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 );
}
}
}
