Adapt to Modia (branch mo_states_in_model_functions)

Author: MartinOtter

Manifest.toml

@@ -128,7 +128,7 @@ function instantiateModel3D!(partiallyInstantiatedModel::Modia.SimulationModel{F
     scene.forceElements = forceElements
     if scene.options.enableContactDetection && scene.collide
         nz = 2
-        zStartIndex = Modia.newZeroCrossings(partiallyInstantiatedModel, nz)
+        zStartIndex = Modia.new_z_segmented_variable!(partiallyInstantiatedModel, nz)
         scene.zStartIndex = zStartIndex
     else
         nz = 0

src/Composition/dynamics.jl

@@ -90,15 +90,15 @@ mutable struct MultibodyData{F <: Modia3D.VarFloatType, TimeType}
             # Define hidden model states and copy initial values into eqInfo
             path = obj.path * "."
             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)
-            freeMotion.ix_hidden_w     = Modia.newHiddenState!(partiallyInstantiatedModel, path*"angularVelocity", path*"der(angularVelocity)", w_init)
-            freeMotion.iextra_isrot123 = Modia.newExtraResult!(partiallyInstantiatedModel, path*"rotation123", freeMotion.isrot123)
-            freeMotion.ix_rot          = Modia.getStateStartIndexFromHiddenStateStartIndex(partiallyInstantiatedModel,freeMotion.ix_hidden_rot)
+            freeMotion.ix_segmented_r     = Modia.new_x_segmented_variable!(partiallyInstantiatedModel, path*"translation"    , path*"der(translation)"    , freeMotion.r)
+            freeMotion.ix_segmented_v     = Modia.new_x_segmented_variable!(partiallyInstantiatedModel, path*"velocity"       , path*"der(velocity)"       , freeMotion.v)
+            freeMotion.ix_segmented_rot   = Modia.new_x_segmented_variable!(partiallyInstantiatedModel, path*"rotation"       , path*"der(rotation)"       , freeMotion.rot)
+            freeMotion.ix_segmented_w     = Modia.new_x_segmented_variable!(partiallyInstantiatedModel, path*"angularVelocity", path*"der(angularVelocity)", w_init)
+            freeMotion.iextra_isrot123 = Modia.new_w_segmented_variable!(partiallyInstantiatedModel, path*"rotation123", freeMotion.isrot123)
+            freeMotion.ix_rot          = Modia.get_x_startIndex_from_x_segmented_startIndex(partiallyInstantiatedModel,freeMotion.ix_segmented_rot)
 
             # Define event indicator to monitor changing sequence of rotation angles
-            freeMotion.iz_rot2 = Modia.newZeroCrossings(partiallyInstantiatedModel, 1)
+            freeMotion.iz_rot2 = Modia.new_z_segmented_variable!(partiallyInstantiatedModel, 1)
         end
 
         for obj in scene.treeForComputation
@@ -631,15 +631,15 @@ singularitySafetyMargin(ang) = abs(rem2pi(ang, RoundNearest)) - 1.5  # is negati
 
 
 """
-    change_rotSequence!(instantiatedModel, freeMotion::FreeMotion, x::AbstractVector, x_hidden::AbstractVector)
+    change_rotSequence!(instantiatedModel, freeMotion::FreeMotion, x::AbstractVector, x_segmented::AbstractVector)
 
 Change rotation sequence of `freeMotion.rot` from `x-axis, y-axis, z-axis` to `x-axis, z-axis, y-axis` or visa versa:
 
-- If `freeMotion.isrot123 = true` , set `freeMotion.isrot123 = false` and `x[..] = x_hidden[..] = rot132fromR(Rfromrot123(freeMotion.rot))`
+- If `freeMotion.isrot123 = true` , set `freeMotion.isrot123 = false` and `x[..] = x_segmented[..] = rot132fromR(Rfromrot123(freeMotion.rot))`
 
-- If `freeMotion.isrot123 = false`, set `freeMotion.isrot123 = true`  and `x[..] = x_hidden[..] = rot123fromR(Rfromrot132(freeMotion.rot))`
+- If `freeMotion.isrot123 = false`, set `freeMotion.isrot123 = true`  and `x[..] = x_segmented[..] = rot123fromR(Rfromrot132(freeMotion.rot))`
 """
-function change_rotSequence!(m::Modia.SimulationModel, freeMotion::FreeMotion, x::AbstractVector, x_hidden::AbstractVector)::Nothing
+function change_rotSequence!(m::Modia.SimulationModel, freeMotion::FreeMotion, x::AbstractVector, x_segmented::AbstractVector)::Nothing
     if freeMotion.isrot123
         freeMotion.rot      = rot132fromR(Rfromrot123(freeMotion.rot))
         freeMotion.isrot123 = false
@@ -650,10 +650,10 @@ function change_rotSequence!(m::Modia.SimulationModel, freeMotion::FreeMotion, x
     Modia.addExtraResult!(m, freeMotion.iextra_isrot123, freeMotion.isrot123)
 
     # Change x-vector
-    startIndex = freeMotion.ix_hidden_rot
-    x_hidden[startIndex  ] = freeMotion.rot[1]
-    x_hidden[startIndex+1] = freeMotion.rot[2]
-    x_hidden[startIndex+2] = freeMotion.rot[3]
+    startIndex = freeMotion.ix_segmented_rot
+    x_segmented[startIndex  ] = freeMotion.rot[1]
+    x_segmented[startIndex+1] = freeMotion.rot[2]
+    x_segmented[startIndex+2] = freeMotion.rot[3]
 
     startIndex = freeMotion.ix_rot
     x[startIndex]   = freeMotion.rot[1]
@@ -679,42 +679,42 @@ end
 """
     setStatesHiddenJoints!(instantiatedModel::Modia.SimulationModel, mbs::MultibodyData, x)
 
-Copy states from the hidden state vector instantiatedModel.x_hidden to the hidden joints into the corresponding Object3Ds
-and copy some state derivatives into instantiatedModel.der_x_hidden.
+Copy states from the hidden state vector instantiatedModel.x_segmented to the hidden joints into the corresponding Object3Ds
+and copy some state derivatives into instantiatedModel.der_x_segmented.
 """
 function setStatesHiddenJoints!(m::Modia.SimulationModel{F,TimeType}, mbs::MultibodyData{F,TimeType}, _x)::Nothing where {F,TimeType}
-    x_hidden     = m.x_hidden
-    der_x_hidden = m.der_x_hidden
+    x_segmented     = m.x_segmented
+    der_x_segmented = m.der_x_segmented
     j1::Int = 0
     j2::Int = 0
     j3::Int = 0
     j4::Int = 0
     for obj in mbs.hiddenJointObjects
-        # Copy x_hidden states into freeMotion states
+        # Copy x_segmented states into freeMotion states
         freeMotion = mbs.freeMotion[obj.jointIndex]
-        j1 = freeMotion.ix_hidden_r  ;  freeMotion.r   = SVector{3,F}(x_hidden[j1], x_hidden[j1+1], x_hidden[j1+2])
-        j2 = freeMotion.ix_hidden_rot;  freeMotion.rot = SVector{3,F}(x_hidden[j2], x_hidden[j2+1], x_hidden[j2+2])
-        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])
+        j1 = freeMotion.ix_segmented_r  ;  freeMotion.r   = SVector{3,F}(x_segmented[j1], x_segmented[j1+1], x_segmented[j1+2])
+        j2 = freeMotion.ix_segmented_rot;  freeMotion.rot = SVector{3,F}(x_segmented[j2], x_segmented[j2+1], x_segmented[j2+2])
+        j3 = freeMotion.ix_segmented_v  ;  freeMotion.v   = SVector{3,F}(x_segmented[j3], x_segmented[j3+1], x_segmented[j3+2])
+        j4 = freeMotion.ix_segmented_w  ;  freeMotion.w   = SVector{3,F}(x_segmented[j4], x_segmented[j4+1], x_segmented[j4+2])
         if freeMotion.wResolvedInParent
             freeMotion.w = Modia3D.resolve2(freeMotion.rot, freeMotion.w, rotation123 = freeMotion.isrot123)
         end
 
         # 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_x_segmented[j1]   = freeMotion.v[1]
