Skip to content

Commit 833ff6e

Browse files
AndreaNeumayrAndreaNeumayr
authored
Merge pull request #46 from ModiaSim/gh_ForceElementInfrastructure
Gh force element infrastructure
2 parents 06bec97 + 5ef8a8d commit 833ff6e

File tree

17 files changed

+271
-75
lines changed

17 files changed

+271
-75
lines changed

‎src/Composition/ForceElements/Bushing.jl‎

Lines changed: 48 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,48 @@
1+
2+
"""
3+
force = Bushing(; obj1, obj2, stiffness, damping)
4+
5+
Return a `force` acting as bushing between `obj1::`[`Object3D`](@ref) and
6+
`obj2::`[`Object3D`](@ref). Vectors `stiffness` and `damping` define the
7+
stiffness resp. damping values in x, y and z direction of `obj1`. The
8+
orientation of `obj2` does no influence the resulting forces.
9+
"""
10+
mutable struct Bushing <: Modia3D.AbstractForceElement
11+
12+
obj1::Modia3D.AbstractObject3D
13+
obj2::Modia3D.AbstractObject3D
14+
15+
stiffness::SVector{3,Float64}
16+
damping::SVector{3,Float64}
17+
18+
function Bushing(; obj1::Modia3D.AbstractObject3D,
19+
obj2::Modia3D.AbstractObject3D,
20+
stiffness::AbstractVector = Modia3D.ZeroVector3D,
21+
damping::AbstractVector = Modia3D.ZeroVector3D)
22+
23+
stiff = Modia3D.convertAndStripUnit(SVector{3,Float64}, u"N/m" , stiffness)
24+
damp = Modia3D.convertAndStripUnit(SVector{3,Float64}, u"N*s/m", damping)
25+
26+
return new(obj1, obj2, stiff, damp)
27+
28+
end
29+
end
30+
31+
32+
function initializeForceElement(force::Bushing)
33+
force.obj1.hasForceElement = true
34+
force.obj2.hasForceElement = true
35+
return nothing
36+
end
37+
38+
function evaluateForceElement(force::Bushing)
39+
r12 = measFramePosition(force.obj2; frameOrig=force.obj1, frameCoord=force.obj1)
40+
v12 = measFrameTransVelocity(force.obj2; frameOrig=force.obj1, frameCoord=force.obj1, frameObsrv=force.obj1)
41+
f12 = force.stiffness .* r12 + force.damping .* v12
42+
applyFrameForcePair!(force.obj2, force.obj1, f12; frameCoord=force.obj1)
43+
return nothing
44+
end
45+
46+
function terminateForceElement(force::Bushing)
47+
return nothing
48+
end

‎src/Composition/ForceElements/SpringDamperPtP.jl‎

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
2+
"""
3+
force = SpringDamperPtP(; obj1, obj2, stiffness, damping)
4+
5+
Return a `force` acting as point-to-point parallel spring-damper between
6+
`obj1::`[`Object3D`](@ref) and `obj2::`[`Object3D`](@ref). Values
7+
`stiffness` and `damping` define the stiffness resp. damping coefficient.
8+
"""
9+
mutable struct SpringDamperPtP <: Modia3D.AbstractForceElement
10+
11+
obj1::Modia3D.AbstractObject3D
12+
obj2::Modia3D.AbstractObject3D
13+
14+
stiffness::Float64
15+
damping::Float64
16+
17+
function SpringDamperPtP(; obj1::Modia3D.AbstractObject3D,
18+
obj2::Modia3D.AbstractObject3D,
19+
stiffness::Float64 = 0.0,
20+
damping::Float64 = 0.0)
21+
22+
stiff = Modia3D.convertAndStripUnit(Float64, u"N/m" , stiffness)
23+
damp = Modia3D.convertAndStripUnit(Float64, u"N*s/m", damping)
24+
25+
return new(obj1, obj2, stiff, damp)
26+
27+
end
28+
end
29+
30+
31+
function initializeForceElement(force::SpringDamperPtP)
32+
force.obj1.hasForceElement = true
33+
force.obj2.hasForceElement = true
34+
return nothing
35+
end
36+
37+
function evaluateForceElement(force::SpringDamperPtP)
38+
(pos, norm) = measFrameDistance(force.obj2; frameOrig=force.obj1)
39+
vel = measFrameDistVelocity(force.obj2; frameOrig=force.obj1)
40+
frc = force.stiffness * pos + force.damping * vel
41+
f12 = frc * norm
42+
applyFrameForcePair!(force.obj2, force.obj1, f12; frameCoord=force.obj1)
43+
end
44+
45+
function terminateForceElement(force::SpringDamperPtP)
46+
return nothing
47+
end

