expose orientation utilities

Project.toml

@@ -6,13 +6,14 @@ version = "0.1.0"
 [deps]
 CoordinateTransformations = "150eb455-5306-5404-9cee-2592286d6298"
 DataInterpolations = "82cc6244-b520-54b8-b5a6-8a565e85f1d0"
+FileIO = "5789e2e9-d7fb-5bc7-8068-2c6fae9b9549"
 JuliaSimCompiler = "8391cb6b-4921-5777-4e45-fd9aab8cb88d"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
+MeshIO = "7269a6da-0436-5bbc-96c2-40638cbb6118"
 ModelingToolkit = "961ee093-0014-501f-94e3-6117800e7a78"
 ModelingToolkitStandardLibrary = "16a59e39-deab-5bd0-87e4-056b12336739"
 Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc"
-FileIO = "5789e2e9-d7fb-5bc7-8068-2c6fae9b9549"
-MeshIO = "7269a6da-0436-5bbc-96c2-40638cbb6118"
+StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
 
 [weakdeps]
 Makie = "ee78f7c6-11fb-53f2-987a-cfe4a2b5a57a"
@@ -27,6 +28,7 @@ MeshIO = "0.4"
 ModelingToolkit = "9"
 ModelingToolkitStandardLibrary = "2"
 Rotations = "1.4"
+StaticArrays = "1"
 julia = "1"
 
 [extras]

docs/src/examples/pendulum.md

@@ -243,27 +243,25 @@ nothing # hide
 Let's break down how to think about directions and orientations when building 3D mechanisms. In the example above, we started with the shoulder joint, this joint rotated around the gravitational axis, `n = [0, 1, 0]`. When this joint is positioned in joint coordinate `shoulder_joint.phi = 0`, its `frame_a` and `frame_b` will coincide. When the joint rotates, `frame_b` will rotate around the axis `n` of `frame_a`. The `frame_a` of the joint is attached to the world, so the joint will rotate around the world's `y`-axis:
 
 ```@example pendulum
-function get_R(frame, t)
- reshape(sol(t, idxs=vec(ori(frame).R.mat')), 3, 3)
-end
-function get_r(frame, t)
- sol(t, idxs=collect(frame.r_0))
-end
-get_R(model.shoulder_joint.frame_b, 0)
+get_rot(sol, model.shoulder_joint.frame_b, 0)
 ```
 we see that at time $t = 0$, we have no rotation of `frame_b` around the $y$ axis of the world (frames are always resolved in the world frame), but a second into the simulation, we have:
 ```@example pendulum
-get_R(model.shoulder_joint.frame_b, 1)
+R1 = get_rot(sol, model.shoulder_joint.frame_b, 1)
+```
+Here, the `frame_b` has rotated around the $y$ axis of the world (if you are not familiar with rotation matrices, we can ask for the rotation axis and angle)
+```@example pendulum
+using Multibody.Rotations
+(rotation_axis(R1), rotation_angle(R1))
 ```
-Here, the `frame_b` has rotated around the $y$ axis of the world.
 
 The next body is the upper arm. This body has an extent of `0.6` in the $z$ direction, as measured in its local `frame_a`
 ```@example pendulum
-get_r(model.upper_arm.frame_b, 0)
+get_trans(sol, model.upper_arm.frame_b, 0)
 ```
 One second into the simulation, the upper arm has rotated around the $y$ axis of the world
 ```@example pendulum
-get_r(model.upper_arm.frame_b, 1)
+rb1 = get_trans(sol, model.upper_arm.frame_b, 1)
 ```
 
 If we look at the variable `model.upper_arm.r`, we do not see this rotation!
@@ -272,9 +270,14 @@ arm_r = sol(1, idxs=collect(model.upper_arm.r))
 ```
 The reason is that this variable is resolved in the local `frame_a` and not in the world frame. To transform this variable to the world frame, we may multiply with the rotation matrix of `frame_a` which is always resolved in the world frame:
 ```@example pendulum
