Enable rotations in Bushing

- Enable rotational force laws.
- Add test model BoxBushing.
- Fix optional argument handling in function measFrameRotation.

Author: GerhardHippmann

src/Composition/ForceElements/Bushing.jl

@@ -1,15 +1,44 @@
 
 """
     force = Bushing(; obj1, obj2,
-        nominalForce = Modia3D.ZeroVector3D,
-        stiffness    = Modia3D.ZeroVector3D,
-        damping      = Modia3D.ZeroVector3D )
+        nominalForce  = Modia3D.ZeroVector3D,
+        stiffness     = Modia3D.ZeroVector3D,
+        damping       = Modia3D.ZeroVector3D,
+        nominalTorque = Modia3D.ZeroVector3D,
+        rotStiffness  = Modia3D.ZeroVector3D,
+        rotDamping    = 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`.
+- `stiffness` defines linear stiffness coefficients in x-, y- and
+  z-direction.
+- `damping` defines linear damping coefficients in x-, y- and
+  z-direction.
+- `nominalTorque` defines nominal torques about alpha, beta and gamma
+  directions.
+- `rotStiffness` defines linear stiffness coefficients about alpha-,
+  beta- and gamma-direction.
+- `rotDamping` defines linear damping coefficients about alpha-,
+  beta- and gamma-direction.
+- `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
 
@@ -19,19 +48,76 @@ mutable struct Bushing <: Modia3D.AbstractForceElement
     nominalForce::SVector{3,Float64}
     stiffness::SVector{3,Float64}
     damping::SVector{3,Float64}
+    nominalTorque::SVector{3,Float64}
+    rotStiffness::SVector{3,Float64}
+    rotDamping::SVector{3,Float64}
+    largeAngles::Bool
 
     function Bushing(; obj1::Object3D,
                        obj2::Object3D,
                        nominalForce::AbstractVector = Modia3D.ZeroVector3D,
                        stiffness::AbstractVector = Modia3D.ZeroVector3D,
-                       damping::AbstractVector = Modia3D.ZeroVector3D)
+                       damping::AbstractVector = Modia3D.ZeroVector3D,
+                       nominalTorque::AbstractVector = Modia3D.ZeroVector3D,
+                       rotStiffness::AbstractVector = Modia3D.ZeroVector3D,
+                       rotDamping::AbstractVector = Modia3D.ZeroVector3D,
+                       largeAngles::Bool = false )
+
+        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)
+        nomTorque = Modia3D.convertAndStripUnit(SVector{3,Float64}, u"N*m"      , nominalTorque)
+        rotStiff  = Modia3D.convertAndStripUnit(SVector{3,Float64}, u"N*m/rad"  , rotStiffness)
+        rotDamp   = Modia3D.convertAndStripUnit(SVector{3,Float64}, u"N*m*s/rad", rotDamping)
 
-        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, stiff, damp, nomTorque, rotStiff, rotDamp, largeAngles)
 
-        return new(obj1, obj2, nomForce, stiff, damp)
+    end
+end
+
+
+# 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 +129,18 @@ 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
+    mom = force.rotStiffness .* ang + force.rotDamping .* angd + force.nominalTorque
+    t12 = torqueFromMoments(force.largeAngles, mom, sico)
 
     applyFrameForcePair!(force.obj2, force.obj1, 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
 

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,46 @@
+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
+
+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,
+        stiffness=[50.0, 100.0, 200.0], damping=[1.0, 2.0, 4.0],
+        rotStiffness=[5.0, 10.0, 20.0], rotDamping=[0.1, 0.2, 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