Enable FreeMotion adaptive rotation sequence

- Implemented by [email protected]
- Fix zero crossing function: abs(rot[2]) < 1.5

Author: GerhardHippmann

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/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, 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,62 @@ 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
+
+
+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( abs(rot[2]) - 1.5 )
+        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)
         ]
 )