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