Ejemplos de Traversal::getParentLinkFromLinkIndex en C++ (Cpp)

Ejemplo n.º 1

bool FreeFloatingJacobianUsingLinkPos(const Model& model,
                                      const Traversal& traversal,
                                      const JointPosDoubleArray& jointPositions,
                                      const LinkPositions& world_H_links,
                                      const LinkIndex jacobianLinkIndex,
                                      const Transform& jacobFrame_X_world,
                                      const Transform& baseFrame_X_jacobBaseFrame,
                                            MatrixDynSize& jacobian)
{
    // We zero the jacobian
    jacobian.zero();

    // Compute base part
    const Transform & world_H_base = world_H_links(traversal.getBaseLink()->getIndex());
    toEigen(jacobian).block(0,0,6,6) = toEigen((jacobFrame_X_world*world_H_base*baseFrame_X_jacobBaseFrame).asAdjointTransform());

    // Compute joint part
    // We iterate from the link up in the traveral until we reach the base
    LinkIndex visitedLinkIdx = jacobianLinkIndex;

    while (visitedLinkIdx != traversal.getBaseLink()->getIndex())
    {
        LinkIndex parentLinkIdx = traversal.getParentLinkFromLinkIndex(visitedLinkIdx)->getIndex();
        IJointConstPtr joint = traversal.getParentJointFromLinkIndex(visitedLinkIdx);

        size_t dofOffset = joint->getDOFsOffset();
        for(int i=0; i < joint->getNrOfDOFs(); i++)
        {
            toEigen(jacobian).block(0,6+dofOffset+i,6,1) =
                toEigen(jacobFrame_X_world*(world_H_links(visitedLinkIdx)*joint->getMotionSubspaceVector(i,visitedLinkIdx,parentLinkIdx)));
        }

        visitedLinkIdx = parentLinkIdx;
    }

    return true;
}