From 9708749b799e4d8a2419d981a4215fbb2d5c7894 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Fri, 14 Jun 2024 13:52:02 +0200 Subject: [PATCH 1/3] expose orientation utilities --- Project.toml | 6 ++- docs/src/examples/pendulum.md | 43 +++++++-------- ext/Render.jl | 17 +----- src/Multibody.jl | 4 +- src/orientation.jl | 32 ++++++++++++ test/runtests.jl | 6 +++ test/test_orientation_getters.jl | 90 ++++++++++++++++++++++++++++++++ 7 files changed, 156 insertions(+), 42 deletions(-) create mode 100644 test/test_orientation_getters.jl diff --git a/Project.toml b/Project.toml index d9b65774..f992313b 100644 --- a/Project.toml +++ b/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] diff --git a/docs/src/examples/pendulum.md b/docs/src/examples/pendulum.md index 30ae0a04..b8d33d51 100644 --- a/docs/src/examples/pendulum.md +++ b/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)`. \ No newline at end of file +the vector is now coinciding with `get_trans(sol, model.lower_arm.frame_b, 12)`. diff --git a/ext/Render.jl b/ext/Render.jl index 6963e80c..fdbc99c9 100644 --- a/ext/Render.jl +++ b/ext/Render.jl @@ -9,9 +9,6 @@ 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) @@ -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) diff --git a/src/Multibody.jl b/src/Multibody.jl index a356b7dc..d8a89219 100644 --- a/src/Multibody.jl +++ b/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 diff --git a/src/orientation.jl b/src/orientation.jl index f1a9721d..0e13ea1e 100644 --- a/src/orientation.jl +++ b/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 \ No newline at end of file diff --git a/test/runtests.jl b/test/runtests.jl index c275766d..f35b5b5c 100644 --- a/test/runtests.jl +++ b/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]);])) + diff --git a/test/test_orientation_getters.jl b/test/test_orientation_getters.jl new file mode 100644 index 00000000..526bc874 --- /dev/null +++ b/test/test_orientation_getters.jl @@ -0,0 +1,90 @@ +using Test +@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 = RDamper(d = 0.07) + damper2 = RDamper(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)`. From 4b9000c11a73021b5234929a9e9935e1a33aedb8 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Fri, 14 Jun 2024 14:03:52 +0200 Subject: [PATCH 2/3] import Rotational --- test/test_orientation_getters.jl | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/test/test_orientation_getters.jl b/test/test_orientation_getters.jl index 526bc874..b6675500 100644 --- a/test/test_orientation_getters.jl +++ b/test/test_orientation_getters.jl @@ -1,4 +1,5 @@ using Test +import ModelingToolkitStandardLibrary.Mechanical.Rotational @mtkmodel FurutaPendulum begin @components begin world = W() @@ -8,8 +9,8 @@ using Test lower_arm = BodyShape(; m = 0.1, isroot = false, r = [0, 0.6, 0], radius=0.04) tip = Body(; m = 0.3, isroot = false) - damper1 = RDamper(d = 0.07) - damper2 = RDamper(d = 0.07) + damper1 = Rotational.Damper(d = 0.07) + damper2 = Rotational.Damper(d = 0.07) end @equations begin connect(world.frame_b, shoulder_joint.frame_a) From 318dcf1f9d8014a44f127eaa5b0acb04df825b9e Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Fri, 14 Jun 2024 14:44:26 +0200 Subject: [PATCH 3/3] transpose --- ext/Render.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ext/Render.jl b/ext/Render.jl index fdbc99c9..e4668317 100644 --- a/ext/Render.jl +++ b/ext/Render.jl @@ -1,7 +1,7 @@ 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 @@ -11,7 +11,7 @@ using MeshIO, FileIO 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)