当前位置: 首页>>代码示例>>C++>>正文


C++ Traversal::getParentLinkFromLinkIndex方法代码示例

本文整理汇总了C++中Traversal::getParentLinkFromLinkIndex方法的典型用法代码示例。如果您正苦于以下问题:C++ Traversal::getParentLinkFromLinkIndex方法的具体用法?C++ Traversal::getParentLinkFromLinkIndex怎么用?C++ Traversal::getParentLinkFromLinkIndex使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在Traversal的用法示例。


在下文中一共展示了Traversal::getParentLinkFromLinkIndex方法的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;
}
开发者ID:,项目名称:,代码行数:37,代码来源:


注:本文中的Traversal::getParentLinkFromLinkIndex方法示例由纯净天空整理自Github/MSDocs等开源代码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。