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

  Updating registry at `~/.julia/registries/General`
  Updating git-repo `https://github.com/JuliaRegistries/General.git`
[?25l[2K[?25h┌ Warning: Module JSON with build ID 69537206423 is missing from the cache.
│ This may mean JSON [682c06a0-de6a-54ab-a142-c8b1cf79cde6] does not support precompilation but is imported by a module that does.
└ @ Base loading.jl:947
┌ Warning: Module JSON with build ID 69537206423 is missing from the cache.
│ This may mean JSON [682c06a0-de6a-54ab-a142-c8b1cf79cde6] does not support precompilation but is imported by a module that does.
└ @ Base loading.jl:947
┌ Warning: Module JSON with build ID 69537206423 is missing from the cache.
│ This may mean JSON [682c06a0-de6a-54ab-a142-c8b1cf79cde6] does not support precompilation but is imported by a module that does.
└ @ Base loading.jl:947
┌ Warning: Module JSON with build ID 69537206423 is missing from the cache.
│ This may mean JSON [682c06a0-de6a-54ab-a142-c8b1cf79cde6] does not support precompilation but is imported by a module that does.
└ @ Base loading.jl:947
┌ Warning: Module JSON with build ID 69537206423 is missing from the cache.
│ This may mean JSON [682c06a0-de6a-54ab-a142-c8b1cf79cde6] does not support precompilation but is imported by a module that does.
└ @ Base loading.jl:947
┌ Warning: Module JSON with build ID 69537206423 is missing from the cache.
│ This may mean JSON [682c06a0-de6a-54ab-a142-c8b1cf79cde6] does not support precompilation but is imported by a module that does.
└ @ Base loading.jl:947
┌ Warning: Module JSON with build ID 69537206423 is missing from the cache.
│ This may mean JSON [682c06a0-de6a-54ab-a142-c8b1cf79cde6] does not support precompilation but is imported by a module that does.
└ @ Base loading.jl:947
[ 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/travis/.julia/conda/3

  added / updated specs:
    - sympy


The following packages will be downloaded:

    package                    |            build
    ---------------------------|-----------------
    fastcache-1.1.0            |   py37h7b6447c_0          30 KB
    gmp-6.1.2                  |       h6c8ec71_1         514 KB
    gmpy2-2.0.8                |   py37h10f8cd9_2         150 KB
    mpc-1.1.0                  |       h10f8cd9_1          90 KB
    mpfr-4.0.1                 |       hdf1c602_3         429 KB
    mpmath-1.1.0               |           py37_0         766 KB
    sympy-1.4                  |           py37_0         7.9 MB
    ------------------------------------------------------------
                                           Total:         9.9 MB

The following NEW packages will be INSTALLED:

  fastcache          pkgs/main/linux-64::fastcache-1.1.0-py37h7b6447c_0
  gmp                pkgs/main/linux-64::gmp-6.1.2-h6c8ec71_1
  gmpy2              pkgs/main/linux-64::gmpy2-2.0.8-py37h10f8cd9_2
  mpc                pkgs/main/linux-64::mpc-1.1.0-h10f8cd9_1
  mpfr               pkgs/main/linux-64::mpfr-4.0.1-hdf1c602_3
  mpmath             pkgs/main/linux-64::mpmath-1.1.0-py37_0
  sympy              pkgs/main/linux-64::sympy-1.4-py37_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)

1×9 LinearAlgebra.Transpose{SymPy.Sym,Array{SymPy.Sym,1}}:
 m_1  m_2  I_1  I_2  l_1  l_2  c_1  c_2  g

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)

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))

2×2 Array{SymPy.Sym,2}:
 I_1 + I_2 + 2*c_2*l_1*m_2*cos(q_2) + l_1^2*m_2  I_2 + c_2*l_1*m_2*cos(q_2)
                     I_2 + c_2*l_1*m_2*cos(q_2)                         I_2

Kinetic energy

simplify(kinetic_energy(x))

     2        2                   2
I₁⋅v₁    I₂⋅v₁               I₂⋅v₂               2
────── + ────── + I₂⋅v₁⋅v₂ + ────── + c₂⋅l₁⋅m₂⋅v₁ ⋅cos(q₂) + c₂⋅l₁⋅m₂⋅v₁⋅v₂⋅co
  2        2                   2

          2      2
        l₁ ⋅m₂⋅v₁
s(q₂) + ──────────
            2

Potential energy

simplify(gravitational_potential_energy(x))

-g⋅(c₁⋅m₁⋅cos(q₁) + c₂⋅m₂⋅cos(q₁ + q₂) + l₁⋅m₂⋅cos(q₁))

This page was generated using Literate.jl.