- Fix bugs with respect to Object(..., fixedToParent=false, angularVelocityResolvedInParent=true)

- Fix bugs in some of the newly introduced functions in Frames.jl
- Adapt test models for angularVelocityResolvedInParent=true: Basic/BouncingSphere3D.jl, FreeShaft.jl, FreeShaftAdaptiveRotSequence.jl, Collision/TwoCollidingBalls.jl
- Add tests for Frames.jl

Author: MartinOtter

src/Composition/joints/joints.jl

@@ -89,7 +89,7 @@ mutable struct MultibodyData{F <: Modia3D.VarFloatType, TimeType}
 
             # Define hidden model states and copy initial values into eqInfo
             path = obj.path * "."
-            w_init = freeMotion.wResolvedInParent ? resolve2(freeMotion.rot, freeMotion.w, rotation123=freeMotion.isrot123) : freeMotion.w
+            w_init = freeMotion.wResolvedInParent ? Modia3D.resolve1(freeMotion.rot, freeMotion.w, rotation123=freeMotion.isrot123) : freeMotion.w
             freeMotion.ix_hidden_r     = Modia.newHiddenState!(partiallyInstantiatedModel, path*"translation"    , path*"der(translation)"    , freeMotion.r)
             freeMotion.ix_hidden_v     = Modia.newHiddenState!(partiallyInstantiatedModel, path*"velocity"       , path*"der(velocity)"       , freeMotion.v)
             freeMotion.ix_hidden_rot   = Modia.newHiddenState!(partiallyInstantiatedModel, path*"rotation"       , path*"der(rotation)"       , freeMotion.rot)
