Merge pull request #138 from ModiaSim/gh_sensorResultElement

GerhardHippmann · web-flow · commit 33856e3b785f · 2024-01-17T15:50:25.000+01:00
Add result element SensorResult
diff --git a/docs/src/Components/ResultElements.md b/docs/src/Components/ResultElements.md
@@ -4,6 +4,12 @@
 CurrentModule = Modia3D.Composition
 ```
 
+## SensorResult
+
+```@docs
+SensorResult
+```
+
 ## ContactResult
 
 ```@docs
diff --git a/docs/src/index.md b/docs/src/index.md
@@ -96,6 +96,7 @@ julia -JModia3D_sysimage.so  (otherwise)
 
 ### Forthcoming version
 
+- Add Result Element SensorResult
 - Enable kinematic chain flipping for structure variable systems
 - Avoid function code generation for linear stiffness and damping in SpringDamperPtP and Bushing
 
diff --git a/src/Composition/ResultElements/ContactResult.jl b/src/Composition/ResultElements/ContactResult.jl
@@ -111,19 +111,17 @@ function evaluateResultElement(model::Modia.InstantiatedModel{F,TimeType}, scene
         torqueVector = SVector{3,F}(0, 0, 0)
     end
 
-    if Modia.storeResults(model)
-        Modia.copy_w_segmented_value_to_result(model, result.penetrationResultIndex, penetration)
-        Modia.copy_w_segmented_value_to_result(model, result.penetrationVelocityResultIndex, penetrationVelocity)
-        Modia.copy_w_segmented_value_to_result(model, result.tangentialVelocityResultIndex, tangentialVelocity)
-        Modia.copy_w_segmented_value_to_result(model, result.angularVelocityResultIndex, angularVelocity)
-        Modia.copy_w_segmented_value_to_result(model, result.normalForceResultIndex, normalForce)
-        Modia.copy_w_segmented_value_to_result(model, result.tangentialForceResultIndex, tangentialForce)
-        Modia.copy_w_segmented_value_to_result(model, result.torqueResultIndex, torque)
-        Modia.copy_w_segmented_value_to_result(model, result.positionVectorResultIndex, positionVector)
-        Modia.copy_w_segmented_value_to_result(model, result.normalVectorResultIndex, normalVector)
-        Modia.copy_w_segmented_value_to_result(model, result.forceVectorResultIndex, forceVector)
-        Modia.copy_w_segmented_value_to_result(model, result.torqueVectorResultIndex, torqueVector)
-    end
+    Modia.copy_w_segmented_value_to_result(model, result.penetrationResultIndex, penetration)
+    Modia.copy_w_segmented_value_to_result(model, result.penetrationVelocityResultIndex, penetrationVelocity)
+    Modia.copy_w_segmented_value_to_result(model, result.tangentialVelocityResultIndex, tangentialVelocity)
+    Modia.copy_w_segmented_value_to_result(model, result.angularVelocityResultIndex, angularVelocity)
+    Modia.copy_w_segmented_value_to_result(model, result.normalForceResultIndex, normalForce)
+    Modia.copy_w_segmented_value_to_result(model, result.tangentialForceResultIndex, tangentialForce)
+    Modia.copy_w_segmented_value_to_result(model, result.torqueResultIndex, torque)
+    Modia.copy_w_segmented_value_to_result(model, result.positionVectorResultIndex, positionVector)
+    Modia.copy_w_segmented_value_to_result(model, result.normalVectorResultIndex, normalVector)
+    Modia.copy_w_segmented_value_to_result(model, result.forceVectorResultIndex, forceVector)
+    Modia.copy_w_segmented_value_to_result(model, result.torqueVectorResultIndex, torqueVector)
 
     return nothing
 end
diff --git a/src/Composition/ResultElements/SensorResult.jl b/src/Composition/ResultElements/SensorResult.jl
@@ -0,0 +1,110 @@
+
+"""
+    result = SensorResult(; object, objectOrigin, objectCoordinateRef, objectObserveRef)
+
+Return a `result` providing kinematic measurements of
+`object::`[`Object3D`](@ref) relative to
+`objectOrigin::`[`Object3D`](@ref), resolved in coordinates of
+`objectCoordinateRef::`[`Object3D`](@ref) and observed in
+`objectObserveRef::`[`Object3D`](@ref).
+
+In case of `objectOrigin` is undefined, world is used as sensor
+origin, i.e. results are absolute measurements. In case of
+`objectCoordinateRef` is undefined, `objectOrigin` is used as coordinate
+reference frame. In case of `objectObserveRef = nothing`,
+`objectCoordinateRef` is used as observer reference frame (so that
+velocity and acceleration results are consistent time derivatives of
+position results).
+
+# Results
+- `translation` is the position vector from `objectOrigin` to `object`.
+- `velocity` is the translational velocity vector from `objectOrigin` to `object`, observed in `objectObserveRef`.
+- `acceleration` is the translational acceleration vector from `objectOrigin` to `object`, observed in `objectObserveRef`.
+- `rotation` contains the [Cardan (Tait–Bryan) angles](https://en.wikipedia.org/wiki/Euler_angles#Chained_rotations_equivalence) (rotation sequence x-y-z) from `objectOrigin` to `object`.
+- `angularVelocity` is the rotational velocity vector from `objectOrigin` to `object`.
+- `angularAcceleration` is the rotational acceleration vector from `objectOrigin` to `object`, observed in `objectObserveRef`.
+- `distance` is the point-to-point distance between `objectOrigin` and `object`.
+- `distanceVelocity` is the point-to-point velocity between `objectOrigin` and `object`.
+- `distanceAcceleration` is the point-to-point acceleration between `objectOrigin` and `object`.
+"""
+mutable struct SensorResult{F <: Modia3D.VarFloatType} <: Modia3D.AbstractResultElement
+
+    path::String
+
+    object::Object3D{F}
+    objectOrigin::Union{Object3D{F}, Nothing}
+    objectCoordinateRef::Union{Object3D{F}, Nothing}
+    objectObserveRef::Union{Object3D{F}, Nothing}
+
+    translationResultIndex::Int
+    velocityResultIndex::Int
+    accelerationResultIndex::Int
+    rotationResultIndex::Int
+    angularVelocityResultIndex::Int
+    angularAccelerationResultIndex::Int
+    distanceResultIndex::Int
+    distanceVelocityResultIndex::Int
+    distanceAccelerationResultIndex::Int
+
+    function SensorResult{F}(; path::String = "",
+                               object::Object3D{F},
+                               objectOrigin::Union{Object3D{F}, Nothing}=nothing,
+                               objectCoordinateRef::Union{Object3D{F}, Nothing}=objectOrigin,
+                               objectObserveRef::Union{Object3D{F}, Nothing}=objectCoordinateRef ) where F <: Modia3D.VarFloatType
+        return new(path, object, objectOrigin, objectCoordinateRef, objectObserveRef)
+    end
+end
+SensorResult(; kwargs...) = SensorResult{Float64}(; kwargs...)
+
+
+function initializeResultElement(model::Modia.InstantiatedModel{F,TimeType}, result::SensorResult{F}) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
+    result.object.computeAcceleration = true
+    if (!isnothing(result.objectOrigin))
+        result.objectOrigin.computeAcceleration = true
+    end
+    if (!isnothing(result.objectObserveRef))
+        result.objectObserveRef.computeAcceleration = true
+    end
+
+    result.translationResultIndex          = Modia.new_w_segmented_variable!(model, result.path*".translation"          , SVector{3,F}(0, 0, 0), "m")
+    result.velocityResultIndex             = Modia.new_w_segmented_variable!(model, result.path*".velocity"             , SVector{3,F}(0, 0, 0), "m/s")
+    result.accelerationResultIndex         = Modia.new_w_segmented_variable!(model, result.path*".acceleration"         , SVector{3,F}(0, 0, 0), "m/s^2")
+    result.rotationResultIndex             = Modia.new_w_segmented_variable!(model, result.path*".rotation"             , SVector{3,F}(0, 0, 0), "rad")
+    result.angularVelocityResultIndex      = Modia.new_w_segmented_variable!(model, result.path*".angularVelocity"      , SVector{3,F}(0, 0, 0), "rad/s")
+    result.angularAccelerationResultIndex  = Modia.new_w_segmented_variable!(model, result.path*".angularAcceleration"  , SVector{3,F}(0, 0, 0), "rad/s^2")
+    result.distanceResultIndex             = Modia.new_w_segmented_variable!(model, result.path*".distance"             , F(0)                 , "m")
+    result.distanceVelocityResultIndex     = Modia.new_w_segmented_variable!(model, result.path*".distanceVelocity"     , F(0)                 , "m/s")
+    result.distanceAccelerationResultIndex = Modia.new_w_segmented_variable!(model, result.path*".distanceAcceleration" , F(0)                 , "m/s^2")
+
+    return nothing
+end
+
+function evaluateResultElement(model::Modia.InstantiatedModel{F,TimeType}, scene::Modia3D.Composition.Scene{F}, result::SensorResult{F}, time::TimeType) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat}
+
+    translation = measFramePosition(result.object; frameOrig=result.objectOrigin, frameCoord=result.objectCoordinateRef)
+    velocity = measFrameTransVelocity(result.object; frameOrig=result.objectOrigin, frameCoord=result.objectCoordinateRef, frameObsrv=result.objectObserveRef)
+    acceleration = measFrameTransAcceleration(result.object; frameOrig=result.objectOrigin, frameCoord=result.objectCoordinateRef, frameObsrv=result.objectObserveRef)
+    rotationMatrix = measFrameRotation(result.object; frameOrig=result.objectOrigin)
+    rotation = rot123fromR(rotationMatrix)
+    angularVelocity = measFrameRotVelocity(result.object; frameOrig=result.objectOrigin, frameCoord=result.objectCoordinateRef)
+    angularAcceleration = measFrameRotAcceleration(result.object; frameOrig=result.objectOrigin, frameCoord=result.objectCoordinateRef, frameObsrv=result.objectObserveRef)
+    (distance, dir) = measFrameDistance(result.object; frameOrig=result.objectOrigin)
+    distanceVelocity = measFrameDistVelocity(result.object; frameOrig=result.objectOrigin)
+    distanceAcceleration = measFrameDistAcceleration(result.object; frameOrig=result.objectOrigin)
+
+    Modia.copy_w_segmented_value_to_result(model, result.translationResultIndex, translation)
+    Modia.copy_w_segmented_value_to_result(model, result.velocityResultIndex, velocity)
+    Modia.copy_w_segmented_value_to_result(model, result.accelerationResultIndex, acceleration)
+    Modia.copy_w_segmented_value_to_result(model, result.rotationResultIndex, rotation)
+    Modia.copy_w_segmented_value_to_result(model, result.angularVelocityResultIndex, angularVelocity)
+    Modia.copy_w_segmented_value_to_result(model, result.angularAccelerationResultIndex, angularAcceleration)
+    Modia.copy_w_segmented_value_to_result(model, result.distanceResultIndex, distance)
+    Modia.copy_w_segmented_value_to_result(model, result.distanceVelocityResultIndex, distanceVelocity)
+    Modia.copy_w_segmented_value_to_result(model, result.distanceAccelerationResultIndex, distanceAcceleration)
+
+    return nothing
+end
+
+function terminateResultElement(result::SensorResult{F}) where F <: Modia3D.VarFloatType
+    return nothing
+end
diff --git a/src/Composition/_module.jl b/src/Composition/_module.jl
@@ -117,6 +117,7 @@ include("scene.jl") # must be included after superObjects.jl
 include(joinpath("joints", "joints.jl"))
 include(joinpath("joints", "changeJointState.jl"))
 
