Some missing methods

[baggepinnen]

While translating the example https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/master/examples/6.%20Symbolics%20using%20SymPy/6.%20Symbolics%20using%20SymPy.jl from using SymPy to using Symbolics.jl, I encountered the following missing functions

Base.typemax
Base.typemin
Base.eps

Which I naively defined as

Base.eps(::Type{Num}) = Num(0)
Base.typemin(::Type{Num}) = Num(-Inf)
Base.typemax(::Type{Num}) = Num(Inf)

When I came to the interesting part of the example

julia> simplify.(mass_matrix(x))
MethodError: no method matching AbstractFloat(::Num)
Closest candidates are:
  (::Type{T})(::Real, ::RoundingMode) where T<:AbstractFloat at rounding.jl:200
  (::Type{T})(::T) where T<:Number at boot.jl:760
  (::Type{T})(::AbstractChar) where T<:Union{AbstractChar, Number} at char.jl:50
  ...

Stacktrace:
  [1] float(x::Num)
    @ Base ./float.jl:206
  [2] sincos(x::Num)
    @ Base.Math ./special/trig.jl:203
  [3] Tuple
    @ ~/.julia/packages/Rotations/QaTBi/src/angleaxis_types.jl:52 [inlined]
  [4] convert
    @ ~/.julia/packages/StaticArrays/LJQEe/src/convert.jl:10 [inlined]
  [5] Rotations.RotMatrix3{Num}(x::Rotations.AngleAxis{Num})
    @ Rotations ~/.julia/packages/Rotations/QaTBi/src/core_types.jl:83
  [6] convert(#unused#::Type{Rotations.RotMatrix3{Num}}, rot::Rotations.AngleAxis{Num})
    @ Rotations ~/.julia/packages/Rotations/QaTBi/src/core_types.jl:20
  [7] joint_transform
    @ ~/.julia/dev/RigidBodyDynamics/src/joint_types/revolute.jl:61 [inlined]
  [8] joint_transform
    @ ~/.julia/dev/RigidBodyDynamics/src/joint.jl:191 [inlined]
  [9] macro expansion

which appears to be a problem with sincos, and indeed, Base.sincos(x::Num) = sin(x), cos(x) let's me continue a bit further. THe expression I get back contains a lot of transpose(I_1 + transpose(...)) which would be nice to get rid of since all variables inside are scalars.

┆Issue is synchronized with this Trello card by Unito

[ChrisRackauckas]

Here's a version that works:

using RigidBodyDynamics
using StaticArrays
using Symbolics


###### Modifications

Base.eps(::Type{Num}) = 0
Base.typemin(::Type{Num}) = -Inf
Base.typemax(::Type{Num}) = Inf
@register Base.rem2pi(x,y::RoundingMode)
Base.float(x::Num) = x

function RigidBodyDynamics.principal_value(aa::RigidBodyDynamics.AngleAxis{T}) where {T}
    theta = rem2pi(aa.theta, RoundNearest)
    Symbolics.IfElse.ifelse(theta < zero(T),RigidBodyDynamics.AngleAxis(-theta, -aa.axis_x, -aa.axis_y, -aa.axis_z, false),
                  RigidBodyDynamics.AngleAxis( theta,  aa.axis_x,  aa.axis_y,  aa.axis_z, false))
end

Base.@propagate_inbounds function RigidBodyDynamics.gravitational_potential_energy(state::RigidBodyDynamics.MechanismState, body::Union{<:RigidBodyDynamics.RigidBody, RigidBodyDynamics.BodyID}, safe=true)
    inertia = RigidBodyDynamics.spatial_inertia(body)
    m = inertia.mass
    #m > 0 || return zero(RigidBodyDynamics.cache_eltype(state))
    com = RigidBodyDynamics.transform_to_root(state, body, safe) * RigidBodyDynamics.center_of_mass(inertia)
    -m * RigidBodyDynamics.dot(state.mechanism.gravitational_acceleration, RigidBodyDynamics.FreeVector3D(com))
end


## Now script


# ## 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 = @variables m_1 m_2 I_1 I_2
lengths = @variables l_1 l_2 c_1 c_2
gravitational_acceleration = @variables g
params = [inertias..., lengths..., gravitational_acceleration...]
transpose(params)


# ## Create double pendulum `Mechanism`
# A `Mechanism` contains the joint layout and inertia parameters, but no state information.

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

# 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] = Num(Variable(Symbol("q_$i")))
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] = Num(Variable(Symbol("v_$i")))
end


# ## Compute dynamical quantities in symbolic form

# Mass matrix
simplify.(mass_matrix(x))


# Kinetic energy
simplify(kinetic_energy(x))

# Potential energy
simplify(gravitational_potential_energy(x))

@YingboMa what do you think of those overloads?

I think the RigidBodyDynamics ones require @MasonProtter's abstract tracing to handle the branching though. I don't see how SymPy could do this correctly?

[ChrisRackauckas]

Oh I see, those two branches are eliminated by the assumptions. This is a nice use case of assumptions in metadata.

[YingboMa]

This is slightly different from the metadata system that we have currently. We don't propagate the metadata for now. Propagating metadata requires a bit of work. For instance, we need to propagate positivity for exp(x), x^(n::iseven), etc. Handling those cases for various assumptions could be a nice GSoC project.