+        der_x_segmented[j1+1] = freeMotion.v[2]
+        der_x_segmented[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)
+            change_rotSequence!(m, freeMotion, _x, x_segmented)
         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_x_segmented[j2]   = der_rot[1]
+        der_x_segmented[j2+1] = der_rot[2]
+        der_x_segmented[j2+2] = der_rot[3]
     end
     return nothing
 end
@@ -785,24 +785,24 @@ end
 """
     setHiddenStatesDerivatives!(instantiatedModel, mbs)
 
-Copy derivatives of hidden states to instantiatedModel.der_x_hidden
+Copy derivatives of hidden states to instantiatedModel.der_x_segmented
 """
 function setHiddenStatesDerivatives!(m::Modia.SimulationModel{F,TimeType}, mbs::MultibodyData{F,TimeType})::Nothing where {F,TimeType}
     for obj in mbs.hiddenJointObjects
         freeMotion = mbs.freeMotion[obj.jointIndex]
 
-        j1 = freeMotion.ix_hidden_v
-        m.der_x_hidden[j1  ] = freeMotion.a[1]
-        m.der_x_hidden[j1+1] = freeMotion.a[2]
-        m.der_x_hidden[j1+2] = freeMotion.a[3]
+        j1 = freeMotion.ix_segmented_v
+        m.der_x_segmented[j1  ] = freeMotion.a[1]
+        m.der_x_segmented[j1+1] = freeMotion.a[2]
+        m.der_x_segmented[j1+2] = freeMotion.a[3]
 
