Merge pull request #41 from ModiaSim/gh_freeMotionAdaptiveRotationSequence

Gh free motion adaptive rotation sequence

Author: AndreaNeumayr

src/Composition/dynamics.jl

@@ -93,7 +93,7 @@ struct MultibodyData{FloatType}
     jointObjects::Vector{Object3D}  # References to Object3Ds that have a joint
     jointStartIndex::Vector{Int}    # Start index of joint in qdd
     jointNdof::Vector{Int}          # Number-of-degrees-of-freedom of joint
-    zStartIndex::Int                # eventHandler.z[zStartIndex] is first index of crossing function
+    zStartIndex::Int                # eventHandler.z[zStartIndex] is first index of crossing functions for contact detection
                                     # (or zero, if nableContactDetection=false)
     nz::Int                         # Number of used zero crossing functions
     residuals::Vector{FloatType}    # Residuals - length(residuals) = nqdd
@@ -141,6 +141,7 @@ function setModiaJointVariables!(id::Int, _leq_mode, instantiatedModel::ModiaLan
             if scene.options.enableContactDetection
                 nz = 2
                 zStartIndex = ModiaLang.addZeroCrossings(instantiatedModel, nz)
+                scene.zStartIndex = zStartIndex
             else
                 nz = 0
                 zStartIndex = 0

src/Composition/joints/joints.jl

@@ -314,7 +314,7 @@ function computeKinematics!(scene::Scene, tree::Vector{Object3D}, time)::Nothing
             freeMotion  = scene.freeMotion[obj.jointIndex]
 
             obj.r_rel = freeMotion.r
-            obj.R_rel = Rfromrot123(freeMotion.rot)
+            obj.R_rel = freeMotion.isrot123 ? Rfromrot123(freeMotion.rot) : Rfromrot132(freeMotion.rot)
 
             obj.r_abs = obj.r_rel
             obj.R_abs = obj.R_rel
@@ -329,7 +329,7 @@ function computeKinematics!(scene::Scene, tree::Vector{Object3D}, time)::Nothing
             freeMotion  = scene.freeMotion[obj.jointIndex]
 
             obj.r_rel = freeMotion.r
-            obj.R_rel = Rfromrot123(freeMotion.rot)
+            obj.R_rel = freeMotion.isrot123 ? Rfromrot123(freeMotion.rot) : Rfromrot132(freeMotion.rot)
 
             obj.r_abs = parent.r_abs + parent.R_abs'*obj.r_rel
             obj.R_abs = obj.R_rel*parent.R_abs
@@ -489,7 +489,8 @@ function setJointVariables_q_qd_f!(scene::Scene, objects::Vector{Object3D}, star
             freeMotion.rot = SVector{3,Float64}(args_i[2])
             freeMotion.v   = SVector{3,Float64}(args_i[3])
             freeMotion.w   = SVector{3,Float64}(args_i[4])
-            
+            freeMotion.isrot123 = args_i[5]
+
         else
            error("Bug in Modia3D/src/Composition/specifics/specifics.jl (setJointVariables!): jointKind = $jointKind is not known")
         end

src/Composition/joints/object3DMotion.jl

@@ -32,7 +32,8 @@ mutable struct FreeMotion <: Modia3D.AbstractJoint
     ndof::Int
 
     r::SVector{3,Float64}
-    rot::SVector{3,Float64}        # cardan angles (rotation sequence x-y-z)
+    rot::SVector{3,Float64}        # cardan angles
+    isrot123::Bool                 # = true: rotation sequence x-y-z, otherwise x-z-y
 
     v::SVector{3,Float64}
     w::SVector{3,Float64}          # angular velocity vector
@@ -49,7 +50,8 @@ mutable struct FreeMotion <: Modia3D.AbstractJoint
                           r::AbstractVector   = Modia3D.ZeroVector3D,
                           rot::AbstractVector = Modia3D.ZeroVector3D,
                           v::AbstractVector   = Modia3D.ZeroVector3D,
-                          w::AbstractVector   = Modia3D.ZeroVector3D)
+                          w::AbstractVector   = Modia3D.ZeroVector3D,
+                          next_isrot123::Bool = true)  # dummy argument, is ignored
 
         #(parent,obj,cutJoint) = attach(obj1, obj2)
         #if cutJoint
