RigidBodyDynamics.jl- 6. Symbolics using SymPy

[JulienCalbert]

When I try to execute the following code that Thiago had written (and which is on the L2C drive) about driving a double pendulum with a symbolic representation:

using RigidBodyDynamics
using StaticArrays
using Symbolics

inertias = @variables m_1 m_2 I_1 I_2 positive = true
lengths = @variables l_1 l_2 c_1 c_2 real = true
gravitational_acceleration = @variables g real = true
params = [inertias..., lengths..., gravitational_acceleration...]
transpose(params)

T = Num 
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

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

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)

x = MechanismState(double_pendulum);

q = configuration(x)

v = velocity(x)

simplify.(mass_matrix(x)) # This gives you the general inertia matrix M(x) of the mechanism

@variables v_1 v_2 # joint velocities
@variables v̇_1 v̇_2 # joint accelarations

v̇ = similar(velocity(x)) # the joint acceleration vector, i.e., the time derivative of the joint velocity vector v
v̇[joint1][1] = v̇_1
v̇[joint2][1] = v̇_2

simplify.(inverse_dynamics(x, v̇)) # this gives you τ

I get the following error with julia-1.6, while it works with julia-1.5:

ERROR: LoadError: MethodError: no method matching decompose(::Num)
Closest candidates are:
  decompose(::Integer) at float.jl:555
  decompose(::Float16) at float.jl:557
  decompose(::Float32) at float.jl:568
  ...
Stacktrace:
  [1] isfinite(x::Num)
    @ Base .\float.jl:453
  [2] isinf(x::Num)
    @ Base .\float.jl:461
  [3] abs(q::Quaternions.Quaternion{Num})
    @ Quaternions C:\Users\jcalbert\.julia\packages\Quaternions\au6LK\src\Quaternion.jl:143
  [4] sign(x::Quaternions.Quaternion{Num})
    @ Base .\number.jl:135
  [5] QuatRotation
    @ C:\Users\jcalbert\.julia\packages\Rotations\VYTDG\src\unitquaternion.jl:22 [inlined]
  [6] QuatRotation
    @ C:\Users\jcalbert\.julia\packages\Rotations\VYTDG\src\unitquaternion.jl:46 [inlined]
  [7] Rotations.QuatRotation(w::Num, x::Num, y::Num, z::Num, normalize::Bool)
    @ Rotations C:\Users\jcalbert\.julia\packages\Rotations\VYTDG\src\unitquaternion.jl:50
  [8] Rotations.QuatRotation(w::Num, x::Num, y::Num, z::Num)
    @ Rotations C:\Users\jcalbert\.julia\packages\Rotations\VYTDG\src\unitquaternion.jl:49
  [9] rotation_between(from::SVector{3, Num}, to::SVector{3, Num})
    @ Rotations C:\Users\jcalbert\.julia\packages\Rotations\VYTDG\src\unitquaternion.jl:328
 [10] Revolute{Num}(axis::SVector{3, Num})
    @ RigidBodyDynamics C:\Users\jcalbert\.julia\packages\RigidBodyDynamics\8B04X\src\joint_types\revolute.jl:16
 [11] Revolute(axis::SVector{3, Num})
    @ RigidBodyDynamics C:\Users\jcalbert\.julia\packages\RigidBodyDynamics\8B04X\src\joint_types\revolute.jl:26
 [12] top-level scope
    @ C:\Users\jcalbert\Downloads\Thiago-double_pendulum\Symbolic_model.jl:33
 [13] include(fname::String)
    @ Base.MainInclude .\client.jl:444
 [14] top-level scope
    @ REPL[6]:1
in expression starting at C:\Users\jcalbert\Downloads\Thiago-double_pendulum\Symbolic_model.jl:33 

Similarly, the following code in the example does not work in Julia-1.6 either. https://github.com/JuliaRobotics/RigidBodyDynamics.jl/tree/master/examples/6.%20Symbolics%20using%20SymPy

[blegat]

It seems that https://github.com/JuliaGeometry/Quaternions.jl/pull/122 broke our example. Maybe isinf could be defined for Symbolics numbers ?

[blegat]

A similar error was thrown here: https://github.com/JuliaSymbolics/Symbolics.jl/issues/91 but it was fixed in https://github.com/korsbo/Latexify.jl/pull/145 without defining isinf in Symbolics.jl

[blegat]

This issue is relevant: https://github.com/JuliaSymbolics/Symbolics.jl/issues/97 We can just use type piracy for the methods that are missing