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 );
}
}
}
void
tau::Robot
::solveKinematics( int debugLevel )
{
for( size_t i = 1; i < bodies.size(); ++i )
{
if( debugLevel > 0 ) errorLog() << "------\n-----Kinematics for Body " << i << " -------\n";
Body* b = bodies[i];
// since b is not the root, it has both a joint and a parent
assert( b->parent() );
assert( b->joint() );
Joint* j = b->joint(); // joint connecting i to the parent body, \lambda(i)
assert( j );
if( debugLevel > 0 ) errorLog() << "joint lambda(i), treeXform:\n" << j->treeXForm() << "\n";
if( debugLevel > 0 ) errorLog() << "joint lambda(i), xform(q="
<< j->q()
<< "):\n" << j->xform() << "\n";
XForm body_from_parent = j->xform() * j->treeXForm();
// if( debugLevel > 0 ) errorLog() << "xform, body_from_parent:\n" << body_from_parent << "\n";
// Recursively compute the base-to-body transform
if( b->parent()->isBase() )
{
if( debugLevel > 0 ) errorLog() << "body parent is base\n";
if( debugLevel > 0 ) errorLog() << "\txform, body_from_parent:\n" << body_from_parent << "\n----\n";
b->setXForm_body_from_base( body_from_parent );
}
else
{
if( debugLevel > 0 ) errorLog() << "body parent is NOT base\n";
XForm parent_from_base = b->parent()->xForm_body_from_base();
XForm body_i_from_base = body_from_parent * parent_from_base;
// XForm body_i_from_base = parent_from_base * body_from_parent;
b->setXForm_body_from_base( body_i_from_base );
if( debugLevel > 0 ) errorLog() << "\txform, parent_from_base:\n" << parent_from_base << "\n----\n";
if( debugLevel > 0 ) errorLog() << "\txform, body_from_parent:\n" << body_from_parent << "\n----\n";
if( debugLevel > 0 ) errorLog() << "\txform, body_i from base:\n" << body_i_from_base << "\n----\n";
}
// if( debugLevel > 0 ) errorLog() << "{}^{F_i}X_{F_0} = \n"
// << b->xForm_body_from_base() << "\n\n";
// SMV z;
// z[5] = 1.0;
// if( debugLevel > 0 ) errorLog() << " applying body_from_base to unit-z vector (translation) : "
// << ( b->xForm_body_from_base().apply( z ) ).second()
// << "\n";
// depends on q, dq and ddq being set already
if( debugLevel > 1 ) errorLog() << "q=" << j->q()
<< ", dq=" << j->dq()
<< ", ddq=" << j->ddq()
<< "\n";
SMV v_J = j->v_J();
if( debugLevel > 1 ) errorLog() << "v_J=" << v_J << "\n";
SMV c_J = j->c_J();
if( debugLevel > 1 ) errorLog() << "c_J=" << c_J << "\n";
MatrixNxN S = j->motionSubspace();
if( debugLevel > 1 ) errorLog() << "S=" << S << "\n";
VectorN ddq = j->ddq();
SMV v_i = body_from_parent.apply( b->parent()->v() ) + v_J;
if( debugLevel > 1 ) errorLog() << "v_i=" << v_i << "\n";
SMV a_i = body_from_parent.apply( b->parent()->a() )
+ SMV(S * ddq)
+ c_J
+ SMV(v_i.skewSym() * v_J.asVector6())
;
if( debugLevel > 1 ) errorLog() << "a_i=" << a_i << "\n";
SFV ext_f = b->externalForce();
if( debugLevel > 2 ) errorLog() << "v_i=\n" << v_i << "\n";
Matrix6x6 vSkewSymAsForce = SFV(v_i.asVector6()).skewSym();
if( debugLevel > 2 ) errorLog() << "vSkewSymAsForce=\n" << vSkewSymAsForce << "\n";
const SpatialInertia& inertia = b->inertia();
if( debugLevel > 3 ) errorLog() << "inertia=\n" << inertia << "\n"
<< "spatial matrix: \n" << inertia.inertiaMatrix() << "\n";
SFV f_i = SFV( (inertia * a_i).asVector6()
+ ( vSkewSymAsForce * ( inertia * v_i ).asVector6() )
- (b->xForm_body_from_base().apply( ext_f )).asVector6() );
if( debugLevel > 1 ) errorLog() << "f_i=" << f_i << "\n";
// here we have v_i, a_i and f_i representing the full description of body i,
// store it in the body in preparation for the next loop (in solveInverseDynamics),
// where these are used to compute the torques.
b->setSpatialVelocity( v_i );
b->setSpatialAcceleration( a_i );
b->setSpatialForce( f_i );
}
}
