Revolute(..) and Prismatic(..) joints can define axis of rotation/translation optionally as vector, e.g., axis = [1.0, 2.0, 3.0].

Author: MartinOtter

docs/src/index.md

@@ -101,7 +101,9 @@ julia -JModia3D_sysimage.so  (otherwise)
 - Additional keyword arguments of Object3D: `Object3D(..., fixedInParent=true, velocity=[0.0, 0.0, 0.0], angularVelocity=[0.0, 0.0, 0.0])`.
   A freely moving Object3D is defined with `Object3D(..., fixedInParent=false, ...)`. The states and other code for such Object3Ds are
   no longer visible in the generated code (so compilation is faster).
-  
+
+- `Revolute(..)` and `Prismatic(..)` joints can define axis of rotation/translation optionally as vector, e.g., `axis = [1.0, 2.0, 3.0]`.
+
 - New variants of functions: `Modia3D.rot1(angle,v), Modia3D.rot2(angle,v), Modia3D.rot3(angle,v), Modia3D.resolve1(rotation,v2), Modia3D.resolve2(rotation,v1)`.
 
 **Deprecated**
@@ -110,13 +112,13 @@ julia -JModia3D_sysimage.so  (otherwise)
   Note, Object3D has variables `translation, rotation, velocity, angularVelocity` instead of `r, rot, v, w` of `FreeMotion`.
   Furthermore, `angularVelocity` is resolved in the parent `Object3D` whereas `w` in `FreeMotion(obj1=.., obj2=..., ..)` is resolved in
   `obj2` and not in `obj1`. This means in particular that the init/start value `FreeMotion(.., w=Var(start=w_start)...)` needs
-  to be transformed in Object3D with `Object3D(..., fixedInParent=false, rotation=xxx, angularVelocity = Modia3D.resolve1(rotation,w_start))`.
-
+  to be transformed in Object3D with `Object3D(..., fixedInParent=false, rotation=XXX, angularVelocity = Modia3D.resolve1(XXX,w_start))`.
+  As a side effect, 
 
 **Non-backwards compatible changes**
 
 - Since Modia3D 0.11.0 is based on Modia 0.9.0, the non-backwards compatible changes of Modia have also an effect on Modia3D
-  (for details, see the release notes of Modia 0.9.0).
+  (for details, see the release notes of Modia 0.9.0). Typically, this should give problems only in seldomly occuring corner cases.
 
 
 ### Version 0.10.4

src/Composition/joints/Prismatic.jl

@@ -5,26 +5,19 @@
 #   Modia3D.Composition (Modia3D/Composition/_module.jl)
 #
 
-
-
-get_eAxis(::Type{F}, axis::Int) where F <: Modia3D.VarFloatType = axis==  1 ? SVector{3,F}(  1.0,  0.0,  0.0) :
-                       axis==  2 ? SVector{3,F}(  0.0,  1.0,  0.0) :
-                       axis==  3 ? SVector{3,F}(  0.0,  0.0,  1.0) :
-                       axis== -1 ? SVector{3,F}( -1.0,  0.0,  0.0) :
-                       axis== -2 ? SVector{3,F}(  0.0, -1.0,  0.0) :
-                       axis== -3 ? SVector{3,F}(  0.0,  0.0, -1.0) :
-                       error("Modia3D.Composition.Prismatic: axis = ", axis, " but must be 1, 2, 3, -1, -2, or -3.")
-
 """
     joint = Prismatic(; obj1, obj2, axis=1, s=0, v=0, canCollide=true)
 
-Return a `joint` that translates `obj2::`[`Object3D`](@ref) with respect to
-`obj1::`[`Object3D`](@ref) along coordinate axis `axis` (`axis = 1,2,3,-1,-2,-3`)
-of `obj1`. The initial position is `s` and the initial velocity is `v`. Negative
-`axis` values describe axes in negative axis directions, i.e. the signs of the values
-of `s` and `v` are reversed. If `canCollide=false`, no collision detection will occur
-between `obj1` and `obj2` (and `Object3D`s that are directly or indirectly rigidly
-fixed to `obj1` or `obj2`).
+Return a Prismatic `joint` that translates `obj2::`[`Object3D`](@ref) with respect to
+`obj1::`[`Object3D`](@ref) along coordinate axis `axis` (`axis = 1,2,3,-1,-2,-3`) 
+of `obj1` with signed distance `s`. Optionally, `axis` can be a vector, such as `axis = [1.0, 2.0, 3.0]`, 
+defining the direction of the axis of translation in `obj1`. The initial position is `s` 
+and the initial velocity is `v`.
+
+If `canCollide=false`, no collision detection will occur between `obj1` and `obj2` (and `Object3D`s that are
+directly or indirectly rigidly fixed to `obj1` or `obj2`). Note, if both `obj1` and `obj2` have
+solids defined that are overlapping along the axis of translation, then `canCollide=false`
+is the only meaningful value.
 
 It is currently not supported that a `Prismatic` joint *closes a kinematic loop*.
 """
