本文整理汇总了C++中Joint::setComputedTorque方法的典型用法代码示例。如果您正苦于以下问题：C++ Joint::setComputedTorque方法的具体用法？C++ Joint::setComputedTorque怎么用？C++ Joint::setComputedTorque使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Joint的用法示例。
在下文中一共展示了Joint::setComputedTorque方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: solveKinematics
void
tau::Robot
::solveInverseDynamics( int debugLevel )
{
solveKinematics( debugLevel );
for( int i = bodies.size() - 1; i > 0; --i )
{
// compute:
// \tau_i = S_i^T f_i
if( debugLevel > 0 ) errorLog() << "------\n-----InvDyn for Body " << i << " -------\n";
Body* b = bodies[i];
Joint* j = b->joint();
MatrixNxN S_i = j->motionSubspace();
if( debugLevel > 0 ) errorLog() << "\tmotion subspace: \n" << S_i << "\n";
SFV f_i = b->spatialForce();
if( debugLevel > 0 ) errorLog() << "\tbody's spatialForce: \n" << f_i << "\n";
VectorN tau_i = S_i.transpose() * f_i.asVector6(); // vector size is number of DOFs
if( debugLevel > 0 ) errorLog() << "\ttau_i (size should be # of dofs): \n" << tau_i << "\n";
j->setComputedTorque( tau_i );
if( ! b->parent()->isBase() )
{
// update the force on the parent
//
// f_{\lambda(i)} = f_{\lambda(i)} + {}^{\lambda(i)}X_i^* f_i
//
XForm body_from_parent = j->xform() * j->treeXForm();
XForm parent_from_body = body_from_parent.inverse();
SFV f_i_inParentFrame = parent_from_body.apply( f_i ); // could use invapply for eff.
// SFV f_p = b->parent()->spatialForce();
if( debugLevel > 0 ) errorLog() << "\tf_i_inParentFrame:\n" << f_i_inParentFrame << "\n";
// SFV f_p_new = SFV( f_p.asVector6() + body_from_parent.invApply( f_i ).asVector6() );
SFV f_p_new = b->parent()->spatialForce() + body_from_parent.invApply( f_i );
b->parent()->setSpatialForce( f_p_new );
}
}
}