Common issues

Long kinematic chains

Symptom

Running RobCoGen raises some exception related to Maxima (e.g. Maxima timeout exception).

The problem

The RobCoGen approach is not well suited for transforms/Jacobians over long kinematic chains, that is, matrices which are the result of a long series of compositions. This is because in such cases the analytical expressions of the matrix coefficients tend to be very long. Very long expressions can cause runtime errors in Maxima.

More background

RobCoGen approach

Internally, RobCoGen uses a symbolic engine (Maxima) that determines the analytical expression of each non-constant coefficient of a matrix, and then generates code that evaluates numerically the same expression.

This approach works really well for matrices that correspond to short kinematic chains. For example, the coordinate transforms from parent to child links are the composition of only two primitive transforms: the one between the joint frame and the predecessor frame (which is a constant), and the one between the joint frame and the successor frame (which depends on the joint status variable). RobCoGen determines the (short) analytical expressions of the few non-constant coefficients of the resulting matrix, and computes those instead of doing matrix multiplication.

Efficiency

If the analytical expressions are very long, e.g. when the matrix is the result of a long series of products, Maxima can have some problems. And even if Maxima could cope with arbitrary long expressions, the point is that the code implementing such long expressions might very well be less efficient than performing regular matrix multiplication.

I do not know where the boundary is: what is the number of matrix multiplications beyond which analytical expressions are less efficient than doing simple matrix multiplication. At the moment Maxima deals without problems with expressions that are already long enough, I think, to be considered inefficient.

Solution

1

Sometimes the Maxima error goes away by increasing the linel configuration option in core.cfg. However, the generated code might not be that efficient (see above the "More background" section).

2

A better solution is to combine the RobCoGen-erated matrices with regular matrix multiplication in your user code.

For example, instead of generating the transform base_X_endeffector, generate base_X_somelink and somelink_X_endeffector, and then in your user code write:

base_X_endeffector = base_X_somelink * somelink_X_endeffector

In other words, use the right tool for the given problem: efficient robocgen code for transforms over short chains, and regular composition for long chains.

Unfortunately, there is no similar solution for the case of Jacobians. If you need the Jacobian over a long kinematic chain, it is best to compute it by hand with the canonical algorithm.

For C++, the iit-rbd repository offers a couple of functions to help. The following snippet demonstrates their usage in the case of the Fancy robot (the robot whose model and code is included in the distribution of robcogen).

#include <iit/rbd/utils.h>
#include <iit/robcogen/utils.h>
#include <iit/robcogen/jacs.h>

#include <iit/robots/fancy/transforms.h>
#include <iit/robots/fancy/jacobians.h>
#include <iit/robots/fancy/traits.h>

using namespace iit;
using namespace iit::Fancy;

int main(int argc, char** argv)
{
    JointState q;
    robcogen::utils::rand_jstate<Traits>(q);

    HomogeneousTransforms xh;
    Jacobians jacs;

    typename Jacobians::Type_fr_base0_J_ee::MatrixType JbyHand;
    // This is the Jacobian generated by RobCoGen
    jacs.fr_base0_J_ee(q);

    // Now we compute the same Jacobian by hand, via an interative procedure.
    // The homogeneous coordinate transforms (i.e. forward kinematics) are
    // required, but we only need the transforms in the form 'parent_X_child',
    // as we compose them by hand, via multiplication.
    HomogeneousTransforms::MatrixType H = xh.fr_base0_X_fr_link1(q);
    rbd::Vector3d p0 = rbd::Utils::positionVector( H );
    rbd::Vector3d z0 = rbd::Utils::zAxis( H );

    H = H * xh.fr_link1_X_fr_link2(q);         // H is now 'base0_X_link2'
    rbd::Vector3d z1 = rbd::Utils::zAxis( H );

    H = H * xh.fr_link2_X_fr_link3(q);
    rbd::Vector3d p2 = rbd::Utils::positionVector( H );
    rbd::Vector3d z2 = rbd::Utils::zAxis( H );

    H = H * xh.fr_link3_X_fr_link4(q);
    rbd::Vector3d z3 = rbd::Utils::zAxis( H );

    H = H * xh.fr_link4_X_fr_link5(q);
    rbd::Vector3d p4 = rbd::Utils::positionVector( H );
    rbd::Vector3d z4 = rbd::Utils::zAxis( H );

    // Use the next two lines if you have generated the transform fr_link5_X_ee
    // H = H * xh.fr_link5_X_ee;
    // rbd::Vector3d p_ee = rbd::Utils::positionVector( H );
    // Otherwise use 'fr_base0_X_ee':
    rbd::Vector3d p_ee = rbd::Utils::positionVector( xh.fr_base0_X_ee(q) );

    // After gathering all the required position vectors and joint axes,
    // we can compute the Jacobian columns, one by one


    iit::robcogen::geometricJacobianColumn_revolute(
        p_ee, p0, z0, JbyHand.col(0)
    );

    iit::robcogen::geometricJacobianColumn_prismatic(
        z1, JbyHand.col(1)
    );

    iit::robcogen::geometricJacobianColumn_revolute(
        p_ee, p2, z2, JbyHand.col(2)
    );

    iit::robcogen::geometricJacobianColumn_prismatic(
        z3, JbyHand.col(3)
    );

    iit::robcogen::geometricJacobianColumn_revolute(
        p_ee, p4, z4, JbyHand.col(4)
    );

    std::cout << "RobCoGen Jacobian:" << std::endl;
    std::cout << jacs.fr_base0_J_ee << std::endl;
    std::cout << std::endl << "Jacobian constructed by hand:" << std::endl;
    std::cout << JbyHand << std::endl;
    std::cout << std::endl << "Difference between the two:" << std::endl;
    std::cout << jacs.fr_base0_J_ee - JbyHand << std::endl;

    return 0;
}