本文整理汇总了C++中Traversal::getParentJointFromLinkIndex方法的典型用法代码示例。如果您正苦于以下问题：C++ Traversal::getParentJointFromLinkIndex方法的具体用法？C++ Traversal::getParentJointFromLinkIndex怎么用？C++ Traversal::getParentJointFromLinkIndex使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Traversal的用法示例。
在下文中一共展示了Traversal::getParentJointFromLinkIndex方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: FreeFloatingJacobianUsingLinkPos
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;
}