‎src/Composition/_module.jl‎

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,8 @@ export Revolute, setAngle!, connect
3232
export Prismatic, setDistance!
3333
export FreeMotion
3434

35+
export Bushing, SpringDamperPtP
36+
3537
export multibodyResiduals!, setModiaJointVariables!
3638

3739
export distanceAndAngles, distance, planarRotationAngle
@@ -93,12 +95,15 @@ include("object3D.jl")
9395
include("supportPoints.jl")
9496
include("superObjects.jl")
9597

96-
# Joints must be included before scene, because joint memory is stored in scene
98+
# Joints and Force Elements must be included before scene, because memory is stored in scene
9799
include(joinpath("joints", "FreeMotion.jl"))
98100
include(joinpath("joints", "Fix.jl"))
99101
include(joinpath("joints", "Revolute.jl"))
100102
include(joinpath("joints", "Prismatic.jl"))
101103

104+
include(joinpath("ForceElements", "Bushing.jl"))
105+
include(joinpath("ForceElements", "SpringDamperPtP.jl"))
106+
102107
include("contactPairs.jl")
103108
include(joinpath(Modia3D.path, "src", "contactDetection", "ContactDetectionMPR", "ContactDetectionMPR_handler.jl"))
104109

‎src/Composition/assignObjects.jl‎

Lines changed: 4 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -79,14 +79,13 @@ end
7979

8080

8181
function assignAccVelo(tree::Vector{Object3D}, obj::Object3D)
82-
# compute velocity of this object
83-
if canCollide(obj) && !hasChildJoint(obj)
84-
push!(tree, obj)
85-
end
86-
# compute acceleration of this object
8782
if hasChildJoint(obj) #|| hasCutJoint(obj)
83+
# compute acceleration of this object
8884
obj.computeAcceleration = true
8985
push!(tree, obj)
86+
elseif hasForceElement(obj) || canCollide(obj)
87+
# compute velocity of this object
88+
push!(tree, obj)
9089
end
9190
end
9291

‎src/Composition/dynamics.jl‎