@@ -698,25 +698,24 @@ function setStatesHiddenJoints!(m::Modia.SimulationModel{F,TimeType}, mbs::Multi
         j3 = freeMotion.ix_hidden_v  ;  freeMotion.v   = SVector{3,F}(x_hidden[j3], x_hidden[j3+1], x_hidden[j3+2])
         j4 = freeMotion.ix_hidden_w  ;  freeMotion.w   = SVector{3,F}(x_hidden[j4], x_hidden[j4+1], x_hidden[j4+2])
         if freeMotion.wResolvedInParent
-            freeMotion.w = resolve2(freeMotion.rot, freeMotion.w, rotation123 = freeMotion.isrot123)
+            freeMotion.w = Modia3D.resolve2(freeMotion.rot, freeMotion.w, rotation123 = freeMotion.isrot123)
         end
 
-        # Copy some freeMotion state derivatives into der_x_hidden
-            # der(r) = v
-            der_x_hidden[j1]   = freeMotion.v[1]
-            der_x_hidden[j1+1] = freeMotion.v[2]
-            der_x_hidden[j1+2] = freeMotion.v[3]
+        # der(r) = v
+        der_x_hidden[j1]   = freeMotion.v[1]
+        der_x_hidden[j1+1] = freeMotion.v[2]
+        der_x_hidden[j1+2] = freeMotion.v[3]
 
-            if Modia.positive(m, freeMotion.iz_rot2, singularitySafetyMargin(freeMotion.rot[2]), freeMotion.str_rot2, nothing)
-                # freeMotion.rot[2] mapped to range -pi .. pi is either >= 1.5 or <= -1.5 (so comes close to singular position pi/2 = 1.57...)
-                change_rotSequence!(m, freeMotion, _x, x_hidden)
-            end
+        if Modia.positive(m, freeMotion.iz_rot2, singularitySafetyMargin(freeMotion.rot[2]), freeMotion.str_rot2, nothing)
+            # freeMotion.rot[2] mapped to range -pi .. pi is either >= 1.5 or <= -1.5 (so comes close to singular position pi/2 = 1.57...)
+            change_rotSequence!(m, freeMotion, _x, x_hidden)
+        end
 
-            # der(rot) = J123or132(rot,isrot123) * w
-            der_rot = J123or132(freeMotion.rot, freeMotion.isrot123)*freeMotion.w
-            der_x_hidden[j2]   = der_rot[1]
-            der_x_hidden[j2+1] = der_rot[2]
-            der_x_hidden[j2+2] = der_rot[3]
+        # der(rot) = J123or132(rot,isrot123) * w
+        der_rot = J123or132(freeMotion.rot, freeMotion.isrot123)*freeMotion.w
+        der_x_hidden[j2]   = der_rot[1]
+        der_x_hidden[j2+1] = der_rot[2]
+        der_x_hidden[j2+2] = der_rot[3]
     end
     return nothing
 end
@@ -799,7 +798,7 @@ function setHiddenStatesDerivatives!(m::Modia.SimulationModel{F,TimeType}, mbs::
         m.der_x_hidden[j1+2] = freeMotion.a[3]
 
         j2 = freeMotion.ix_hidden_w
-        z = freeMotion.wResolvedInParent ? resolve1(obj.R_rel, freeMotion.z) : freeMotion.z
+        z = freeMotion.wResolvedInParent ? Modia3D.resolve1(obj.R_rel, freeMotion.z) : freeMotion.z
         m.der_x_hidden[j2  ] = z[1]
         m.der_x_hidden[j2+1] = z[2]
         m.der_x_hidden[j2+2] = z[3]

src/Composition/joints/object3DMotion.jl

@@ -46,7 +46,7 @@ Return a `joint` that describes the free movement of `obj2::`[`Object3D`](@ref)
 with respect to `obj1::`[`Object3D`](@ref). The initial position is `r`
 (resolved in `obj1`) and the initial orientation is `rot` in [Cardan (Tait–Bryan) angles](https://en.wikipedia.org/wiki/Euler_angles#Chained_rotations_equivalence)
 (rotation sequence x-y-z from `obj1` to `obj2`). `v` (resolved in `obj1`) and `w`
-(resolved in `obj2`) are the initial cartesian translational and rotational
+(resolved in `obj1`) are the initial cartesian translational and rotational
 velocity vectors.
 """
 mutable struct FreeMotion{F <: Modia3D.VarFloatType} <: Modia3D.AbstractJoint
@@ -75,22 +75,23 @@ mutable struct FreeMotion{F <: Modia3D.VarFloatType} <: Modia3D.AbstractJoint
     iqdd_hidden::Int      # qdd_hidden[iqdd_hidden:iqdd_hidden+5] are the elements of qdd that are stored in [a,z] if hiddenStates
     ix_rot::Int           # startIndex of rot with respect to x-vector
     str_rot2::String      # String to be used for zero crossing logging of iz_rot2
-    wResolvedInParent::Bool  # = true, if w is resolved in obj1 (= parent); = false, if w is resolved in obj2.
+    wResolvedInParent::Bool  # = true: For external interface (w in state, der(w) in der(x)) w is resolved in obj1 (= parent);
+                             # = false: For external interface w is resolved in obj2 (!).
 
     obj1::Modia3D.AbstractObject3D
     obj2::Modia3D.AbstractObject3D
 
     ndof::Int
 
-    r::SVector{3,F}
-    rot::SVector{3,F}        # cardan angles
+    r::SVector{3,F}          # Relative position vector from obj1 to obj2, resolved in obj1
+    rot::SVector{3,F}        # Rotation angles from obj1 to obj2 with rotation sequence isrot123
     isrot123::Bool           # = true: rotation sequence x-y-z, otherwise x-z-y
 
-    v::SVector{3,F}          # = der(r)
-    w::SVector{3,F}          # angular velocity vector
+    v::SVector{3,F}          # = der(r), that is relative velocity of obj2 with respect to obj1, resolved in obj1
+    w::SVector{3,F}          # Relative angular velocity vector of obj2 with respect to obj1, resolved in obj2 (!)
 
-    a::SVector{3,F}          # = der(v)
-    z::SVector{3,F}          # = der(w)
+    a::SVector{3,F}          # = der(v), that is relative acceleration of obj2 with respect to obj1, resolved in obj1
+    z::SVector{3,F}          # = der(w), that is relative angular velocity of obj2 with respect to obj1, resolved in obj2 (!)
 
     function FreeMotion{F}(; obj1::Modia3D.AbstractObject3D,
                              obj2::Modia3D.AbstractObject3D,
@@ -126,13 +127,17 @@ mutable struct FreeMotion{F <: Modia3D.VarFloatType} <: Modia3D.AbstractJoint
         #        "    obj2 has a root, but obj1 has not root. This is currently not supported (exchange obj1 and obj2)!")
         #end
 
+        isrot123 = true
         r   = Modia3D.convertAndStripUnit(SVector{3,F}, u"m", r)
         rot = Modia3D.convertAndStripUnit(SVector{3,F}, u"rad", rot)
         v   = Modia3D.convertAndStripUnit(SVector{3,F}, u"m/s"  , v)
         w   = Modia3D.convertAndStripUnit(SVector{3,F}, u"rad/s", w)
+        if wResolvedInParent
+            # Transform w from obj1 to obj2
+            w = Modia3D.resolve2(rot, w, rotation123=isrot123)
+        end
         a   = Modia3D.ZeroVector3D(F)
         z   = Modia3D.ZeroVector3D(F)
-        isrot123 = true
         str_rot2 = "singularitySafetyMargin(" * path * ".rotation[2])"
 
         obj2.joint      = new(path, hiddenStates, -1, -1, -1, -1, -1, -1, -1, -1, str_rot2, wResolvedInParent, obj1, obj2, 6, r, rot, isrot123, v, w, a, z)

src/Frames/_module.jl

@@ -31,6 +31,7 @@ export qrot1, qrot2, qrot3, qrot123,          qrot_e, qrot_nxy, from_R
 
 export resolve1, resolve2, absoluteRotation, relativeRotation, inverseRotation
 export planarRotationAngle, eAxis
+export angularVelocityResolvedInParentStates!
 
 export Path, t_pathEnd, interpolate, interpolate_r
 export skew

src/Frames/rotationMatrix.jl

@@ -53,7 +53,7 @@ but compute this efficiently by taking the zeros in rot1(..) into account.
 """
 @inline function rot1(angle::Number, v::AbstractVector)
    (s,c) = sincos(angle)
-   SVector{3,typeof(s)}(v[1], v[2]*c + v[3]*s, -v[2]*s + v[3]*c)
+   SVector{3}(v[1], v[2]*c + v[3]*s, -v[2]*s + v[3]*c)
 end
 
 
@@ -81,7 +81,7 @@ but compute this efficiently by taking the zeros in rot2(..) into account.
 """
 @inline function rot2(angle::Number, v::AbstractVector)
    (s,c) = sincos(angle)
-   SVector{3,typeof(s)}(v[1]*c - v[3]*s, v[2], v[1]*s + v[3]*c)
+   SVector{3}(v[1]*c - v[3]*s, v[2], v[1]*s + v[3]*c)
 end
 
 
@@ -101,15 +101,15 @@ end
 
 
 """
-    Modia3D.rot3(angle::F,v::AbstractVector) where F
+    Modia3D.rot3(angle,v::AbstractVector)
 
 Return in principal Modia3D.rot3(angle)*v,
 but compute this efficiently by taking the zeros in rot3(..) into account.
 `angle` can be provided in radian or in degree, e.g. `using Unitful; Modia3D.rot3(90u"°", v)`.
 """
 @inline function rot3(angle::Number, v::AbstractVector)
    (s,c) = sincos(angle)
-   SVector{3,typeof(s)}(c*v[1] + s*v[2], -v[1]*s + v[2]*c, v[3])
+   SVector{3}(c*v[1] + s*v[2], -v[1]*s + v[2]*c, v[3])
 end
 
 
@@ -209,13 +209,15 @@ vector `v1::SVector{3,F}` (vector resolved in frame 1) given
 
 - Rotation matrix `R::SMatrix{3,3,F,9}` (rotate frame 1 into frame 2) or
 - Quaternion vector `q::SVector{4,F}` (rotate frame 1 into frame 2) or
-- Angles vector `rotation::SVector{3,F}` (= rotation123 ? [angleX, angleY, angleZ] : [angleX, angleZ, angleY]).
+- Angles vector `rotation::AbstractVector`
+  (rotation123=true: rotate around X with rotation[1], around Y with rotation[2], around Z with rotation[3];
+   rotation123=false: rotate around X with rotation[1], around Z with rotation[2], around Y with rotation[3]).
 """
 resolve1(R::SMatrix{3,3,F,9}, v2::SVector{3,F})   where F <: Modia3D.VarFloatType = R'*v2
 resolve1(R::SMatrix{3,3,F,9}, v2::AbstractVector) where F <: Modia3D.VarFloatType = R'*SVector{3,F}(v2)
-resolve1(rotation::SVector{3,F}, v2::AbstractVector; rotation123=true) where F <: Modia3D.VarFloatType =
-    rotation123 ? rot1(rotation[1], rot2(rotation[2], rot3(rotation[3],v2))) :
-                  rot1(rotation[1], rot3(rotation[3], rot2(rotation[2],v2)))
+resolve1(rotation::AbstractVector, v2::AbstractVector; rotation123=true) where F <: Modia3D.VarFloatType =
+    rotation123 ? rot1(-rotation[1], rot2(-rotation[2], rot3(-rotation[3],v2))) :
+                  rot1(-rotation[1], rot3(-rotation[2], rot2(-rotation[3],v2)))
 
 
 """
@@ -228,13 +230,27 @@ vector `v2::SVector{3,F}` (vector resolved in frame 2) given
 
 - Rotation matrix `R::SMatrix{3,3,F,9}` (rotate frame 1 into frame 2) or
 - Quaternion vector `q::SVector{4,F}` (rotate frame 1 into frame 2) or
-- Angles vector `rotation::SVector{3,F}` (= rotation123 ? [angleX, angleY, angleZ] : [angleX, angleZ, angleY]).
+- Angles vector `rotation::AbstractVector`
+  (rotation123=true: rotate around X with rotation[1], around Y with rotation[2], around Z with rotation[3];
+   rotation123=false: rotate around X with rotation[1], around Z with rotation[2], around Y with rotation[3]).
 """
 resolve2(R::SMatrix{3,3,F,9}, v1::SVector{3,F})    where F <: Modia3D.VarFloatType = R*v1
 resolve2(R::SMatrix{3,3,F,9}, v1::AbstractVector)  where F <: Modia3D.VarFloatType = R*SVector{3,F}(v1)
-resolve2(rotation::SVector{3,F}, v1::AbstractVector; rotation123=true) where F <: Modia3D.VarFloatType =
+resolve2(rotation::AbstractVector, v1::AbstractVector; rotation123=true) where F <: Modia3D.VarFloatType =
     rotation123 ? rot3(rotation[3], rot2(rotation[2], rot1(rotation[1], v1))) :
-                  rot2(rotation[2], rot3(rotation[3], rot1(rotation[1], v1)))
+                  rot2(rotation[3], rot3(rotation[2], rot1(rotation[1], v1)))
+
+
+"""
+    Modia3D.angularVelocityResolvedInParentStates!(states, startIndexRotation, rotation123)
+"""
+function angularVelocityResolvedInParentStates!(states::AbstractVector, startIndexRotation::Int, rotation123::Bool)::Nothing
+    rotation = states[startIndexRotation:startIndexRotation+2]
+    w2       = states[startIndexRotation+3:startIndexRotation+5]
+    states[startIndexRotation+3:startIndexRotation+5] = Modia3D.resolve1(rotation, w2, rotation123=rotation123)
+    return nothing
+end
+
 
 
 """

test/Basic/BouncingSphere3D.jl

@@ -9,7 +9,7 @@ BouncingSphere = Model3D(
                                        visualMaterial=VisualMaterial(color="DarkGreen"),
                                        solidMaterial="Steel",
                                        collision=true)),
-    sphere    = Object3D(parent=:world, fixedToParent=false, translation=[0.0, 1.0, 0.0],
+    sphere    = Object3D(parent=:world, fixedToParent=false, translation=[0.0, 1.0, 0.0], angularVelocityResolvedInParent=true,
                          feature=Solid(shape=Sphere(diameter=0.2),
                                        visualMaterial=VisualMaterial(color="Blue"),
                                        solidMaterial="Steel",
@@ -18,7 +18,9 @@ BouncingSphere = Model3D(
 )
 
 bouncingSphere = @instantiateModel(BouncingSphere, unitless=true, logCode=true)
-simulate!(bouncingSphere, stopTime=2.2, dtmax=0.1, log=true, logStates=true)
+
+requiredFinalStates = [-1.1158498420134682e-7, 0.09999999283736953, 1.9605796328352922e-10, -1.3226335283305713e-12, 3.726293803759173e-13, 6.442335795625085e-16, -4.97188563948566e-9, 6.3453729183502065e-15, -2.8482188606342696e-6, 6.351235882032115e-15, -4.677163261435682e-20, 1.3039308706792498e-11]
+simulate!(bouncingSphere, stopTime=2.2, dtmax=0.1, log=true, logStates=true, requiredFinalStates=requiredFinalStates)
 printResultInfo(bouncingSphere)
 
 @usingModiaPlot

test/Basic/FreeShaft.jl

@@ -9,7 +9,7 @@ Shaft = Model(
     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(parent=:world, fixedToParent=false,
+    shaft = Object3D(parent=:world, fixedToParent=false, angularVelocityResolvedInParent=true,
                                    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))),
@@ -18,7 +18,7 @@ Shaft = Model(
 model = Model3D(
     shaft = Shaft | Map(Length=1.0, Diameter=0.2, shaft = Map(velocity=[0.0, 0.1, 0.6],
                                                               rotation=[30, 20, 10]u"°",
-                                                              angularVelocity=[1.0, 2.0, 3.0], angularVelocityResolvedInParent=false)
+                                                              angularVelocity=Modia3D.resolve1([30, 20, 10]u"°",[1.0, 2.0, 3.0]), angularVelocityResolvedInParent=true)
                        )
 )
 #@showModel model
@@ -29,6 +29,7 @@ shaft = @instantiateModel(model, unitless=true, log=false, logStateSelection=fal
 
 stopTime = 5.0
 requiredFinalStates=[-1.7224720653038268e-14, 0.4999999999999575, -0.750000016852883, -1.5681975307975595e-14, 0.0999999999999862, -0.8999999999999555, 12.518049838490617, -0.9018643787079454, 13.979844144766544, 2.0096818051930816, -0.9803470582617598, 2.9999999999999933]
+Modia3D.angularVelocityResolvedInParentStates!(requiredFinalStates, 7, true)
 simulate!(shaft, stopTime=stopTime, log=true, logStates=false, requiredFinalStates=requiredFinalStates)
 
 @usingModiaPlot

test/Basic/FreeShaftAdaptiveRotSequence.jl

@@ -13,23 +13,25 @@ Shaft = Model3D(
     shaft = Object3D(parent = :world, fixedToParent = false,
                      rotation=[30.0, 90.0, 10.0]u"°",
                      velocity=[0.0, 0.1, 0.0]u"m/s",
-                     angularVelocity=[-2.0, 0.0, 0.0]u"rad/s", angularVelocityResolvedInParent=false,
+                     angularVelocity=Modia3D.resolve1([30.0, 90.0, 10.0]u"°", [-2.0, 0.0, 0.0]u"rad/s"), angularVelocityResolvedInParent=true,
                      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))),
 )
 
+
 shaft = @instantiateModel(Shaft, unitless=true, log=false, logStateSelection=false, logCode=false)
 
 stopTime = 7.0
 dtmax = 0.1
 #requiredFinalStates = [0.0, 0.7, -7.350074420637136, 0.6981317007977381, 1.4336293856408397, 1.5707963267949017, 0.0, 0.1, -2.1, -2.0, 0.0, 0.0]
 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=false, logStates=false, requiredFinalStates=requiredFinalStates)
+Modia3D.angularVelocityResolvedInParentStates!(requiredFinalStates, 7, false)
+simulate!(shaft, stopTime=stopTime, dtmax=dtmax, log=true, logEvents=true, logStates=false, requiredFinalStates=requiredFinalStates)
 #printResultInfo(shaft)
 
 @usingModiaPlot
-plot(shaft, ["shaft.rotation", "shaft.rotation123", "shaft.angularVelocity", "shaft.translation", "shaft.velocity"], figure=1)
+plot(shaft, ["shaft.rotation", "shaft.rotation123", "shaft.angularVelocity", "shaft.translation", "shaft.velocity"], figure=2)
 
 end

test/Basic/Pendulum.jl

@@ -10,7 +10,8 @@ Pendulum = Model3D(
 )
 
 pendulum = @instantiateModel(Pendulum, unitless=true, log=false, logDetails=false, logCode=true, logStateSelection=false, logCalculations=false)
-simulate!(pendulum, stopTime=3.0, log=true)
+requiredFinalStates = [5.970529829666215, -1.1395482781332746]
+simulate!(pendulum, stopTime=3.0, log=true, requiredFinalStates = requiredFinalStates)
 
 @usingModiaPlot
 plot(pendulum, "rev.phi")

test/Collision/TwoCollidingBalls.jl

@@ -31,15 +31,15 @@ collidingBalls = Model3D(
     ball1 = Object3D(parent=:world, fixedToParent=false,
                      translation = [-1.3, 0.0, diameter/2],
                      velocity    = [3.0, 0.0, 0.0],
-                     rotation    = [pi/2, 0.0, 0.0],
+                     rotation    = [pi/2, 0.0, 0.0], angularVelocityResolvedInParent=true,
                      feature=Solid(shape=Sphere(diameter=diameter),
                                    solidMaterial="BilliardBall",
                                    visualMaterial=vmatSolids,
                                    collision=true)),
 
     ball2 = Object3D(parent=:world, fixedToParent=false,
                      translation = [0.0, 0.0, diameter/2],
-                     rotation    = [pi/2, 0.0, 0.0],
+                     rotation    = [pi/2, 0.0, 0.0], angularVelocityResolvedInParent=true,
                      feature=Solid(shape=Sphere(diameter=diameter),
                                    solidMaterial="BilliardBall",
                                    visualMaterial=vmatSolids,
@@ -50,10 +50,14 @@ twoCollidingBalls = @instantiateModel(collidingBalls, unitless=true, log=false,
 
 stopTime = 1.5
 tolerance = 1e-6
+dtmax = 0.1
 requiredFinalStates = [0.4489974852019629, 4.971482124288075e-6, 0.029996861274764766, 0.5336096206429409, 4.775435383850775e-6, -8.566849073992856e-7, 1.5708013764495665, 1.2298784775382833e-5, -55.4441088791202, -6.408457808447407e-5, -9.894396552324208e-5, -17.78647176388158, 1.4032405177409832, -4.464837913922324e-6, 0.029997744288682248, 1.4383823207541608, -4.574293232521244e-6, -2.3103945635420407e-6, 1.570796533109225, 1.2115305686199998e-8, -44.07171252097457, 0.00015344214722251976, 2.5704782290946986e-6, -47.9463685248874]
-simulate!(twoCollidingBalls, stopTime=stopTime, tolerance=tolerance, log=true, logStates=false, logEvents=false, requiredFinalStates=requiredFinalStates)
+Modia3D.angularVelocityResolvedInParentStates!(requiredFinalStates,  7, true)
+Modia3D.angularVelocityResolvedInParentStates!(requiredFinalStates, 19, true)
+simulate!(twoCollidingBalls, stopTime=stopTime, tolerance=tolerance, dtmax=dtmax, log=true, logStates=true, logEvents=true, requiredFinalStates=requiredFinalStates)
 
 @usingModiaPlot
 plot(twoCollidingBalls, ["ball1.translation" "ball1.rotation"; "ball1.velocity" "ball1.angularVelocity"], figure=1)
+plot(twoCollidingBalls, ["ball2.translation" "ball2.rotation"; "ball2.velocity" "ball2.angularVelocity"], figure=2)
 
 end