Skip to content

Commit 09f926a

Browse files
committed
simplify and document orientation interface
1 parent 58bc962 commit 09f926a

File tree

6 files changed

+68
-33
lines changed

6 files changed

+68
-33
lines changed

docs/make.jl

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -33,6 +33,7 @@ makedocs(;
3333
"Ropes, cables and chains" => "examples/ropes_and_cables.md",
3434
"Bodies in space" => "examples/space.md",
3535
],
36+
"Rotations and orientation" => "rotations.md",
3637
"3D rendering" => "rendering.md",
3738
])
3839

docs/src/examples/free_motion.md

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -53,7 +53,7 @@ nothing # hide
5353
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`.
5454

5555
```@example FREE_MOTION
56-
using Multibody.Rotations: QuatRotation, RotXYZ
56+
using Multibody.Rotations: QuatRotation, RotXYZ, params
5757
@named begin
5858
body = BodyShape(m = 1, I_11 = 1, I_22 = 1, I_33 = 1, r = [0.4, 0, 0],
5959
r_0 = [0.2, -0.5, 0.1], isroot = true, quat=true)
@@ -80,8 +80,8 @@ ssys = structural_simplify(IRSystem(model))
8080
prob = ODEProblem(ssys, [
8181
collect(body.body.v_0 .=> 0);
8282
collect(body.body.w_a .=> 0);
83-
collect(body.body.Q) .=> vec(QuatRotation(RotXYZ(deg2rad.((10,10,10))...)));
84-
collect(body.body.Q̂) .=> vec(QuatRotation(RotXYZ(deg2rad.((10,10,10))...)));
83+
collect(body.body.Q) .=> params(QuatRotation(RotXYZ(deg2rad.((10,10,10))...)));
84+
collect(body.body.Q̂) .=> params(QuatRotation(RotXYZ(deg2rad.((10,10,10))...)));
8585
], (0, 4))
8686
8787
sol = solve(prob, Rodas5P())

docs/src/examples/three_springs.md

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,7 @@ eqs = [connect(world.frame_b, bar1.frame_a)
4040
ssys = structural_simplify(IRSystem(model))
4141
prob = ODEProblem(ssys, [], (0, 10))
4242
43-
sol = solve(prob, FBDF(), u0=prob.u0 .+ 1e-12*randn(length(prob.u0)))
43+
sol = solve(prob, Rodas4())
4444
@assert SciMLBase.successful_retcode(sol)
4545
4646
Plots.plot(sol, idxs = [body1.r_0...])

docs/src/rotations.md

Lines changed: 60 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,60 @@
1+
# Working with orientation and rotation
2+
3+
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.
4+
5+
## Euler angles
6+
[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.
7+
8+
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``.
9+
10+
## Quaternions
11+
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.
12+
13+
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)`.
14+
15+
### Examples using quaternions
16+
- [Free motions](@ref) (second example on the page)
17+
- [Three springs](@ref)
18+
- [Bodies in space](@ref) (may use, see comment)
19+
20+
## Rotation matrices
21+
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.
22+
23+
24+
## Conversion between formats
25+
You may convert between different representations of orientation using the appropriate constructors from Rotations.jl, for example:
26+
```@example ORI
27+
using Multibody.Rotations
28+
using Multibody.Rotations: params
29+
using Multibody.Rotations.Quaternions
30+
using LinearAlgebra
31+
32+
R = RotMatrix{3}(I(3))
33+
```
34+
35+
```@example ORI
36+
# Convert R to a quaternion
37+
Q = QuatRotation(R)
38+
```
39+
40+
```@example ORI
41+
# Convert Q to a 4-vector
42+
Qvec = params(Q)
43+
```
44+
45+
```@example ORI
46+
# Convert R to Euler angles in the sequence XYZ
47+
E = RotXYZ(R)
48+
```
49+
50+
```@example ORI
51+
# Convert E to a 3-vector
52+
Evec = params(E)
53+
```
54+
55+
## Conventions for modeling
56+
See [Orientations and directions](@ref)
57+
58+
59+
## Orientation API
60+
See [Orientation utilities](@ref)

src/orientation.jl

Lines changed: 0 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -247,33 +247,7 @@ function connect_loop(F1, F2)
247247
end
248248

249249
## Quaternions
250-
struct Quaternion <: Orientation
251-
Q
252-
w::Any
253-
end
254-
255-
Base.getindex(Q::Quaternion, i) = Q.Q[i]
256-
257-
"""
258-
Never call this function directly from a component constructor, instead call `f = Frame(); R = ori(f)` and add `f` to the subsystems.
259-
"""
260-
function NumQuaternion(; Q = [1.0, 0, 0, 0.0], w = zeros(3), name, varw = false)
261-
# 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
262-
# Q = at_variables_t(:Q, 1:4, default = Q) #[description="Orientation rotation matrix ∈ SO(3)"]
263-
@variables Q(t)[1:4] = [1.0,0,0,0]
264-
if varw
265-
@variables w(t)[1:3]=w [description="angular velocity"]
266-
# w = at_variables_t(:w, 1:3, default = w)
267-
else
268-
w = get_w(Q)
269-
end
270-
Q, w = collect.((Q, w))
271-
Quaternion(Q, w)
272-
end
273-
274250

275-
orientation_constraint(q::AbstractVector) = q'q - 1
276-
orientation_constraint(q::Quaternion) = orientation_constraint(q.Q)
277251

278252
# function angular_velocity2(q::AbstractVector, q̇)
279253
# 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]]
@@ -292,7 +266,6 @@ to_q(Q::AbstractVector) = SA[Q[4], Q[1], Q[2], Q[3]]
292266
to_q(Q::Rotations.QuatRotation) = to_q(vec(Q))
293267
to_mb(Q::AbstractVector) = SA[Q[2], Q[3], Q[4], Q[1]]
294268
to_mb(Q::Rotations.QuatRotation) = to_mb(vec(Q))
295-
Base.vec(Q::Rotations.QuatRotation) = SA[Q.q.s, Q.q.v1, Q.q.v2, Q.q.v3]
296269

297270
# function angular_velocity1(Q, der_Q)
298271
# 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)

test/test_quaternions.jl

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -265,6 +265,7 @@ using ModelingToolkit
265265
# using Plots
266266
using JuliaSimCompiler
267267
using OrdinaryDiffEq
268+
using Multibody.Rotations: params
268269

269270
@testset "FreeBody" begin
270271
t = Multibody.t
@@ -300,8 +301,8 @@ using OrdinaryDiffEq
300301
collect(body.body.w_a .=> 0);
301302
collect(body.body.v_0 .=> 0);
302303
# collect(body.body.phi .=> deg2rad(10));
303-
collect(body.body.Q) .=> vec(QuatRotation(RotXYZ(deg2rad.((10,10,10))...)));
304-
collect(body.body.Q̂) .=> vec(QuatRotation(RotXYZ(deg2rad.((10,10,10))...)));
304+
collect(body.body.Q) .=> params(QuatRotation(RotXYZ(deg2rad.((10,10,10))...)));
305+
collect(body.body.Q̂) .=> params(QuatRotation(RotXYZ(deg2rad.((10,10,10))...)));
305306
], (0, 10))
306307

307308
# @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

0 commit comments

Comments
 (0)