Merge pull request #50 from ModiaSim/gh_extendForceElementFunctionality

Extend force element functionality

Author: AndreaNeumayr

src/Composition/ForceElements/Bushing.jl

@@ -1,29 +1,36 @@
 
 """
-force = Bushing(; obj1, obj2, stiffness, damping)
+    force = Bushing(; obj1, obj2,
+        nominalForce = Modia3D.ZeroVector3D,
+        stiffness    = Modia3D.ZeroVector3D,
+        damping      = Modia3D.ZeroVector3D )
 
 Return a `force` acting as bushing between `obj1::`[`Object3D`](@ref) and
-`obj2::`[`Object3D`](@ref). Vectors `stiffness` and `damping` define the
-stiffness resp. damping values in x, y and z direction of `obj1`. The
-orientation of `obj2` does not influence the resulting forces.
+`obj2::`[`Object3D`](@ref). Vectors `stiffness`, `damping` and
+`nominalForce` define the stiffness, damping and nominal force values in
+x, y and z direction of `obj1`. The orientation of `obj2` does not
+influence the resulting forces.
 """
 mutable struct Bushing <: Modia3D.AbstractForceElement
 
-    obj1::Modia3D.AbstractObject3D
-    obj2::Modia3D.AbstractObject3D
+    obj1::Object3D
+    obj2::Object3D
 
+    nominalForce::SVector{3,Float64}
     stiffness::SVector{3,Float64}
     damping::SVector{3,Float64}
 
-    function Bushing(; obj1::Modia3D.AbstractObject3D,
-                       obj2::Modia3D.AbstractObject3D,
+    function Bushing(; obj1::Object3D,
+                       obj2::Object3D,
+                       nominalForce::AbstractVector = Modia3D.ZeroVector3D,
                        stiffness::AbstractVector = Modia3D.ZeroVector3D,
-                       damping::AbstractVector   = Modia3D.ZeroVector3D)
+                       damping::AbstractVector = Modia3D.ZeroVector3D)
 
-        stiff = Modia3D.convertAndStripUnit(SVector{3,Float64}, u"N/m" , stiffness)
-        damp  = Modia3D.convertAndStripUnit(SVector{3,Float64}, u"N*s/m", damping)
+        nomForce = Modia3D.convertAndStripUnit(SVector{3,Float64}, u"N"    , nominalForce)
+        stiff    = Modia3D.convertAndStripUnit(SVector{3,Float64}, u"N/m"  , stiffness)
+        damp     = Modia3D.convertAndStripUnit(SVector{3,Float64}, u"N*s/m", damping)
 
-        return new(obj1, obj2, stiff, damp)
+        return new(obj1, obj2, nomForce, stiff, damp)
 
     end
 end
@@ -38,7 +45,9 @@ end
 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
+
+    f12 = force.stiffness .* r12 + force.damping .* v12 + force.nominalForce
+
     applyFrameForcePair!(force.obj2, force.obj1, f12; frameCoord=force.obj1)
     return nothing
 end

src/Composition/ForceElements/SpringDamperPtP.jl

@@ -1,29 +1,58 @@
 
 """
-force = SpringDamperPtP(; obj1, obj2, stiffness, damping)
+    force = SpringDamperPtP(; obj1, obj2,
+        nominalLength  = 0.0,
+        nominalForce   = 0.0,
+        springForceLaw = 0.0,
+        damperForceLaw = 0.0 )
 
 Return a `force` acting as point-to-point parallel spring-damper between
-`obj1::`[`Object3D`](@ref) and `obj2::`[`Object3D`](@ref). Values
-`stiffness` and `damping` define the stiffness resp. damping coefficient.
+`obj1::`[`Object3D`](@ref) and `obj2::`[`Object3D`](@ref).
+
+# Arguments
+
+- `nominalLength` defines the nominal length, i.e. the distance between
+  `obj1` and `obj2` where the deflection of the spring is zero.
+- `nominalForce` defines the nominal force, i.e. the force that acts when
+  spring and damper forces are zero. Positive values represent tension.
+- `springForceLaw` defines the force law of the spring:
+  A `Float64` value represents a linear stiffness coefficient.
+  An univariate `Function` is used to compute the spring force dependent
+  of its deflection. Positive values represent tension.
+- `damperForceLaw` defines the force law of the damper:
+  A `Float64` value represents a linear damping coefficient.
+  An univariate `Function` is used to compute the damper force dependent
+  of its deflection velocity. Positive values represent expansion.
 """
 mutable struct SpringDamperPtP <: Modia3D.AbstractForceElement
 
-    obj1::Modia3D.AbstractObject3D
-    obj2::Modia3D.AbstractObject3D
-
-    stiffness::Float64
-    damping::Float64
+    obj1::Object3D
+    obj2::Object3D
 
