Merge pull request #52 from ModiaSim/gh_extendBushingFunctionality

Gh extend bushing functionality

Author: AndreaNeumayr

src/Composition/ForceElements/Bushing.jl

@@ -1,37 +1,171 @@
 
 """
     force = Bushing(; obj1, obj2,
-        nominalForce = Modia3D.ZeroVector3D,
-        stiffness    = Modia3D.ZeroVector3D,
-        damping      = Modia3D.ZeroVector3D )
+        nominalForce      = Modia3D.ZeroVector3D,
+        springForceLaw    = Modia3D.ZeroVector3D,
+        damperForceLaw    = Modia3D.ZeroVector3D,
+        nominalTorque     = Modia3D.ZeroVector3D,
+        rotSpringForceLaw = Modia3D.ZeroVector3D,
+        rotDamperForceLaw = Modia3D.ZeroVector3D,
+        largeAngles       = false )
 
 Return a `force` acting as bushing between `obj1::`[`Object3D`](@ref) and
-`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.
+`obj2::`[`Object3D`](@ref). The force directions are defined by `obj1`,
+i.e. the orientation of `obj2` does not influence the resulting forces.
+
+# Arguments
+
+- `nominalForce` defines the nominal force vector, i.e. the force that
+  acts when spring and damper forces are zero. Positive values act in
+  positive axis directions at `obj1` and in opposite directions at `obj2`.
+- `springForceLaw` defines the force law of the spring in x-, y- and
+  z-direction:
+  - A `Float64` value represents a linear stiffness coefficient.
+  - An univariate `Function` is used to compute the spring force
+    dependent of its deflection.
+- `damperForceLaw` defines the force law of the damper in x-, y- and
+  z-direction:
+  - A `Float64` value represents a linear damping coefficient.
+  - An univariate `Function` is used to compute the damper force
+    dependent of its deflection velocity.
+- `nominalTorque` defines nominal torques about alpha, beta and gamma
+  directions.
+- `rotSpringForceLaw` defines the force law of the rotational spring
+  about alpha-, beta- and gamma-direction:
+  - A `Float64` value represents a linear damping coefficient.
+  - An univariate `Function` is used to compute the spring force
+    dependent of its deflection.
+- `rotDamperForceLaw` defines the force law of the rotational damper
+  about alpha-, beta- and gamma-direction:
+  - A `Float64` value represents a linear damping coefficient.
+  - An univariate `Function` is used to compute the damper force
+    dependent of its deflection velocity.
+- `largeAngles` can be used to enable large angle mode.
+  - When disabled, small deformation angles (< 10°) are assumed. This
+    option deals equally with rotations [alpha, beta gamma] about the
+    axes [x, y, z] of `obj1`, but causes approximation errors for
+    larger angles.
+  - When enabled, the deformation angles and torque directions are
+    calculated as [Cardan (Tait–Bryan) angles](https://en.wikipedia.org/wiki/Euler_angles#Chained_rotations_equivalence)
+    (rotation sequence x-y-z from `obj1` to `obj2`). This option
+    supports angles up to nearly 90°, but introduces local rotation
+    directions [alpha, beta gamma] which differ from the axes [x, y, z]
+    of `obj1` and increases computation effort.
 """
 mutable struct Bushing <: Modia3D.AbstractForceElement
 
     obj1::Object3D
     obj2::Object3D
 
     nominalForce::SVector{3,Float64}