@@ -34,53 +27,51 @@ mutable struct Prismatic{F <: Modia3D.VarFloatType} <: Modia3D.AbstractJoint
     obj1::Object3D{F}
     obj2::Object3D{F}
 
-    posAxis::Int             # = 1,2,3
-    posMovement::Bool        # = true, if movement along posAxis, otherwise in negative posAxis
-    eAxis::SVector{3,F}   # Unit vector in direction of axis
-    ndof::Int
-    canCollide::Bool         # = false, if no collision between obj1 and obj2
+    eAxis::SVector{3,F}   # Translation axis with norm(eAxis) = 1
+    canCollide::Bool      # = false, if no collision between obj1 and obj2
 
     s::F
     v::F
     a::F
 
     function Prismatic{F}(; obj1::Object3D{F},
-                         obj2::Object3D{F},
-                         path::String="",
-                         axis::Int = 1,
-                         s::Real   = F(0.0),
-                         v::Real   = F(0.0),
-                         canCollide::Bool = true) where F <: Modia3D.VarFloatType
+                            obj2::Object3D{F},
+                            path::String="",
+                            axis::Union{Int, AbstractVector} = 1,
+                            s::Real = F(0.0),
+                            v::Real = F(0.0),
+                            canCollide::Bool = true) where F <: Modia3D.VarFloatType
 
         (parent,obj,cutJoint) = attach(obj1, obj2, name = "Prismatic joint")  # an error is triggered if cutJoint=true
 
-        if !(1 <= abs(axis) <= 3)
-            error("\nError from Prismatic joint connecting ", Modia3D.fullName(obj1), " with ", Modia3D.fullName(obj2), ":\n",
-                "    axis = $axis, but must be 1,2,3 or -1,-2-3.!")
+        if typeof(axis) <: Int
+            eAxis =  abs(axis) == 1 ? SVector{3,F}(1, 0, 0) :
+                    (abs(axis) == 2 ? SVector{3,F}(0, 1, 0) :
+                    (abs(axis) == 3 ? SVector{3,F}(0, 0, 1) :
+                        error("\nError from $path = Prismatic(obj1 = :($(obj1.path)), obj2 = :($(obj2.path)), axis = $axis, ...):\n",
+                              "    axis = must be 1,2,3 or -1,-2,-3 or a vector with 3 elements!")))
+            if axis < 0
+                eAxis = -eAxis
+            end
+        else
+            eAxis = normalize(SVector{3,F}(ustrip.(axis)))
+            if !all(isfinite,eAxis)
+                error("\nError from $path = Prismatic(obj1 = :($(obj1.path)), obj2 = :($(obj2.path)), axis = $axis, ...):\n",
+                      "    normalize(axis) results in vector $eAxis that has non-finite elements!")
+            end
         end
 
-        if !(typeof(s) <: AbstractFloat) || !(typeof(v) <: AbstractFloat)
-            error("\nError from Prismatic joint connecting ", Modia3D.fullName(obj1), " with ", Modia3D.fullName(obj2), ":\n",
-                "    s and/or v are not <: AbstractFloat!")
-        end
-
-        axis  = obj === obj2 ? axis : -axis
-        eAxis = get_eAxis(F, axis)
-
+        eAxis = obj === obj2 ? eAxis : -eAxis
         s = Modia3D.convertAndStripUnit(F, u"m"  , s)
         v = Modia3D.convertAndStripUnit(F, u"m/s", v)
 