@@ -68,18 +70,19 @@ mutable struct FreeMotion <: Modia3D.AbstractJoint
         w   = Modia3D.convertAndStripUnit(SVector{3,Float64}, u"rad/s", w)
         a   = Modia3D.ZeroVector3D
         z   = Modia3D.ZeroVector3D
+        isrot123 = true    # next_isrot123 is ignored.
 
         residue_f = Modia3D.ZeroVector3D
         residue_t = Modia3D.ZeroVector3D
 
-        obj2.joint      = new(path, obj1, obj2, 6, r, rot, v, w, a, z, residue_f, residue_t)
+        obj2.joint      = new(path, obj1, obj2, 6, r, rot, isrot123, v, w, a, z, residue_f, residue_t)
         obj2.parent     = obj1
         obj2.jointKind  = FreeMotionKind
         obj2.jointIndex = 0
         obj2.ndof       = 6
         obj2.canCollide = true
         obj2.r_rel      = r
-        obj2.R_rel      = Rfromrot123(rot)
+        obj2.R_rel      = isrot123 ? Rfromrot123(rot) : Rfromrot132(rot)
 
         push!(obj1.children, obj2)
 

src/Composition/scene.jl

@@ -405,7 +405,8 @@ mutable struct Scene <: Modia3D.AbstractScene
     noCPairs::Vector{Vector{Int64}}           # Indices of frames (with respect to collSuperObjs) that can't collide in general (e.g. objects are connected via joints)
     noCPairsHelp::Dict{Modia3D.AbstractJoint,Vector{Int64}}
     allowedToMove::Vector{Union{Bool,Nothing}}
-    AABB::Vector{Vector{Basics.BoundingBox}}    # Bounding boxes of elements that can collide
+    AABB::Vector{Vector{Basics.BoundingBox}}  # Bounding boxes of elements that can collide
+    zStartIndex::Int                          # start index of collision zero crossing functions
     #forceElements::Vector{Int64}
     exportAnimation::Bool                     # animation file export is enabled
     animation::Vector{animationStep}          # animation data of visible Object3Ds
@@ -495,6 +496,7 @@ mutable struct Scene <: Modia3D.AbstractScene
             Dict{Modia3D.AbstractJoint,Vector{Int64}}(),
             Vector{Union{Bool}}[],
             Vector{Vector{Basics.BoundingBox}}[],
+            1,
             exportAnimation,
             Vector{animationStep}[],
             0,

src/ModiaInterface/_module.jl

@@ -16,7 +16,7 @@ export multibodyResiduals!, setModiaJointVariables!
 export Fix
 export Revolute, RevoluteWithFlange
 export Prismatic, PrismaticWithFlange
-export J123, J132, FreeMotion
+export J123, J132, J123or132, singularRem, FreeMotion, change_rotSequenceInNextIteration!
 
 export buildModia3D
 

src/ModiaInterface/buildModia3D.jl

@@ -66,6 +66,10 @@ function getJointInfo!(model, jointInfo, path)::Nothing
         constructor = model[:_constructor]
         if typeof(constructor) <: OrderedDict && haskey(constructor, :ndof)
             ndof = constructor[:ndof]
+            if haskey(model, :_rotName)
+                # Hack to provide the full path name of FreeMotion.rot to the FreeMotion object
+                model[:_rotName] = string(path)*".rot"
+            end
             push!(jointInfo, (path=path, ndof=ndof))
             return
         end

src/ModiaInterface/model3D.jl

@@ -131,18 +131,63 @@ function J132(rot132::AbstractVector)
 
 end
 
