int ChainIkSolverVel_pinv_nso::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
{
if (nj != chain.getNrOfJoints())
return (error = E_NOT_UP_TO_DATE);
if (nj != q_in.rows() || nj != qdot_out.rows() || nj != opt_pos.rows() || nj != weights.rows())
return (error = E_SIZE_MISMATCH);
//Let the ChainJntToJacSolver calculate the jacobian "jac" for
//the current joint positions "q_in"
error = jnt2jac.JntToJac(q_in,jac);
if (error < E_NOERROR) return error;
//Do a singular value decomposition of "jac" with maximum
//iterations "maxiter", put the results in "U", "S" and "V"
//jac = U*S*Vt
svdResult = svd_eigen_HH(jac.data,U,S,V,tmp,maxiter);
if (0 != svdResult)
{
qdot_out.data.setZero() ;
return error = E_SVD_FAILED;
}
unsigned int i;
// We have to calculate qdot_out = jac_pinv*v_in
// Using the svd decomposition this becomes(jac_pinv=V*S_pinv*Ut):
// qdot_out = V*S_pinv*Ut*v_in
// S^-1
for (i = 0; i < nj; ++i) {
Sinv(i) = fabs(S(i))<eps ? 0.0 : 1.0/S(i);
}
for (i = 0; i < 6; ++i) {
tmp(i) = v_in(i);
}
qdot_out.data = V * Sinv.asDiagonal() * U.transpose() * tmp.head(6);
// Now onto NULL space
// Given the cost function g, and the current joints q, desired joints qd, and weights w:
// t = g(q) = 1/2 * Sum( w_i * (q_i - qd_i)^2 )
//
// The jacobian Jc is:
// t_dot = Jc(q) * q_dot
// Jc = dt/dq = w_j * (q_i - qd_i) [1 x nj vector]
//
// The pseudo inverse (^-1) is
// Jc^-1 = w_j * (q_i - qd_i) / A [nj x 1 vector]
// A = Sum( w_i^2 * (q_i - qd_i)^2 )
//
// We can set the change as the step needed to remove the error times a value alpha:
// t_dot = -2 * alpha * t
//
// When we put it together and project into the nullspace, the final result is
// q'_out += (I_n - J^-1 * J) * Jc^-1 * (-2 * alpha * g(q))
double g = 0; // g(q)
double A = 0; // normalizing term
for (i = 0; i < nj; ++i) {
double qd = q_in(i) - opt_pos(i);
g += 0.5 * qd*qd * weights(i);
A += qd*qd * weights(i)*weights(i);
}
if (A > 1e-9) {
// Calculate inverse Jacobian Jc^-1
for (i = 0; i < nj; ++i) {
tmp(i) = weights(i)*(q_in(i) - opt_pos(i)) / A;
}
// Calculate J^-1 * J * Jc^-1 = V*S^-1*U' * U*S*V' * tmp
tmp2 = V * Sinv.asDiagonal() * U.transpose() * U * S.asDiagonal() * V.transpose() * tmp;
for (i = 0; i < nj; ++i) {
//std::cerr << i <<": "<< qdot_out(i) <<", "<< -2*alpha*g * (tmp(i) - tmp2(i)) << std::endl;
qdot_out(i) += -2*alpha*g * (tmp(i) - tmp2(i));
}
}
//return the return value of the svd decomposition
return (error = E_NOERROR);
}
int ChainIkSolverVel_pinv_nso::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
{
//Let the ChainJntToJacSolver calculate the jacobian "jac" for
//the current joint positions "q_in"
jnt2jac.JntToJac(q_in,jac);
//Do a singular value decomposition of "jac" with maximum
//iterations "maxiter", put the results in "U", "S" and "V"
//jac = U*S*Vt
int ret = svd.calculate(jac,U,S,V,maxiter);
double sum;
unsigned int i,j;
// We have to calculate qdot_out = jac_pinv*v_in
// Using the svd decomposition this becomes(jac_pinv=V*S_pinv*Ut):
// qdot_out = V*S_pinv*Ut*v_in
//first we calculate Ut*v_in
for (i=0;i<jac.columns();i++) {
sum = 0.0;
for (j=0;j<jac.rows();j++) {
sum+= U[j](i)*v_in(j);
}
//If the singular value is too small (<eps), don't invert it but
//set the inverted singular value to zero (truncated svd)
tmp(i) = sum*(fabs(S(i))<eps?0.0:1.0/S(i));
}
//tmp is now: tmp=S_pinv*Ut*v_in, we still have to premultiply
//it with V to get qdot_out
for (i=0;i<jac.columns();i++) {
sum = 0.0;
for (j=0;j<jac.columns();j++) {
sum+=V[i](j)*tmp(j);
}
//Put the result in qdot_out
qdot_out(i)=sum;
}
//Now onto NULL space
for(i = 0; i < jac.columns(); i++)
tmp(i) = weights(i)*(opt_pos(i) - q_in(i));
//Vtn*tmp
for (i=jac.rows()+1;i<jac.columns();i++) {
tmp2(i-(jac.rows()+1)) = 0.0;
for (j=0;j<jac.columns();j++) {
tmp2(i-(jac.rows()+1)) +=V[j](i)*tmp(j);
}
}
for (i=0;i<jac.columns();i++) {
sum = 0.0;
for (j=jac.rows()+1;j<jac.columns();j++) {
sum +=V[i](j)*tmp2(j);
}
qdot_out(i) += alpha*sum;
}
//return the return value of the svd decomposition
return ret;
}