-        posAxis     = abs(axis)
-        posMovement = axis > 0
-
-        obj.joint      = new(path, parent, obj, posAxis, posMovement, eAxis, 1, canCollide, s, v, F(0.0) )
+        obj.joint      = new(path, parent, obj, eAxis, canCollide, s, v, F(0.0) )
         obj.jointKind  = PrismaticKind
         obj.jointIndex = 0
         obj.ndof       = 1
         obj.canCollide = canCollide
         obj.r_rel      = eAxis*s
         obj.R_rel      = Modia3D.NullRotation(F)
-
         parent.hasChildJoint = true
         return obj.joint
     end

src/Composition/joints/Revolute.jl

@@ -4,19 +4,20 @@
 # This file is part of module
 #   Modia3D.Composition (Modia3D/Composition/_module.jl)
 #
-import Modia3D.Frames
-
 
 """
     joint = Revolute(;obj1, obj2, axis=3, phi=0, w=0, canCollide=false)
 
 Return a Revolute `joint` that rotates `obj1::`[`Object3D`](@ref) into
-`obj2::`[`Object3D`](@ref) along the axis `axis` of `obj1` (`axis = 1,2,3,-1,-2,-3`).
-The initial start angle is `phi` and the initial angular velocity
-is `w`. Negative `axis` values describe axes in negative axis directions, i.e.
-the signs of the values of `phi` and `w` are reversed. If `canCollide=false`, no
-collision detection will occur between `obj1` and `obj2` (and `Object3D`s that are
-directly or indirectly rigidly fixed to `obj1` or `obj2`).
+`obj2::`[`Object3D`](@ref) along the axis `axis` of `obj1` (`axis = 1,2,3,-1,-2,-3`) with angle `phi`.
+Optionally, `axis` can be a vector, such as `axis = [1.0, 2.0, 3.0]`, defining the direction of the
+axis of rotation in `obj1`. The initial start angle is `phi` and the initial angular velocity
+is `w`.
+
+If `canCollide=false`, no collision detection will occur between `obj1` and `obj2` (and `Object3D`s that are
+directly or indirectly rigidly fixed to `obj1` or `obj2`). Note, if both `obj1` and `obj2` have
+solids defined that are overlapping around the axis of rotation, then collision will be permanently
+occuring and `canCollide=false` is the only meaningful value.
 
 It is currently not supported that a `Revolute` joint *closes a kinematic loop*.
 """
@@ -26,49 +27,51 @@ mutable struct Revolute{F <: Modia3D.VarFloatType} <: Modia3D.AbstractJoint
     obj1::Object3D{F}
     obj2::Object3D{F}
 
-    posAxis::Int      # = 1,2,3
-    posMovement::Bool # = true, if movement along posAxis, otherwise in negative posAxis
-    ndof::Int
-    canCollide::Bool  # = false, if no collision between obj1 and obj2
+    eAxis::SVector{3,F}   # Rotation axis with norm(eAxis) = 1
+    canCollide::Bool      # = false, if no collision between obj1 and obj2
 
     phi::F
     w::F
     a::F
 
     function Revolute{F}(; obj1::Object3D{F},
-                        obj2::Object3D{F},
-                        path::String="",
-                        axis::Int = 3,
-                        phi::Real = F(0.0),
-                        w::Real   = F(0.0),
-                        canCollide::Bool = false) where F <: Modia3D.VarFloatType
+                           obj2::Object3D{F},
+                           path::String="",
+                           axis::Union{Int, AbstractVector} = 3,
+                           phi::Real = F(0.0),
+                           w::Real   = F(0.0),
+                           canCollide::Bool = false) where F <: Modia3D.VarFloatType
 
         (parent,obj,cutJoint) = attach(obj1, obj2, name = "Revolute joint")  # an error is triggered if cutJoint=true
 