+
+
+"""
+    next_isrot123 = change_rotSequenceInNextIteration!(rot::AbstractVector, isrot123::Bool, instantiatedModel::SimulationModel, x, rot_name)
+
+Change rotation sequence of `rot` from `x-axis, y-axis, z-axis` to `x-axis, z-axis, y-axis` or visa versa in the next event iteration:
+
+- If `isrot123 = true`, return `next_isrot123 = false` and `x[..] = rot132fromR(Rfromrot123(rot))`
+
+- If `isrot123 = false`, return `next_isrot123 = true` and `x[..] = rot123fromR(Rfromrot132(rot))`
+"""
+function change_rotSequenceInNextIteration!(rot::AbstractVector, isrot123::Bool, instantiatedModel::SimulationModel, x, rot_name)::Bool
+    if isrot123
+        #println("        switch $rot_name 123 -> 132")
+        next_rot      = Modia3D.rot132fromR(Modia3D.Rfromrot123(rot))
+        next_isrot123 = false
+    else
+        #println("        switch $rot_name 132 -> 123")
+        next_rot      = Modia3D.rot123fromR(Modia3D.Rfromrot132(rot))
+        next_isrot123 = true
+    end
+
+    # Change x-vector with the next_rot values
+    eqInfo = instantiatedModel.equationInfo
+    startIndex = eqInfo.x_info[ eqInfo.x_dict[rot_name] ].startIndex
+    x[startIndex]   = next_rot[1]
+    x[startIndex+1] = next_rot[2]
+    x[startIndex+2] = next_rot[3]
+    return next_isrot123
+end
+
+
+singularRem(ang) = abs(rem2pi(ang, RoundNearest)) - 1.5  # is negative/positive in valid/singular angle range
+J123or132(rot, isrot123) = isrot123 ? J123(rot) : J132(rot)
+
+
 FreeMotion(; obj1, obj2, r=Var(init=zeros(3)), rot=Var(init=zeros(3)), v=Var(init=zeros(3)), w=Var(init=zeros(3))) = Model(; _constructor = Par(value = :(Modia3D.FreeMotion), _path = true, ndof = 6),
     obj1 = Par(value = obj1),
     obj2 = Par(value = obj2),
     r    = r,
     rot  = rot,
     v    = v,
     w    = w,
+
+    next_isrot123 = Var(start=true),
+    _rotName = "???",  # is changed by buildModia3D to the full path name of "rot"
+
     equations = :[
-        der(r)   = v
-        der(rot) = J123(rot) * w
+        der(r) = v
+
+        isrot123 = pre(next_isrot123)
+        rot2_singularity = positive(singularRem(rot[2]))
+        next_isrot123 = if rot2_singularity; change_rotSequenceInNextIteration!(rot, isrot123, instantiatedModel, _x, _rotName) else isrot123 end
+        der(rot) = J123or132(rot,isrot123) * w
+
         der(v) = qdd[1:3]
         der(w) = qdd[4:6]
-        variables = getVariables(r, rot, v, w)
+        variables = getVariables(r, rot, v, w, isrot123)
         ]
 )

src/contactDetection/ContactDetectionMPR/handler.jl