-    function SpringDamperPtP(; obj1::Modia3D.AbstractObject3D,
-                               obj2::Modia3D.AbstractObject3D,
-                               stiffness::Float64 = 0.0,
-                               damping::Float64 = 0.0)
+    nominalLength::Float64
+    nominalForce::Float64
+    springForceFunction::Function
+    damperForceFunction::Function
 
-        stiff = Modia3D.convertAndStripUnit(Float64, u"N/m"  , stiffness)
-        damp  = Modia3D.convertAndStripUnit(Float64, u"N*s/m", damping)
+    function SpringDamperPtP(; obj1::Object3D,
+                               obj2::Object3D,
+                               nominalLength::Float64 = 0.0,
+                               nominalForce::Float64 = 0.0,
+                               springForceLaw::Union{Float64, Function} = 0.0,
+                               damperForceLaw::Union{Float64, Function} = 0.0 )
 
-        return new(obj1, obj2, stiff, damp)
+        nomLength = Modia3D.convertAndStripUnit(Float64, u"m", nominalLength)
+        nomForce  = Modia3D.convertAndStripUnit(Float64, u"N", nominalForce)
+        if (typeof(springForceLaw) == Float64)
+            stiffness = Modia3D.convertAndStripUnit(Float64, u"N/m", springForceLaw)
+            springForceLaw = eval(:(fc(pos) = $stiffness * pos))
+        end
+        if (typeof(damperForceLaw) == Float64)
+            damping = Modia3D.convertAndStripUnit(Float64, u"N*s/m", damperForceLaw)
+            damperForceLaw = eval(:(fd(vel) = $damping * vel))
+        end
 
+        return new(obj1, obj2, nomLength, nomForce, springForceLaw, damperForceLaw)
     end
 end
 
@@ -37,8 +66,12 @@ end
 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
-    f12 = frc * norm
+
+    defl = pos - force.nominalLength
+    fc = force.springForceFunction(defl)
+    fd = force.damperForceFunction(vel)
+    f12 = (fc + fd + force.nominalForce) * norm
+
     applyFrameForcePair!(force.obj2, force.obj1, f12; frameCoord=force.obj1)
 end
 

test/ForceElements/BoxNonLinearSpringDamperPtP.jl

@@ -0,0 +1,55 @@
+module BoxNonLinearSpringDamperPtP
+
+using ModiaLang
+
+import Modia3D
+using  Modia3D.ModiaInterface
+
+interpolatedForceLaws = false
+l0 = 0.1
+f0 = 5.0
+fc(x) = sign(x) * 100.0 * abs(x)^1.2
+fd(v) = sign(v) *   2.0 * abs(v)^0.8
+if (interpolatedForceLaws)
+    using Interpolations
+    xc = -1.0:0.1:1.0
+    yc = [fc(x) for x in xc]
+    fc(x) = CubicSplineInterpolation(xc, yc)(x)
+    xd = -10.0:0.1:10.0
+    yd = [fd(x) for x in xd]
+    fd(v) = LinearInterpolation(xd, yd)(v)
+end
+
+SpringDamper = Model(
+    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=:Length)),
+    worldFrame = Object3D(parent=:world,
+                          feature=Visual(shape=CoordinateSystem(length=:Length))),
+    box = Object3D(feature=Solid(shape=Box(lengthX=:Length, lengthY=:Length, lengthZ=:Length),
+                                 massProperties=MassProperties(; mass=:Mass, Ixx=:IMoment, Iyy=:IMoment, Izz=:IMoment),
+                                 visualMaterial=:(visualMaterial))),
+    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, nominalLength=l0, nominalForce=f0, springForceLaw=fc, damperForceLaw=fd)
+)
+
+springDamper = @instantiateModel(buildModia3D(SpringDamper), aliasReduction=false, unitless=true)
+
+stopTime = 5.0
+dtmax = 0.1
+if (interpolatedForceLaws)
+    requiredFinalStates = [-0.018609840194419268, -0.018610063010666804, -0.22128548799442815, 0.3091309586280587, 0.3091307182143652, 0.019491580480008697, -0.01289771100508473, 0.012897263597266288, 8.547830805609731e-5, -0.24873244248570697, 0.2487317559006742, 6.865934996471178e-7]
+else
+    requiredFinalStates = [-0.021471022585945587, -0.021471338063312084, -0.22257979374805176, 0.3258966598257503, 0.3258956068893249, 0.007390650203910464, -0.014365742162847027, 0.014364567804398353, 0.00010407183716342378, -0.2720750966146867, 0.27207423475429426, 8.60986118342952e-7]
+end
+simulate!(springDamper, stopTime=stopTime, dtmax=dtmax, log=true, requiredFinalStates=requiredFinalStates)
+
+@usingModiaPlot
+plot(springDamper, ["joint.r", "joint.v", "joint.rot", "joint.w"], figure=1)
+
+end