-get_R(model.upper_arm.frame_a, 1)*arm_r
+get_rot(sol, model.upper_arm.frame_a, 1)*arm_r
 ```
 We now get the same result has when we asked for the translation vector of `frame_b` above.
+```@example pendulum
+using Test # hide
+get_rot(sol, model.upper_arm.frame_a, 1)*arm_r ≈ rb1 # hide
+nothing # hide
+```
 
 
 Slightly more formally, let $R_A^B$ denote the rotation matrix that rotates a vector expressed in a frame $A$ into one that is expressed in frame $B$, i.e., $r_B = R_B^A r_A$. We have then just performed the transformation $r_W = R_W^A r_A$, where $W$ denotes the world frame, and $A$ denotes `body.frame_a`.
@@ -283,26 +286,20 @@ The next joint, the elbow joint, has the rotational axis `n = [0, 0, 1]`. This i
 
 The lower arm is finally having an extent along the $y$-axis. At the final time when the pendulum motion has been fully damped, we see that the second frame of this body ends up with an $y$-coordinate of `-0.6`:
 ```@example pendulum
-get_r(model.lower_arm.frame_b, 12)
+get_trans(sol, model.lower_arm.frame_b, 12)
 ```
 
 If we rotate the vector of extent of the lower arm to the world frame, we indeed see that the only coordinate that is nonzero is the $y$ coordinate:
 ```@example pendulum
-get_R(model.lower_arm.frame_a, 12)*sol(12, idxs=collect(model.lower_arm.r))
+get_rot(sol, model.lower_arm.frame_a, 12)*sol(12, idxs=collect(model.lower_arm.r))
 ```
 
-The reason that the latter vector differs from `get_r(model.lower_arm.frame_b, 12)` above is that `get_r(model.lower_arm.frame_b, 12)` has been _translated_ as well. To both translate and rotate `model.lower_arm.r` into the world frame, we must use the full transformation matrix $T_W^A \in SE(3)$:
+The reason that the latter vector differs from `get_trans(sol, model.lower_arm.frame_b, 12)` above is that `get_trans(sol, model.lower_arm.frame_b, 12)` has been _translated_ as well. To both translate and rotate `model.lower_arm.r` into the world frame, we must use the full transformation matrix $T_W^A \in SE(3)$:
 
 ```@example pendulum
-function get_T(frame, t)
- R = get_R(frame, t)
- r = get_r(frame, t)
- [R r; 0 0 0 1]
-end
-
 r_A = sol(12, idxs=collect(model.lower_arm.r))
 r_A = [r_A; 1] # Homogeneous coordinates
 
