int ChainIkSolverVel_pinv_givens::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
{
toggle!=toggle;
jnt2jac.JntToJac(q_in,jac);
for(unsigned int i=0;i<6;i++)
v_in_eigen(i)=v_in(i);
for(unsigned int i=0;i<m;i++){
for(unsigned int j=0;j<n;j++)
if(transpose)
jac_eigen(i,j)=jac(j,i);
else
jac_eigen(i,j)=jac(i,j);
}
int ret = svd_eigen_Macie(jac_eigen,U,S,V,B,tempi,1e-15,toggle);
//std::cout<<"# sweeps: "<<ret<<std::endl;
if(transpose)
UY = (V.transpose() * v_in_eigen).lazy();
else
UY = (U.transpose() * v_in_eigen).lazy();
for (unsigned int i = 0; i < n; i++){
double wi = UY(i);
double alpha = S(i);
if (alpha != 0)
alpha = 1.0 / alpha;
else
alpha = 0.0;
SUY(i)= alpha * wi;
}
if(transpose)
qdot_eigen = (U * SUY).lazy();
else
qdot_eigen = (V * SUY).lazy();
for (unsigned int j=0;j<chain.getNrOfJoints();j++)
qdot_out(j)=qdot_eigen(j);
return ret;
}
int augmented_solver::CartToJnt(const JntArray& q_in, Twist& v_in, JntArray& qdot_out, JntArray& qdot_base_out)
{
double damping_factor = 0.01;
//Let the ChainJntToJacSolver calculate the jacobian "jac" for
//the current joint positions "q_in"
jnt2jac.JntToJac(q_in,jac);
//v_in.vel.z(0.01);
//Create standard platform jacobian
Eigen::Matrix<double,6,3> jac_base;
jac_base.setZero();
if(base_is_actived_)
{
jac_base(0,0) = base_to_arm_factor_;
jac_base(1,1) = base_to_arm_factor_;
jac_base(5,2) = base_to_arm_factor_;
}
//Put full jacobian matrix together
Eigen::Matrix<double, 6, Eigen::Dynamic> jac_full;
jac_full.resize(6,chain.getNrOfJoints() + jac_base.cols());
jac_full << jac.data, jac_base;
int num_dof = chain.getNrOfJoints() + jac_base.cols();
if(DEBUG)
std::cout << "Combined jacobian:\n " << jac_full << "\n";
//Weighting Matrices
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> W_v;
W_v.resize(num_dof,num_dof);
W_v.setZero();
for(int i=0 ; i<num_dof ; i++)
W_v(i,i) = damping_factor;
Eigen::Matrix<double, 6,6> W_e;
W_e.setZero();
for(unsigned int i=0 ; i<6 ; i++)
W_e(i,i) = 1;
if(DEBUG)
std::cout << "Weight matrix defined\n";
//W_e.setIdentity(6,6);
//Definition of additional task
Eigen::Matrix<double, 7, 1> z_in;
z_in.setZero();
z_in(0,0) = 1.57;
z_in(1,0) = 1.57;
z_in(2,0) = 1.57;
z_in(3,0) = 1.57;
z_in(4,0) = 1.57;
z_in(5,0) = 1.57;
z_in(6,0) = 1.57;
Eigen::Matrix<double, 7 , 10> jac_c;
jac_c.setZero();
for(unsigned int i=0 ; i<7 ; i++)
jac_c(i,i) = 1;
Eigen::Matrix<double, 7, 7> W_c;
W_c.setZero();
//Inversion
// qdot_out = (jac_full^T * W_e * jac_full + jac_augmented^T * W_c * jac_augmented + W_v)^-1(jac_full^T * W_e * v_in + jac_augmented^T * W_c * z_in)
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> damped_inversion;
damped_inversion.resize(num_dof,num_dof);
damped_inversion = (jac_full.transpose() * W_e * jac_full) + (jac_c.transpose() * W_c * jac_c) + W_v;
if(DEBUG)
std::cout << "Inversion done\n";
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> q_dot_conf_control;
Eigen::Matrix<double, 6, 1> v_in_eigen;
v_in_eigen.setZero();
v_in_eigen(0,0) = v_in.vel.x();
v_in_eigen(1,0) = v_in.vel.y();
v_in_eigen(2,0) = v_in.vel.z();
v_in_eigen(3,0) = v_in.rot.x();
v_in_eigen(4,0) = v_in.rot.y();
v_in_eigen(5,0) = v_in.rot.z();
q_dot_conf_control = damped_inversion.inverse() * ((jac_full.transpose() * W_e * v_in_eigen) + (jac_c.transpose() * W_c * z_in));
if(DEBUG)
std::cout << "Endergebnis: \n" << q_dot_conf_control << "\n";
//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(i) = sum*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;
}
if(DEBUG)
std::cout << "Solution SVD: " << qdot_out(0) << " " << qdot_out(1) << " " << qdot_out(2) << " " << qdot_out(3) << " " << qdot_out(4) << " " << qdot_out(5) << " " << qdot_out(6) << "\n====\n";
//return the return value of the svd decomposition
//New calculation
for(unsigned int i=0;i<7;i++)
{
qdot_out(i)=q_dot_conf_control(i,0);
}
if(base_is_actived_)
{
for(unsigned int i = 7; i<7+3; i++)
qdot_base_out(i-7) = q_dot_conf_control(i,0);
}
if(DEBUG)
std::cout << "Solution ConfControl: " << qdot_out(0) << " " << qdot_out(1) << " " << qdot_out(2) << " " << qdot_out(3) << " " << qdot_out(4) << " " << qdot_out(5) << " " << qdot_out(6) << "\n====\n";
return ret;
}
