## Setup¶

In :
using Pkg
Pkg.activate(@__DIR__)
pkg"instantiate"
pkg"precompile"

  Updating registry at ~/.julia/registries/General
Updating git-repo ssh://[email protected]/JuliaRegistries/General.git
Precompiling project...

In :
using RigidBodyDynamics
using StaticArrays
using SymPy

┌ Info: Recompiling stale cache file /Users/tkoolen/.julia/compiled/v1.0/RigidBodyDynamics/WeevQ.ji for RigidBodyDynamics [366cf18f-59d5-5db9-a4de-86a9f6786172]


## Create symbolic parameters¶

• Masses: $m_1, m_2$
• Mass moments of inertia (about center of mass): $I_1, I_2$
• Link lengths: $l_1, l_2$
• Center of mass locations (w.r.t. preceding joint axis): $c_1, c_2$
• Gravitational acceleration: $g$
In :
inertias = @syms m_1 m_2 I_1 I_2 positive = true
lengths = @syms l_1 l_2 c_1 c_2 real = true
gravitational_acceleration = @syms g real = true
params = [inertias..., lengths..., gravitational_acceleration...]
transpose(params)

Out:
\begin{bmatrix}m_{1}&m_{2}&I_{1}&I_{2}&l_{1}&l_{2}&c_{1}&c_{2}&g\end{bmatrix}

## Create double pendulum Mechanism¶

A Mechanism contains the joint layout and inertia parameters, but no state information.

In :
T = Sym # the 'scalar type' of the Mechanism we'll construct
axis = SVector(zero(T), one(T), zero(T)) # axis of rotation for each of the joints
double_pendulum = Mechanism(RigidBody{T}("world"); gravity = SVector(zero(T), zero(T), g))
world = root_body(double_pendulum) # the fixed 'world' rigid body

# Attach the first (upper) link to the world via a revolute joint named 'shoulder'
inertia1 = SpatialInertia(CartesianFrame3D("upper_link"), moment=I_1 * axis * axis', com=SVector(zero(T), zero(T), c_1), mass=m_1)
body1 = RigidBody(inertia1)
joint1 = Joint("shoulder", Revolute(axis))
joint1_to_world = one(Transform3D{T}, frame_before(joint1), default_frame(world));
attach!(double_pendulum, world, body1, joint1, joint_pose = joint1_to_world);

# Attach the second (lower) link to the world via a revolute joint named 'elbow'
inertia2 = SpatialInertia(CartesianFrame3D("lower_link"), moment=I_2 * axis * axis', com=SVector(zero(T), zero(T), c_2), mass=m_2)
body2 = RigidBody(inertia2)
joint2 = Joint("elbow", Revolute(axis))
joint2_to_body1 = Transform3D(frame_before(joint2), default_frame(body1), SVector(zero(T), zero(T), l_1))
attach!(double_pendulum, body1, body2, joint2, joint_pose = joint2_to_body1)

Out:
Spanning tree:
Vertex: world (root)
Vertex: upper_link, Edge: shoulder
Vertex: lower_link, Edge: elbow
No non-tree joints.

## Create MechanismState associated with the double pendulum Mechanism¶

A MechanismState stores all state-dependent information associated with a Mechanism.

In :
x = MechanismState(double_pendulum);

In :
# Set the joint configuration vector of the MechanismState to a new vector of symbolic variables
q = configuration(x)
for i in eachindex(q)
q[i] = symbols("q_$i", real = true) end  In : # Set the joint velocity vector of the MechanismState to a new vector of symbolic variables v = velocity(x) for i in eachindex(v) v[i] = symbols("v_$i", real = true)
end


## Compute dynamical quantities in symbolic form¶

In :
# Mass matrix
simplify.(mass_matrix(x))

Out:
\begin{bmatrix}I_{1} + I_{2} + 2 c_{2} l_{1} m_{2} \cos{\left (q_{2} \right )} + l_{1}^{2} m_{2}&I_{2} + c_{2} l_{1} m_{2} \cos{\left (q_{2} \right )}\\I_{2} + c_{2} l_{1} m_{2} \cos{\left (q_{2} \right )}&I_{2}\end{bmatrix}
In :
# Kinetic energy
simplify(kinetic_energy(x))

Out:
\begin{equation*}\frac{I_{1} v_{1}^{2}}{2} + \frac{I_{2} v_{1}^{2}}{2} + I_{2} v_{1} v_{2} + \frac{I_{2} v_{2}^{2}}{2} + c_{2} l_{1} m_{2} v_{1}^{2} \cos{\left (q_{2} \right )} + c_{2} l_{1} m_{2} v_{1} v_{2} \cos{\left (q_{2} \right )} + \frac{l_{1}^{2} m_{2} v_{1}^{2}}{2}\end{equation*}
In :
# Potential energy
simplify(gravitational_potential_energy(x))

Out:
\begin{equation*}- g \left(c_{1} m_{1} \cos{\left (q_{1} \right )} + c_{2} m_{2} \cos{\left (q_{1} + q_{2} \right )} + l_{1} m_{2} \cos{\left (q_{1} \right )}\right)\end{equation*}