chainiksolver_vel_pinv_mimic.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2016, CRI group, NTU, Singapore
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of CRI group nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Francisco Suarez-Ruiz */
00036 
00037 #include <moveit/lma_kinematics_plugin/chainiksolver_vel_pinv_mimic.h>
00038 #include <ros/console.h>
00039 
00040 namespace KDL
00041 {
00042 ChainIkSolverVel_pinv_mimic::ChainIkSolverVel_pinv_mimic(const Chain& _chain, int _num_mimic_joints,
00043                                                          int _num_redundant_joints, bool _position_ik, double _eps,
00044                                                          int _maxiter)
00045   : chain(_chain)
00046   , jnt2jac(chain)
00047   , jac(chain.getNrOfJoints())
00048   , U(6, JntArray(chain.getNrOfJoints() - _num_mimic_joints))
00049   , S(chain.getNrOfJoints() - _num_mimic_joints)
00050   , V(chain.getNrOfJoints() - _num_mimic_joints, JntArray(chain.getNrOfJoints() - _num_mimic_joints))
00051   , tmp(chain.getNrOfJoints() - _num_mimic_joints)
00052   , jac_reduced(chain.getNrOfJoints() - _num_mimic_joints)
00053   , qdot_out_reduced(chain.getNrOfJoints() - _num_mimic_joints)
00054   , U_translate(MatrixXd::Zero(3, chain.getNrOfJoints() - _num_mimic_joints))
00055   , S_translate(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints))
00056   , V_translate(MatrixXd::Zero(chain.getNrOfJoints() - _num_mimic_joints, chain.getNrOfJoints() - _num_mimic_joints))
00057   , tmp_translate(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints))
00058   , jac_locked(chain.getNrOfJoints() - _num_redundant_joints - _num_mimic_joints)
00059   , qdot_out_reduced_locked(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints)
00060   , qdot_out_locked(chain.getNrOfJoints() - _num_redundant_joints)
00061   , svd(jac_reduced)
00062   , eps(_eps)
00063   , maxiter(_maxiter)
00064   , num_mimic_joints(_num_mimic_joints)
00065   , position_ik(_position_ik)
00066   , U_locked(MatrixXd::Zero(6, chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
00067   , S_locked(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
00068   , V_locked(MatrixXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints,
00069                             chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
00070   , tmp_locked(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
00071   , U_translate_locked(MatrixXd::Zero(3, chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
00072   , S_translate_locked(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
00073   , V_translate_locked(MatrixXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints,
00074                                       chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
00075   , tmp_translate_locked(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
00076   , num_redundant_joints(_num_redundant_joints)
00077   , redundant_joints_locked(false)
00078 {
00079   mimic_joints_.resize(chain.getNrOfJoints());
00080   for (std::size_t i = 0; i < mimic_joints_.size(); ++i)
00081     mimic_joints_[i].reset(i);
00082 }
00083 
00084 ChainIkSolverVel_pinv_mimic::~ChainIkSolverVel_pinv_mimic()
00085 {
00086 }
00087 
00088 bool ChainIkSolverVel_pinv_mimic::setMimicJoints(const std::vector<lma_kinematics_plugin::JointMimic>& mimic_joints)
00089 {
00090   if (mimic_joints.size() != chain.getNrOfJoints())
00091     return false;
00092 
00093   for (std::size_t i = 0; i < mimic_joints.size(); ++i)
00094   {
00095     if (mimic_joints[i].map_index >= chain.getNrOfJoints())
00096       return false;
00097   }
00098   mimic_joints_ = mimic_joints;
00099   return true;
00100 }
00101 
00102 bool ChainIkSolverVel_pinv_mimic::setRedundantJointsMapIndex(
00103     const std::vector<unsigned int>& redundant_joints_map_index)
00104 {
00105   if (redundant_joints_map_index.size() != chain.getNrOfJoints() - num_mimic_joints - num_redundant_joints)
00106   {
00107     ROS_ERROR("Map index size: %d does not match expected size. No. of joints: %d, num_mimic_joints: %d, "
00108               "num_redundant_joints: %d",
00109               (int)redundant_joints_map_index.size(), (int)chain.getNrOfJoints(), (int)num_mimic_joints,
00110               (int)num_redundant_joints);
00111     return false;
00112   }
00113 
00114   for (std::size_t i = 0; i < redundant_joints_map_index.size(); ++i)
00115   {
00116     if (redundant_joints_map_index[i] >= chain.getNrOfJoints() - num_mimic_joints)
00117       return false;
00118   }
00119   locked_joints_map_index = redundant_joints_map_index;
00120   return true;
00121 }
00122 
00123 bool ChainIkSolverVel_pinv_mimic::jacToJacReduced(const Jacobian& jac, Jacobian& jac_reduced_l)
00124 {
00125   jac_reduced_l.data.setZero();
00126   for (std::size_t i = 0; i < chain.getNrOfJoints(); ++i)
00127   {
00128     Twist vel1 = jac_reduced_l.getColumn(mimic_joints_[i].map_index);
00129     Twist vel2 = jac.getColumn(i);
00130     Twist result = vel1 + (mimic_joints_[i].multiplier * vel2);
00131     jac_reduced_l.setColumn(mimic_joints_[i].map_index, result);
00132   }
00133   return true;
00134 }
00135 
00136 bool ChainIkSolverVel_pinv_mimic::jacToJacLocked(const Jacobian& jac, Jacobian& jac_locked)
00137 {
00138   jac_locked.data.setZero();
00139   for (std::size_t i = 0; i < chain.getNrOfJoints() - num_mimic_joints - num_redundant_joints; ++i)
00140   {
00141     jac_locked.setColumn(i, jac.getColumn(locked_joints_map_index[i]));
00142   }
00143   return true;
00144 }
00145 
00146 int ChainIkSolverVel_pinv_mimic::CartToJntRedundant(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
00147 {
00148   qdot_out.data.setZero();
00149   // Let the ChainJntToJacSolver calculate the jacobian "jac" for
00150   // the current joint positions "q_in". This will include the mimic joints
00151   if (num_mimic_joints > 0)
00152   {
00153     jnt2jac.JntToJac(q_in, jac);
00154     // Now compute the actual jacobian that involves only the active DOFs
00155     jacToJacReduced(jac, jac_reduced);
00156   }
00157   else
00158     jnt2jac.JntToJac(q_in, jac_reduced);
00159 
00160   // Now compute the jacobian with redundant joints locked
00161   jacToJacLocked(jac_reduced, jac_locked);
00162 
00163   // Do a singular value decomposition of "jac" with maximum
00164   // iterations "maxiter", put the results in "U", "S" and "V"
00165   // jac = U*S*Vt
00166 
00167   int ret;
00168   if (!position_ik)
00169     ret = svd_eigen_HH(jac_locked.data, U_locked, S_locked, V_locked, tmp_locked, maxiter);
00170   else
00171     ret =
00172         svd_eigen_HH(jac_locked.data.topLeftCorner(3, chain.getNrOfJoints() - num_mimic_joints - num_redundant_joints),
00173                      U_translate_locked, S_translate_locked, V_translate_locked, tmp_translate_locked, maxiter);
00174 
00175   double sum;
00176   unsigned int i, j;
00177 
00178   // We have to calculate qdot_out = jac_pinv*v_in
00179   // Using the svd decomposition this becomes(jac_pinv=V*S_pinv*Ut):
00180   // qdot_out = V*S_pinv*Ut*v_in
00181 
00182   unsigned int columns, rows;
00183   if (!position_ik)
00184     rows = jac_locked.rows();
00185   else
00186     rows = 3;
00187 
00188   // first we calculate Ut*v_in
00189   for (i = 0; i < jac_locked.columns(); i++)
00190   {
00191     sum = 0.0;
00192     for (j = 0; j < rows; j++)
00193     {
00194       if (!position_ik)
00195         sum += U_locked(j, i) * v_in(j);
00196       else
00197         sum += U_translate_locked(j, i) * v_in(j);
00198     }
00199     // If the singular value is too small (<eps), don't invert it but
00200     // set the inverted singular value to zero (truncated svd)
00201     if (!position_ik)
00202       tmp(i) = sum * (fabs(S_locked(i)) < eps ? 0.0 : 1.0 / S_locked(i));
00203     else
00204       tmp(i) = sum * (fabs(S_translate_locked(i)) < eps ? 0.0 : 1.0 / S_translate_locked(i));
00205   }
00206   // tmp is now: tmp=S_pinv*Ut*v_in, we still have to premultiply
00207   // it with V to get qdot_out
00208   for (i = 0; i < jac_locked.columns(); i++)
00209   {
00210     sum = 0.0;
00211     for (j = 0; j < jac_locked.columns(); j++)
00212     {
00213       if (!position_ik)
00214         sum += V_locked(i, j) * tmp(j);
00215       else
00216         sum += V_translate_locked(i, j) * tmp(j);
00217     }
00218     // Put the result in qdot_out_reduced if mimic joints exist, otherwise in qdot_out
00219     if (num_mimic_joints > 0)
00220       qdot_out_reduced_locked(i) = sum;
00221     else
00222       qdot_out_locked(i) = sum;
00223   }
00224   ROS_DEBUG_STREAM_NAMED("lma", "Solution:");
00225 
00226   if (num_mimic_joints > 0)
00227   {
00228     for (i = 0; i < chain.getNrOfJoints() - num_mimic_joints - num_redundant_joints; ++i)
00229     {
00230       qdot_out_reduced(locked_joints_map_index[i]) = qdot_out_reduced_locked(i);
00231     }
00232     for (i = 0; i < chain.getNrOfJoints(); ++i)
00233     {
00234       qdot_out(i) = qdot_out_reduced(mimic_joints_[i].map_index) * mimic_joints_[i].multiplier;
00235     }
00236   }
00237   else
00238   {
00239     for (i = 0; i < chain.getNrOfJoints() - num_redundant_joints; ++i)
00240     {
00241       qdot_out(locked_joints_map_index[i]) = qdot_out_locked(i);
00242     }
00243   }
00244   // Reset the flag
00245   // redundant_joints_locked = false;
00246   // return the return value of the svd decomposition
00247   return ret;
00248 }
00249 
00250 int ChainIkSolverVel_pinv_mimic::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
00251 {
00252   if (redundant_joints_locked)
00253     return CartToJntRedundant(q_in, v_in, qdot_out);
00254 
00255   // Let the ChainJntToJacSolver calculate the jacobian "jac" for
00256   // the current joint positions "q_in". This will include the mimic joints
00257   if (num_mimic_joints > 0)
00258   {
00259     jnt2jac.JntToJac(q_in, jac);
00260     // Now compute the actual jacobian that involves only the active DOFs
00261     jacToJacReduced(jac, jac_reduced);
00262   }
00263   else
00264     jnt2jac.JntToJac(q_in, jac_reduced);
00265 
00266   // Do a singular value decomposition of "jac" with maximum
00267   // iterations "maxiter", put the results in "U", "S" and "V"
00268   // jac = U*S*Vt
00269 
00270   int ret;
00271   if (!position_ik)
00272     ret = svd.calculate(jac_reduced, U, S, V, maxiter);
00273   else
00274     ret = svd_eigen_HH(jac_reduced.data.topLeftCorner(3, chain.getNrOfJoints() - num_mimic_joints), U_translate,
00275                        S_translate, V_translate, tmp_translate, maxiter);
00276 
00277   double sum;
00278   unsigned int i, j;
00279 
00280   // We have to calculate qdot_out = jac_pinv*v_in
00281   // Using the svd decomposition this becomes(jac_pinv=V*S_pinv*Ut):
00282   // qdot_out = V*S_pinv*Ut*v_in
00283 
00284   unsigned int columns, rows;
00285   if (!position_ik)
00286     rows = jac_reduced.rows();
00287   else
00288     rows = 3;
00289 
00290   // first we calculate Ut*v_in
00291   for (i = 0; i < jac_reduced.columns(); i++)
00292   {
00293     sum = 0.0;
00294     for (j = 0; j < rows; j++)
00295     {
00296       if (!position_ik)
00297         sum += U[j](i) * v_in(j);
00298       else
00299         sum += U_translate(j, i) * v_in(j);
00300     }
00301     // If the singular value is too small (<eps), don't invert it but
00302     // set the inverted singular value to zero (truncated svd)
00303     if (!position_ik)
00304       tmp(i) = sum * (fabs(S(i)) < eps ? 0.0 : 1.0 / S(i));
00305     else
00306       tmp(i) = sum * (fabs(S_translate(i)) < eps ? 0.0 : 1.0 / S_translate(i));
00307   }
00308   // tmp is now: tmp=S_pinv*Ut*v_in, we still have to premultiply
00309   // it with V to get qdot_out
00310   for (i = 0; i < jac_reduced.columns(); i++)
00311   {
00312     sum = 0.0;
00313     for (j = 0; j < jac_reduced.columns(); j++)
00314     {
00315       if (!position_ik)
00316         sum += V[i](j) * tmp(j);
00317       else
00318         sum += V_translate(i, j) * tmp(j);
00319     }
00320     // Put the result in qdot_out_reduced if mimic joints exist, otherwise in qdot_out
00321     if (num_mimic_joints > 0)
00322       qdot_out_reduced(i) = sum;
00323     else
00324       qdot_out(i) = sum;
00325   }
00326   ROS_DEBUG_STREAM_NAMED("lma", "Solution:");
00327   if (num_mimic_joints > 0)
00328   {
00329     for (i = 0; i < chain.getNrOfJoints(); ++i)
00330     {
00331       qdot_out(i) = qdot_out_reduced(mimic_joints_[i].map_index) * mimic_joints_[i].multiplier;
00332     }
00333   }
00334   // return the return value of the svd decomposition
00335   return ret;
00336 }
00337 }


moveit_kinematics
Author(s): Dave Coleman , Ioan Sucan , Sachin Chitta
autogenerated on Wed Jan 17 2018 03:32:16