Lines changed: 31 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,8 @@
1-
function getJointsAndObject3DsWithoutParents!(evaluatedParameters,
2-
object3DWithoutParents::Vector{Object3D},
3-
jointObjects::Vector{Object3D},
4-
path::String)::Nothing
1+
function getJointsAndForceElementsAndObject3DsWithoutParents!(evaluatedParameters,
2+
object3DWithoutParents::Vector{Object3D},
3+
jointObjects::Vector{Object3D},
4+
forceElements::Vector{Modia3D.AbstractForceElement},
5+
path::String)::Nothing
56
for (key,value) in evaluatedParameters # zip(keys(evaluatedParameters), evaluatedParameters)
67
if typeof(value) == Object3D
78
if value.parent === value
@@ -13,8 +14,11 @@ function getJointsAndObject3DsWithoutParents!(evaluatedParameters,
1314
elseif typeof(value) <: Modia3D.AbstractJoint
1415
push!(jointObjects, value.obj2)
1516

17+
elseif typeof(value) <: Modia3D.AbstractForceElement
18+
push!(forceElements, value)
19+
1620
elseif typeof(value) <: OrderedDict
17-
getJointsAndObject3DsWithoutParents!(value, object3DWithoutParents, jointObjects, path)
21+
getJointsAndForceElementsAndObject3DsWithoutParents!(value, object3DWithoutParents, jointObjects, forceElements, path)
1822
end
1923
end
2024
return nothing
@@ -24,21 +28,21 @@ multiBodyName(instantiatedModel, mbsPath) = mbsPath == "" ? instantiatedModel.mo
2428
instantiatedModel.modelName * "." * mbsPath
2529

2630
"""
27-
(world, jointObjects) = checkMultibodySystemAndGetWorldAndJoints(instantiatedModel, id)
31+
(world, jointObjects, forceElements) = checkMultibodySystemAndGetWorldAndJointsAndForceElements(instantiatedModel, id)
2832
2933
Recursively traverse model and perform the following actions:
3034
3135
- Search for an OrderedDict that has `key = :_id, value = id`
3236
(identifies the root of the multibody system).
33-
3437
- Search from the root of the multibody system and perform the following actions:
3538
- Check that from all Object3Ds exactly one of them has no parent.
3639
Return this parent as `world`.
3740
- Check that only `world` has potentially a `feature` entry that
3841
is a SceneOption.
3942
- Return a vector of joint objects as `joints`.
43+
- Return a vector of all force element objects.
4044
"""
41-
function checkMultibodySystemAndGetWorldAndJoints(instantiatedModel::ModiaLang.SimulationModel{FloatType,ParType,EvaluatedParType,TimeType}, id::Int) where {FloatType,ParType,EvaluatedParType,TimeType}
45+
function checkMultibodySystemAndGetWorldAndJointsAndForceElements(instantiatedModel::ModiaLang.SimulationModel{FloatType,ParType,EvaluatedParType,TimeType}, id::Int) where {FloatType,ParType,EvaluatedParType,TimeType}
4246
# Find root mbs of multibody system
4347
(mbsRoot, mbsPath) = ModiaLang.getIdParameter(instantiatedModel.evaluatedParameters, ParType, id)
4448
if isnothing(mbsRoot)
@@ -47,7 +51,8 @@ function checkMultibodySystemAndGetWorldAndJoints(instantiatedModel::ModiaLang.S
4751

4852
object3DWithoutParents = Object3D[]
4953
jointObjects = Object3D[]
50-
getJointsAndObject3DsWithoutParents!(mbsRoot, object3DWithoutParents, jointObjects, mbsPath)
54+
forceElements = Modia3D.AbstractForceElement[]
55+
getJointsAndForceElementsAndObject3DsWithoutParents!(mbsRoot, object3DWithoutParents, jointObjects, forceElements, mbsPath)
5156

5257
if length(object3DWithoutParents) == 0
5358
error("\n", multiBodyName(instantiatedModel, mbsPath), ": There is either no Object3D or all of them have a parent!\n",
@@ -60,7 +65,7 @@ function checkMultibodySystemAndGetWorldAndJoints(instantiatedModel::ModiaLang.S
6065
error("\n", instantiatedModel.modelName, ": The following ", length(object3DWithoutParents), " Object3Ds have no parent\n",
6166
"(note, there must be exactly one Object3D that has no parent):\n", object3DNames, "\n")
6267
end
63-
return (object3DWithoutParents[1], jointObjects)
68+
return (object3DWithoutParents[1], jointObjects, forceElements)
6469
end
6570

6671

@@ -120,7 +125,7 @@ function setModiaJointVariables!(id::Int, _leq_mode, instantiatedModel::ModiaLan
120125
# Instantiate the Modia3D system
121126
#mbsPar = getIdParameter(parameters, id)
122127
#mbsObj = instantiateDependentObjects(instantiatedModel.modelModule, mbsPar)
123-
(world, jointObjects) = checkMultibodySystemAndGetWorldAndJoints(instantiatedModel, id)
128+
(world, jointObjects, forceElements) = checkMultibodySystemAndGetWorldAndJointsAndForceElements(instantiatedModel, id)
124129

125130
# Construct startIndex vector and number of degrees of freedom per joint
126131
nJoints = length(jointObjects)
@@ -134,10 +139,16 @@ function setModiaJointVariables!(id::Int, _leq_mode, instantiatedModel::ModiaLan
134139
end
135140
nqdd = j-1
136141

142+
# Initialize force elements
143+
for force in forceElements
144+
initializeForceElement(force)
145+
end
146+
137147
# Construct MultibodyData
138148
scene = initAnalysis2!(world)
139149
residuals = zeros(FloatType,nqdd)
140150
cache_h = zeros(FloatType,nqdd)
151+
scene.forceElements = forceElements
141152
if scene.options.enableContactDetection
142153
nz = 2
143154
zStartIndex = ModiaLang.addZeroCrossings(instantiatedModel, nz)
@@ -256,11 +267,15 @@ For Modia3D:
256267

257268
function multibodyResiduals3!(sim, scene, world, time, storeResults, isTerminal, leq_mode)
258269
tree = scene.treeForComputation
270+
forceElements = scene.forceElements
259271
visualize = scene.visualize # && sim.model.visualiz
260272
exportAnimation = scene.exportAnimation
261273

262274
if isTerminal #if Modia.isTerminalOfAllSegments(sim)
263275
TimerOutputs.@timeit sim.timer "Modia3D_4 isTerminal" begin
276+
for force in forceElements
277+
terminateForceElement(force)
278+
end
264279
if exportAnimation
265280
Modia3D.exportAnimation(scene)
266281
end
@@ -306,6 +321,11 @@ function multibodyResiduals3!(sim, scene, world, time, storeResults, isTerminal,
306321
end
307322
end # end forward recursion
308323

324+
# Evaluate force elements
325+
for force in forceElements
326+
evaluateForceElement(force)
327+
end
328+
309329
# Compute contact forces/torques
310330
if scene.collide
311331
computeContactForcesAndTorques(sim, scene, world, time, nothing)

‎src/Composition/frameForceTorque.jl‎

Lines changed: 16 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,7 @@ Apply torque vector `torque` resolved in frame `frameCoord` at frame `frameApply
55
66
If `frameCoord` is omitted `torque` is resolved in absolute coordinates.
77
"""
8-
function applyFrameTorque!(frameApply::Object3D, torque::SVector{3,Float64}; frameCoord::Object3D)
8+
function applyFrameTorque!(frameApply::Object3D, torque::SVector{3,Float64}; frameCoord::Union{Object3D, Nothing}=nothing)
99
if !isnothing(frameCoord)
1010
torque_abs = frameCoord.R_abs' * torque # World_torque := R_CoordWorld^T * Coord_torque
1111
else
@@ -23,7 +23,7 @@ Apply force vector `force` resolved in frame `frameCoord` at origin of frame `fr
2323
2424
If `frameCoord` is omitted `force` is resolved in absolute coordinates.
2525
"""
26-
function applyFrameForce!(frameApply::Object3D, force::SVector{3,Float64}; frameCoord::Object3D)
26+
function applyFrameForce!(frameApply::Object3D, force::SVector{3,Float64}; frameCoord::Union{Object3D, Nothing}=nothing)
2727
if !isnothing(frameCoord)
2828
force_abs = frameCoord.R_abs' * force # World_force := R_CoordWorld^T * Coord_force
2929
else
@@ -41,9 +41,9 @@ Apply force and torque vectors `force` and `torque` resolved in frame `frameCoor
4141
4242
If `frameCoord` is omitted `force` and `torque` are resolved in absolute coordinates.
4343
"""
44-
function applyFrameForceTorque!(frameApply::Object3D, force::SVector{3,Float64}, torque::SVector{3,Float64}; frameCoord::Object3D)
45-
applyFrameForce!(frameApply, force; frameCoord)
46-
applyFrameTorque!(frameApply, torque; frameCoord)
44+
function applyFrameForceTorque!(frameApply::Object3D, force::SVector{3,Float64}, torque::SVector{3,Float64}; frameCoord::Union{Object3D, Nothing}=nothing)
45+
applyFrameForce!(frameApply, force; frameCoord=frameCoord)
46+
applyFrameTorque!(frameApply, torque; frameCoord=frameCoord)
4747
return nothing
4848
end
4949

@@ -55,9 +55,9 @@ Apply torque vector `torque` resolved in frame `frameCoord` at origins of frames
5555
5656
If `frameCoord` is omitted torque` is resolved in absolute coordinates.
5757
"""
58-
function applyFrameTorquePair!(frameMeas::Object3D, frameOrig::Object3D, torque::SVector{3,Float64}; frameCoord::Object3D)
59-
applyFrameTorque!(frameMeas, -torque; frameCoord)
60-
applyFrameTorque!(frameOrig, torque; frameCoord)
58+
function applyFrameTorquePair!(frameMeas::Object3D, frameOrig::Object3D, torque::SVector{3,Float64}; frameCoord::Union{Object3D, Nothing}=nothing)
59+
applyFrameTorque!(frameMeas, -torque; frameCoord=frameCoord)
60+
applyFrameTorque!(frameOrig, torque; frameCoord=frameCoord)
6161
return nothing
6262
end
6363

@@ -70,11 +70,11 @@ In addition a compensation torque is applied at frame `frameOrig` to satisfy tor
7070
7171
If `frameCoord` is omitted `torque` is resolved in absolute coordinates.
7272
"""
73-
function applyFrameForcePair!(frameMeas::Object3D, frameOrig::Object3D, force::SVector{3,Float64}; frameCoord::Object3D)
74-
applyFrameForce!(frameMeas, -force; frameCoord)
75-
applyFrameForce!(frameOrig, force; frameCoord)
76-
r_OrigMeas = measFramePosition(frameMeas; frameOrig, frameCoord)
77-
applyFrameTorque!(frameOrig, cross(r_OrigMeas, force); frameCoord) # Coord_t := Coord_r_OrigMeas x Coord_force
73+
function applyFrameForcePair!(frameMeas::Object3D, frameOrig::Object3D, force::SVector{3,Float64}; frameCoord::Union{Object3D, Nothing}=nothing)
74+
applyFrameForce!(frameMeas, -force; frameCoord=frameCoord)
75+
applyFrameForce!(frameOrig, force; frameCoord=frameCoord)
76+
r_OrigMeas = measFramePosition(frameMeas; frameOrig=frameOrig, frameCoord=frameCoord)
77+
applyFrameTorque!(frameOrig, cross(r_OrigMeas, force); frameCoord=frameCoord) # Coord_t := Coord_r_OrigMeas x Coord_force
7878
return nothing
7979
end
8080

@@ -87,8 +87,8 @@ In addition a compensation torque is applied at frame `frameOrig` to satisfy tor
8787
8888
If `frameCoord` is omitted `force` and `torque` are resolved in absolute coordinates.
8989
"""
90-
function applyFrameForceTorquePair!(frameMeas::Object3D, frameOrig::Object3D, force::SVector{3,Float64}; frameCoord::Object3D)
91-
applyFrameTorquePair!(frameMeas, frameOrig, torque; frameCoord)
92-
applyFrameForcePair!(frameMeas, frameOrig, force; frameCoord)
90+
function applyFrameForceTorquePair!(frameMeas::Object3D, frameOrig::Object3D, force::SVector{3,Float64}; frameCoord::Union{Object3D, Nothing}=nothing)
91+
applyFrameTorquePair!(frameMeas, frameOrig, torque; frameCoord=frameCoord)
92+
applyFrameForcePair!(frameMeas, frameOrig, force; frameCoord=frameCoord)
9393
return nothing
9494
end

0 commit comments

Comments
 (0)