Skip to content

Commit f2e76f7

Browse files
baggepinnenbaggepinnen
committed
add components for prescribed motion
1 parent 036c0da commit f2e76f7

File tree

5 files changed

+273
-2
lines changed

5 files changed

+273
-2
lines changed

‎docs/make.jl

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,7 @@ makedocs(;
3232
"Spherical pendulum" => "examples/spherical_pendulum.md",
3333
"Gearbox" => "examples/gearbox.md",
3434
"Free motions" => "examples/free_motion.md",
35+
"Prescribed motions" => "examples/prescribed_pose.md",
3536
"Kinematic loops" => "examples/kinematic_loops.md",
3637
"Industrial robot" => "examples/robot.md",
3738
"Ropes, cables and chains" => "examples/ropes_and_cables.md",

‎docs/src/examples/prescribed_pose.md

Lines changed: 179 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,179 @@
1+
# Prescribed movements
2+
3+
The movement of a frame can be prescribed using any of the components
4+
- [`Position`](@ref)
5+
- [`Pose`](@ref)
6+
7+
The motion of joints [`Revolute`](@ref) and [`Prismatic`](@ref) can also be prescribed by using `axisflange = true` and attaching a `ModelingToolkitStandardLibrary.Rotational.Position` or `ModelingToolkitStandardLibrary.TranslationalModelica.Position` to the axis flange of the joint.
8+
9+
10+
Prescribed motions can be useful to, e.g.,
11+
- Use the 3D rendering capabilities to visualize the movement of a mechanism moving in a prescribed way without simulating the system.
12+
- Simplify simulations by prescribing the movement of a part of the mechanism.
13+
14+
15+
In the example below, we prescribe the motion of a suspension system using [`Pose`](@ref). We load the `Rotations` package in order to get access to the constructor `RotXYZ` which allows us to specify a rotation matrix using Euler angles. The component prescribing the motion is
16+
```julia
17+
body_upright = Pose(; r = [0, 0.17 + 0.1sin(t), 0], R = RotXYZ(0, 0.5sin(t), 0))
18+
```
19+
This makes the `body_upright` frame move in the vertical direction and rotate around the y-axis. Notice how the reference translation `r` and the reference orientation `R` are symbolic expressions that depend on time `t`. To make a frame follow recorded data, you may use [DataInterpolations.jl](https://docs.sciml.ai/DataInterpolations/stable/) to create interpolation objects that when accessed with `t` return the desired position and orientation.
20+
21+
```@example PRESCRIBED_POSE
22+
using Multibody
23+
using Multibody.Rotations # To specify orientations using Euler angles
24+
using ModelingToolkit
25+
using Plots
26+
using OrdinaryDiffEq
27+
using LinearAlgebra
28+
using JuliaSimCompiler
29+
using Test
30+
31+
t = Multibody.t
32+
D = Differential(t)
33+
W(args...; kwargs...) = Multibody.world
34+
35+
n = [1, 0, 0]
36+
AB = 146.5 / 1000
37+
BC = 233.84 / 1000
38+
CD = 228.60 / 1000
39+
DA = 221.43 / 1000
40+
BP = 129.03 / 1000
41+
DE = 310.31 / 1000
42+
t5 = 19.84 |> deg2rad
43+
44+
@mtkmodel QuarterCarSuspension begin
45+
@structural_parameters begin
46+
spring = true
47+
(jc = [0.5, 0.5, 0.5, 0.7])#, [description = "Joint color"]
48+
mirror = false
49+
end
50+
@parameters begin
51+
cs = 4000, [description = "Damping constant [Ns/m]"]
52+
ks = 44000, [description = "Spring constant [N/m]"]
53+
rod_radius = 0.02
54+
jr = 0.03, [description = "Radius of revolute joint"]
55+
end
56+
begin
57+
dir = mirror ? -1 : 1
58+
rRod1_ia = AB*normalize([0, -0.1, 0.2dir])
59+
rRod2_ib = BC*normalize([0, 0.2, 0dir])
60+
end
61+
@components begin
62+
r123 = JointRRR(n_a = n*dir, n_b = n*dir, rRod1_ia, rRod2_ib, rod_radius=0.018, rod_color=jc)
63+
r2 = Revolute(; n=n*dir, radius=jr, color=jc)
64+
b1 = FixedTranslation(radius = rod_radius, r = CD*normalize([0, -0.1, 0.3dir])) # CD
65+
chassis = FixedTranslation(r = DA*normalize([0, 0.2, 0.2*sin(t5)*dir]), render=false)
66+
chassis_frame = Frame()
67+
68+
if spring
69+
springdamper = SpringDamperParallel(c = ks, d = cs, s_unstretched = 1.3*BC, radius=rod_radius, num_windings=10)
70+
end
71+
if spring
72+
spring_mount_F = FixedTranslation(r = 0.7*CD*normalize([0, -0.1, 0.3dir]), render=false)
73+
end
74+
if spring
75+
spring_mount_E = FixedTranslation(r = 1.3DA*normalize([0, 0.2, 0.2*sin(t5)*dir]), render=true)
76+
end
77+
end
78+
begin
79+
A = chassis.frame_b
80+
D = chassis.frame_a
81+
end
82+
@equations begin
83+
# Main loop
84+
connect(A, r123.frame_a)
85+
connect(r123.frame_b, b1.frame_b)
86+
connect(b1.frame_a, r2.frame_b)
87+
connect(r2.frame_a, D)
88+
89+
# Spring damper
90+
if spring
91+
connect(springdamper.frame_b, spring_mount_E.frame_b)
92+
connect(b1.frame_a, spring_mount_F.frame_a)
93+
connect(D, spring_mount_E.frame_a)
94+
connect(springdamper.frame_a, spring_mount_F.frame_b)
95+
end
96+
97+
connect(chassis_frame, chassis.frame_a)
98+
end
99+
end
100+
101+
@mtkmodel ExcitedWheelAssembly begin
102+
@parameters begin
103+
rod_radius = 0.02
104+
end
105+
@components begin
106+
chassis_frame = Frame()
107+
suspension = QuarterCarSuspension(; rod_radius)
108+
109+
wheel = SlippingWheel(
110+
radius = 0.2,
111+
m = 15,
112+
I_axis = 0.06,
113+
I_long = 0.12,
114+
x0 = 0.0,
115+
z0 = 0.0,
116+
der_angles = [0, 0, 0],
117+
iscut = true,
118+
)
119+
120+
end
121+
@equations begin
122+
connect(wheel.frame_a, suspension.r123.frame_ib)
123+
connect(chassis_frame, suspension.chassis_frame)
124+
end
125+
end
126+
127+
128+
@mtkmodel SuspensionWithExcitationAndMass begin
129+
@parameters begin
130+
ms = 1500/4, [description = "Mass of the car [kg]"]
131+
rod_radius = 0.02
132+
end
133+
@components begin
134+
world = W()
135+
mass = Body(m=ms, r_cm = 0.5DA*normalize([0, 0.2, 0.2*sin(t5)]))
136+
excited_suspension = ExcitedWheelAssembly(; rod_radius)
137+
prescribed_motion = Pose(; r = [0, 0.1 + 0.1sin(t), 0], R = RotXYZ(0, 0.5sin(t), 0))
138+
end
139+
@equations begin
140+
connect(prescribed_motion.frame_b, excited_suspension.chassis_frame, mass.frame_a)
141+
end
142+
143+
end
144+
145+
@named model = SuspensionWithExcitationAndMass()
146+
model = complete(model)
147+
ssys = structural_simplify(IRSystem(model))
148+
display([unknowns(ssys) diag(ssys.mass_matrix)])
149+
150+
defs = [
151+
model.excited_suspension.suspension.ks => 30*44000
152+
model.excited_suspension.suspension.cs => 30*4000
153+
model.excited_suspension.suspension.r2.phi => -0.6031*(1)
154+
]
155+
prob = ODEProblem(ssys, defs, (0, 2π))
156+
sol = solve(prob, Rodas5P(autodiff=false), initializealg = BrownFullBasicInit())
157+
@test SciMLBase.successful_retcode(sol)
158+
Multibody.render(model, sol, show_axis=false, x=-0.8, y=0.7, z=0.1, lookat=[0,0.1,0.0], filename="prescribed_motion.gif") # Video
159+
nothing # hide
160+
```
161+
162+
![prescribed motion](prescribed_motion.gif)
163+
164+
Even though we formulated an `ODEProblem` and called `solve`, we do not actually perform any simulation here! If we look at the mass matrix of the system
165+
```@example PRESCRIBED_POSE
166+
ssys.mass_matrix
167+
```
168+
we see that it is all zeros. This means that there are no differential equations at all in the system, only algebraic equations. The solver will thus only solve for the algebraic variables using a nonlinear root finder. In general, prescribing the value of some state variables removes the need for the solver to solve for them, which can be useful for simplifying simulations. Using the "simulation" above, we can use the solution object to, e.g., find the compression of the spring and the forces acting on the ground over time.
169+
170+
```@example PRESCRIBED_POSE
171+
plot(sol, idxs=[model.excited_suspension.suspension.springdamper.s, -model.excited_suspension.suspension.springdamper.f, model.excited_suspension.wheel.wheeljoint.f_n], labels=["Spring length [m]" "Spring force [N] " "Normal force [N]"], layout=(2,1), sp=[1 2 2])
172+
```
173+
Here, we see that the total spring force and the normal force acting on the ground are not equal, this is due to the spring not applying force only in the vertical direction. We can also compute the slip velocity, the velocity with which the contact between the wheel and the ground is sliding along the ground due to the prescribed motion.
174+
175+
```@example PRESCRIBED_POSE
176+
wj = model.excited_suspension.wheel.wheeljoint
177+
plot(sol, idxs=[wj.v_slip, wj.v_slip_long, wj.v_slip_lat], labels=["Slip velocity magnitude" "Longitudinal slip velocity" "Lateral slip velocity"], ylabel="Velocity [m/s]")
178+
```
179+

‎src/Multibody.jl

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -223,7 +223,7 @@ include("frames.jl")
223223
export PartialTwoFrames
224224
include("interfaces.jl")
225225

226-
export World, world, Mounting1D, Fixed, FixedTranslation, FixedRotation, Body, BodyShape, BodyCylinder, BodyBox, Rope
226+
export World, world, Mounting1D, Fixed, Position, Pose, FixedTranslation, FixedRotation, Body, BodyShape, BodyCylinder, BodyBox, Rope
227227
include("components.jl")
228228

229229
export Revolute, Prismatic, Planar, Spherical, Universal,

‎src/components.jl

Lines changed: 88 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -79,6 +79,11 @@ function inner_gravity(point_gravity, mu, g, n, r)
7979
end
8080

8181

82+
"""
83+
Fixed(; name, r = [0, 0, 0], render = true)
84+
85+
A component rigidly attached to the world frame with translation `r` between the world and the `frame_b`. The position vector `r` is resolved in the world frame.
86+
"""
8287
@component function Fixed(; name, r = [0, 0, 0], render = true)
8388
systems = @named begin frame_b = Frame() end
8489
@parameters begin
@@ -91,6 +96,89 @@ end
9196
add_params(sys, [render]; name)
9297
end
9398

99+
"""
100+
Position(; name, r = [0, 0, 0], render = true, fixed_oreintation = true)
101+
102+
Forced movement of a flange according to a reference position `r`. Similar to [`Fixed`](@ref), but `r` is not a parameter, and may thus be any time-varying symbolic expression. The reference position vector `r` is resolved in the world frame. Example: `r = [sin(t), 0, 0]`.
103+
104+
# Arguments:
105+
- `r`: Position vector from world frame to frame_b, resolved in world frame
106+
- `render`: Render the component in animations
107+
- `fixed_oreintation`: If `true`, the orientation of the frame is fixed to the world frame. If `false`, the orientation is free to change.
108+
109+
See also [`Pose`](@ref) for a component that allows for forced orientation as well.
110+
"""
111+
@component function Position(; name, r = [0, 0, 0], render = true, fixed_oreintation = true)
112+
systems = @named begin frame_b = Frame() end
113+
@parameters begin
114+
render = render, [description = "Render the component in animations"]
115+
end
116+
@variables begin
117+
p(t)[1:3], [description = "Position vector from world frame to frame_b, resolved in world frame"]
118+
v(t)[1:3], [description = "Absolute velocity of frame_b, resolved in world frame"]
119+
a(t)[1:3], [description = "Absolute acceleration of frame_b, resolved in world frame"]
120+
end
121+
eqs = [
122+
collect(frame_b.r_0 .~ r)
123+
collect(frame_b.r_0 .~ p)
124+
collect(v .~ D.(p))
125+
collect(a .~ D.(v))
126+
]
127+
if fixed_oreintation
128+
append!(eqs, ori(frame_b) ~ nullrotation())
129+
end
130+
sys = compose(ODESystem(eqs, t; name=:nothing), systems...)
131+
add_params(sys, [render]; name)
132+
end
133+
134+
"""
135+
Pose(; name, r = [0, 0, 0], R, q, render = true)
136+
137+
Forced movement of a flange according to a reference position `r` and reference orientation `R`. The reference arrays `r` and `R` are resolved in the world frame, and may be any symbolic expression. As an alternative to specifying `R`, it is possible to specify a quaternion `q` (4-vector quaternion with real part first).
138+
139+
Example usage:
140+
```
141+
using Multibody.Rotations
142+
R = RotXYZ(0, 0.5sin(t), 0)
143+
@named rot = Multibody.Pose(; r=[sin(t), 0, 0], R)
144+
```
145+
146+
# Arguments
147+
- `r`: Position vector from world frame to frame_b, resolved in world frame
148+
- `R`: Reference orientation matrix
149+
- `q`: Reference quaternion (optional alternative to `R`)
150+
- `render`: Render the component in animations
151+
- `normalize`: If a quaternion is provided, normalize the quaternion (default = true)
152+
153+
See also [`Position`](@ref) for a component that allows for only forced translation.
154+
"""
155+
@component function Pose(; name, r = [0, 0, 0], R=nothing, q=nothing, render = true, normalize=true)
156+
systems = @named begin frame_b = Frame() end
157+
@parameters begin
158+
render = render, [description = "Render the component in animations"]
159+
end
160+
@variables begin
161+
p(t)[1:3], [description = "Position vector from world frame to frame_b, resolved in world frame"]
162+
v(t)[1:3], [description = "Absolute velocity of frame_b, resolved in world frame"]
163+
a(t)[1:3], [description = "Absolute acceleration of frame_b, resolved in world frame"]
164+
end
165+
eqs = [
166+
collect(frame_b.r_0 .~ r)
167+
collect(frame_b.r_0 .~ p)
168+
collect(v .~ D.(p))
169+
collect(a .~ D.(v))
170+
if R !== nothing
171+
ori(frame_b).R ~ R
172+
elseif q !== nothing
173+
ori(frame_b) ~ from_Q(q, 0; normalize)
174+
else
175+
error("Either R or q must be provided")
176+
end
177+
]
178+
sys = compose(ODESystem(eqs, t; name=:nothing), systems...)
179+
add_params(sys, [render]; name)
180+
end
181+
94182
@component function Mounting1D(; name, n = [1, 0, 0], phi0 = 0)
95183
systems = @named begin
96184
flange_b = Rotational.Flange()

‎src/orientation.jl

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -256,8 +256,11 @@ end
256256
# end
257257

258258
Base.:/(q::Rotations.Quaternions.Quaternion, x::Num) = Rotations.Quaternions.Quaternion(q.s / x, q.v1 / x, q.v2 / x, q.v3 / x)
259-
function from_Q(Q2, w)
259+
function from_Q(Q2, w; normalize=false)
260260
# Q2 = to_q(Q) # Due to different conventions
261+
if normalize
262+
Q2 = Q2 / _norm(Q2)
263+
end
261264
q = Rotations.QuatRotation(Q2, false)
262265
R = RotMatrix(q)
263266
RotationMatrix(R, w)

0 commit comments

Comments
 (0)