Enable force elements for all Object3Ds

GerhardHippmann · GerhardHippmann · commit 5ef8a8dd2561 · 2021-11-02T11:46:13.000+01:00
- Enable velocity and force&amp;torque computation for all Object3Ds referenced by force elements.
- Update test BoxSpringDamperPtP.
- Remove unused argument time from evaluateForceElement.
- Fix some error messages, comments and indentations.
diff --git a/src/Composition/ForceElements/Bushing.jl b/src/Composition/ForceElements/Bushing.jl
@@ -30,10 +30,12 @@ end
 
 
 function initializeForceElement(force::Bushing)
+    force.obj1.hasForceElement = true
+    force.obj2.hasForceElement = true
     return nothing
 end
 
-function evaluateForceElement(time, force::Bushing)
+function evaluateForceElement(force::Bushing)
     r12 = measFramePosition(force.obj2; frameOrig=force.obj1, frameCoord=force.obj1)
     v12 = measFrameTransVelocity(force.obj2; frameOrig=force.obj1, frameCoord=force.obj1, frameObsrv=force.obj1)
     f12 = force.stiffness .* r12 + force.damping .* v12
diff --git a/src/Composition/ForceElements/SpringDamperPtP.jl b/src/Composition/ForceElements/SpringDamperPtP.jl
@@ -29,10 +29,12 @@ end
 
 
 function initializeForceElement(force::SpringDamperPtP)
+    force.obj1.hasForceElement = true
+    force.obj2.hasForceElement = true
     return nothing
 end
 
-function evaluateForceElement(time, force::SpringDamperPtP)
+function evaluateForceElement(force::SpringDamperPtP)
     (pos, norm) = measFrameDistance(force.obj2; frameOrig=force.obj1)
     vel = measFrameDistVelocity(force.obj2; frameOrig=force.obj1)
     frc = force.stiffness * pos + force.damping * vel
diff --git a/src/Composition/assignObjects.jl b/src/Composition/assignObjects.jl
@@ -79,14 +79,13 @@ end
 
 
 function assignAccVelo(tree::Vector{Object3D}, obj::Object3D)
-    # compute velocity of this object
-    if canCollide(obj) && !hasChildJoint(obj)
-        push!(tree, obj)
-    end
-    # compute acceleration of this object
     if hasChildJoint(obj) #|| hasCutJoint(obj)
+        # compute acceleration of this object
         obj.computeAcceleration = true
         push!(tree, obj)
+    elseif hasForceElement(obj) || canCollide(obj)
+        # compute velocity of this object
+        push!(tree, obj)
     end
 end
 
diff --git a/src/Composition/dynamics.jl b/src/Composition/dynamics.jl
@@ -1,8 +1,8 @@
 function getJointsAndForceElementsAndObject3DsWithoutParents!(evaluatedParameters,
-                                                           object3DWithoutParents::Vector{Object3D},
-                                                           jointObjects::Vector{Object3D},
-                                                           forceElements::Vector{Modia3D.AbstractForceElement},
-                                                           path::String)::Nothing
+                                                              object3DWithoutParents::Vector{Object3D},
+                                                              jointObjects::Vector{Object3D},
+                                                              forceElements::Vector{Modia3D.AbstractForceElement},
+                                                              path::String)::Nothing
     for (key,value) in evaluatedParameters   # zip(keys(evaluatedParameters), evaluatedParameters)
         if typeof(value) == Object3D
             if value.parent === value
