本文整理汇总了C++中rbd::MultiBody::jointIndexById方法的典型用法代码示例。如果您正苦于以下问题：C++ MultiBody::jointIndexById方法的具体用法？C++ MultiBody::jointIndexById怎么用？C++ MultiBody::jointIndexById使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类rbd::MultiBody的用法示例。
在下文中一共展示了MultiBody::jointIndexById方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1:
MotionSpringConstr::MotionSpringConstr(const rbd::MultiBody& mb,
std::vector<std::vector<double>> lTorqueBounds,
std::vector<std::vector<double>> uTorqueBounds,
const std::vector<SpringJoint>& springs):
MotionConstrCommon(mb),
torqueL_(),
torqueU_(),
springs_()
{
int vars = mb.nrDof() - mb.joint(0).dof();
torqueL_.resize(vars);
torqueU_.resize(vars);
// remove the joint 0
lTorqueBounds[0] = {};
uTorqueBounds[0] = {};
rbd::paramToVector(lTorqueBounds, torqueL_);
rbd::paramToVector(uTorqueBounds, torqueU_);
springs_.reserve(springs.size());
for(const SpringJoint& sj: springs)
{
int index = mb.jointIndexById(sj.jointId);
int posInDof = mb.jointPosInDof(index) - mb.joint(0).dof();
springs_.push_back({index, posInDof, sj.K, sj.C, sj.O});
}
}示例2:
OrientationTrackingTask::OrientationTrackingTask(const rbd::MultiBody& mb,
int bodyId, const Eigen::Vector3d& bodyPoint, const Eigen::Vector3d& bodyAxis,
const std::vector<int>& trackingJointsId,
const Eigen::Vector3d& trackedPoint):
bodyIndex_(mb.bodyIndexById(bodyId)),
bodyPoint_(bodyPoint),
bodyAxis_(bodyAxis),
zeroJacIndex_(),
trackedPoint_(trackedPoint),
jac_(mb, bodyId),
eval_(3),
shortJacMat_(3, jac_.dof()),
jacMat_(3, mb.nrDof()),
jacDotMat_(3, mb.nrDof())
{
std::set<int> trackingJointsIndex;
for(int id: trackingJointsId)
{
trackingJointsIndex.insert(mb.jointIndexById(id));
}
int jacPos = 0;
for(int i: jac_.jointsPath())
{
const rbd::Joint& curJoint = mb.joint(i);
if(trackingJointsIndex.find(i) == std::end(trackingJointsIndex))
{
for(int j = 0; j < curJoint.dof(); ++j)
{
zeroJacIndex_.push_back(jacPos + j);
}
}
jacPos += curJoint.dof();
}
}