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