1. Quickstart - double pendulum
This example is also available as a Jupyter notebook that can be run locally The notebook can be found in the examples directory of the package. If the notebooks are missing, you may need to run using Pkg; Pkg.build().
Setup
In addition to RigidBodyDynamics, we'll be using the StaticArrays package, used throughout RigidBodyDynamics, which provides stack-allocated, fixed-size arrays:
using RigidBodyDynamics
using LinearAlgebra
using StaticArraysCreating a double pendulum Mechanism
We're going to create a simple Mechanism that represents a double pendulum. The Mechanism type can be thought of as an interconnection of rigid bodies and joints.
We'll start by creating a 'root' rigid body, representing the fixed world, and using it to create a new Mechanism:
g = -9.81 # gravitational acceleration in z-direction
world = RigidBody{Float64}("world")
doublependulum = Mechanism(world; gravity = SVector(0, 0, g))Spanning tree: Vertex: world (root) No non-tree joints.
Note that the RigidBody type is parameterized on the 'scalar type', here Float64.
We'll now add a second body, called 'upper link', to the Mechanism. We'll attach it to the world with a revolute joint, with the $y$-axis as the axis of rotation. We'll start by creating a SpatialInertia, which stores the inertial properties of the new body:
axis = SVector(0., 1., 0.) # joint axis
I_1 = 0.333 # moment of inertia about joint axis
c_1 = -0.5 # center of mass location with respect to joint axis
m_1 = 1. # mass
frame1 = CartesianFrame3D("upper_link") # the reference frame in which the spatial inertia will be expressed
inertia1 = SpatialInertia(frame1,
moment=I_1 * axis * axis',
com=SVector(0, 0, c_1),
mass=m_1)SpatialInertia expressed in "upper_link" (id = 1): mass: 1.0 center of mass: Point3D in "upper_link": [0.0, 0.0, -0.5] moment of inertia (about origin of "upper_link" (id = 1): [0.0 0.0 0.0; 0.0 0.333 0.0; 0.0 0.0 0.0]
Note that the created SpatialInertia is annotated with the frame in which it is expressed (in the form of a CartesianFrame3D). This is a common theme among RigidBodyDynamics objects. Storing frame information with the data obviates the need for the complicated variable naming conventions that are used in some other libraries to disambiguate the frame in which quantities are expressed. It also enables automated reference frame checks.
We'll now create the second body:
upperlink = RigidBody(inertia1)RigidBody: "upper_link"
and a new revolute joint called 'shoulder':
shoulder = Joint("shoulder", Revolute(axis))Joint "shoulder": Revolute joint with axis [0.0, 1.0, 0.0]
Creating a Joint automatically constructs two new CartesianFrame3D objects: a frame directly before the joint, and one directly after. To attach the new body to the world by this joint, we'll have to specify where the frame before the joint is located on the parent body (here, the world):
before_shoulder_to_world = one(Transform3D,
frame_before(shoulder), default_frame(world))Transform3D from "before_shoulder" to "world": rotation: 0.0 rad about [1.0, 0.0, 0.0], translation: [0.0, 0.0, 0.0]
Now we can attach the upper link to the world:
attach!(doublependulum, world, upperlink, shoulder,
joint_pose = before_shoulder_to_world)Spanning tree: Vertex: world (root) Vertex: upper_link, Edge: shoulder No non-tree joints.
which changes the tree representation of the Mechanism.
We can attach the lower link in similar fashion:
l_1 = -1. # length of the upper link
I_2 = 0.333 # moment of inertia about joint axis
c_2 = -0.5 # center of mass location with respect to joint axis
m_2 = 1. # mass
inertia2 = SpatialInertia(CartesianFrame3D("lower_link"),
moment=I_2 * axis * axis',
com=SVector(0, 0, c_2),
mass=m_2)
lowerlink = RigidBody(inertia2)
elbow = Joint("elbow", Revolute(axis))
before_elbow_to_after_shoulder = Transform3D(
frame_before(elbow), frame_after(shoulder), SVector(0, 0, l_1))
attach!(doublependulum, upperlink, lowerlink, elbow,
joint_pose = before_elbow_to_after_shoulder)Spanning tree:
Vertex: world (root)
Vertex: upper_link, Edge: shoulder
Vertex: lower_link, Edge: elbow
No non-tree joints.Now our double pendulum Mechanism is complete.
Note: instead of defining the Mechanism in this way, it is also possible to load in a URDF file (an XML file format used in ROS), using the parse_urdf function, e.g.:
srcdir = dirname(pathof(RigidBodyDynamics))
urdf = joinpath(srcdir, "..", "test", "urdf", "Acrobot.urdf")
parse_urdf(urdf)Spanning tree:
Vertex: world (root)
Vertex: upper_link, Edge: shoulder
Vertex: lower_link, Edge: elbow
No non-tree joints.The state of a Mechanism
A Mechanism stores the joint/rigid body layout, but no state information. State information is separated out into a MechanismState object:
state = MechanismState(doublependulum)MechanismState{Float64, Float64, Float64, …}(…)Let's first set the configurations and velocities of the joints:
set_configuration!(state, shoulder, 0.3)
set_configuration!(state, elbow, 0.4)
set_velocity!(state, shoulder, 1.)
set_velocity!(state, elbow, 2.);The joint configurations and velocities are stored as Vectors (denoted $q$ and $v$ respectively in this package) inside the MechanismState object:
q = configuration(state)
v = velocity(state)2-element SegmentedVector{JointID,Float64,Base.OneTo{JointID},Array{Float64,1}}:
1.0
2.0Kinematics
We are now ready to do kinematics. Here's how you transform a point at the origin of the frame after the elbow joint to world frame:
transform(state, Point3D(frame_after(elbow), zero(SVector{3})),
default_frame(world))Point3D in "world": [-0.29552020666133955, 0.0, -0.955336489125606]
Other objects like Wrenches, Twists, and SpatialInertias can be transformed in similar fashion.
You can also ask for the homogeneous transform to world:
transform_to_root(state, frame_after(elbow))Transform3D from "after_elbow" to "world": rotation: 0.7000000000000001 rad about [7.089183103716176e-63, 1.0, 0.0], translation: [-0.29552020666133955, 0.0, -0.955336489125606]
Or a relative transform:
relative_transform(state, frame_after(elbow), frame_after(shoulder))Transform3D from "after_elbow" to "after_shoulder": rotation: 0.39999999999999997 rad about [1.2235735295359127e-62, 1.0, 0.0], translation: [9.370825333944079e-18, 0.0, -0.9999999999999999]
and here's the center of mass of the double pendulum:
center_of_mass(state)Point3D in "world": [-0.38269457680542746, 0.0, -0.9077129136653266]
Dynamics
A MechanismState can also be used to compute quantities related to the dynamics of the Mechanism. Here we compute the mass matrix:
mass_matrix(state)2×2 LinearAlgebra.Symmetric{Float64,Array{Float64,2}}:
2.58706 0.79353
0.79353 0.333Note that there is also a zero-allocation version, mass_matrix! (the ! at the end of a method is a Julia convention signifying that the function is 'in-place', i.e. modifies its input data).
We can do inverse dynamics as follows (note again that there is a non-allocating version of this method as well):
v̇ = similar(velocity(state)) # the joint acceleration vector, i.e., the time derivative of the joint velocity vector v
v̇[shoulder][1] = 1
v̇[elbow][1] = 2
inverse_dynamics(state, v̇)2-element SegmentedVector{JointID,Float64,Base.OneTo{JointID},Array{Float64,1}}:
10.124916215693656
4.814127424056643Simulation
Let's simulate the double pendulum for 5 seconds, starting from the state we defined earlier. For this, we can use the basic simulate function:
ts, qs, vs = simulate(state, 5., Δt = 1e-3);simulate returns a vector of times (ts) and associated joint configurations (qs) and velocities (vs). You can of course plot the trajectories using your favorite plotting package (see e.g. Plots.jl). The MeshCatMechanisms or RigidBodyTreeInspector packages can also be used for 3D animation of the double pendulum in action. See also RigidBodySim.jl for a more full-fledge simulation environment.
A lower level interface for simulation/ODE integration with more options is also available. Consult the documentation for more information. In addition, RigidBodySim.jl offers a more full-featured simulation environment.
This page was generated using Literate.jl.