Develop a lightweight spatial algebra library

[amcastro-tri]

Right now all operations involving spatial algebra are carried out essentially by hand.
Examples of this can be seen:

• transformSpatialInertia called from updateCompositeRigidBodyInertias in turn called by inverseDynamics.

This is a very error-prone approach and is cause of confusion to developers even when well familiar with these concepts.

I proposal is to write a very lightweight library built on top of Eigen to provide very expressive spatial algebra representations of spatial vectors, inertias, wrenches and twists. This library could even implement a frame checking capability (which could be disabled with a macro at compile time) to avoid mixing operations with spatial quantities on different frames.

[sherm1]

I'd love to see spatial objects treated nicely in Drake, with properly overloaded operators so that expressions involving them can be understood at a glance, and unit tests providing some confidence. I'm not sure all the current computations actually need to get done -- some appear to be artifacts of computing the multibody dynamics in inefficient matrix form rather than in operator form.

I'm skeptical about the frame-checking stuff -- if the frames are known but being misused then good notation would fix that. If OTOH the frames are not known then how do the quantities get marked with correct frames? And is it just the expressed-in frame? What about the measured-in frame and the differentiated-with-respect-to frame? (Common to have angular velocity of frame B measured in frame A expressed in World, for example.)

In any case I think there ought to be low-level, purely numerical objects available on which more elaborate, frame-checked classes could be built. These objects should come with a standardized notation discipline like this one.

BTW, Simbody has a set of spatial object classes and utilities, SpatialInertia, ArticulatedInertia, spatial shift matrices, and inline operators that provide optimal computations with minimal syntax.

[amcastro-tri]

This is a great set of slides @sherm1! thank you!. I used before the convention you describe in slide 8 but (to simplify things) with the additional convention that bodies twists where in a frame with its origin at the body's center of mass but aligned with the world's (static) coordinate frame.
I also like the "real example" in slide 15. Here you check if body A is the ground (static) and you do an extra set of computations to properly express twists in a moving (dynamic) frame. I think we could just encode that in our implementation by just flagging frames as static or dynamic and then your example would not need all the extra code. Though that would require an extra "if" in the implementation...

[jwnimmer-tri]

So I'm all for rolling our own of what we need, so that its tailored to purpose and easy to use, but are there existing libraries that would meet the need?

For example, in Driving we use https://github.com/strasdat/Sophus and it seems at least vaguely related to what you're trying to solve here. (I admit to not knowing the domain well, so maybe its something else entirely.)

[sherm1]

I can't find documentation for Sophus, and the test cases are indecipherable and undocumented. At first glance it looks like a long-winded way of representing Rotations (SO(3)) and Transforms (SE(3)). I know some people are fond of thinking of those simple objects in Lie algebra terms, but I've never understood why that's a good thing.

Also there are a lot more spatial quantities than just those dealing with spatial pose (like spatial forces, inertias, accelerations, etc.). Maybe those are in Sophus somewhere too? Do you know of some documentation, @jwnimmer-tri? Without that I don't get why anyone would want to use this package.

[jwnimmer-tri]

Sorry, I don't have any other specific information. I know that others also have found that code difficult.

[sherm1]

I suspect that using this in driving is a Really Bad Idea. It will have the effect of making what could have been straightforward code understandable by any graphics or mechanics-savvy engineer into something only a mathematician could love.

[rpoyner-tri]

@sherm1 +1. Empirically, both Grant and I fell down the Sophus rathole when we first started to work with driving. It's not a terribly deep pit, but it is certainly unfamiliar language for otherwise familiar concepts.

[amcastro-tri]

FWIW. I already have a minimum implementation of this in my own branch for MBT. Probably the most developed component in MBT so far so I'll start pushing soon. However so far I focused on capabilities we do not have in libraries like Eigen (I don't know about Sophus). Those are: spatial vectors, shift operators, rotational inertias, spatial inertias and a set of the most common operations between those.
As I work on MBT I refine those capabilities specifically targeting our needs. The main motivation is to essentially have a nice set of abstractions (physical rather than mathematical) where all the Eigen template handling can be hidden as an implementation detail and with precise documentation on what those abstractions do. Also lightweight and speed are important.