Computing the inertia matrix

The equations of motion of a manipulator can be written as:

\begin{equation*} \bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd = \bftau + \bftau_g(\bfq) \end{equation*}

where, denoting by \(n = \dim(\bfq)\) the degree of freedom of the robot,

  • \(\bfM(\q)\) is the \(n \times n\) inertia matrix,
  • \(\bfC(\q)\) is the \(n \times n \times n\) Coriolis tensor,
  • \(\bftau\) is the \(n\)-dimensional vector of actuated joint torques,
  • \(\bftau_g(\q)\) is the \(n\)-dimensional vector of gravity torques.

We will see in this post one way to compute the inertia matrix \(\bfM(\bfq)\) with OpenRAVE.

Inverse dynamics with OpenRAVE

The matrices and tensors of the formula above can be computed by inverse dynamics. For instance, in OpenRAVE one can use Robot.ComputeInverseDynamics to compute each of the three terms:

\begin{equation*} \begin{array}{ccc} m := \bfM(\bfq) \qdd & c := \qd^\top \bfC(\bfq) \qd & g := -\bftau_g(\bfq). \end{array} \end{equation*}

By setting \(\qdd\) to the vector \(\bfe_i\) of the canonical basis of \(\mathbb{R}^n\), one can then compute the \(i^{\textrm{th}}\) line of the inertia matrix \(\bfM(\q).\) This is known as the unit vector method, which was proposed by Walker and Orin (1982). Here is a sample Python implementation:

def compute_inertia_matrix(robot, q, external_torque=None):
    n = len(q)
    M = zeros((n, n))
    with robot:
        robot.SetDOFValues(q)
        for (i, e_i) in enumerate(eye(n)):
            m, c, g = robot.ComputeInverseDynamics(
                e_i, external_torque, returncomponents=True)
            M[:, i] = m
    return M

Note how there is no need to set the robot's velocity in the function above, as ComputeInverseDynamics computes separately the three terms m, c and g.

To go further

The unit vector method is not considered efficient any more, as it has now been superseded by the composite rigid-body algorithm (CRBA) described in Chapter 6 of Roy Featherstone's Rigid Body Dynamics Algorithms. This algorithm is implemented in most common rigid-body dynamics libraries such as RBDL, RigidBodyDynamics.jl and Pinocchio.

Pages of this website are under the CC-BY 4.0 license.