Spatial quantities and spatial arithmetic
"Spatial quantities" includes all necessary quantities used in 3D rigid body kinematics and dynamics computations and includes spatial velocity, acceleration, forces, momentum, and inertia. Velocity, acceleration, forces, and momentum are vectors (six-dimensional vectors) while inertia is densely represented as a 6x6 matrix (with a sparse representation depending on the inertia type).
Each spatial quantity must be defined with respect to a particular frame (even if that frame is the "global" or absolute frame, which is denoted by a null shared_ptr or the GLOBAL constant): each of the data structures discussed below has a field, pose that determines the pose that the quantity is defined within.
#include <Ravelin/SAcceld.h>
#include <Ravelin/SAccelf.h>
#include <Ravelin/SVelocityd.h>
#include <Ravelin/SVelocityf.h>
#include <Ravelin/SForced.h>
#include <Ravelin/SForcef.h>
#include <Ravelin/SMomentumd.h>
#include <Ravelin/SMomentumf.h>
#include <Ravelin/SpatialRBInertiad.h>
#include <Ravelin/SpatialRBInertiaf.h>
#include <Ravelin/SpatialABInertiad.h>
#include <Ravelin/SpatialABInertiaf.h>
Spatial velocities and accelerations are defined using the convention in Featherstone's 1987 book (Robot Dynamics Algorithms), with the top three components of the vector representing the angular velocity/acceleration and the bottom three components representing the linear velocity/acceleration.
Example: working with rigid body spatial velocities
// create the pose object
shared_ptr<Pose3d> P(new Pose3d);
// we'll have this pose represent the position and orientation of the rigid body's inertial frame
// ... (init the position and orientation here)
// setup the velocity
// note that the linear and angular velocity vectors must be defined in the proper frame as well
SVelocityd v(P);
v.set_linear(Vector3d(2,2,2,P)); // linear velocity of [ 2 2 2 ]
v.set_angular(Vector3d(0,1,0,P)); // angular velocity of [ 0 1 0 ]
// do some other things...
// get the linear velocity out (note that the conversion discards the frame information)
double lx = P[3];
double ly = P[4];
double lz = P[5];
// set the angular velocity (dangerous: no frame checking!)
P[0] = ax;
P[1] = ay;
P[2] = az;
// convert the velocity to the global frame
SVelocityd v0 = Pose3d::transform(GLOBAL, v);
Spatial forces and momenta are defined using the convention in Featherstone's 1987 book (Robot Dynamics Algorithms), with the top three components of the vector representing the force/linear momentum components and the bottom three components representing the torque/angular momentum components.
The SpatialRBInertiaf and SpatialRBInertiad classes represents the core properties of rigid body dynamics inertia- mass, center-of-mass, and inertia matrix using a relatively sparse representation (only 10 floating point values are necessary because the inertia matrix is symmetric; Ravelin stores the full matrix and thus consumes 13 floating point values). The key values are: m, the mass of the rigid body; J, the inertia matrix; and h the vector from the origin of the reference frame to the center of mass. These values are used to represent the following 6x6 matrix:
| -hx*m I*m |
| J - hx*hx*m hx*m |
where hx is the skew symmetric matrix determined by the tensor form of h and I is the 3x3 identity matrix.
Example: working with rigid body spatial forces and inertias
// name the global frame in 3D
const shared_ptr<const Pose3d> GLOBAL_3D;
// create the pose object
shared_ptr<Pose3d> P(new Pose3d);
// we'll have this pose represent the position and orientation of the rigid body's inertial frame
// ... (init the position and orientation here)
// setup the inertia in the inertial frame, which we assume is defined at the center-of-mass of the body
SpatialRBInertiad J(P);
J.m = 1.0; // mass of 1.0
J.h.set_zero(); // inertia defined relative to center-of-mass
J.J.set_identity(); // the rotational inertia
J.pose = P;
// setup the force
// note that the force and torque vectors must be defined in the proper frame as well
SForced f(P);
f.set_force(Vector3d(1,1,1,P)); // force of [ 1 1 1 ]
f.set_torque(Vector3d(0,0,0,P)); // no torque
// get the torque out (note that the conversion discards the frame information)
double tx = P[3];
double ty = P[4];
double tz = P[5];
// compute the acceleration in the global frame
SAcceld a0 = Pose3d::transform(GLOBAL_3D, J.inverse_mult(f));
Spatial "articulated body inertias" come from the O(n) recursive algorithms of Featherstone and others that treat an articulated body as a handle. Such matrices have the form:
| H' M |
| J H |
where H, J, and M are 3x3 matrices. The spatial articulated body inertia class does not construct or use the 6x6 matrix (unless specifically asked to by the user). All arithmetic and inversion is performed using the sparse representations of the matrix.