+include(joinpath("ResultElements", "SensorResult.jl"))
 include(joinpath("ResultElements", "ContactResult.jl"))
 include("sensors.jl")
 include("frameMeasurements.jl")
diff --git a/src/Composition/dynamics.jl b/src/Composition/dynamics.jl
@@ -551,8 +551,10 @@ function computeGeneralizedForces!(mbs::MultibodyData{F,TimeType}, qdd_hidden::V
             end
 
         elseif leq_mode == -2
-            # Compute only terms needed at a communication point (currently: Only visualization + export animation)
+            # Compute only terms needed at a communication point (user-defined Object3D r/R_abs, result elements, visualization/animation export)
             TimerOutputs.@timeit instantiatedModel.timer "Modia3D_3" begin
+                # Compute kinematics (yields absolute accelerations for result elements)
+                TimerOutputs.@timeit instantiatedModel.timer "Modia3D_3 computePositionsVelocitiesAccelerations!" computePositionsVelocitiesAccelerations!(mbs, tree, time)
                 # objects can have interactionManner
                 if scene.options.useOptimizedStructure
                     for obj in scene.pureResultObject3Ds
diff --git a/src/Composition/frameMeasurements.jl b/src/Composition/frameMeasurements.jl
@@ -132,7 +132,7 @@ If `frameObsrv` is omitted `wd` is observed in world frame.
 function measFrameRotAcceleration(frameMeas::Object3D{F}; frameOrig::Union{Object3D{F}, Nothing}=nothing, frameCoord::Union{Object3D{F}, Nothing}=nothing, frameObsrv::Union{Object3D{F}, Nothing}=nothing) where F <: Modia3D.VarFloatType
     wd_OrigMeas = frameMeas.R_abs' * copy(frameMeas.z)  # World_wd_WorldMeas := R_MeasWorld^T * Meas_wd_WorldMeas
     if !isnothing(frameOrig)
-        wd_OrigMeas = wd_OrigMeas - (frame.Orig.R_abs' * frameOrig.z)  # World_wd_OrigMeas := World_wd_WorldMeas - R_OrigWorld^T * Orig_wd_WorldOrig
+        wd_OrigMeas = wd_OrigMeas - (frameOrig.R_abs' * frameOrig.z)  # World_wd_OrigMeas := World_wd_WorldMeas - R_OrigWorld^T * Orig_wd_WorldOrig
     end
     if !isnothing(frameObsrv)
         w_OrigMeas = measFrameRotVelocity(frameMeas; frameOrig=frameOrig)
diff --git a/src/ModiaInterface/_module.jl b/src/ModiaInterface/_module.jl
@@ -19,7 +19,7 @@ export Revolute, RevoluteWithFlange
 export Prismatic, PrismaticWithFlange
 export singularRem, FreeMotion, change_rotSequenceInNextIteration!
 export WorldForce, WorldTorque, Bushing, SpringDamperPtP, PolygonalContactModel
-export ContactResult
+export SensorResult, ContactResult
 export FreeMotion2
 
 export ModelActions, ActionAttach, ActionRelease, ActionReleaseAndAttach, ActionDelete, EventAfterPeriod, ActionWait, ActionFlipChain
diff --git a/src/ModiaInterface/model3D.jl b/src/ModiaInterface/model3D.jl
@@ -30,6 +30,7 @@ WorldTorque(          ; kwargs...) = Par(; _constructor = :(Modia3D.Composition.
 Bushing(              ; kwargs...) = Par(; _constructor = :(Modia3D.Composition.Bushing{FloatType})              , _path = true, kwargs...)
 SpringDamperPtP(      ; kwargs...) = Par(; _constructor = :(Modia3D.Composition.SpringDamperPtP{FloatType})      , _path = true, kwargs...)
 PolygonalContactModel(; kwargs...) = Par(; _constructor = :(Modia3D.Composition.PolygonalContactModel{FloatType}), _path = true, kwargs...)
+SensorResult(         ; kwargs...) = Par(; _constructor = :(Modia3D.Composition.SensorResult{FloatType})         , _path = true, kwargs...)
 ContactResult(        ; kwargs...) = Par(; _constructor = :(Modia3D.Composition.ContactResult{FloatType})        , _path = true, kwargs...)
 
 MassPropertiesFromShape()              = Par(; _constructor = :(Modia3D.Shapes.MassPropertiesFromShape{FloatType}))
diff --git a/test/ForceElements/BoxBushing.jl b/test/ForceElements/BoxBushing.jl
@@ -30,7 +30,8 @@ BoxBushing = Model3D(
                                  visualMaterial=:(visualMaterial))),
     force = Bushing(obj1=:world, obj2=:box,
                     springForceLaw=[fc, 100.0, 200.0], damperForceLaw=[1.0, fd, 4.0],
-                    rotSpringForceLaw=[5.0, 10.0, mc], rotDamperForceLaw=[0.1, md, 0.4], largeAngles=largeAngles)
+                    rotSpringForceLaw=[5.0, 10.0, mc], rotDamperForceLaw=[0.1, md, 0.4], largeAngles=largeAngles),
+    result = SensorResult(object=:box)
 )
 
 boxBushing = @instantiateModel(BoxBushing, unitless=true, logCode=true)
@@ -45,9 +46,10 @@ end
 simulate!(boxBushing, stopTime=stopTime, dtmax=dtmax, log=true, requiredFinalStates=requiredFinalStates)
 
 @usingModiaPlot
-plot(boxBushing, ["box.translation", "box.velocity", "box.rotation", "box.angularVelocity"], figure=1)
-plot(boxBushing, ["force.translation", "force.velocity", "force.rotation", "force.rotationVelocity"], figure=2)
-plot(boxBushing, ["force.springForce", "force.damperForce", "force.forceVector"], figure=3)
-plot(boxBushing, ["force.springTorque", "force.damperTorque", "force.torque", "force.torqueVector"], figure=4)
+plot(boxBushing, ["result.translation", "result.velocity", "result.acceleration"], figure=1)
+plot(boxBushing, ["result.rotation", "result.angularVelocity", "result.angularAcceleration"], figure=2)
+plot(boxBushing, ["force.translation", "force.velocity", "force.rotation", "force.rotationVelocity"], figure=3)
+plot(boxBushing, ["force.springForce", "force.damperForce", "force.forceVector"], figure=4)
+plot(boxBushing, ["force.springTorque", "force.damperTorque", "force.torque", "force.torqueVector"], figure=5)
 
 end
diff --git a/test/ForceElements/BoxForceSensorResult.jl b/test/ForceElements/BoxForceSensorResult.jl
@@ -0,0 +1,49 @@
+module BoxForceSensorResultSimulation
+
+using Modia3D
+
+BoxForceSensorResult = Model3D(
+    Length = 0.1,
+    Mass = 1.0,
+    IMoment = 0.1,
+    visualMaterial = VisualMaterial(color="IndianRed1", transparency=0.5),
+    world = Object3D(feature=Scene(gravityField=UniformGravityField(g=9.81, n=[0, 0, -1]),
+                                   nominalLength=:(2*Length))),
+    worldFrame = Object3D(parent=:world,
+                          feature=Visual(shape=CoordinateSystem(length=:Length))),
+    coordRefFrame = Object3D(parent=:world,
+                             rotation=[deg2rad(23), deg2rad(45), deg2rad(67)],
+                             feature=Visual(shape=CoordinateSystem(length=:(Length/2)))),
+    box1 = Object3D(parent=:world, fixedToParent=false,
+                    translation=[0.2, 0.1, 0.05],
+                    rotation=[0.12, 0.06, 0.03],
+                    feature=Solid(shape=Box(lengthX=:Length, lengthY=:Length, lengthZ=:Length),
+                                  massProperties=MassProperties(; mass=:Mass, Ixx=:IMoment, Iyy=:IMoment, Izz=:IMoment),
+                                  visualMaterial=VisualMaterial(color="IndianRed1", transparency=0.5))),
+    box2 = Object3D(parent=:world, fixedToParent=false,
+                    feature=Solid(shape=Box(lengthX=:Length, lengthY=:Length, lengthZ=:Length),
+                                  massProperties=MassProperties(; mass=:Mass, Ixx=:IMoment, Iyy=:IMoment, Izz=:IMoment),
+                                  visualMaterial=VisualMaterial(color="Turquoise", transparency=0.5))),
+    box2CornerFrame = Object3D(parent=:box2,
+                               feature=Visual(shape=CoordinateSystem(length=:(Length/2))),
+                               translation=:[Length/2, Length/2, Length/2]),
+    force1 = Bushing(obj1=:world, obj2=:box1,
+                     springForceLaw=[50.0, 100.0, 200.0], damperForceLaw=[1.0, 2.0, 4.0],
+                     rotSpringForceLaw=[5.0, 10.0, 20.0], rotDamperForceLaw=[0.1, 0.2, 0.4], largeAngles=false),
+    force2 = SpringDamperPtP(obj1=:world, obj2=:box2CornerFrame, springForceLaw=100.0, damperForceLaw=2.0),
+    sensor = SensorResult(object=:box1, objectOrigin=:box2, objectCoordinateRef=:coordRefFrame, objectObserveRef=:box1)
+)
+
+boxForceSensorResult = @instantiateModel(BoxForceSensorResult, unitless=true)
+
+stopTime = 6.0
+dtmax = 0.1
+requiredFinalStates = [-0.0016107113217414199, -0.0002480705395490782, -0.049050349752476706, 0.07029162030881492, 2.2312117805489287e-5, -6.372913904848077e-6, -0.0010449353993963431, -0.0001514824927969645, -2.4593890216411865e-7, 0.042388263052657665, 0.00010086233694909848, -5.551141011115388e-6, 0.05646126021462138, 0.0564613884676648, -0.12716607892200024, 0.2920400985011993, 0.2920398970084984, 0.04549365682408229, 2.1980798016988854, -0.6805794882560942, 1.213379900814701, 0.29439337230022455, -0.29439394783827416, 5.036863759629742e-7]
+simulate!(boxForceSensorResult, stopTime=stopTime, dtmax=dtmax, log=true, requiredFinalStates=requiredFinalStates)
+
+@usingModiaPlot
+plot(boxForceSensorResult, ["sensor.translation", "sensor.velocity", "sensor.acceleration"], figure=1)
+plot(boxForceSensorResult, ["sensor.rotation", "sensor.angularVelocity", "sensor.angularAcceleration"], figure=2)
+plot(boxForceSensorResult, ["sensor.distance", "sensor.distanceVelocity", "sensor.distanceAcceleration"], figure=3)
+
+end
diff --git a/test/includeTests.jl b/test/includeTests.jl
@@ -41,6 +41,7 @@ Test.@testset "Force Elements" begin
     include(joinpath("ForceElements", "BoxBushing.jl"))
     include(joinpath("ForceElements", "BoxSpringDamperPtP.jl"))
     include(joinpath("ForceElements", "BoxNonLinearSpringDamperPtP.jl"))
+    include(joinpath("ForceElements", "BoxForceSensorResult.jl"))
     if testsExtend >= normalTests
         include(joinpath("ForceElements", "PCMBubbleFunnel.jl"))
     end
