int TreeIdSolver_RNE::CartToJnt(const KDL::JntArray &q, const KDL::JntArray &q_dot, const KDL::JntArray &q_dotdot, const Twist& base_velocity, const Twist& base_acceleration, const Wrenches& f_ext,JntArray &torques, Wrench& base_force)
{
if( q.rows() != undirected_tree.getNrOfDOFs() ||
q_dot.rows() != q.rows() ||
q_dotdot.rows() != q.rows() ||
f_ext.size() != undirected_tree.getNrOfLinks() ||
torques.rows() != q.rows() ) return -1;
base_force = Wrench::Zero();
rneaKinematicLoop(undirected_tree,q,q_dot,q_dotdot,traversal,base_velocity,base_acceleration,v,a);
rneaDynamicLoop(undirected_tree,q,traversal,v,a,f_ext,f,torques,base_force);
return 0;
}
int ChainIdSolver_RNE_FB::CartToJnt(const KDL::JntArray &q, const KDL::JntArray &q_dot, const KDL::JntArray &q_dotdot, const Twist& base_velocity, const Twist& base_acceleration, const Wrenches& f_ext, KDL::JntArray &torques, Wrench& base_force)
{
//Check sizes when in debug mode
if(q.rows()!=nj || q_dot.rows()!=nj || q_dotdot.rows()!=nj || torques.rows()!=nj || f_ext.size()!=ns)
return -1;
unsigned int j=0;
//Sweep from root to leaf
for(unsigned int i=0;i<ns;i++){
double q_,qdot_,qdotdot_;
Segment segm;
segm = chain.getSegment(i);
if(segm.getJoint().getType()!=Joint::None){
q_=q(j);
qdot_=q_dot(j);
qdotdot_=q_dotdot(j);
j++;
}else
q_=qdot_=qdotdot_=0.0;
//Calculate segment properties: X,S,vj,cj
X[i]=segm.pose(q_);//Remark this is the inverse of the
//frame for transformations from
//the parent to the current coord frame
//Transform velocity and unit velocity to segment frame
Twist vj=X[i].M.Inverse(segm.twist(q_,qdot_));
S[i]=X[i].M.Inverse(segm.twist(q_,1.0));
//We can take cj=0, see remark section 3.5, page 55 since the unit velocity vector S of our joints is always time constant
//calculate velocity and acceleration of the segment (in segment coordinates)
if(i==0){
v[i]=X[i].Inverse(base_velocity)+vj;
a[i]=X[i].Inverse(base_acceleration)+S[i]*qdotdot_+v[i]*vj;
}else{
v[i]=X[i].Inverse(v[i-1])+vj;
a[i]=X[i].Inverse(a[i-1])+S[i]*qdotdot_+v[i]*vj;
}
//Calculate the force for the joint
//Collect RigidBodyInertia and external forces
RigidBodyInertia Ii=segm.getInertia();
f[i]=Ii*a[i]+v[i]*(Ii*v[i])-f_ext[i];
//std::cout << "aLink " << segm.getName() << "\na= " << a[i] << "\nv= " << v[i] << "\nf= " << f[i] << "\nf_ext= " << f_ext[i] << std::endl;
//std::cout << "a["<<i<<"]=" << a[i] << "\n f["<<i<<"]=" << f[i] << "\n S["<<i<<"]=" << S[i] << std::endl;
}
//Sweep from leaf to root
j=nj-1;
for(int i=ns-1;i>=0;i--){
Segment segm;
segm = chain.getSegment(i);
if(segm.getJoint().getType()!=Joint::None)
torques(j--)=dot(S[i],f[i]);
if(i!=0)
f[i-1]=f[i-1]+X[i]*f[i];
}
base_force = X[0]*f[0];
//debug
for(int i=0; i < (int)ns; i++) {
Segment segm;
segm = chain.getSegment(i);
//std::cout << "bLink " << segm.getName() << " a= " << a[i] << " f= " << f[i] << std::endl;
}
return 0;
}