-    stiffness::SVector{3,Float64}
-    damping::SVector{3,Float64}
+    springForceFunction::SVector{3,Function}
+    damperForceFunction::SVector{3,Function}
+    nominalTorque::SVector{3,Float64}
+    rotSpringForceFunction::SVector{3,Function}
+    rotDamperForceFunction::SVector{3,Function}
+    largeAngles::Bool
 
     function Bushing(; obj1::Object3D,
                        obj2::Object3D,
                        nominalForce::AbstractVector = Modia3D.ZeroVector3D,
-                       stiffness::AbstractVector = Modia3D.ZeroVector3D,
-                       damping::AbstractVector = Modia3D.ZeroVector3D)
+                       springForceLaw::AbstractVector = Modia3D.ZeroVector3D,
+                       damperForceLaw::AbstractVector = Modia3D.ZeroVector3D,
+                       nominalTorque::AbstractVector = Modia3D.ZeroVector3D,
+                       rotSpringForceLaw::AbstractVector = Modia3D.ZeroVector3D,
+                       rotDamperForceLaw::AbstractVector = Modia3D.ZeroVector3D,
+                       largeAngles::Bool = false )
+        for dir in 1:3
+            @assert(typeof(springForceLaw[dir]) == Float64 || isa(springForceLaw[dir], Function))
+            @assert(typeof(damperForceLaw[dir]) == Float64 || isa(damperForceLaw[dir], Function))
+            @assert(typeof(rotSpringForceLaw[dir]) == Float64 || isa(rotSpringForceLaw[dir], Function))
+            @assert(typeof(rotDamperForceLaw[dir]) == Float64 || isa(rotDamperForceLaw[dir], Function))
+        end
+
+        nomForce  = Modia3D.convertAndStripUnit(SVector{3,Float64}, u"N"  , nominalForce)
+        nomTorque = Modia3D.convertAndStripUnit(SVector{3,Float64}, u"N*m", nominalTorque)
+        springForceFunction = Vector{Function}(undef, 3)
+        damperForceFunction = Vector{Function}(undef, 3)
+        rotSpringForceFunction = Vector{Function}(undef, 3)
+        rotDamperForceFunction = Vector{Function}(undef, 3)
+        irand = rand(Int)
+        for dir in 1:3
+            if (typeof(springForceLaw[dir]) == Float64)
+                stiffness = Modia3D.convertAndStripUnit(Float64, u"N/m", springForceLaw[dir])
+                fsymb = Symbol("fc", dir, "_", irand)  # todo: replace irand by force.path
+                springForceFunction[dir] = eval(:($fsymb(pos) = $stiffness * pos))
+            else
+                springForceFunction[dir] = springForceLaw[dir]
+            end
+            if (typeof(damperForceLaw[dir]) == Float64)
+                damping = Modia3D.convertAndStripUnit(Float64, u"N*s/m", damperForceLaw[dir])
+                fsymb = Symbol("fd", dir, "_", irand)  # todo: replace irand by force.path
+                damperForceFunction[dir] = eval(:($fsymb(vel) = $damping * vel))
+            else
+                damperForceFunction[dir] = damperForceLaw[dir]
+            end
+            if (typeof(rotSpringForceLaw[dir]) == Float64)
+                stiffness = Modia3D.convertAndStripUnit(Float64, u"N*m/rad", rotSpringForceLaw[dir])
+                fsymb = Symbol("mc", dir, "_", irand)  # todo: replace irand by force.path
+                rotSpringForceFunction[dir] = eval(:($fsymb(ang) = $stiffness * ang))
+            else
+                rotSpringForceFunction[dir] = rotSpringForceLaw[dir]
+            end
+            if (typeof(rotDamperForceLaw[dir]) == Float64)
+                damping = Modia3D.convertAndStripUnit(Float64, u"N*m*s/rad", rotDamperForceLaw[dir])
+                fsymb = Symbol("md", dir, "_", irand)  # todo: replace irand by force.path
+                rotDamperForceFunction[dir] = eval(:($fsymb(angd) = $damping * angd))
+            else
+                rotDamperForceFunction[dir] = rotDamperForceLaw[dir]
+            end
+        end
 
-        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, nomForce, springForceFunction, damperForceFunction, nomTorque, rotSpringForceFunction, rotDamperForceFunction, largeAngles)
+    end
+end
 
-        return new(obj1, obj2, nomForce, stiff, damp)
 
