-
Notifications
You must be signed in to change notification settings - Fork 8
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #138 from ModiaSim/gh_sensorResultElement
Add result element SensorResult
- Loading branch information
Showing
12 changed files
with
192 additions
and
21 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters