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,
00032                                                          int _num_redundant_joints, bool _position_ik, double _eps,
00033                                                          int _maxiter)
00034   : chain(_chain)
00035   , jnt2jac(chain)
00036   , jac(chain.getNrOfJoints())
00037   , U(6, JntArray(chain.getNrOfJoints() - _num_mimic_joints))
00038   , S(chain.getNrOfJoints() - _num_mimic_joints)
00039   , V(chain.getNrOfJoints() - _num_mimic_joints, JntArray(chain.getNrOfJoints() - _num_mimic_joints))
00040   , tmp(chain.getNrOfJoints() - _num_mimic_joints)
00041   , jac_reduced(chain.getNrOfJoints() - _num_mimic_joints)
00042   , qdot_out_reduced(chain.getNrOfJoints() - _num_mimic_joints)
00043   , U_translate(MatrixXd::Zero(3, chain.getNrOfJoints() - _num_mimic_joints))
00044   , S_translate(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints))
00045   , V_translate(MatrixXd::Zero(chain.getNrOfJoints() - _num_mimic_joints, chain.getNrOfJoints() - _num_mimic_joints))
00046   , tmp_translate(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints))
00047   , jac_locked(chain.getNrOfJoints() - _num_redundant_joints - _num_mimic_joints)
00048   , qdot_out_reduced_locked(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints)
00049   , qdot_out_locked(chain.getNrOfJoints() - _num_redundant_joints)
00050   , svd(jac_reduced)
00051   , eps(_eps)
00052   , maxiter(_maxiter)
00053   , num_mimic_joints(_num_mimic_joints)
00054   , position_ik(_position_ik)
00055   , U_locked(MatrixXd::Zero(6, chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
00056   , S_locked(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
00057   , V_locked(MatrixXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints,
00058                             chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
00059   , tmp_locked(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
00060   , U_translate_locked(MatrixXd::Zero(3, chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
00061   , S_translate_locked(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
00062   , V_translate_locked(MatrixXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints,
00063                                       chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
00064   , tmp_translate_locked(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
00065   , num_redundant_joints(_num_redundant_joints)
00066   , redundant_joints_locked(false)
00067 {
00068   mimic_joints_.resize(chain.getNrOfJoints());
00069   for (std::size_t i = 0; i < mimic_joints_.size(); ++i)
00070     mimic_joints_[i].reset(i);
00071 }
00072 
00073 ChainIkSolverVel_pinv_mimic::~ChainIkSolverVel_pinv_mimic()
00074 {
00075 }
00076 
00077 bool ChainIkSolverVel_pinv_mimic::setMimicJoints(const std::vector<kdl_kinematics_plugin::JointMimic>& mimic_joints)
00078 {
00079   if (mimic_joints.size() != chain.getNrOfJoints())
00080     return false;
00081 
00082   for (std::size_t i = 0; i < mimic_joints.size(); ++i)
00083   {
00084     if (mimic_joints[i].map_index >= chain.getNrOfJoints())
00085       return false;
00086   }
00087   mimic_joints_ = mimic_joints;
00088   return true;
00089 }
00090 
00091 bool ChainIkSolverVel_pinv_mimic::setRedundantJointsMapIndex(
00092     const std::vector<unsigned int>& redundant_joints_map_index)
00093 {
00094   if (redundant_joints_map_index.size() != chain.getNrOfJoints() - num_mimic_joints - num_redundant_joints)
00095   {
00096     ROS_ERROR("Map index size: %d does not match expected size. No. of joints: %d, num_mimic_joints: %d, "
00097               "num_redundant_joints: %d",
00098               (int)redundant_joints_map_index.size(), (int)chain.getNrOfJoints(), (int)num_mimic_joints,
00099               (int)num_redundant_joints);
00100     return false;
00101   }
00102 
00103   for (std::size_t i = 0; i < redundant_joints_map_index.size(); ++i)
00104   {
00105     if (redundant_joints_map_index[i] >= chain.getNrOfJoints() - num_mimic_joints)
00106       return false;
00107   }
00108   locked_joints_map_index = redundant_joints_map_index;
00109   return true;
00110 }
00111 
00112 bool ChainIkSolverVel_pinv_mimic::jacToJacReduced(const Jacobian& jac, Jacobian& jac_reduced_l)
00113 {
00114   jac_reduced_l.data.setZero();
00115   for (std::size_t i = 0; i < chain.getNrOfJoints(); ++i)
00116   {
00117     Twist vel1 = jac_reduced_l.getColumn(mimic_joints_[i].map_index);
00118     Twist vel2 = jac.getColumn(i);
00119     Twist result = vel1 + (mimic_joints_[i].multiplier * vel2);
00120     jac_reduced_l.setColumn(mimic_joints_[i].map_index, result);
00121   }
00122   return true;
00123 }
00124 
00125 bool ChainIkSolverVel_pinv_mimic::jacToJacLocked(const Jacobian& jac, Jacobian& jac_locked)
00126 {
00127   jac_locked.data.setZero();
00128   for (std::size_t i = 0; i < chain.getNrOfJoints() - num_mimic_joints - num_redundant_joints; ++i)
00129   {
00130     jac_locked.setColumn(i, jac.getColumn(locked_joints_map_index[i]));
00131   }
00132   return true;
00133 }
00134 
00135 int ChainIkSolverVel_pinv_mimic::CartToJntRedundant(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
00136 {
00137   qdot_out.data.setZero();
00138   // Let the ChainJntToJacSolver calculate the jacobian "jac" for
00139   // the current joint positions "q_in". This will include the mimic joints
00140   if (num_mimic_joints > 0)
00141   {
00142     jnt2jac.JntToJac(q_in, jac);
00143     // Now compute the actual jacobian that involves only the active DOFs
00144     jacToJacReduced(jac, jac_reduced);
00145   }
00146   else
00147     jnt2jac.JntToJac(q_in, jac_reduced);
00148 
00149   // Now compute the jacobian with redundant joints locked
00150   jacToJacLocked(jac_reduced, jac_locked);
00151 
00152   // Do a singular value decomposition of "jac" with maximum
00153   // iterations "maxiter", put the results in "U", "S" and "V"
00154   // jac = U*S*Vt
00155 
00156   int ret;
00157   if (!position_ik)
00158     ret = svd_eigen_HH(jac_locked.data, U_locked, S_locked, V_locked, tmp_locked, maxiter);
00159   else
00160     ret =
00161         svd_eigen_HH(jac_locked.data.topLeftCorner(3, chain.getNrOfJoints() - num_mimic_joints - num_redundant_joints),
00162                      U_translate_locked, S_translate_locked, V_translate_locked, tmp_translate_locked, maxiter);
00163 
00164   double sum;
00165   unsigned int i, j;
00166 
00167   // We have to calculate qdot_out = jac_pinv*v_in
00168   // Using the svd decomposition this becomes(jac_pinv=V*S_pinv*Ut):
00169   // qdot_out = V*S_pinv*Ut*v_in
00170 
00171   unsigned int rows;
00172   if (!position_ik)
00173     rows = jac_locked.rows();
00174   else
00175     rows = 3;
00176 
00177   // first we calculate Ut*v_in
00178   for (i = 0; i < jac_locked.columns(); i++)
00179   {
00180     sum = 0.0;
00181     for (j = 0; j < rows; j++)
00182     {
00183       if (!position_ik)
00184         sum += U_locked(j, i) * v_in(j);
00185       else
00186         sum += U_translate_locked(j, i) * v_in(j);
00187     }
00188     // If the singular value is too small (<eps), don't invert it but
00189     // set the inverted singular value to zero (truncated svd)
00190     if (!position_ik)
00191       tmp(i) = sum * (fabs(S_locked(i)) < eps ? 0.0 : 1.0 / S_locked(i));
00192     else
00193       tmp(i) = sum * (fabs(S_translate_locked(i)) < eps ? 0.0 : 1.0 / S_translate_locked(i));
00194   }
00195   // tmp is now: tmp=S_pinv*Ut*v_in, we still have to premultiply
00196   // it with V to get qdot_out
00197   for (i = 0; i < jac_locked.columns(); i++)
00198   {
00199     sum = 0.0;
00200     for (j = 0; j < jac_locked.columns(); j++)
00201     {
00202       if (!position_ik)
00203         sum += V_locked(i, j) * tmp(j);
00204       else
00205         sum += V_translate_locked(i, j) * tmp(j);
00206     }
00207     // Put the result in qdot_out_reduced if mimic joints exist, otherwise in qdot_out
00208     if (num_mimic_joints > 0)
00209       qdot_out_reduced_locked(i) = sum;
00210     else
00211       qdot_out_locked(i) = sum;
00212   }
00213   ROS_DEBUG_STREAM_NAMED("kdl", "Solution:");
00214 
00215   if (num_mimic_joints > 0)
00216   {
00217     for (i = 0; i < chain.getNrOfJoints() - num_mimic_joints - num_redundant_joints; ++i)
00218     {
00219       qdot_out_reduced(locked_joints_map_index[i]) = qdot_out_reduced_locked(i);
00220     }
00221     for (i = 0; i < chain.getNrOfJoints(); ++i)
00222     {
00223       qdot_out(i) = qdot_out_reduced(mimic_joints_[i].map_index) * mimic_joints_[i].multiplier;
00224     }
00225   }
00226   else
00227   {
00228     for (i = 0; i < chain.getNrOfJoints() - num_redundant_joints; ++i)
00229     {
00230       qdot_out(locked_joints_map_index[i]) = qdot_out_locked(i);
00231     }
00232   }
00233   // Reset the flag
00234   // redundant_joints_locked = false;
00235   // return the return value of the svd decomposition
00236   return ret;
00237 }
00238 
00239 int ChainIkSolverVel_pinv_mimic::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
00240 {
00241   if (redundant_joints_locked)
00242     return CartToJntRedundant(q_in, v_in, qdot_out);
00243 
00244   // Let the ChainJntToJacSolver calculate the jacobian "jac" for
00245   // the current joint positions "q_in". This will include the mimic joints
00246   if (num_mimic_joints > 0)
00247   {
00248     jnt2jac.JntToJac(q_in, jac);
00249     // Now compute the actual jacobian that involves only the active DOFs
00250     jacToJacReduced(jac, jac_reduced);
00251   }
00252   else
00253     jnt2jac.JntToJac(q_in, jac_reduced);
00254 
00255   // Do a singular value decomposition of "jac" with maximum
00256   // iterations "maxiter", put the results in "U", "S" and "V"
00257   // jac = U*S*Vt
00258 
00259   int ret;
00260   if (!position_ik)
00261     ret = svd.calculate(jac_reduced, U, S, V, maxiter);
00262   else
00263     ret = svd_eigen_HH(jac_reduced.data.topLeftCorner(3, chain.getNrOfJoints() - num_mimic_joints), U_translate,
00264                        S_translate, V_translate, tmp_translate, maxiter);
00265 
00266   double sum;
00267   unsigned int i, j;
00268 
00269   // We have to calculate qdot_out = jac_pinv*v_in
00270   // Using the svd decomposition this becomes(jac_pinv=V*S_pinv*Ut):
00271   // qdot_out = V*S_pinv*Ut*v_in
00272 
00273   unsigned int rows;
00274   if (!position_ik)
00275     rows = jac_reduced.rows();
00276   else
00277     rows = 3;
00278 
00279   // first we calculate Ut*v_in
00280   for (i = 0; i < jac_reduced.columns(); i++)
00281   {
00282     sum = 0.0;
00283     for (j = 0; j < rows; j++)
00284     {
00285       if (!position_ik)
00286         sum += U[j](i) * v_in(j);
00287       else
00288         sum += U_translate(j, i) * v_in(j);
00289     }
00290     // If the singular value is too small (<eps), don't invert it but
00291     // set the inverted singular value to zero (truncated svd)
00292     if (!position_ik)
00293       tmp(i) = sum * (fabs(S(i)) < eps ? 0.0 : 1.0 / S(i));
00294     else
00295       tmp(i) = sum * (fabs(S_translate(i)) < eps ? 0.0 : 1.0 / S_translate(i));
00296   }
00297   // tmp is now: tmp=S_pinv*Ut*v_in, we still have to premultiply
00298   // it with V to get qdot_out
00299   for (i = 0; i < jac_reduced.columns(); i++)
00300   {
00301     sum = 0.0;
00302     for (j = 0; j < jac_reduced.columns(); j++)
00303     {
00304       if (!position_ik)
00305         sum += V[i](j) * tmp(j);
00306       else
00307         sum += V_translate(i, j) * tmp(j);
00308     }
00309     // Put the result in qdot_out_reduced if mimic joints exist, otherwise in qdot_out
00310     if (num_mimic_joints > 0)
00311       qdot_out_reduced(i) = sum;
00312     else
00313       qdot_out(i) = sum;
00314   }
00315   ROS_DEBUG_STREAM_NAMED("kdl", "Solution:");
00316   if (num_mimic_joints > 0)
00317   {
00318     for (i = 0; i < chain.getNrOfJoints(); ++i)
00319     {
00320       qdot_out(i) = qdot_out_reduced(mimic_joints_[i].map_index) * mimic_joints_[i].multiplier;
00321     }
00322   }
00323   // return the return value of the svd decomposition
00324   return ret;
00325 }
00326 }


moveit_kinematics
Author(s): Dave Coleman , Ioan Sucan , Sachin Chitta
autogenerated on Mon Apr 23 2018 10:25:23