-        j2 = freeMotion.ix_hidden_w
+        j2 = freeMotion.ix_segmented_w
         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]
+        m.der_x_segmented[j2  ] = z[1]
+        m.der_x_segmented[j2+1] = z[2]
+        m.der_x_segmented[j2+2] = z[3]
 
-        #println("   j1=$j1, j2=$j2, freeMotion.a[2] = ", freeMotion.a[2], ", m.der_x_hidden = ", m.der_x_hidden)
+        #println("   j1=$j1, j2=$j2, freeMotion.a[2] = ", freeMotion.a[2], ", m.der_x_segmented = ", m.der_x_segmented)
     end
     return nothing
 end

src/Composition/joints/joints.jl

@@ -53,10 +53,10 @@ mutable struct FreeMotion{F <: Modia3D.VarFloatType} <: Modia3D.AbstractJoint
     path::String
 
     # Hidden states are stored in the following way:
-    #   x_hidden[ix_r:...] = [r, rot, v, w]
+    #   x_segmented[ix_r:...] = [r, rot, v, w]
     #
     # Derivatives of hidden states are stored in the following way
-    #   der_x_hidden[ix_r:...] = [der(r), der(rot), der(v), der(w)]
+    #   der_x_segmented[ix_r:...] = [der(r), der(rot), der(v), der(w)]
     #
     # with
     #   der(r)   = v
@@ -65,10 +65,10 @@ mutable struct FreeMotion{F <: Modia3D.VarFloatType} <: Modia3D.AbstractJoint
     #   der(w)   = ...
 
     hiddenStates::Bool    # = true, if state is not visible in generated code
