Store r_abs, R_abs of visual shapes in result

Author: MartinOtter

Project.toml

@@ -12,7 +12,7 @@ HTTP = "0.9"
 JSON = "0.21"
 Measurements = "2"
 MeshIO = "0.4.10"
-Modia = "0.12.0, 0.11.0"
+Modia = "0.12.0"
 MonteCarloMeasurements = "1"
 OrderedCollections = "1"
 Reexport = "1.0"

src/Composition/dynamics.jl

@@ -174,10 +174,23 @@ function initSegment_Model3D!(partiallyInstantiatedModel::Modia.InstantiatedMode
             zStartIndex = 0
         end
 
+        # objIndices[i,1]: Index of r_abs of Object3D i
+        #           [i,2]: Index of R_abs of Object3D i
+        objIndices = Matrix{Int}(undef, length(scene.updateVisuElements), 2)
+        for (i,obj) in enumerate(scene.updateVisuElements)
+            if typeof(obj.feature) == Modia3D.Composition.EmptyObject3DFeature
+                objIndices[i,1] = 0
+                objIndices[i,2] = 0
+            else
+                objIndices[i,1] = Modia.new_w_segmented_variable!(partiallyInstantiatedModel, obj.path*".r_abs", Modia3D.ZeroVector3D(F), "m")
+                objIndices[i,2] = Modia.new_w_segmented_variable!(partiallyInstantiatedModel, obj.path*".R_abs", Modia3D.NullRotation(F), "")   
+            end
+        end      
+        
         mbsBuild.mbs = MultibodyData{F,TimeType}(partiallyInstantiatedModel, modelPath, world, scene,
-                                                revoluteObjects, prismaticObjects, freeMotionObjects, hiddenJointObjects,
-                                                mbsBuild.revoluteIndices, mbsBuild.prismaticIndices, mbsBuild.freeMotionIndices,
-                                                zStartIndex, nz)
+                                                 revoluteObjects, prismaticObjects, freeMotionObjects, hiddenJointObjects,
+                                                 mbsBuild.revoluteIndices, mbsBuild.prismaticIndices, mbsBuild.freeMotionIndices,
+                                                 objIndices, zStartIndex, nz)
         parameters[:qdd_hidden] = zeros(F, length(mbsBuild.mbs.hiddenGenForces))
 
         if log