+# Compute deformation angles from rotation matrix
+function anglesFromRotation(largeAngles::Bool, R12::SMatrix{3,3,Float64}, w12::SVector{3,Float64})
+    if largeAngles
+        sbe = clamp(R12[3,1], -1.0, 1.0)
+        cbe = sqrt(1.0 - sbe*sbe)
+        if (cbe > 1e-12)
+            sal = -R12[3,2]/cbe
+            cal =  R12[3,3]/cbe
+            al = atan(-R12[3,2], R12[3,3])
+            be = asin(sbe)
+            ga = atan(-R12[2,1], R12[1,1])
+            ald = w12[1] + (sal*w12[2] - cal*w12[3])*sbe/cbe
+            bed = cal*w12[2] + sal*w12[3]
+            gad = (-sal*w12[2] + cal*w12[3])/cbe
+            return (@SVector[al, be, ga], @SVector[ald, bed, gad], @SMatrix[sal sbe; cal cbe])
+        else
+            @error("Gimbal lock of Bushing transformation.")
+            return (@SVector[0.0, 0.0, 0.0], @SVector[0.0, 0.0, 0.0], @SMatrix[0.0 0.0; 0.0 0.0])
+        end
+    else
+        al = R12[2,3]
+        be = R12[3,1]
+        ga = R12[1,2]
+        ald = w12[1]
+        bed = w12[2]
+        gad = w12[3]
+        if (max(abs(al), abs(be), abs(ga)) > 0.174)
+            @warn("Bushing angle exceeds 10 deg.")
+        end
+        return (@SVector[al, be, ga], @SVector[ald, bed, gad], @SMatrix[0.0 0.0; 0.0 0.0])
+    end
+end
+
+# Compute torque vector from force law moments
+function torqueFromMoments(largeAngles::Bool, moments::SVector{3,Float64}, sico::SMatrix{2,2,Float64,4})
+    if largeAngles
+        tx = moments[1] + sico[1,2]*moments[3]
+        ty = sico[2,1]*moments[2] - sico[1,1]*sico[2,2]*moments[3]
+        tz = sico[1,1]*moments[2] + sico[2,1]*sico[2,2]*moments[3]
+        return @SVector[tx, ty, tz]
+    else
+        return moments
     end
 end
 
@@ -43,12 +177,22 @@ function initializeForceElement(force::Bushing)
 end
 
 function evaluateForceElement(force::Bushing)
+    R12 = measFrameRotation(force.obj2; frameOrig=force.obj1)
     r12 = measFramePosition(force.obj2; frameOrig=force.obj1, frameCoord=force.obj1)
+    w12 = measFrameRotVelocity(force.obj2; frameOrig=force.obj1, frameCoord=force.obj1)
     v12 = measFrameTransVelocity(force.obj2; frameOrig=force.obj1, frameCoord=force.obj1, frameObsrv=force.obj1)
+    (ang, angd, sico) = anglesFromRotation(force.largeAngles, R12, w12)
 
-    f12 = force.stiffness .* r12 + force.damping .* v12 + force.nominalForce
+    f12 = Vector{Float64}(undef, 3)
+    mom = Vector{Float64}(undef, 3)
+    for dir in 1:3
+        f12[dir] = force.springForceFunction[dir](r12[dir]) + force.damperForceFunction[dir](v12[dir]) + force.nominalForce[dir]
+        mom[dir] = force.rotSpringForceFunction[dir](ang[dir]) + force.rotDamperForceFunction[dir](angd[dir]) + force.nominalTorque[dir]
+    end
+    t12 = torqueFromMoments(force.largeAngles, SVector{3}(mom), sico)
 
-    applyFrameForcePair!(force.obj2, force.obj1, f12; frameCoord=force.obj1)
+    applyFrameForcePair!(force.obj2, force.obj1, SVector{3}(f12); frameCoord=force.obj1)
+    applyFrameTorquePair!(force.obj2, force.obj1, t12; frameCoord=force.obj1)
     return nothing
 end
 

src/Composition/ForceElements/SpringDamperPtP.jl

