本文整理汇总了C++中DyLink::isFreeJoint方法的典型用法代码示例。如果您正苦于以下问题：C++ DyLink::isFreeJoint方法的具体用法？C++ DyLink::isFreeJoint怎么用？C++ DyLink::isFreeJoint使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类DyLink的用法示例。
在下文中一共展示了DyLink::isFreeJoint方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: a
void ForwardDynamicsABM::calcABMPhase3()
{
const LinkTraverse& traverse = body->linkTraverse();
DyLink* root = static_cast<DyLink*>(traverse[0]);
if(root->isFreeJoint()){
// - | Ivv trans(Iwv) | * | dvo | = | pf |
// | Iwv Iww | | dw | | ptau |
Eigen::Matrix<double, 6, 6> M;
M << root->Ivv(), root->Iwv().transpose(),
root->Iwv(), root->Iww();
Eigen::Matrix<double, 6, 1> f;
f << root->pf(),
root->ptau();
f *= -1.0;
Eigen::Matrix<double, 6, 1> a(M.colPivHouseholderQr().solve(f));
root->dvo() = a.head<3>();
root->dw() = a.tail<3>();
} else {
root->dvo().setZero();
root->dw().setZero();
}
const int n = traverse.numLinks();
for(int i=1; i < n; ++i){
DyLink* link = static_cast<DyLink*>(traverse[i]);
const DyLink* parent = link->parent();
if(!link->isFixedJoint()){
link->ddq() = (link->uu() - (link->hhv().dot(parent->dvo()) + link->hhw().dot(parent->dw()))) / link->dd();
link->dvo().noalias() = parent->dvo() + link->cv() + link->sv() * link->ddq();
link->dw().noalias() = parent->dw() + link->cw() + link->sw() * link->ddq();
}else{
link->ddq() = 0.0;
link->dvo() = parent->dvo();
link->dw() = parent->dw();
}
}
}