More asserts in URDF write.jl

[togo59]

As requested by @tkoolen here is a sort of minimal working example:

I construct a one-body, one-joint "robot". The revolute (z-axis) joint "A3" connects between "world" and a rigid body called "a3_a3b". What I would like is for (perhaps) URDF write.jl to emit an informative error to help users easily figure out where they've gone wrong. E.g.:

Set up: This is "my" inertia before it gets loaded into RBD

julia> inertia
3×3×1 Array{Float64,3}:
[:, :, 1] =
 12.48   -0.007       0.458
 -0.007  88.133       0.0003354
  0.458   0.0003354  78.551

julia> iner = spatial_inertia(rigidLink[1])
SpatialInertia expressed in "after_A3":
mass: 159.923
center of mass: Point3D in "after_A3": [0.0, 0.0, 1.4608]
moment of inertia:
[12.48 -0.007 0.458; -0.007 88.133 0.0003354; 0.458 0.0003354 78.551]

The above is correct.

julia> maximal_coordinates(mech)
(Spanning tree:
Vertex: world (root)
  Vertex: a3_a3b, Edge: a3_a3b
Non-tree joints:
A3, predecessor: world, successor: a3_a3b, 
Dict{RigidBody{Float64},Joint{Float64,JT} where JT<:JointType{Float64}}(a3_a3b=>a3_a3b), 
Dict(a3_a3b=>a3_a3b,world=>world), 
Dict{Joint{Float64,JT} where JT<:JointType{Float64},Joint{Float64,JT} where JT<:JointType{Float64}}(A3=>A3))

Not quite sure why edge and vertex (above) are the same, especially when they are different below. From now on I type from line 18 of write.jl into the REPL.

julia> canonicalized = deepcopy(mech)
Spanning tree:
Vertex: world (root)
  Vertex: a3_a3b, Edge: A3
No non-tree joints.

julia> RigidBodyDynamics.canonicalize_graph!(canonicalized)
Vertex: world (root)
  Vertex: a3_a3b, Edge: A3

The above makes perfect sense.

julia> origin = center_of_mass(iner)
Point3D in "after_A3": [0.0, 0.0, 1.4608]

Correct.

julia> centroidal = CartesianFrame3D()
CartesianFrame3D: "anonymous" (id = 116)

julia> to_centroidal_frame = Transform3D(iner.frame, centroidal, -origin.v)
Transform3D from "after_A3" to "anonymous":
rotation: 0.0 rad about [1.0, 0.0, 0.0], translation: [-0.0, -0.0, -1.4608]

Starting to look odd: -ve translation? Is "annonymous" the inertial FoR? Not "world"?

julia> inert = transform(iner, to_centroidal_frame)
SpatialInertia expressed in "anonymous":
mass: 159.923
center of mass: Point3D in "anonymous": [0.0, 0.0, 0.0]
moment of inertia:
[-328.786 -0.007 0.458; -0.007 -253.133 0.0003354; 0.458 0.0003354 78.551]

The above is wrong. (Note I used a new variable name "inert" to preserve the "iner" variable.)

julia> iner.frame
CartesianFrame3D: "after_A3" (id = 111)

julia> inert.frame
CartesianFrame3D: "anonymous" (id = 116)

julia> iner
SpatialInertia expressed in "after_A3":
mass: 159.923
center of mass: Point3D in "after_A3": [0.0, 0.0, 1.4608]
moment of inertia:
[12.48 -0.007 0.458; -0.007 88.133 0.0003354; 0.458 0.0003354 78.551]

Correct

julia> inert
SpatialInertia expressed in "anonymous":
mass: 159.923
center of mass: Point3D in "anonymous": [0.0, 0.0, 0.0]
moment of inertia:
[-328.786 -0.007 0.458; -0.007 -253.133 0.0003354; 0.458 0.0003354 78.551]

Incorrect.

I believe that this is a brain issue (mine, e.g. not knowing how to use RBD) not a coding error on anyone's part. But some additional @asserts scattered throughout the process would be extremely useful!

[tkoolen]

To summarize as a simple runnable example:

using RigidBodyDynamics
using StaticArrays

moment = [12.48 -0.007 0.458; -0.007 88.133 0.0003354; 0.458 0.0003354 78.551]
com = [0.0, 0.0, 1.4608]
m = 159.923
frame1 = CartesianFrame3D()
frame2 = CartesianFrame3D()

inertia1 = SpatialInertia(frame1, moment, com * m, m)
tf = Transform3D(inertia1.frame, frame2, @SVector [-0.0, -0.0, -1.4608])
inertia2 = transform(inertia1, tf)

@show inertia2

results in

SpatialInertia expressed in "anonymous":
mass: 159.923
center of mass: Point3D in "anonymous": [0.0, 0.0, 0.0]
moment of inertia:
[-328.786 -0.007 0.458; -0.007 -253.133 0.0003354; 0.458 0.0003354 78.551]

This is because inertia1 is actually nonphysical, despite the fact that the moment of inertia part satisfies the triangle inequality. This can be seen by doing an eigenvalue decomposition of the full 6x6 inertia matrix:

julia> using LinearAlgebra

julia> eigvals(SMatrix(inertia1))
6-element SArray{Tuple{6},Float64,1,6}:
 -158.77064513722854
 -112.32906311867893
   78.55128470628988
  159.923
  331.17335978337456
  360.385063766243

What's happening is that inertia1's moment of inertia is too small for its center of mass offset. If all of the mass were concentrated at the center of mass, the parallel axis theorem tells us that the xx and yy components of the moment of inertia are 159.923 * 1.4608^2 = 341.2655492787201, which is larger than the xx and yy components specified in inertia1. Having the mass not be concentrated at the center of mass would only increase these values.

You probably assumed that the moment of inertia is expressed in a frame with the center of mass as the origin. This is not the case; everything is expressed in inertia1.frame.

There's been at least one other person who has been confused about this though; I really should stress this point more in the documentation and add a convenience constructor that allows you to specify the moment of inertia about the center of mass.

[togo59]

I'm extremely grateful for your careful and detailed responses. The fact that the inertia terms for only three of my six links were wrong is rather embarrassing as those were the three for which I attempted to re-calculate the values following a misunderstanding between the drawing office and me in defining the components in their CAD models, from which the original inertia tensors were produced. (Ouch!)

[togo59]

Working perfectly now! 🥇

[tkoolen]

Good to hear!

[togo59]

This is my robot arm. The joints are very compliant and hugely non-linear. I have to make it stop wobbling using adaptive feedback control. I love my job!

[tkoolen]

Wow, very cool!