@@ -74,14 +74,14 @@ function selectContactPairs!(sim, scene::Composition.Scene, ch::Composition.Cont
         # z[1] ... zero crossing function from contact to no contact
         # max. distance (= min. penetration) of active contact pairs
         if isempty(ch.contactDict)
-            @inbounds simh.z[1] = -42.0
+            simh.z[scene.zStartIndex] = -42.0
         else
             (pair, key)  = findmax(ch.contactDict)
-            @inbounds simh.z[1] = pair.distanceWithHysteresis
+            simh.z[scene.zStartIndex] = pair.distanceWithHysteresis
         end
         # z[2] ... zero crossing function from no contact to contact
         # min. distance of inactive contact pairs
-        @inbounds simh.z[2] = ch.noContactMinVal
+        simh.z[1+scene.zStartIndex] = ch.noContactMinVal
     end
     ch.distanceComputed = true
     return nothing

test/Basic/ShaftFreeMotionAdaptiveRotSequence.jl

@@ -0,0 +1,40 @@
+module ShaftFreeMotionAdaptiveRotSequence
+
+using ModiaLang
+
+import Modia3D
+using  Modia3D.ModiaInterface
+
+Shaft = Model(
+    Length = 1.0,
+    Diameter = 0.2,
+    visualMaterial = VisualMaterial(color="IndianRed1", transparency=0.5),
+    world = Object3D(feature=Scene(gravityField=UniformGravityField(g=0.3, n=[0, 0, -1]))),
+    worldFrame = Object3D(parent=:world,
+                          feature=Visual(shape=CoordinateSystem(length=:Diameter))),
+    shaft = Object3D(feature=Solid(shape=Cylinder(axis=3, diameter=:Diameter, length=:Length),
+                                   massProperties=MassProperties(; mass=84.7154, Ixx=7.2711, Iyy=7.2711, Izz=0.4230),
+                                   visualMaterial=:(visualMaterial))),
+    shaftFrame = Object3D(parent=:shaft, feature=Visual(shape=CoordinateSystem(length=0.4))),
+    free = FreeMotion(obj1=:world, obj2=:shaft, v   = Var(init = [0.0, 0.1, 0.0]),
+                                                rot = Var(start = [deg2rad(30), deg2rad(90), deg2rad(10)]),
+                                                w   = Var(init = [-2.0, 0.0, 0.0]))
+)
+
+#@showModel model
+
+shaft = @instantiateModel(buildModia3D(Shaft), aliasReduction=false, unitless=true, log=false, logStateSelection=false, logCode=true)
+
+#@showModel shaft.p[1]
+
+stopTime = 7.0
+dtmax = 0.1
+requiredFinalStates = [0.0, 0.7, -7.350074420637136, 0.0, 0.1, -2.1, 0.6981317007977381, 1.4336293856408397, 1.5707963267949017, -2.0, 0.0, 0.0]
+simulate!(shaft, stopTime=stopTime, dtmax=dtmax, log=true, logEvents=true, logStates=true, requiredFinalStates=requiredFinalStates)
+
+@usingModiaPlot
+plot(shaft, "free.rot", figure=1)
+plot(shaft, ["free.r", "free.v", "free.w"], figure=2)
+plot(shaft, "free.isrot123", figure=3)
+
+end

test/Collision/BouncingCapsules.jl

@@ -71,7 +71,7 @@ bouncingCapsules = @instantiateModel(buildModia3D(BouncingCapsules), unitless=tr
 
 stopTime = 1.7
 tolerance = 1e-8
-requiredFinalStates = [-0.7964014792034407, 1.078911579441921, 0.23035036015771998, -0.10969534535624982, 0.4797449365169936, -0.08174820851332286, 4.48271590323064, 1.340697608488989, -1.5392203655926255, 2.4243895450782937, 0.15531689730526202, -0.03915705292205236, 0.22862079147109046, -0.7970862046473598, 1.0806110815754162, -0.06460242891125959, -0.11431031161481991, 0.4851429973131076, 3.385569081576411, 0.1916283718909546, -1.6236830622142888, -0.06474401604033102, 2.4344556240272657, 0.2553659880626085, 1.0818397597553406, 0.22945140558263866, -0.7966137267737096, 0.4853616788402741, -0.07085422346062514, -0.09972138522555073, -1.5640086933709134, 0.19867779732620508, 3.386466331485274, 0.21808527847860343, -0.05743721435841237, 2.4419014448796816]
+requiredFinalStates = [-0.7964020579786201, 1.078911664590739, 0.23035071499249554, -0.11077716509170218, 0.4796475444580211, -0.08226573462808072, 2.944349048428646, -0.22998195076361025, 1.5634033775705642, 2.4243768357473545, 0.1525224150890733, -0.038478509902694046, 0.22862096684511138, -0.7970870633627486, 1.080611113333237, -0.06458397074339108, -0.11429374835354875, 0.4851467302974838, 3.3855692118273724, 0.19162868836235442, -1.6236815609521178, -0.06476968434945787, 2.4344563197987243, 0.25547076791303613, 1.0818397999295593, 0.22945161590171845, -0.7966147351633047, 0.4853706855582012, -0.07080711655835899, -0.09965214358471511, -1.5640102628973194, 0.198677718499869, 3.386466629188658, 0.2183469805758011, -0.05750372064225383, 2.441900102029629]
 simulate!(bouncingCapsules, stopTime=stopTime, tolerance=tolerance, log=true, logStates=true, logEvents=true, requiredFinalStates=requiredFinalStates)
 
 @usingModiaPlot