Merge pull request #83 from JuliaComputing/rotations

simplify and document orientation interface

Author: baggepinnen

docs/make.jl

@@ -33,6 +33,7 @@ makedocs(;
                  "Ropes, cables and chains" => "examples/ropes_and_cables.md",
                  "Bodies in space" => "examples/space.md",
              ],
+             "Rotations and orientation" => "rotations.md",
              "3D rendering" => "rendering.md",
          ])
 

docs/src/examples/free_motion.md

@@ -53,7 +53,7 @@ nothing # hide
 If we instead model a body suspended in springs without the presence of any joint, we need to _give the body state variables_. We do this by saying `isroot = true` when we create the body, we also use quaternions to represent angular state using `quat = true`.
 
 ```@example FREE_MOTION
-using Multibody.Rotations: QuatRotation, RotXYZ
+using Multibody.Rotations: QuatRotation, RotXYZ, params
 @named begin
     body = BodyShape(m = 1, I_11 = 1, I_22 = 1, I_33 = 1, r = [0.4, 0, 0],
                      r_0 = [0.2, -0.5, 0.1], isroot = true, quat=true)
@@ -80,8 +80,8 @@ ssys = structural_simplify(IRSystem(model))
 prob = ODEProblem(ssys, [
     collect(body.body.v_0 .=> 0);
     collect(body.body.w_a .=> 0);
-    collect(body.body.Q) .=> vec(QuatRotation(RotXYZ(deg2rad.((10,10,10))...)));
-    collect(body.body.Q̂) .=> vec(QuatRotation(RotXYZ(deg2rad.((10,10,10))...)));
+    collect(body.body.Q) .=> params(QuatRotation(RotXYZ(deg2rad.((10,10,10))...)));
+    collect(body.body.Q̂) .=> params(QuatRotation(RotXYZ(deg2rad.((10,10,10))...)));
 ], (0, 4))
 
 sol = solve(prob, Rodas5P())

docs/src/examples/three_springs.md

@@ -40,7 +40,7 @@ eqs = [connect(world.frame_b, bar1.frame_a)
 ssys = structural_simplify(IRSystem(model))
 prob = ODEProblem(ssys, [], (0, 10))
 
-sol = solve(prob, FBDF(), u0=prob.u0 .+ 1e-12*randn(length(prob.u0)))
+sol = solve(prob, Rodas4())
 @assert SciMLBase.successful_retcode(sol)
 
 Plots.plot(sol, idxs = [body1.r_0...])

docs/src/rotations.md

@@ -0,0 +1,60 @@
+# Working with orientation and rotation
+
+Orientations and rotations in 3D can be represented in multiple different ways. Components which (may) have a 3D angular state, such as [`Body`](@ref), [`Spherical`](@ref) and [`FreeMotion`](@ref), offer the user to select the orientation representation, either Euler angles or quaternions.
+
+## Euler angles
+[Euler angles](https://en.wikipedia.org/wiki/Euler_angles) represent orientation using rotations around three axes, and thus uses three numbers to represent the orientation. The benefit of this representation is that it is minimal (only three numbers used), but the drawback is that any 3-number orientation representation suffers from a kinematic singularity. This representation is beneficial when you know that the singularity will not come into play in your simulation.
+
+Most components that may use Euler angles also allow you to select the sequence of axis around which to perform the rotations, e.g., `sequence = [1,2,3]` performs rotations around ``x`` first, then ``y`` and ``z``.
+
+## Quaternions
+A [quaternion](https://en.wikipedia.org/wiki/Quaternion) represents an orientation using 4 numbers. This is a non-minimal representation, but in return it is also singularity free. Multibody.jl uses non-unit quaternions to represent orientation when `quat = true` is provided to components that support this option. The convention used for quaternions is ``[s, v_1, v_2, v_3]``, sometimes also referred to as ``[w, i, j, k]``, i.e., the real/scalar part comes first, followed by the three imaginary numbers.
+
+Multibody.jl depends on [Rotations.jl](https://github.com/JuliaGeometry/Rotations.jl) which in turn uses [Quaternions.jl](https://github.com/JuliaGeometry/Quaternions.jl) for quaternion computations. If you manually create quaternions using these packages, you may convert them to a vector to provide, e.g., initial conditions, using `vec(Q)`.
+
+### Examples using quaternions
+- [Free motions](@ref) (second example on the page)
+- [Three springs](@ref)
+- [Bodies in space](@ref) (may use, see comment)
+
+## Rotation matrices
+Rotation matrices represent orientation using a ``3\times 3`` matrix ``\in SO(3)``. These are used in the equations of multibody components and connectors, but should for the most part be invisible to the user. In particular, they should never appear as state variables after simplification. 
+
+
+## Conversion between formats
+You may convert between different representations of orientation using the appropriate constructors from Rotations.jl, for example:
+```@example ORI
+using Multibody.Rotations
+using Multibody.Rotations: params
+using Multibody.Rotations.Quaternions
+using LinearAlgebra
+
+R = RotMatrix{3}(I(3))
+```
+
+```@example ORI
+# Convert R to a quaternion
+Q = QuatRotation(R)
+```
+
+```@example ORI
+# Convert Q to a 4-vector
+Qvec = params(Q)
+```
+
+```@example ORI
+# Convert R to Euler angles in the sequence XYZ
+E = RotXYZ(R)
+```
+
+```@example ORI
+# Convert E to a 3-vector
+Evec = params(E)
+```
+
+## Conventions for modeling
+See [Orientations and directions](@ref)
+
+
+## Orientation API
+See [Orientation utilities](@ref)

examples/fourbar.jl

@@ -247,34 +247,8 @@ function connect_loop(F1, F2)
 end
 
 ## Quaternions
-struct Quaternion <: Orientation
-    Q
-    w::Any
-end
-
-Base.getindex(Q::Quaternion, i) = Q.Q[i]
-
-"""
-Never call this function directly from a component constructor, instead call `f = Frame(); R = ori(f)` and add `f` to the subsystems.
-"""
-function NumQuaternion(; Q = [1.0, 0, 0, 0.0], w = zeros(3), name, varw = false)
-    # The reason for not calling this directly is that all R vaiables have to have the same name since they are treated as connector variables (otherwise a connection error is thrown). A component with more than one rotation matrix will thus have two different R variables that overwrite each other
-    # Q = at_variables_t(:Q, 1:4, default = Q) #[description="Orientation rotation matrix ∈ SO(3)"]
-    @variables Q(t)[1:4] = [1.0,0,0,0]
-    if varw
-        @variables w(t)[1:3]=w [description="angular velocity"]
-        # w = at_variables_t(:w, 1:3, default = w)
-    else
-        w = get_w(Q)
-    end
-    Q, w = collect.((Q, w))
-    Quaternion(Q, w)
-end
 
 
-orientation_constraint(q::AbstractVector) = q'q - 1
-orientation_constraint(q::Quaternion) = orientation_constraint(q.Q)
-
 # function angular_velocity2(q::AbstractVector, q̇)
 #     Q = [q[4] q[3] -q[2] -q[1]; -q[3] q[4] q[1] -q[2]; q[2] -q[1] q[4] -q[3]]
 #     2 * Q * q̇
@@ -292,7 +266,6 @@ to_q(Q::AbstractVector) = SA[Q[4], Q[1], Q[2], Q[3]]
 to_q(Q::Rotations.QuatRotation) = to_q(vec(Q))
 to_mb(Q::AbstractVector) = SA[Q[2], Q[3], Q[4], Q[1]]
 to_mb(Q::Rotations.QuatRotation) = to_mb(vec(Q))
-Base.vec(Q::Rotations.QuatRotation) = SA[Q.q.s, Q.q.v1, Q.q.v2, Q.q.v3]
 
 # function angular_velocity1(Q, der_Q)
 #     2*([Q[4] -Q[3] Q[2] -Q[1]; Q[3] Q[4] -Q[1] -Q[2]; -Q[2] Q[1] Q[4] -Q[3]]*der_Q)
@@ -332,78 +305,16 @@ Returns a `RotationMatrix` object.
 """
 function axis_rotation(sequence, angle; name = :R)
     if sequence == 1
-        return RotationMatrix(rotx(angle), zeros(3))
+        return RotationMatrix(Rotations.RotX(angle), zeros(3))
     elseif sequence == 2
-        return RotationMatrix(roty(angle), zeros(3))
+        return RotationMatrix(Rotations.RotY(angle), zeros(3))
     elseif sequence == 3
-        return RotationMatrix(rotz(angle), zeros(3))
+        return RotationMatrix(Rotations.RotZ(angle), zeros(3))
     else
         error("Invalid sequence $sequence")
     end
 end
 
-"""
-    rotx(t, deg = false)
-
-Generate a rotation matrix for a rotation around the x-axis.
-
-- `t`: The angle of rotation (in radians, unless `deg` is set to true)
-- `deg`: (Optional) If true, the angle is in degrees
-
-Returns a 3x3 rotation matrix.
-"""
-function rotx(t, deg = false)
-    if deg
-        t *= pi / 180
-    end
-    ct = cos(t)
-    st = sin(t)
-    R = [1 0 0
-         0 ct -st
-         0 st ct]
-end
-
-"""
-    roty(t, deg = false)
-
-Generate a rotation matrix for a rotation around the y-axis.
-
-- `t`: The angle of rotation (in radians, unless `deg` is set to true)
-- `deg`: (Optional) If true, the angle is in degrees
-
-Returns a 3x3 rotation matrix.
-"""
-function roty(t, deg = false)
-    if deg
-        t *= pi / 180
-    end
-    ct = cos(t)
-    st = sin(t)
-    R = [ct 0 st
-         0 1 0
-         -st 0 ct]
-end
-
-"""
-    rotz(t, deg = false)
-
-Generate a rotation matrix for a rotation around the z-axis.
-
-- `t`: The angle of rotation (in radians, unless `deg` is set to true)
-- `deg`: (Optional) If true, the angle is in degrees
-
-Returns a 3x3 rotation matrix.
-"""
-function rotz(t, deg = false)
-    if deg
-        t *= pi / 180
-    end
-    ct = cos(t)
-    st = sin(t)
-    R = [ct -st 0
-         st ct 0
-         0 0 1]
-end
 
 function from_nxy(n_x, n_y)
     e_x = norm(n_x) < 1e-10 ? [1.0, 0, 0] : _normalize(n_x)

src/orientation.jl

@@ -265,6 +265,7 @@ using ModelingToolkit
 # using Plots
 using JuliaSimCompiler
 using OrdinaryDiffEq
+using Multibody.Rotations: params
 
 @testset "FreeBody" begin
     t = Multibody.t
@@ -300,8 +301,8 @@ using OrdinaryDiffEq
                         collect(body.body.w_a .=> 0);
                     collect(body.body.v_0 .=> 0);
                     #   collect(body.body.phi .=> deg2rad(10));
-                        collect(body.body.Q) .=> vec(QuatRotation(RotXYZ(deg2rad.((10,10,10))...)));
-                        collect(body.body.Q̂) .=> vec(QuatRotation(RotXYZ(deg2rad.((10,10,10))...)));
+                        collect(body.body.Q) .=> params(QuatRotation(RotXYZ(deg2rad.((10,10,10))...)));
+                        collect(body.body.Q̂) .=> params(QuatRotation(RotXYZ(deg2rad.((10,10,10))...)));
                     ], (0, 10))
 
     # @test_skip begin # The modelica example uses angles_fixed = true, which causes the body component to run special code for variable initialization. This is not yet supported by MTK