Rigid and multi rigid bodies

Rigid bodies

Rigid bodies (of type RigidBodyd and RigidBodyf) have three linear and three angular degrees of freedom (i.e., the bodies are defined in 3D). A rigid body object stores its position, orientation, velocity, acceleration, inertia, and forces acting upon it. Bodies can be disabled, meaning that they are considered to be fixed, giving them zero velocity and (effectively) infinite inertia.

Each rigid body has a number of poses associated with it than be queried, or occasionally set:

• pose: this pose is defined with respect to an arbitrary point on the rigid body
• inertial pose: this point is defined at the center of mass on the rigid body. The inertial pose could be defined such that the inertia matrix becomes a diagonal matrix, but Ravelin does not currently attempt this (small) optimization.
• gc pose / mixed pose: this pose is collocated in absolute space with the primary pose but is aligned with the global frame (i.e., identity orientation). This is the pose used for generalized quantities (e.g., generalized inertia).

Header files

#include <Ravelin/RigidBodyd.h>
#include <Ravelin/RigidBodyf.h>

Articulated bodies

Articulated bodies (also known as multi-rigid bodies) are comprised of joints (derived from type Jointd or Jointf) and links (multiple pointers to RigidBodyf' and RigidBodydobjects). Currently, all articulated bodies are of typeRCArticulatedBodydorRCArticulatedBodyf`, meaning that the bodies are defined in reduced (aka minimal/independent) coordinates. Ravelin will eventually support articulated bodies defined purely in absolute coordinates (only for convenience).

Implicit and explicit joint constraints

Constraints for explicit joints never need to be considered directly; they are explicitly accounted for in the coordinates of an articulated body defined using independent coordinates. Implicit joint constraints are defined using equations that take the form C(q)=0, where q is the independent coordinates of a rigid or multi-rigid body.

When implicit constraints are used, the dynamics of multi-rigid bodies can no longer be modeled using ordinary differential equations: differential algebraic equations (DAEs) are necessary. The DAE for such a multi-rigid body system looks like:

M*qdd = f + J'*F
C(q) = 0

where M is the generalized inertia matrix, qdd is the generalized acceleration, f are the generalized external forces, J is the Jacobian of C, ' is the transposition operator, and F is the vector of constraint forces.

Joint types

• Fixed (FixedJointd and FixedJointf): a joint that "welds" two rigid bodies together (highly efficient for articulated bodies defined in independent coordinates, highly inefficient when the joint generates implicit constraints)
• Revolute (RevoluteJointd and RevoluteJointf): a joint that admits a single rotational degree of freedom
• Spherical (SphericalJointd and SphericalJointf): a true spherical joint, meaning that the joint can suffer from gimbal lock, where two axes become aligned and a rotational degree of freedom is lost
• Universal (UniversalJointd and UniversalJointf): a joint that admits two degrees of freedom along two orthogonal rotational axes
• Prismatic (PrismaticJointd and PrismaticJointf): a joint that admits one translational degree of freedom along an axis

Dynamics algorithms

For articulated bodies defined in independent coordinates (RCArticulatedBodyd and RCArticulatedBodyf), two dynamics algorithms are available (described below). Each algorithm will be faster in certain circumstances. The algorithm can be selected by changing the algorithm_type field of the articulated body.

• eFeatherstone: Featherstone's Articulated Body Algorithm. This is an algorithm that runs in linear time in the number of degrees of freedom of the articulated body.
• eCRB: the Composite Rigid Body Algorithm. This is a cubic time algorithm (in the number of degrees of freedom of the articulated body), which is nevertheless often faster than the linear time Featherstone algorithm

The code contains an implementation of Featherstone's branch induced sparsity Cholesky factorization, but it is disabled by default. Our experience indicates that LAPACK's SIMD computations result in faster Cholesky factorizations, even for highly branched bodies.

Dynamics computation frames

Like the dynamics algorithms, dynamics can be computed in different frames. Particular frame choices will result in faster computations for certain bodies. Computation frame can be selected by calling the set_computation_frame(.) function for the body (rigid or articulated). The computation frame choices follow:

• eGlobal: all calculations are computed with respect to a frame at the origin. This selection readily admits numerical errors as bodies move away from the origin.
• eLink: all calculations are computed with respect to the rigid body's/each link's reference pose
• eLinkCOM: all calculations are computed with respect to a frame centered at the rigid body's/each link's center of mass but oriented with the world (i.e., the "mixed" or "generalized coordinate" pose)
• eLinkInertia: all calculations are computed with respect to the rigid body's/each link's inertial frame
• eJoint: calculations are computed in a frame attached to each joint (dynamics are computed with respect to the link pose for single rigid bodies and for the base link of multi-rigid bodies)

Dynamic bodies

Every rigid body and every articulated body is derived from type DynamicBodyd or DynamicBodyf. All generalized quantity functions are defined for every dynamic body, meaning that dynamics can be computed for any body using matrix/vector arithmetic. For example:

void calculate_dynamics(boost::shared_ptr<Ravelin::DynamicBodyd> body)
{
  // get the force and the inertia
  Ravelin::VectorNd f;
  Ravelin::MatrixNd M;
  body->get_generalized_forces(f);
  body->get_generalized_inertia(M);
  
  // the inertia matrix is symmetric, positive definite, so we apply a Cholesky factorization
  LinAlgd::factor_chol(M);

  // solve for the generalized accelerations
  a = f;
  LinAlgd::solve_chol_fast(M, a);
}

"Super" bodies

It is often convenient to get the "super" body of a DynamicBody. The super body for a rigid body is just the rigid body. The super body for a rigid body that is a link of a multi-rigid body will return a pointer to the RCArticulatedBody. See the following example:

boost::shared_ptr<Ravelin::RCArticulatedBodyd> rcab = boost::dynamic_pointer_cast<Ravelin::RCArticulatedBodyd>(body->get_super_body());

Header files

#include <Ravelin/RCArticulatedBodyd.h>
#include <Ravelin/RCArticulatedBodyf.h>

Forward kinematics

Ravelin computes forward kinematics for a multi-rigid-body in a lazy fashion: poses are connected using relative references between links and joints. The connection network, shown below, appears complex but most users should never have to access this network directly: this diagram is only shown in case the programmer wishes to do something extraordinary (like change the kinematic structure after the multi-rigid-body has been setup).

The network is built automatically using the Joint::set_location(const VECTOR3 location&, boost::shared_ptr<RIGIDBODY> inboard, boost::shared_ptr<RIGIDBODY> outboard) function.

Ravelin throws std::exception if the programmer attempts to change this network (by, for example, manually changing the joint location via Joint::set_location()). If the programmer does indeed wish to change the kinematic structure, that exception should be caught.