using RigidBodyDynamics
using StaticArrays
using SymPy
WARNING: Method definition macroexpand(Module, Any) in module Compat at /home/twan/code/RigidBodyDynamics/v0.5/Compat/src/Compat.jl:1491 overwritten in module MacroTools at /home/twan/code/RigidBodyDynamics/v0.5/MacroTools/src/utils.jl:64.
inertias = @syms m_1 m_2 I_1 I_2 positive = true
lengths = @syms l_1 l_2 c_1 c_2 real = true
gravitationalAcceleration = @syms g real = true
params = [inertias..., lengths..., gravitationalAcceleration...]
Mechanism
¶A Mechanism
contains the joint layout and inertia parameters, but no state information.
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
doublePendulum = Mechanism(RigidBody{T}("world"); gravity = SVector(zero(T), zero(T), g))
world = root_body(doublePendulum) # the fixed 'world' rigid body
# Attach the first (upper) link to the world via a revolute joint named 'shoulder'
inertia1 = SpatialInertia(CartesianFrame3D("upper_link"), I_1 * axis * axis.', m_1 * SVector(zero(T), zero(T), c_1), m_1)
body1 = RigidBody(inertia1)
joint1 = Joint("shoulder", Revolute(axis))
joint1ToWorld = eye(RigidBodyDynamics.Transform3DS{T}, frame_before(joint1), default_frame(world))
attach!(doublePendulum, world, joint1, joint1ToWorld, body1)
# Attach the second (lower) link to the world via a revolute joint named 'elbow'
inertia2 = SpatialInertia(CartesianFrame3D("lower_link"), I_2 * axis * axis.', m_2 * SVector(zero(T), zero(T), c_2), m_2)
body2 = RigidBody(inertia2)
joint2 = Joint("elbow", Revolute(axis))
joint2ToBody1 = Transform3D(frame_before(joint2), default_frame(body1), SVector(zero(T), zero(T), l_1))
attach!(doublePendulum, body1, joint2, joint2ToBody1, body2)
Spanning tree: Vertex: world (root) Vertex: upper_link, Edge: shoulder Vertex: lower_link, Edge: elbow No non-tree joints.
MechanismState
associated with the double pendulum Mechanism
¶A MechanismState
stores all state-dependent information associated with a Mechanism
.
x = MechanismState(T, doublePendulum);
# Set the joint configuration vector of the MechanismState to a new vector of symbolic variables
configuration(x)[:] = [symbols("q_$i", real = true) for i = 1 : num_positions(x)]
# Set the joint velocity vector of the MechanismState to a new vector of symbolic variables
velocity(x)[:] = [symbols("v_$i", real = true) for i = 1 : num_positions(x)]
# Mass matrix
M = mass_matrix(x)
map!(simplify, M.data, M.data) # Note: M is a Symmetric matrix type; need to simplify the underlying data
full(M) # convert to full form so that it is pretty-printed (minor bug in SymPy.jl)
# Kinetic energy
simplify(kinetic_energy(x))
# Potential energy
simplify(gravitational_potential_energy(x))