diff --git a/src/Composition/ForceElements/Bushing.jl b/src/Composition/ForceElements/Bushing.jl index 13174c3c..833bdd3b 100644 --- a/src/Composition/ForceElements/Bushing.jl +++ b/src/Composition/ForceElements/Bushing.jl @@ -1,15 +1,56 @@ """ 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 @@ -17,21 +58,114 @@ mutable struct Bushing <: Modia3D.AbstractForceElement 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 diff --git a/src/Composition/ForceElements/SpringDamperPtP.jl b/src/Composition/ForceElements/SpringDamperPtP.jl index 2ca731f8..021f8ee2 100644 --- a/src/Composition/ForceElements/SpringDamperPtP.jl +++ b/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 diff --git a/src/Composition/frameMeasurements.jl b/src/Composition/frameMeasurements.jl index e8b599b5..dcc070ac 100644 --- a/src/Composition/frameMeasurements.jl +++ b/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 diff --git a/test/ForceElements/BoxBushing.jl b/test/ForceElements/BoxBushing.jl new file mode 100644 index 00000000..5ca3ffe7 --- /dev/null +++ b/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 diff --git a/test/ForceElements/BoxNonLinearSpringDamperPtP.jl b/test/ForceElements/BoxNonLinearSpringDamperPtP.jl index f2efb664..783a80b6 100644 --- a/test/ForceElements/BoxNonLinearSpringDamperPtP.jl +++ b/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 diff --git a/test/ForceElements/HarmonicOscillator.jl b/test/ForceElements/HarmonicOscillator.jl index 5cbc96dd..941c47c3 100644 --- a/test/ForceElements/HarmonicOscillator.jl +++ b/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) diff --git a/test/includeTests.jl b/test/includeTests.jl index 1703b396..3af18972 100644 --- a/test/includeTests.jl +++ b/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