-        if !(1 <= abs(axis) <= 3)
-            error("\nError from Revolute joint connecting ", Modia3D.fullName(obj1), " with ", Modia3D.fullName(obj2), ":\n",
-                "    axis = $axis, but must be 1,2,3 or -1,-2-3.!")
+        if typeof(axis) <: Int
+            eAxis =  abs(axis) == 1 ? SVector{3,F}(1, 0, 0) :
+                    (abs(axis) == 2 ? SVector{3,F}(0, 1, 0) :
+                    (abs(axis) == 3 ? SVector{3,F}(0, 0, 1) :
+                        error("\nError from $path = Revolute(obj1 = :($(obj1.path)), obj2 = :($(obj2.path)), axis = $axis, ...):\n",
+                              "    axis = must be 1,2,3 or -1,-2,-3 or a vector with 3 elements!")))
+            if axis < 0
+                eAxis = -eAxis
+            end
+        else
+            eAxis = normalize(SVector{3,F}(ustrip.(axis)))
+            if !all(isfinite,eAxis)
+                error("\nError from $path = Revolute(obj1 = :($(obj1.path)), obj2 = :($(obj2.path)), axis = $axis, ...):\n",
+                      "    normalize(axis) results in vector $eAxis that has non-finite elements!")
+            end
         end
 
-        if !(typeof(phi) <: AbstractFloat) || !(typeof(w) <: AbstractFloat)
-            error("\nError from Revolute joint connecting ", Modia3D.fullName(obj1), " with ", Modia3D.fullName(obj2), ":\n",
-                "    phi and/or w are not <: AbstractFloat!")
-        end
-
-        axis = obj === obj2 ? axis : -axis
+        eAxis = obj === obj2 ? eAxis : -eAxis
         phi = Modia3D.convertAndStripUnit(F, u"rad"  , phi)
         w   = Modia3D.convertAndStripUnit(F, u"rad/s", w)
 
-        posAxis     = abs(axis)
-        posMovement = axis > 0
-
-        obj.joint = new(path, parent, obj, posAxis, posMovement, 1, canCollide, phi, w, F(0.0))
+        obj.joint = new(path, parent, obj, eAxis, canCollide, phi, w, F(0.0))
         obj.jointKind  = RevoluteKind
         obj.jointIndex = 0
         obj.ndof       = 1
         obj.canCollide = canCollide
         obj.r_rel      = Modia3D.ZeroVector3D(F)
-        obj.R_rel      = Frames.rotAxis(posAxis, posMovement, phi)
+        obj.R_rel      = Modia3D.rot_e(eAxis, phi)
         parent.hasChildJoint = true
         return obj.joint
     end

src/Composition/joints/joints.jl

