chainiksolver_vel_pinv_mimic.cpp
Go to the documentation of this file.
00001 // Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00002 
00003 // Version: 1.0
00004 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00005 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00006 // URL: http://www.orocos.org/kdl
00007 
00008 // This library is free software; you can redistribute it and/or
00009 // modify it under the terms of the GNU Lesser General Public
00010 // License as published by the Free Software Foundation; either
00011 // version 2.1 of the License, or (at your option) any later version.
00012 
00013 // This library is distributed in the hope that it will be useful,
00014 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00015 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00016 // Lesser General Public License for more details.
00017 
00018 // You should have received a copy of the GNU Lesser General Public
00019 // License along with this library; if not, write to the Free Software
00020 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00021 
00022 // Modified to account for "mimic" joints, i.e. joints whose motion has a
00023 // linear relationship to that of another joint.
00024 // Copyright  (C)  2013  Sachin Chitta, Willow Garage
00025 
00026 #include <moveit/kdl_kinematics_plugin/chainiksolver_vel_pinv_mimic.hpp>
00027 #include <ros/console.h>
00028 
00029 namespace KDL
00030 {
00031 ChainIkSolverVel_pinv_mimic::ChainIkSolverVel_pinv_mimic(const Chain& _chain, int _num_mimic_joints, int _num_redundant_joints, bool _position_ik, double _eps, int _maxiter):
00032   chain(_chain),
00033   jnt2jac(chain),
00034   jac(chain.getNrOfJoints()),
00035   U(6,JntArray(chain.getNrOfJoints()-_num_mimic_joints)),
00036   S(chain.getNrOfJoints()-_num_mimic_joints),
00037   V(chain.getNrOfJoints()-_num_mimic_joints,JntArray(chain.getNrOfJoints()-_num_mimic_joints)),
00038   tmp(chain.getNrOfJoints()-_num_mimic_joints),
00039   jac_reduced(chain.getNrOfJoints()-_num_mimic_joints),
00040   qdot_out_reduced(chain.getNrOfJoints()-_num_mimic_joints),
00041   U_translate(MatrixXd::Zero(3,chain.getNrOfJoints()-_num_mimic_joints)),
00042   S_translate(VectorXd::Zero(chain.getNrOfJoints()-_num_mimic_joints)),
00043   V_translate(MatrixXd::Zero(chain.getNrOfJoints()-_num_mimic_joints,chain.getNrOfJoints()-_num_mimic_joints)),
00044   tmp_translate(VectorXd::Zero(chain.getNrOfJoints()-_num_mimic_joints)),
00045   jac_locked(chain.getNrOfJoints()-_num_redundant_joints-_num_mimic_joints),
00046   qdot_out_reduced_locked(chain.getNrOfJoints()-_num_mimic_joints-_num_redundant_joints),
00047   qdot_out_locked(chain.getNrOfJoints()-_num_redundant_joints),
00048   svd(jac_reduced),
00049   eps(_eps),
00050   maxiter(_maxiter),
00051   num_mimic_joints(_num_mimic_joints),
00052   position_ik(_position_ik),
00053   U_locked(MatrixXd::Zero(6,chain.getNrOfJoints()-_num_mimic_joints-_num_redundant_joints)),
00054   S_locked(VectorXd::Zero(chain.getNrOfJoints()-_num_mimic_joints-_num_redundant_joints)),
00055   V_locked(MatrixXd::Zero(chain.getNrOfJoints()-_num_mimic_joints-_num_redundant_joints,chain.getNrOfJoints()-_num_mimic_joints-_num_redundant_joints)),
00056   tmp_locked(VectorXd::Zero(chain.getNrOfJoints()-_num_mimic_joints-_num_redundant_joints)),
00057   U_translate_locked(MatrixXd::Zero(3,chain.getNrOfJoints()-_num_mimic_joints-_num_redundant_joints)),
00058   S_translate_locked(VectorXd::Zero(chain.getNrOfJoints()-_num_mimic_joints-_num_redundant_joints)),
00059   V_translate_locked(MatrixXd::Zero(chain.getNrOfJoints()-_num_mimic_joints-_num_redundant_joints,chain.getNrOfJoints()-_num_mimic_joints-_num_redundant_joints)),
00060   tmp_translate_locked(VectorXd::Zero(chain.getNrOfJoints()-_num_mimic_joints-_num_redundant_joints)),
00061   num_redundant_joints(_num_redundant_joints),
00062   redundant_joints_locked(false)
00063 {
00064   mimic_joints_.resize(chain.getNrOfJoints());
00065   for(std::size_t i=0; i < mimic_joints_.size(); ++i)
00066     mimic_joints_[i].reset(i);
00067 }
00068 
00069 ChainIkSolverVel_pinv_mimic::~ChainIkSolverVel_pinv_mimic()
00070 {
00071 }
00072 
00073 bool ChainIkSolverVel_pinv_mimic::setMimicJoints(const std::vector<kdl_kinematics_plugin::JointMimic> & mimic_joints)
00074 {
00075   if(mimic_joints.size() != chain.getNrOfJoints())
00076     return false;
00077 
00078   for(std::size_t i=0; i < mimic_joints.size(); ++i)
00079   {
00080     if(mimic_joints[i].map_index >= chain.getNrOfJoints())
00081       return false;
00082   }
00083   mimic_joints_ = mimic_joints;
00084   return true;
00085 }
00086 
00087 bool ChainIkSolverVel_pinv_mimic::setRedundantJointsMapIndex(const std::vector<unsigned int> & redundant_joints_map_index)
00088 {
00089   if(redundant_joints_map_index.size() != chain.getNrOfJoints()-num_mimic_joints-num_redundant_joints)
00090   {
00091     ROS_ERROR("Map index size: %d does not match expected size. No. of joints: %d, num_mimic_joints: %d, num_redundant_joints: %d", 
00092               (int) redundant_joints_map_index.size(),
00093               (int) chain.getNrOfJoints(), 
00094               (int) num_mimic_joints, 
00095               (int) num_redundant_joints);
00096     return false;
00097   }
00098 
00099   for(std::size_t i=0; i < redundant_joints_map_index.size(); ++i)
00100   {
00101     if(redundant_joints_map_index[i] >= chain.getNrOfJoints()-num_mimic_joints)
00102       return false;
00103   }
00104   locked_joints_map_index = redundant_joints_map_index;
00105   return true;
00106 }
00107 
00108 bool ChainIkSolverVel_pinv_mimic::jacToJacReduced(const Jacobian &jac, Jacobian &jac_reduced_l)
00109 {
00110   jac_reduced_l.data.setZero();
00111   for(std::size_t i=0; i < chain.getNrOfJoints(); ++i)
00112   {
00113     Twist vel1 = jac_reduced_l.getColumn(mimic_joints_[i].map_index);
00114     Twist vel2 = jac.getColumn(i);
00115     Twist result = vel1 + (mimic_joints_[i].multiplier*vel2);
00116     jac_reduced_l.setColumn(mimic_joints_[i].map_index,result);
00117   }
00118   return true;
00119 }
00120 
00121 bool ChainIkSolverVel_pinv_mimic::jacToJacLocked(const Jacobian &jac, Jacobian &jac_locked)
00122 {
00123   jac_locked.data.setZero();
00124   for(std::size_t i=0; i < chain.getNrOfJoints()-num_mimic_joints-num_redundant_joints; ++i)
00125   {
00126     jac_locked.setColumn(i, jac.getColumn(locked_joints_map_index[i]));
00127   }
00128   return true;
00129 }
00130 
00131 int ChainIkSolverVel_pinv_mimic::CartToJntRedundant(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
00132 {
00133   qdot_out.data.setZero();
00134   //Let the ChainJntToJacSolver calculate the jacobian "jac" for
00135   //the current joint positions "q_in". This will include the mimic joints
00136   if(num_mimic_joints > 0)
00137   {
00138     jnt2jac.JntToJac(q_in,jac);
00139     //Now compute the actual jacobian that involves only the active DOFs
00140     jacToJacReduced(jac,jac_reduced);
00141   }
00142   else
00143     jnt2jac.JntToJac(q_in,jac_reduced);
00144 
00145   //Now compute the jacobian with redundant joints locked
00146   jacToJacLocked(jac_reduced,jac_locked);
00147 
00148   //Do a singular value decomposition of "jac" with maximum
00149   //iterations "maxiter", put the results in "U", "S" and "V"
00150   //jac = U*S*Vt
00151 
00152   int ret;
00153   if(!position_ik)
00154     ret = svd_eigen_HH(jac_locked.data,U_locked,S_locked,V_locked,tmp_locked,maxiter);
00155   else
00156     ret = svd_eigen_HH(jac_locked.data.topLeftCorner(3,chain.getNrOfJoints()-num_mimic_joints-num_redundant_joints),U_translate_locked,S_translate_locked,V_translate_locked,tmp_translate_locked,maxiter);
00157 
00158   double sum;
00159   unsigned int i,j;
00160 
00161   // We have to calculate qdot_out = jac_pinv*v_in
00162   // Using the svd decomposition this becomes(jac_pinv=V*S_pinv*Ut):
00163   // qdot_out = V*S_pinv*Ut*v_in
00164 
00165   unsigned int columns, rows;
00166   if(!position_ik)
00167     rows = jac_locked.rows();
00168   else
00169     rows = 3;
00170 
00171   //first we calculate Ut*v_in
00172   for (i=0;i<jac_locked.columns();i++) {
00173     sum = 0.0;
00174     for (j=0;j<rows;j++) {
00175       if(!position_ik)
00176         sum+= U_locked(j,i)*v_in(j);
00177       else
00178         sum+= U_translate_locked(j,i)*v_in(j);
00179     }
00180     //If the singular value is too small (<eps), don't invert it but
00181     //set the inverted singular value to zero (truncated svd)
00182     if(!position_ik)
00183       tmp(i) = sum*(fabs(S_locked(i))<eps?0.0:1.0/S_locked(i));
00184     else
00185       tmp(i) = sum*(fabs(S_translate_locked(i))<eps?0.0:1.0/S_translate_locked(i));
00186   }
00187   //tmp is now: tmp=S_pinv*Ut*v_in, we still have to premultiply
00188   //it with V to get qdot_out
00189   for (i=0;i<jac_locked.columns();i++) {
00190     sum = 0.0;
00191     for (j=0;j<jac_locked.columns();j++) {
00192       if(!position_ik)
00193         sum+=V_locked(i,j)*tmp(j);
00194       else
00195         sum+=V_translate_locked(i,j)*tmp(j);
00196     }
00197     //Put the result in qdot_out_reduced if mimic joints exist, otherwise in qdot_out
00198     if(num_mimic_joints > 0)
00199       qdot_out_reduced_locked(i)=sum;
00200     else
00201       qdot_out_locked(i) = sum;
00202   }
00203   ROS_DEBUG_STREAM_NAMED("kdl","Solution:");
00204 
00205   if(num_mimic_joints > 0)
00206   {
00207     for(i=0; i < chain.getNrOfJoints()-num_mimic_joints-num_redundant_joints; ++i)
00208     {
00209       qdot_out_reduced(locked_joints_map_index[i]) = qdot_out_reduced_locked(i);
00210     }
00211     for(i=0; i < chain.getNrOfJoints(); ++i)
00212     {
00213       qdot_out(i) = qdot_out_reduced(mimic_joints_[i].map_index) * mimic_joints_[i].multiplier;
00214     }
00215   }
00216   else
00217   {
00218     for(i=0; i < chain.getNrOfJoints()-num_redundant_joints; ++i)
00219     {
00220       qdot_out(locked_joints_map_index[i]) = qdot_out_locked(i);
00221     }
00222   }
00223   // Reset the flag
00224   // redundant_joints_locked = false;
00225   //return the return value of the svd decomposition
00226   return ret;
00227 }
00228 
00229 int ChainIkSolverVel_pinv_mimic::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
00230 {
00231   if(redundant_joints_locked)
00232     return CartToJntRedundant(q_in, v_in, qdot_out);
00233 
00234   //Let the ChainJntToJacSolver calculate the jacobian "jac" for
00235   //the current joint positions "q_in". This will include the mimic joints
00236   if(num_mimic_joints > 0)
00237   {
00238     jnt2jac.JntToJac(q_in,jac);
00239     //Now compute the actual jacobian that involves only the active DOFs
00240     jacToJacReduced(jac,jac_reduced);
00241   }
00242   else
00243     jnt2jac.JntToJac(q_in,jac_reduced);
00244 
00245   //Do a singular value decomposition of "jac" with maximum
00246   //iterations "maxiter", put the results in "U", "S" and "V"
00247   //jac = U*S*Vt
00248 
00249   int ret;
00250   if(!position_ik)
00251     ret = svd.calculate(jac_reduced,U,S,V,maxiter);
00252   else
00253     ret = svd_eigen_HH(jac_reduced.data.topLeftCorner(3,chain.getNrOfJoints()-num_mimic_joints),U_translate,S_translate,V_translate,tmp_translate,maxiter);
00254 
00255   double sum;
00256   unsigned int i,j;
00257 
00258   // We have to calculate qdot_out = jac_pinv*v_in
00259   // Using the svd decomposition this becomes(jac_pinv=V*S_pinv*Ut):
00260   // qdot_out = V*S_pinv*Ut*v_in
00261 
00262   unsigned int columns, rows;
00263   if(!position_ik)
00264     rows = jac_reduced.rows();
00265   else
00266     rows = 3;
00267 
00268   //first we calculate Ut*v_in
00269   for (i=0;i<jac_reduced.columns();i++) {
00270     sum = 0.0;
00271     for (j=0;j<rows;j++) {
00272       if(!position_ik)
00273         sum+= U[j](i)*v_in(j);
00274       else
00275         sum+= U_translate(j,i)*v_in(j);
00276     }
00277     //If the singular value is too small (<eps), don't invert it but
00278     //set the inverted singular value to zero (truncated svd)
00279     if(!position_ik)
00280       tmp(i) = sum*(fabs(S(i))<eps?0.0:1.0/S(i));
00281     else
00282       tmp(i) = sum*(fabs(S_translate(i))<eps?0.0:1.0/S_translate(i));
00283   }
00284   //tmp is now: tmp=S_pinv*Ut*v_in, we still have to premultiply
00285   //it with V to get qdot_out
00286   for (i=0;i<jac_reduced.columns();i++) {
00287     sum = 0.0;
00288     for (j=0;j<jac_reduced.columns();j++) {
00289       if(!position_ik)
00290         sum+=V[i](j)*tmp(j);
00291       else
00292         sum+=V_translate(i,j)*tmp(j);
00293     }
00294     //Put the result in qdot_out_reduced if mimic joints exist, otherwise in qdot_out
00295     if(num_mimic_joints > 0)
00296       qdot_out_reduced(i)=sum;
00297     else
00298       qdot_out(i) = sum;
00299   }
00300   ROS_DEBUG_STREAM_NAMED("kdl","Solution:");
00301   if(num_mimic_joints > 0)
00302   {
00303     for(i=0; i < chain.getNrOfJoints(); ++i)
00304     {
00305       qdot_out(i) = qdot_out_reduced(mimic_joints_[i].map_index) * mimic_joints_[i].multiplier;
00306     }
00307   }
00308   //return the return value of the svd decomposition
00309   return ret;
00310 }
00311 }


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Aug 26 2015 12:43:30