@@ -16,13 +16,14 @@ Return a `force` acting as point-to-point parallel spring-damper between
 - `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.
+  - 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.
+  - 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
 
@@ -43,12 +44,15 @@ mutable struct SpringDamperPtP <: Modia3D.AbstractForceElement
 
         nomLength = Modia3D.convertAndStripUnit(Float64, u"m", nominalLength)
         nomForce  = Modia3D.convertAndStripUnit(Float64, u"N", nominalForce)
+        irand = rand(Int)
         if (typeof(springForceLaw) == Float64)
             stiffness = Modia3D.convertAndStripUnit(Float64, u"N/m", springForceLaw)
-            springForceLaw = eval(:(fc(pos) = $stiffness * pos))
+            fsymb = Symbol("fc", "_", irand)  # todo: replace irand by force.path
+            springForceLaw = eval(:($fsymb(pos) = $stiffness * pos))
         end
         if (typeof(damperForceLaw) == Float64)
             damping = Modia3D.convertAndStripUnit(Float64, u"N*s/m", damperForceLaw)
+            fsymb = Symbol("fd", "_", irand)  # todo: replace irand by force.path
             damperForceLaw = eval(:(fd(vel) = $damping * vel))
         end
 

src/Composition/frameMeasurements.jl

@@ -5,7 +5,7 @@ Return relative RotationMatrix `R` from frame `frameOrig` into frame `frameMeas`
 
 If `frameOrig` is omitted `R` represents the absolute rotation of `frameMeas`.
 """
-function measFrameRotation(frameMeas::Object3D; frameOrig::Object3D)
+function measFrameRotation(frameMeas::Object3D; frameOrig::Union{Object3D, Nothing}=nothing)
     R_MeasOrig = copy(frameMeas.R_abs)  # R_MeasOrig := R_MeasWorld
     if !isnothing(frameOrig)
         R_MeasOrig = R_MeasOrig * frameOrig.R_abs'  # R_MeasOrig := R_MeasOrig * R_OrigWorld^T

test/ForceElements/BoxBushing.jl

@@ -0,0 +1,50 @@
+module BoxBushingSimulation
+
+using ModiaLang
+
+import Modia3D
+using  Modia3D.ModiaInterface
+
+const largeAngles = true
+if largeAngles
+    startAngles = [0.8, 0.4, 0.2]
+else
+    startAngles = [0.12, 0.06, 0.03]
+end
+fc(p) = 50.0 * p
+fd(v) = 2.0 * v
+mc(a) = 20.0 * a
+md(w) = 0.2 * w
+
+BoxBushing = 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))),
+    joint = FreeMotion(obj1=:world, obj2=:box, r=Var(init=[0.2, 0.1, 0.05]), rot=Var(init=startAngles)),
+    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)
+)
+
+boxBushing = @instantiateModel(buildModia3D(BoxBushing), aliasReduction=false, unitless=true)
+
+stopTime = 5.0
+dtmax = 0.1
+if largeAngles
+    requiredFinalStates = [-0.013214812736859366, 0.0005523898753356359, -0.04904666002334838, 0.0757946142513073, 0.003341416782112683, -4.954082753506823e-5, -0.056708142772310295, 0.0009597147602342456, 4.340509280339362e-5, 0.3809257838547459, 0.001097814904879805, -0.00018842580793933223]
+else
+    requiredFinalStates = [-0.01321449035293881, 0.0005522522622707078, -0.04904666423238891, 0.07579225811468479, 0.0033409499586830368, -4.943166272627969e-5, -0.008049782190986742, 0.00034916853581158153, 1.460039207707777e-6, 0.04510769348383226, 0.0018233644599616554, 9.80056841368889e-6]
+end
+simulate!(boxBushing, stopTime=stopTime, dtmax=dtmax, log=true, requiredFinalStates=requiredFinalStates)
+
+@usingModiaPlot
+plot(boxBushing, ["joint.r", "joint.v", "joint.rot", "joint.w"], figure=1)
+
+end

test/ForceElements/BoxNonLinearSpringDamperPtP.jl

@@ -5,7 +5,7 @@ using ModiaLang
 import Modia3D
 using  Modia3D.ModiaInterface
 
-interpolatedForceLaws = false
+const interpolatedForceLaws = false
 l0 = 0.1
 f0 = 5.0
 fc(x) = sign(x) * 100.0 * abs(x)^1.2

test/ForceElements/HarmonicOscillator.jl

@@ -19,7 +19,7 @@ Oscillator = Model(
                                         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, nominalForce=:[0.0, 0.0, nomForce], stiffness=:[0.0, 0.0, Stiffness], damping=:[0.0, 0.0, Damping])
+    force = Bushing(obj1=:world, obj2=:oscillator, nominalForce=:[0.0, 0.0, nomForce], springForceLaw=:[0.0, 0.0, Stiffness], damperForceLaw=:[0.0, 0.0, Damping])
 )
 
 oscillator = @instantiateModel(buildModia3D(Oscillator), aliasReduction=false, unitless=true)

test/includeTests.jl

@@ -20,6 +20,7 @@ end
 
 Test.@testset "Force Elements" begin
     include(joinpath("ForceElements", "HarmonicOscillator.jl"))
+    include(joinpath("ForceElements", "BoxBushing.jl"))
     include(joinpath("ForceElements", "BoxSpringDamperPtP.jl"))
     include(joinpath("ForceElements", "BoxNonLinearSpringDamperPtP.jl"))
 end