-get_T(model.lower_arm.frame_a, 12)*r_A
+get_frame(sol, model.lower_arm.frame_a, 12)*r_A
 ```
-the vector is now coinciding with `get_r(model.lower_arm.frame_b, 12)`.
+the vector is now coinciding with `get_trans(sol, model.lower_arm.frame_b, 12)`.

ext/Render.jl

@@ -1,20 +1,17 @@
 module Render
 using Makie
 using Multibody
-import Multibody: render, render!, encode, decode
+import Multibody: render, render!, encode, decode, get_rot, get_trans, get_frame
 using Rotations
 using LinearAlgebra
 using ModelingToolkit
 export render
 using MeshIO, FileIO
 
 
-function get_rot(sol, frame, t)
- reshape(sol(t, idxs = vec(ori(frame).R.mat)), 3, 3)
-end
 
 function get_rot_fun(sol, frame)
- syms = vec(ori(frame).R.mat)
+ syms = vec(ori(frame).R.mat')
  getter = ModelingToolkit.getu(sol, syms)
  p = ModelingToolkit.parameter_values(sol)
  function (t)
@@ -35,22 +32,12 @@ function get_fun(sol, syms)
 end
 
 
-"""
- get_frame(sol, frame, t)
-
-Extract a 4×4 transformation matrix ∈ SE(3) from a solution at time `t`.
-"""
-function get_frame(sol, frame, t)
- R = get_rot(sol, frame, t)
- tr = sol(t, idxs = collect(frame.r_0))
- [R' tr; 0 0 0 1]
-end
 
 function get_frame_fun(sol, frame)
  R = get_rot_fun(sol, frame)
  tr = get_fun(sol, collect(frame.r_0))
  function (t)
- [R(t)' tr(t); 0 0 0 1]
+ [R(t) tr(t); 0 0 0 1]
  end
 end
 
@@ -249,7 +236,7 @@ function render!(scene, ::Union{typeof(Revolute), typeof(RevolutePlanarLoopConst
  O = r_0($t)
  n_a = n($t)
  R_w_a = rotfun($t)
- n_w = R_w_a'*n_a # Rotate to the world frame
+ n_w = R_w_a*n_a # Rotate to the world frame
  p1 = Point3f(O + radius*n_w)
  p2 = Point3f(O - radius*n_w)
  Makie.GeometryBasics.Cylinder(p1, p2, radius)

src/Multibody.jl

@@ -5,7 +5,7 @@ using ModelingToolkit
 using JuliaSimCompiler
 import ModelingToolkitStandardLibrary.Mechanical.Rotational
 import ModelingToolkitStandardLibrary.Mechanical.TranslationalModelica as Translational
-
+using StaticArrays
 export Rotational, Translational
 
 export render, render!
@@ -126,7 +126,7 @@ decode(s) = String(UInt8.(s))
 # ODEProblem{true}(fun, u0, tspan, ps; kwargs...)
 # end
 
-export Orientation, RotationMatrix, ori
+export Orientation, RotationMatrix, ori, get_rot, get_trans, get_frame
 include("orientation.jl")
 
 export Frame

src/orientation.jl

@@ -418,4 +418,36 @@ end
 
 function resolve_dyade2(R, D1)
  R*D1*R'
+end
+
+"""
+ get_rot(sol, frame, t)
+
+Extract a 3×3 rotation matrix ∈ SO(3) from a solution at time `t`.
+See also [`get_trans`](@ref), [`get_frame`](@ref)
+"""
+function get_rot(sol, frame, t)
+ Rotations.RotMatrix3(reshape(sol(t, idxs = vec(ori(frame).R.mat')), 3, 3))
+end
+
+"""
+ get_trans(sol, frame, t)
+
+Extract the translational part of a frame from a solution at time `t`.
+See also [`get_rot`](@ref), [`get_frame`](@ref)
+"""
+function get_trans(sol, frame, t)
+ SVector{3}(sol(t, idxs = collect(frame.r_0)))
+end
+
+"""
+ get_frame(sol, frame, t)
+
+Extract a 4×4 transformation matrix ∈ SE(3) from a solution at time `t`.
+See also [`get_trans`](@ref), [`get_rot`](@ref)
+"""
+function get_frame(sol, frame, t)
+ R = get_rot(sol, frame, t)
+ tr = get_trans(sol, frame, t)
+ [R tr; 0 0 0 1]
 end

test/runtests.jl

@@ -35,6 +35,11 @@ end
  include("test_robot.jl")
 end
 
+@testset "orientation_getters" begin
+ @info "Testing orientation_getters"
+ include("test_orientation_getters.jl")
+end
+
 
 # ==============================================================================
 ## Add spring to make a harmonic oscillator ====================================
@@ -986,3 +991,4 @@ sol = solve(prob, Rodas4())
 
 tt = 0:0.1:10
 @test Matrix(sol(tt, idxs = [collect(body.r_0[2:3]);])) ≈ Matrix(sol(tt, idxs = [collect(body2.r_0[2:3]);]))
+

test/test_orientation_getters.jl

@@ -0,0 +1,91 @@
+using Test
+import ModelingToolkitStandardLibrary.Mechanical.Rotational
+@mtkmodel FurutaPendulum begin
+ @components begin
+ world = W()
+ shoulder_joint = Revolute(n = [0, 1, 0], isroot = true, axisflange = true)
+ elbow_joint = Revolute(n = [0, 0, 1], isroot = true, axisflange = true, phi0=0.1)
+ upper_arm = BodyShape(; m = 0.1, isroot = false, r = [0, 0, 0.6], radius=0.04)
+ lower_arm = BodyShape(; m = 0.1, isroot = false, r = [0, 0.6, 0], radius=0.04)
+ tip = Body(; m = 0.3, isroot = false)
+
+ damper1 = Rotational.Damper(d = 0.07)
+ damper2 = Rotational.Damper(d = 0.07)
+ end
+ @equations begin
+ connect(world.frame_b, shoulder_joint.frame_a)
+ connect(shoulder_joint.frame_b, upper_arm.frame_a)
+ connect(upper_arm.frame_b, elbow_joint.frame_a)
+ connect(elbow_joint.frame_b, lower_arm.frame_a)
+ connect(lower_arm.frame_b, tip.frame_a)
+
+ connect(shoulder_joint.axis, damper1.flange_a)
+ connect(shoulder_joint.support, damper1.flange_b)
+
+ connect(elbow_joint.axis, damper2.flange_a)
+ connect(elbow_joint.support, damper2.flange_b)
+
+ end
+end
+
+@named model = FurutaPendulum()
+model = complete(model)
+ssys = structural_simplify(IRSystem(model))
+
+prob = ODEProblem(ssys, [model.shoulder_joint.phi => 0.0, model.elbow_joint.phi => 0.1], (0, 12))
+sol = solve(prob, Rodas4())
+
+
+
+
+
+get_rot(sol, model.shoulder_joint.frame_b, 0)
+
+# we see that at time $t = 0$, we have no rotation of `frame_b` around the $y$ axis of the world (frames are always resolved in the world frame), but a second into the simulation, we have:
+R1 = get_rot(sol, model.shoulder_joint.frame_b, 1)
+
+# Here, the `frame_b` has rotated around the $y$ axis of the world (if you are not familiar with rotation matrices, we can ask for the rotation axis and angle)
+using Multibody.Rotations
+(rotation_axis(R1), rotation_angle(R1))
+
+
+# The next body is the upper arm. This body has an extent of `0.6` in the $z$ direction, as measured in its local `frame_a`
+get_trans(sol, model.upper_arm.frame_b, 0)
+
+# One second into the simulation, the upper arm has rotated around the $y$ axis of the world
+rb1 = get_trans(sol, model.upper_arm.frame_b, 1)
+@test rb1 ≈ [-0.2836231361058395, 0.0, 0.528732367711197]
+
+# If we look at the variable `model.upper_arm.r`, we do not see this rotation!
+arm_r = sol(1, idxs=collect(model.upper_arm.r))
+@test arm_r == [0, 0, 0.6]
+
+# The reason is that this variable is resolved in the local `frame_a` and not in the world frame. To transform this variable to the world frame, we may multiply with the rotation matrix of `frame_a` which is always resolved in the world frame:
+@test get_rot(sol, model.upper_arm.frame_a, 1)*arm_r ≈ rb1
+
+# We now get the same result has when we asked for the translation vector of `frame_b` above.
+
+# Slightly more formally, let $R_A^B$ denote the rotation matrix that rotates a vector expressed in a frame $A$ into one that is expressed in frame $B$, i.e., $r_B = R_B^A r_A$. We have then just performed the transformation $r_W = R_W^A r_A$, where $W$ denotes the world frame, and $A$ denotes `body.frame_a`.
+
+# The next joint, the elbow joint, has the rotational axis `n = [0, 0, 1]`. This indicates that the joint rotates around the $z$-axis of its `frame_a`. Since the upper arm was oriented along the $z$ direction, the joint is rotating around the axis that coincides with the upper arm. 
+
+# The lower arm is finally having an extent along the $y$-axis. At the final time when the pendulum motion has been fully damped, we see that the second frame of this body ends up with an $y$-coordinate of `-0.6`:
+t1 = get_trans(sol, model.lower_arm.frame_b, 12)
+@test t1 ≈ [-0.009040487302666853, -0.59999996727278, 0.599931920189277]
+
+
+# If we rotate the vector of extent of the lower arm to the world frame, we indeed see that the only coordinate that is nonzero is the $y$ coordinate:
+t1rot = get_rot(sol, model.lower_arm.frame_a, 12)*sol(12, idxs=collect(model.lower_arm.r))
+
+@test abs(t1rot[1]) < 1e-2
+@test abs(t1rot[3]) < 1e-2
+
+
+# The reason that the latter vector differs from `get_trans(sol, model.lower_arm.frame_b, 12)` above is that `get_trans(sol, model.lower_arm.frame_b, 12)` has been _translated_ as well. To both translate and rotate `model.lower_arm.r` into the world frame, we must use the full transformation matrix $T_W^A \in SE(3)$:
+
+r_A = sol(12, idxs=collect(model.lower_arm.r))
+r_A = [r_A; 1] # Homogeneous coordinates
+
+@test (get_frame(sol, model.lower_arm.frame_a, 12)*r_A)[1:3] ≈ t1
+
+# the vector is now coinciding with `get_trans(sol, model.lower_arm.frame_b, 12)`.