@@ -347,14 +347,14 @@ function computePositionsVelocitiesAccelerations!(mbs::MultibodyData{F,TimeType}
             revolute  = mbs.revolute[obj.jointIndex]
 
             obj.r_abs = parent.r_abs
-            obj.R_rel = Frames.rotAxis(revolute.posAxis, revolute.posMovement, revolute.phi)
+            obj.R_rel = Frames.rot_e(revolute.eAxis, revolute.phi)
             obj.R_abs = obj.R_rel*parent.R_abs
 
             obj.v0 = parent.v0
             obj.a0 = parent.a0
 
-            w_rel = Frames.axisValue(revolute.posAxis, revolute.posMovement, revolute.w)
-            z_rel = Frames.axisValue(revolute.posAxis, revolute.posMovement, revolute.a)
+            w_rel = revolute.eAxis*revolute.w
+            z_rel = revolute.eAxis*revolute.a
 
             obj.w = obj.R_rel*(parent.w + w_rel)
             obj.z = obj.R_rel*(parent.z + z_rel + cross(parent.w, w_rel))
@@ -449,13 +449,12 @@ function computeAccelerations!(mbs::MultibodyData{F,TimeType}, tree::Vector{Obje
             revolute = mbs.revolute[obj.jointIndex]
 
             obj.a0 = parent.a0
-            z_rel  = Frames.axisValue(revolute.posAxis, revolute.posMovement, revolute.a)
+            z_rel  = revolute.eAxis*revolute.a
             obj.z  = obj.R_rel*(parent.z + z_rel)
 
         elseif jointKind == PrismaticKind
             prismatic = mbs.prismatic[obj.jointIndex]
 
-            eAxis  = prismatic.posMovement ? prismatic.posAxis : -prismatic.posAxis
             a_rel  = prismatic.eAxis*prismatic.a
             obj.a0 = parent.a0 + parent.R_abs'*(a_rel + cross(parent.z, obj.r_rel))
             obj.z  = parent.z
@@ -516,7 +515,7 @@ function computeObject3DForcesTorquesAndGenForces!(mbs::MultibodyData{F,TimeType
             parent.t += obj.R_rel'*obj.t
             obj.f     = -obj.f
             obj.t     = -obj.t
-            revoluteGenForces[obj.jointIndex] = revolute.posMovement ? obj.t[revolute.posAxis] : -obj.t[revolute.posAxis]
+            revoluteGenForces[obj.jointIndex] = dot(revolute.eAxis,obj.t)
 
         elseif jointKind == PrismaticKind
             prismatic = mbs.prismatic[obj.jointIndex]
@@ -858,7 +857,7 @@ getGenForcesHiddenJoints(mbs, qdd_hidden) = mbs.hiddenGenForces
 function setAngle!(revolute::Revolute, phi::F) where F <: Modia3D.VarFloatType
    obj          = revolute.obj2
    revolute.phi = phi
-   obj.R_rel    = Frames.rotAxis(revolute.posAxis, revolute.posMovement, phi)
+   obj.R_rel    = Frames.rot_e(revolute.eAxis, phi)
    obj.R_abs    = obj.R_rel*obj.parent.R_abs
    return revolute
 end

src/Frames/rotationMatrix.jl

@@ -145,10 +145,10 @@ with `angle` if `positive=true` and otherwise with `-angle`.
 """
     R = Modia3D.rot_e(e, angle)
 
-Return rotation matrix that rotates around angle `angle` along unit axis `e`.
-This function assumes that `norm(e) == 1`.
+Return rotation matrix that rotates frame1 around `angle` along unit axis `e`
+arriving at frame2. This function assumes that `norm(e) == 1`.
 """
-@inline function rot_e(e::AbstractVector,angle::Number)
+@inline function rot_e(e::AbstractVector, angle::Number)
     (s,c) = sincos(angle)
     F = typeof(s)
     return e*e' + (NullRotation(F) - e*e')*c - skew(e)*s

src/Modia3D.jl

@@ -5,7 +5,7 @@ module Modia3D
 
 const path = dirname(dirname(@__FILE__))   # Absolute path of package directory
 const Version = "0.11.0-dev"
-const Date = "2022-05-26"
+const Date = "2022-05-29"
 
 println("\nImporting Modia3D Version $Version ($Date)")
 

test/Basic/ModelsForPrecompilation.jl

@@ -13,7 +13,7 @@ Dummy = Model3D(
     rev    = Revolute(obj1=:world, obj2=:frame1),
 
     body2  = Object3D(feature=Solid(shape=Box(lengthX=1.0, lengthY=0.1, lengthZ=0.1), massProperties=MassProperties(mass=1.0))),
-    prism  = Prismatic(obj1=:world , obj2=:body2, axis=2),
+    prism  = Prismatic(obj1=:world , obj2=:body2, axis=[0.0, 20.0, 0.0]),
 
     body3  = Object3D(parent=:world, fixedToParent=:false,
                                     translation=[0.0, 1.0, 0.0],

test/Basic/Pendulum.jl

@@ -6,7 +6,7 @@ Pendulum = Model3D(
     world     = Object3D(feature=Scene()),
     body      = Object3D(feature=Solid(massProperties=MassProperties(mass=1.0))),
     bodyFrame = Object3D(parent=:body, translation=[-0.5, 0.0, 0.0]),
-    rev       = Revolute(obj1=:world, obj2=:bodyFrame)
+    rev       = Revolute(obj1=:world, obj2=:bodyFrame, axis=[0.0, 0.0, 1.0])
 )
 
 pendulum = @instantiateModel(Pendulum, unitless=true, log=false, logDetails=false, logCode=true, logStateSelection=false, logCalculations=false)