test/Basic/BoxSpringDamperPtP.jl test/ForceElements/BoxSpringDamperPtP.jltest/Basic/BoxSpringDamperPtP.jl renamed to test/ForceElements/BoxSpringDamperPtP.jl

@@ -8,20 +8,21 @@ using  Modia3D.ModiaInterface
 SpringDamper = Model(
     Length = 0.1,
     Mass = 1.0,
+    IMoment = 0.1,
     Stiffness = 100.0,
     Damping = 2.0,
     visualMaterial = VisualMaterial(color="IndianRed1", transparency=0.5),
     world = Object3D(feature=Scene(gravityField=UniformGravityField(g=9.81, n=[0, 0, -1]), nominalLength=:Length)),
     worldFrame = Object3D(parent=:world,
                           feature=Visual(shape=CoordinateSystem(length=:Length))),
     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),
+                                 massProperties=MassProperties(; mass=:Mass, Ixx=:IMoment, Iyy=:IMoment, Izz=:IMoment),
                                  visualMaterial=:(visualMaterial))),
     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)
+    force = SpringDamperPtP(obj1=:world, obj2=:boxCornerFrame, springForceLaw=:Stiffness, damperForceLaw=:Damping)
 )
 
 springDamper = @instantiateModel(buildModia3D(SpringDamper), aliasReduction=false, unitless=true)

test/Basic/HarmonicOscillator.jl test/ForceElements/HarmonicOscillator.jltest/Basic/HarmonicOscillator.jl renamed to test/ForceElements/HarmonicOscillator.jl

@@ -8,24 +8,25 @@ using  Modia3D.ModiaInterface
 Oscillator = Model(
     Length = 0.1,
     Mass = 1.0,
+    nomForce = 0.5,
     Stiffness = 100.0,
     Damping = 2.0,
     visualMaterial = VisualMaterial(color="IndianRed1", transparency=0.5),
     world = Object3D(feature=Scene(gravityField=UniformGravityField(g=9.81, n=[0, 0, -1]), nominalLength=:Length)),
     worldFrame = Object3D(parent=:world,
                           feature=Visual(shape=CoordinateSystem(length=:Length))),
     oscillator = 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),
+                                        massProperties=MassProperties(; mass=:Mass, Ixx=0.1, Iyy=0.1, Izz=0.1),
                                         visualMaterial=:(visualMaterial))),
     joint = Prismatic(obj1=:world, obj2=:oscillator, axis=3, s=Var(init=0.0), v=Var(init=0.0)),
-    force = Bushing(obj1=:world, obj2=:oscillator, stiffness=:[0.0, 0.0, Stiffness], damping=:[0.0, 0.0, Damping])
+    force = Bushing(obj1=:world, obj2=:oscillator, nominalForce=:[0.0, 0.0, nomForce], stiffness=:[0.0, 0.0, Stiffness], damping=:[0.0, 0.0, Damping])
 )
 
 oscillator = @instantiateModel(buildModia3D(Oscillator), aliasReduction=false, unitless=true)
 
 stopTime = 5.0
 dtmax = 0.1
-requiredFinalStates = [0.0032763240987720987, -0.09755848695453585]
+requiredFinalStates = [0.0034429013089251376, -0.10253096819858225]
 simulate!(oscillator, stopTime=stopTime, dtmax=dtmax, log=true, requiredFinalStates=requiredFinalStates)
 
 @usingModiaPlot

test/includeTests.jl

@@ -9,8 +9,6 @@ Test.@testset "Basic" begin
     include(joinpath("Basic", "PendulumWithParameterizedDamper.jl"))
     include(joinpath("Basic", "PendulumWithSpring.jl"))
     include(joinpath("Basic", "DoublePendulumWithDampers.jl"))
-    include(joinpath("Basic", "HarmonicOscillator.jl"))
-    include(joinpath("Basic", "BoxSpringDamperPtP.jl"))
     include(joinpath("Basic", "BoxPlanarMotion.jl"))
     include(joinpath("Basic", "ShaftFreeMotion.jl"))
     include(joinpath("Basic", "ShaftFreeMotionAdaptiveRotSequence.jl"))
@@ -20,6 +18,12 @@ Test.@testset "Basic" begin
     end
 end
 
+Test.@testset "Force Elements" begin
+    include(joinpath("ForceElements", "HarmonicOscillator.jl"))
+    include(joinpath("ForceElements", "BoxSpringDamperPtP.jl"))
+    include(joinpath("ForceElements", "BoxNonLinearSpringDamperPtP.jl"))
+end
+
 Test.@testset "Robot" begin
     include(joinpath("Robot", "ServoWithRamp.jl"))
     include(joinpath("Robot", "ServoWithRampAndPrismatic.jl"))