6. Symbolics using SymPy

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

using RigidBodyDynamics
using StaticArrays
using SymPy

 Activating environment at `~/work/RigidBodyDynamics.jl/RigidBodyDynamics.jl/examples/6. Symbolics using SymPy/Project.toml`
[ Info: Installing sympy via the Conda sympy package...
[ Info: Running `conda install -q -y sympy` in root environment
Collecting package metadata (current_repodata.json): ...working... done
Solving environment: ...working... done

## Package Plan ##

  environment location: /home/runner/.julia/conda/3

  added / updated specs:
    - sympy


The following packages will be downloaded:

    package                    |            build
    ---------------------------|-----------------
    gmp-6.2.1                  |       h2531618_2         539 KB
    gmpy2-2.0.8                |   py38hd5f6e3b_3         162 KB
    mpc-1.1.0                  |       h10f8cd9_1          90 KB
    mpfr-4.0.2                 |       hb69a4c5_1         487 KB
    mpmath-1.2.1               |   py38h06a4308_0         766 KB
    sympy-1.8                  |   py38h06a4308_0         9.1 MB
    ------------------------------------------------------------
                                           Total:        11.1 MB

The following NEW packages will be INSTALLED:

  gmp                pkgs/main/linux-64::gmp-6.2.1-h2531618_2
  gmpy2              pkgs/main/linux-64::gmpy2-2.0.8-py38hd5f6e3b_3
  mpc                pkgs/main/linux-64::mpc-1.1.0-h10f8cd9_1
  mpfr               pkgs/main/linux-64::mpfr-4.0.2-hb69a4c5_1
  mpmath             pkgs/main/linux-64::mpmath-1.2.1-py38h06a4308_0
  sympy              pkgs/main/linux-64::sympy-1.8-py38h06a4308_0


Preparing transaction: ...working... done
Verifying transaction: ...working... done
Executing transaction: ...working... done

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$

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)

\[\left[ \begin{array}{rrrrrrrrr}m_{1}&m_{2}&I_{1}&I_{2}&l_{1}&l_{2}&c_{1}&c_{2}&g\end{array}\right]\]

Create double pendulum `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
double_pendulum = Mechanism(RigidBody{T}("world"); gravity = SVector(zero(T), zero(T), g))
world = root_body(double_pendulum) # the fixed 'world' rigid body

RigidBody: "world"

Attach the first (upper) link to the world via a revolute joint named 'shoulder'

inertia1 = SpatialInertia(CartesianFrame3D("upper_link"),
    moment=I_1 * axis * transpose(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 * transpose(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)

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.

x = MechanismState(double_pendulum);

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

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

Mass matrix

simplify.(mass_matrix(x))

\[\left[ \begin{array}{rr}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{array}\right]\]

Kinetic energy

simplify(kinetic_energy(x))

\[\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*}\]

Potential energy

simplify(gravitational_potential_energy(x))

\[\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*}\]

This page was generated using Literate.jl.