RigidBodyDynamics visualization not working

General Usage
1

I am playing with the example from the RigidBodyDynamic.jl documentation with the MeshCatMechanisms.jl. No major changes.

How do I get the last link to show?

using RigidBodyDynamics
using LinearAlgebra
using StaticArrays
using MeshCatMechanisms
using Random

g = -9.81 # gravitational acceleration in z-direction
world = RigidBody{Float64}("world")
doublependulum = Mechanism(world; gravity = SVector(0, 0, g))
axis = SVector(0., 1., 0.) # joint axis

axis = SVector(0., 1., 0.) # joint axis
I_1 = 0.333 # moment of inertia about joint axis
c_1 = -0.5 # center of mass location with respect to joint axis
m_1 = 1. # mass
frame1 = CartesianFrame3D("upper_link") # the reference frame in which the spatial inertia will be expressed
inertia1 = SpatialInertia(frame1,
    moment=I_1 * axis * axis',
    com=SVector(0, 0, c_1),
    mass=m_1)

upperlink = RigidBody(inertia1)
shoulder = Joint("shoulder", Revolute(axis))
before_shoulder_to_world = one(Transform3D, frame_before(shoulder), default_frame(world))
attach!(doublependulum, world, upperlink, shoulder, joint_pose = before_shoulder_to_world)

l_1 = -1. # length of the upper link
I_2 = 0.333 # moment of inertia about joint axis
c_2 = -0.5 # center of mass location with respect to joint axis
m_2 = 1. # mass
inertia2 = SpatialInertia(CartesianFrame3D("lower_link"),
    moment=I_2 * axis * axis',
    com=SVector(0, 0, c_2),
    mass=m_2)
lowerlink = RigidBody(inertia2)
elbow = Joint("elbow", Revolute(axis))
before_elbow_to_after_shoulder = Transform3D(
    frame_before(elbow), frame_after(shoulder), SVector(0, 0, l_1))
attach!(doublependulum, upperlink, lowerlink, elbow,
    joint_pose = before_elbow_to_after_shoulder)

state = MechanismState(doublependulum)

set_configuration!(state, shoulder, 0.3)
set_configuration!(state, elbow, 0.4)
set_velocity!(state, shoulder, 1.)
set_velocity!(state, elbow, 2.);
setdirty!(state)

using MeshCatMechanisms

vis = MechanismVisualizer(doublependulum, Skeleton(inertias = false));
open(vis)

I was playing with adding a point. It worked, but I cannot figure out how the link is supposed to work

frame3 = CartesianFrame3D("end_effector") # the reference frame in which the spatial inertia will be expressed
lower_link_body = findbody(doublependulum, "lower_link")
end_point = Point3D(default_frame(lower_link_body), 0., 0, l_1)

image

2

With Skeleton(inertias = false), MechanismGeometries.jl generates a box geometry (‘rod’) between the origins of each pair of joints that is attached to the same rigid body. This is handy as a quick and dirty visualization for for closed-loop mechanisms, like the four-bar linkage example. But for kinematic trees like the double pendulum, there’s really no way for MechanismGeometries.jl to come up with appropriate geometry to draw for the ‘leaf’ links in the tree (e.g., the lower link for the double pendulum). The mechanism does not store the ‘length’ of the second link (since that’s not necessary for the dynamics), so that can’t be used to generate geometry, and you’re explicitly disabling the inertia ellipsoid visualization, which would use CoM location and moment of inertia.

You can instead define your mechanism along with visual geometries for the links in a URDF file (see e.g. https://github.com/JuliaRobotics/RigidBodyDynamics.jl/blob/master/test/urdf/Acrobot.urdf for a double pendulum, and use URDFVisuals instead of Skeleton as the geometry source). Or you can still define the mechanism layout in code as you’re doing, but manually add appropriate geometries to the nodes in the visualizer tree corresponding to the links using the setelement! function. See https://github.com/JuliaRobotics/MeshCatMechanisms.jl/blob/master/examples/demo.ipynb for some examples, but note that you can add other geometry types as well.

2 Likes
3

The Skeleton geometry source is pretty basic–it only draws sticks between joints, so a terminal link will only be obvious if you allow it to draw the inertias. If you set inertias=true, do you see the link?

1 Like
4

with inertias=true I get an error

ERROR: Principal inertias [-0.25, 0, 0.083] do not satisfy the triangle inequalities, so the equivalent inertial ellipsoid is not well-defined

image
image951×328 57.7 KB

5

Ugh, yeah, that’s https://github.com/JuliaRobotics/RigidBodyDynamics.jl/issues/399 which we haven’t gotten around to. Instead of moment=I_1 * axis * axis' you should be able to use moment=I_1 * [0.01, 1, 1], for example.

6

SDiagonal() produces the same error. I will move to URDF files

7

See Adding external force RigidBodyDynamics mechansms - #4 by tkoolen.

1 Like