@@ -514,11 +527,17 @@ function computeGeneralizedForces!(mbs::MultibodyData{F,TimeType}, qdd_hidden::V
             TimerOutputs.@timeit instantiatedModel.timer "Modia3D_3" begin
                 # objects can have interactionManner (need to rename updateVisuElements)
                 if scene.options.useOptimizedStructure
-                    for obj in scene.updateVisuElements
+                    objIndices = mbs.objIndices
+                    for (i,obj) in enumerate(scene.updateVisuElements)
                         parent = obj.parent
                         obj.r_abs = obj.r_rel ≡ Modia3D.ZeroVector3D(F) ? parent.r_abs : parent.r_abs + parent.R_abs'*obj.r_rel
                         obj.R_abs = obj.R_rel ≡ Modia3D.NullRotation(F) ? parent.R_abs : obj.R_rel*parent.R_abs
 
+                        if objIndices[i,1] > 0
+                            Modia.copy_w_segmented_value_to_result(instantiatedModel, objIndices[i,1], obj.r_abs)
+                            Modia.copy_w_segmented_value_to_result(instantiatedModel, objIndices[i,2], obj.R_abs)
+                        end
+            
                         # is executed only if an internal Object3D called
                         if length( obj.visualizationFrame ) == 1
                             obj.visualizationFrame[1].r_abs = obj.r_abs

src/Composition/joints/joints.jl

@@ -31,6 +31,8 @@ mutable struct MultibodyData{F <: Modia3D.VarFloatType, TimeType}
     freeMotionCache_h::Vector{SVector{3,F}}   # = h_f(q,qd,gravity,contact-forces)
     hiddenCache_h::Vector{F}                  # = h_hidden(q,qd,gravity,contact-forces)
 
+    objIndices::Matrix{Int}                   # objIndices[i,1]: Index of local w_segmented variable r_abs of Object3D i
+                                              #           [i,2]: Index of local w_segmented variable R_abs of Object3D i
     zStartIndex::Int                          # eventHandler.z[zStartIndex] is first index of crossing function
                                               # (or zero, if enableContactDetection=false)
     nz::Int                                   # Number of used zero crossing functions
@@ -43,7 +45,7 @@ mutable struct MultibodyData{F <: Modia3D.VarFloatType, TimeType}
 
     function MultibodyData{F,TimeType}(partiallyInstantiatedModel::Modia.InstantiatedModel{F,TimeType}, modelPath::String, world, scene,
                                        revoluteObjects, prismaticObjects, freeMotionObjects, hiddenJointObjects,
-                                       revoluteIndices, prismaticIndices, freeMotionIndices,
+                                       revoluteIndices, prismaticIndices, freeMotionIndices, objIndices,
                                        zStartIndex, nz) where {F,TimeType}
         # Make joints available
         revoluteJoints   = Vector{Revolute{F}}(  undef, length(revoluteObjects))
@@ -116,7 +118,7 @@ mutable struct MultibodyData{F <: Modia3D.VarFloatType, TimeType}
             zeros(F, length(revoluteObjects)), zeros(F, length(prismaticObjects)),
             zeros(F, length(revoluteObjects)+length(prismaticObjects)), zeros(SVector{3,F}, 2*length(freeMotionObjects)), zeros(F, nhidden_qdd),
             zeros(F, length(revoluteObjects)+length(prismaticObjects)), zeros(SVector{3,F}, 2*length(freeMotionObjects)), zeros(F, nhidden_qdd),
-            zStartIndex, nz, partiallyInstantiatedModel.time,
+            objIndices, zStartIndex, nz, partiallyInstantiatedModel.time,
             zeros(F, 2*3*length(freeMotionObjects)),
             Modia.LinearEquations{F}[])
     end

test/Basic/Pendulum.jl

@@ -12,9 +12,9 @@ Pendulum = Model3D(
 pendulum = @instantiateModel(Pendulum, unitless=true, log=false, logDetails=false, logCode=true, logStateSelection=false, logCalculations=false)
 requiredFinalStates = [5.970529829666215, -1.1395482781332746]
 simulate!(pendulum, stopTime=3.0, log=true, requiredFinalStates = requiredFinalStates)
+showInfo(pendulum)
 
 @usingModiaPlot
-plot(pendulum, "rev.phi")
-
+plot(pendulum, ["rev.phi", "body.r_abs", "body.R_abs"])
 
 end

test/Segmented/TwoStageRocket3D.jl

@@ -69,12 +69,13 @@ rocket = @instantiateModel(TwoStageRocket, unitless=true)
 stopTime = 15.0
 tolerance = 1.0e-6
 requiredFinalStates = [0.0, 1232.2203937213537, 0.0, 0.0, 77.85108394267779, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
-simulate!(rocket, stopTime=stopTime, tolerance=tolerance, log=true, logEvents=false, logStates=false, requiredFinalStates=requiredFinalStates)
-
+simulate!(rocket, stopTime=stopTime, tolerance=tolerance, log=true, logEvents=true, logStates=false, requiredFinalStates=requiredFinalStates)
+showInfo(rocket)
 
 @usingModiaPlot
 plot(rocket, [("stage2.body.translation[2]", "stage1.body.translation[2]" ),
               ("stage2.body.velocity[2]", "stage1.body.velocity[2]"),
               ("stage1.thrust.forceVector[2]", "stage2.thrust.forceVector[2]")])
 
+plot(rocket, ["stage1.body.r_abs[2]", "stage2.body.r_abs[2]"], figure=2)
 end