Open baggepinnen opened 3 years ago
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?
Oh I see, those two branches are eliminated by the assumptions. This is a nice use case of assumptions in metadata.
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.
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
Which I naively defined as
When I came to the interesting part of the example
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 oftranspose(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