int ChainIkSolverVel_wdls::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
{
jnt2jac.JntToJac(q_in,jac);
double sum;
unsigned int i,j;
/*
for (i=0;i<jac.rows();i++) {
for (j=0;j<jac.columns();j++)
tmp_jac(i,j) = jac(i,j);
}
*/
// Create the Weighted jacobian
tmp_jac_weight1 = jac.data.lazyProduct(weight_js);
tmp_jac_weight2 = weight_ts.lazyProduct(tmp_jac_weight1);
// Compute the SVD of the weighted jacobian
int ret = svd_eigen_HH(tmp_jac_weight2,U,S,V,tmp,maxiter);
//Pre-multiply U and V by the task space and joint space weighting matrix respectively
tmp_ts = weight_ts.lazyProduct(U.topLeftCorner(6,6));
tmp_js = weight_js.lazyProduct(V);
// tmp = (Si*U'*Ly*y),
for (i=0;i<jac.columns();i++) {
sum = 0.0;
for (j=0;j<jac.rows();j++) {
if(i<6)
sum+= tmp_ts(j,i)*v_in(j);
else
sum+=0.0;
}
if(S(i)==0||S(i)<eps)
tmp(i) = sum*((S(i)/(S(i)*S(i)+lambda*lambda)));
else
tmp(i) = sum/S(i);
}
/*
// x = Lx^-1*V*tmp + x
for (i=0;i<jac.columns();i++) {
sum = 0.0;
for (j=0;j<jac.columns();j++) {
sum+=tmp_js(i,j)*tmp(j);
}
qdot_out(i)=sum;
}
*/
qdot_out.data=tmp_js.lazyProduct(tmp);
return ret;
}
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_wdls::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())
return (error = E_SIZE_MISMATCH);
error = jnt2jac.JntToJac(q_in,jac);
if ( error < E_NOERROR) return error;
double sum;
unsigned int i,j;
// Initialize (internal) return values
nrZeroSigmas = 0 ;
sigmaMin = 0.;
lambda_scaled = 0.;
/*
for (i=0;i<jac.rows();i++) {
for (j=0;j<jac.columns();j++)
tmp_jac(i,j) = jac(i,j);
}
*/
// Create the Weighted jacobian
tmp_jac_weight1 = jac.data.lazyProduct(weight_js);
tmp_jac_weight2 = weight_ts.lazyProduct(tmp_jac_weight1);
// Compute the SVD of the weighted jacobian
svdResult = svd_eigen_HH(tmp_jac_weight2,U,S,V,tmp,maxiter);
if (0 != svdResult)
{
qdot_out.data.setZero() ;
return (error = E_SVD_FAILED);
}
//Pre-multiply U and V by the task space and joint space weighting matrix respectively
tmp_ts = weight_ts.lazyProduct(U.topLeftCorner(6,6));
tmp_js = weight_js.lazyProduct(V);
// Minimum of six largest singular values of J is S(5) if number of joints >=6 and 0 for <6
if ( jac.columns() >= 6 ) {
sigmaMin = S(5);
}
else {
sigmaMin = 0.;
}
// tmp = (Si*U'*Ly*y),
for (i=0;i<jac.columns();i++) {
sum = 0.0;
for (j=0;j<jac.rows();j++) {
if(i<6)
sum+= tmp_ts(j,i)*v_in(j);
else
sum+=0.0;
}
// If sigmaMin > eps, then wdls is not active and lambda_scaled = 0 (default value)
// If sigmaMin < eps, then wdls is active and lambda_scaled is scaled from 0 to lambda
// Note: singular values are always positive so sigmaMin >=0
if ( sigmaMin < eps )
{
lambda_scaled = sqrt(1.0-(sigmaMin/eps)*(sigmaMin/eps))*lambda ;
}
if(fabs(S(i))<eps) {
if (i<6) {
// Scale lambda to size of singular value sigmaMin
tmp(i) = sum*((S(i)/(S(i)*S(i)+lambda_scaled*lambda_scaled)));
}
else {
tmp(i)=0.0; // S(i)=0 for i>=6 due to cols>rows
}
// Count number of singular values near zero
++nrZeroSigmas ;
}
else {
tmp(i) = sum/S(i);
}
}
/*
// x = Lx^-1*V*tmp + x
for (i=0;i<jac.columns();i++) {
sum = 0.0;
for (j=0;j<jac.columns();j++) {
sum+=tmp_js(i,j)*tmp(j);
}
qdot_out(i)=sum;
}
*/
qdot_out.data=tmp_js.lazyProduct(tmp);
// If number of near zero singular values is greater than the full rank
// of jac, then wdls is active
if ( nrZeroSigmas > (jac.columns()-jac.rows()) ) {
return (error = E_CONVERGE_PINV_SINGULAR); // converged but pinv singular
} else {
return (error = E_NOERROR); // have converged
}
}