@@ -139,14 +139,16 @@ function setModiaJointVariables!(id::Int, _leq_mode, instantiatedModel::ModiaLan
             end
             nqdd = j-1
 
+            # Initialize force elements
+            for force in forceElements
+                initializeForceElement(force)
+            end
+
             # Construct MultibodyData
             scene = initAnalysis2!(world)
             residuals = zeros(FloatType,nqdd)
             cache_h   = zeros(FloatType,nqdd)
             scene.forceElements = forceElements
-            for force in forceElements
-                initializeForceElement(force)
-            end
             if scene.options.enableContactDetection
                 nz = 2
                 zStartIndex = ModiaLang.addZeroCrossings(instantiatedModel, nz)
@@ -321,7 +323,7 @@ function multibodyResiduals3!(sim, scene, world, time, storeResults, isTerminal,
 
             # Evaluate force elements
             for force in forceElements
-                evaluateForceElement(time, force)
+                evaluateForceElement(force)
             end
 
             # Compute contact forces/torques
diff --git a/src/Composition/handler.jl b/src/Composition/handler.jl
@@ -128,7 +128,7 @@ end
 
 # the indices of super objects, which can't collide, are stored in a list
 function fillStackOrBuffer!(scene::Scene, superObj::SuperObjsRow, obj::Object3D, rootSuperObj::Object3D)::Nothing
-    n_children =  length(obj.children)
+    n_children = length(obj.children)
     help = fill(false, n_children)
 
     for i = 1:n_children
@@ -142,16 +142,16 @@ function fillStackOrBuffer!(scene::Scene, superObj::SuperObjsRow, obj::Object3D,
                     push!(superObj.noCPair, length(scene.buffer))
                 end
             else
-            if isMovable(child)
-                push!(scene.buffer, child)
-                child.isRootObj = true
-                child.computeAcceleration = true
-            else
-                push!(scene.stack, child)
-                assignAccVelo(scene.treeAccVelo, child)
-                if !(obj == rootSuperObj)
-                    changeParentToRootObj(rootSuperObj, child)
-                    help[i] = true
+                if isMovable(child)
+                    push!(scene.buffer, child)
+                    child.isRootObj = true
+                    child.computeAcceleration = true
+                else
+                    push!(scene.stack, child)
+                    assignAccVelo(scene.treeAccVelo, child)
+                    if !(obj == rootSuperObj)
+                        changeParentToRootObj(rootSuperObj, child)
+                        help[i] = true
     end; end; end; end; end
 
     if !isempty(obj.children)
diff --git a/src/Composition/joints/joints.jl b/src/Composition/joints/joints.jl
@@ -341,7 +341,7 @@ function computeKinematics!(scene::Scene, tree::Vector{Object3D}, time)::Nothing
             obj.z  = parent.R_abs*parent.z + freeMotion.z
 
         else
-            error("Bug in Modia3D/src/Composition/specifics/specifics.jl (computeKinematics!): jointKind = $jointKind is not known")
+            error("Bug in Modia3D/src/Composition/joints/joints.jl (computeKinematics!): jointKind = $jointKind is not known.")
         end
     end
     return nothing
@@ -396,7 +396,7 @@ function computeKinematics_for_leq_mode_pos!(scene::Scene, tree::Vector{Object3D
             obj.z  = parent.R_abs*parent.z + freeMotion.z
 
         else
-            error("Bug in Modia3D/src/Composition/specifics/specifics.jl (computeKinematics_for_leq_mode_pos!): jointKind = $jointKind is not known")
+            error("Bug in Modia3D/src/Composition/joints/joints.jl (computeKinematics_for_leq_mode_pos!): jointKind = $jointKind is not known.")
         end
     end
     return nothing
@@ -448,7 +448,7 @@ function computeForcesTorquesAndResiduals!(scene::Scene, tree::Vector{Object3D},
             freeMotion.residue_t = obj.t
 
         else
-            error("Bug in Modia3D/src/Composition/specifics/specifics.jl (computeForcesTorquesAndResiduals!): jointKind = $jointKind is not known")
+            error("Bug in Modia3D/src/Composition/joints/joints.jl (computeForcesTorquesAndResiduals!): jointKind = $jointKind is not known.")
         end
     end
     return nothing
@@ -492,7 +492,7 @@ function setJointVariables_q_qd_f!(scene::Scene, objects::Vector{Object3D}, star
             freeMotion.isrot123 = args_i[5]
 
         else
-           error("Bug in Modia3D/src/Composition/specifics/specifics.jl (setJointVariables!): jointKind = $jointKind is not known")
+           error("Bug in Modia3D/src/Composition/joints/joints.jl (setJointVariables_q_qd_f!): jointKind = $jointKind is not known.")
         end
     end
     return nothing
@@ -531,7 +531,7 @@ function setJointVariables_qdd!(scene::Scene, objects::Vector{Object3D}, startIn
             freeMotion.z = SVector{3,Float64}(qdd2[beg+3], qdd2[beg+4], qdd2[beg+5])
 
         else
-           error("Bug in Modia3D/src/Composition/specifics/specifics.jl (setJointVariables!): jointKind = $jointKind is not known")
+           error("Bug in Modia3D/src/Composition/joints/joints.jl (setJointVariables_qdd!): jointKind = $jointKind is not known.")
         end
     end
     return nothing
@@ -571,7 +571,7 @@ function getJointResiduals_for_leq_mode_0!(scene::Scene, objects::Vector{Object3
             residuals[beg+3:beg+5] = freeMotion.residue_t
 
         else
-           error("Bug in Modia3D/src/Composition/specifics/specifics.jl (getJointResiduals_for_leq_mode_0!): jointKind = $jointKind is not known")
+           error("Bug in Modia3D/src/Composition/joints/joints.jl (getJointResiduals_for_leq_mode_0!): jointKind = $jointKind is not known.")
         end
     end
     return nothing
@@ -604,7 +604,7 @@ function getJointResiduals_for_leq_mode_pos!(scene::Scene, objects::Vector{Objec
             residuals[beg+3:beg+5] = freeMotion.residue_t + cache_h[beg+3:beg+5]
 
         else
-           error("Bug in Modia3D/src/Composition/specifics/specifics.jl (getJointResiduals_for_leq_mode_0!): jointKind = $jointKind is not known")
+           error("Bug in Modia3D/src/Composition/joints/joints.jl (getJointResiduals_for_leq_mode_pos!): jointKind = $jointKind is not known.")
         end
     end
     return nothing
diff --git a/src/Composition/object3D.jl b/src/Composition/object3D.jl
@@ -177,7 +177,7 @@ mutable struct Object3D <: Modia3D.AbstractObject3D
         end
 
         if !isnothing(parent)
-            # with parent -> call constructor 3 or 5
+            # with parent -> call constructor 3
             if !isnothing(rotation) && (typeof(rotation) != Frames.RotationMatrix)
                 rotation = Frames.rot123(rotation[1], rotation[2], rotation[3])
             end
diff --git a/test/Basic/BoxSpringDamperPtP.jl b/test/Basic/BoxSpringDamperPtP.jl
@@ -17,18 +17,21 @@ SpringDamper = Model(
     box = Object3D(feature=Solid(shape=Box(lengthX=:Length, lengthY=:Length, lengthZ=:Length),
                                  massProperties=MassProperties(; mass=1.0, Ixx=0.1, Iyy=0.1, Izz=0.1),
                                  visualMaterial=:(visualMaterial))),
-    joint = FreeMotion(obj1=:world, obj2=:box, r=Var(init=[0.1, 0.2, 0.3])),
-    force = SpringDamperPtP(obj1=:world, obj2=:box, stiffness=:Stiffness, damping=:Damping)
+    boxCornerFrame = Object3D(parent=:box,
+                              feature=Visual(shape=CoordinateSystem(length=:(Length/2))),
+                              translation=:[Length/2, Length/2, Length/2]),
+    joint = FreeMotion(obj1=:world, obj2=:box),
+    force = SpringDamperPtP(obj1=:world, obj2=:boxCornerFrame, stiffness=:Stiffness, damping=:Damping)
 )
 
 springDamper = @instantiateModel(buildModia3D(SpringDamper), aliasReduction=false, unitless=true)
 
-stopTime = 6.0
+stopTime = 5.0
 dtmax = 0.1
-requiredFinalStates = [-0.026549009773552276, -0.05309801954749593, -0.10117097852565102, 0.0144068779028959, 0.028813755813570235, 0.02595802582858583, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+requiredFinalStates = [-0.031789887795694936, -0.03179006302845493, -0.13960438757282567, -0.22639388796813553, -0.226393551737028, -0.0958553592848107, -0.04370189950475126, 0.0436602955184432, 0.0009569259867732385, 0.8435396531516037, -0.8435431841758767, 3.531021863184031e-6]
 simulate!(springDamper, stopTime=stopTime, dtmax=dtmax, log=true, requiredFinalStates=requiredFinalStates)
 
 @usingModiaPlot
-plot(springDamper, ["joint.r", "joint.v"], figure=1)
+plot(springDamper, ["joint.r", "joint.v", "joint.rot", "joint.w"], figure=1)
 
 end