-    ix_hidden_r::Int      # instantiatedModel.x_hidden[ix_r_hidden  :ix_hidden_r+2]     are the elements of x_hidden that are stored in r   if hiddenStates
-    ix_hidden_rot::Int    # instantiatedModel.x_hidden[ix_rot_hidden:ix_hidden_rot+2] are the elements of x_hidden that are stored in rot if hiddenStates
-    ix_hidden_v::Int      # instantiatedModel.x_hidden[ix_v_hidden  :ix_hidden_v+2]     are the elements of x_hidden that are stored in v   if hiddenStates
-    ix_hidden_w::Int      # instantiatedModel.x_hidden[ix_w_hidden  :ix_hidden_w+2]     are the elements of x_hidden that are stored in w   if hiddenStates
+    ix_segmented_r::Int      # instantiatedModel.x_segmented[ix_r_hidden  :ix_segmented_r+2]   are the elements of x_segmented that are stored in r   if hiddenStates
+    ix_segmented_rot::Int    # instantiatedModel.x_segmented[ix_rot_hidden:ix_segmented_rot+2] are the elements of x_segmented that are stored in rot if hiddenStates
+    ix_segmented_v::Int      # instantiatedModel.x_segmented[ix_v_hidden  :ix_segmented_v+2]   are the elements of x_segmented that are stored in v   if hiddenStates
+    ix_segmented_w::Int      # instantiatedModel.x_segmented[ix_w_hidden  :ix_segmented_w+2]   are the elements of x_segmented that are stored in w   if hiddenStates
     iextra_isrot123::Int  # Startindex of extra result isrot123
     iz_rot2::Int          # instantiatedModel.eventHandler.z[iz_rot2] is the element of z in which singularRem(rot[2]) is stored if hiddenStates,
                           # to monitor when to switch to a different rotation sequence of rot

src/Composition/joints/object3DMotion.jl

@@ -42,9 +42,9 @@ end
 
 
 """
-    R = Modia3D.from_q(q::SVector{4,F})
+    R = Modia3D.from_q(q)
 
-Return SMatrix{4,4,F,16} `R` from quaternion `q`.
+Return rotation matrix `R::SMatrix{3,3,F,9}` from quaternions `q::SVector{4,F}`.
 """
 from_q(q::SVector{4,F}) where F <: Modia3D.VarFloatType = SMatrix{3,3,F,9}( F(2.0) * (q[1] * q[1] + q[4] * q[4]) - F(1.0),
                                             F(2.0) * (q[2] * q[1] - q[3] * q[4]),
@@ -58,10 +58,9 @@ from_q(q::SVector{4,F}) where F <: Modia3D.VarFloatType = SMatrix{3,3,F,9}( F(2.
 
 
 """
-    q = Modia3D.from_R(R::SMatrix{3,3,F,9};
-                       q_guess = NullQuaternion(F))
+    q = Modia3D.from_R(R; q_guess = NullQuaternion(F))
 
-Return quaternion `SVector{4,F} q` from rotation matrix `SMatrix{3,3,F,9} R`.
+Return quaternion `q::SVector{4,F}` from rotation matrix `R::SMatrix{3,3,F,9}`.
 
 From the two possible solutions `q` the one is returned that is closer
 to `q_guess` (note, `q` and `-q` define the same rotation).

src/Frames/quaternion.jl

@@ -5,7 +5,7 @@ module Modia3D
 
 const path = dirname(dirname(@__FILE__))   # Absolute path of package directory
 const Version = "0.11.0-dev"
-const Date = "2022-05-29"
+const Date = "2022-07-02"
 
 println("\nImporting Modia3D Version $Version ($Date)")
 

src/Modia3D.jl

@@ -21,7 +21,7 @@ bouncingSphere = @instantiateModel(BouncingSphere, unitless=true, logCode=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)
+showInfo(bouncingSphere)
 
 @usingModiaPlot
 plot(bouncingSphere, ["sphere.translation", "sphere.velocity", "sphere.rotation123"], figure=1)

test/Basic/BouncingSphere3D.jl

@@ -29,7 +29,7 @@ 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]
 Modia3D.angularVelocityResolvedInParentStates!(requiredFinalStates, 7, false)
 simulate!(shaft, stopTime=stopTime, dtmax=dtmax, log=true, logEvents=true, logStates=false, requiredFinalStates=requiredFinalStates)
-#printResultInfo(shaft)
+showInfo(shaft)
 
 @usingModiaPlot
 plot(shaft, ["shaft.rotation", "shaft.rotation123", "shaft.angularVelocity", "shaft.translation", "shaft.velocity"], figure=2)