chainiksolver_vel_pinv_mimic.h
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 #ifndef KDL_CHAIN_IKSOLVERVEL_PINV_MIMIC_H
00038 #define KDL_CHAIN_IKSOLVERVEL_PINV_MIMIC_H
00039 
00040 #include "kdl/chainiksolver.hpp"
00041 #include "kdl/chainjnttojacsolver.hpp"
00042 #include "kdl/utilities/svd_HH.hpp"
00043 #include "kdl/utilities/svd_eigen_HH.hpp"
00044 
00045 #include <moveit/lma_kinematics_plugin/joint_mimic.h>
00046 
00047 namespace KDL
00048 {
00058 class ChainIkSolverVel_pinv_mimic : public ChainIkSolverVel
00059 {
00060 public:
00077   explicit ChainIkSolverVel_pinv_mimic(const Chain& chain, int num_mimic_joints = 0, int num_redundant_joints = 0,
00078                                        bool position_ik = false, double eps = 0.00001, int maxiter = 150);
00079 
00080   ~ChainIkSolverVel_pinv_mimic();
00081 
00082   virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
00083 
00084   virtual int CartToJntRedundant(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
00085 
00090   virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out)
00091   {
00092     return -1;
00093   };
00094 
00103   bool setMimicJoints(const std::vector<lma_kinematics_plugin::JointMimic>& _mimic_joints);
00104 
00113   bool setRedundantJointsMapIndex(const std::vector<unsigned int>& redundant_joints_map_index);
00114 
00115   void lockRedundantJoints()
00116   {
00117     redundant_joints_locked = true;
00118   }
00119 
00120   void unlockRedundantJoints()
00121   {
00122     redundant_joints_locked = false;
00123   }
00124 
00125 private:
00126   bool jacToJacReduced(const Jacobian& jac, Jacobian& jac_mimic);
00127   bool jacToJacLocked(const Jacobian& jac, Jacobian& jac_locked);
00128 
00129   const Chain chain;
00130   ChainJntToJacSolver jnt2jac;
00131 
00132   // This set of variables are all used in the default case, i.e. where we are solving for the
00133   // full end-effector pose
00134   Jacobian jac;
00135   std::vector<JntArray> U;
00136   JntArray S;
00137   std::vector<JntArray> V;
00138   JntArray tmp;
00139 
00140   // This is the "reduced" jacobian, i.e. the contributions of the mimic joints have been mapped onto
00141   // the active DOFs here
00142   Jacobian jac_reduced;
00143   JntArray qdot_out_reduced;
00144 
00145   // This is the set of variable used when solving for position only inverse kinematics
00146   Eigen::MatrixXd U_translate;
00147   Eigen::VectorXd S_translate;
00148   Eigen::MatrixXd V_translate;
00149   Eigen::VectorXd tmp_translate;
00150 
00151   // This is the jacobian when the redundant joint is "locked" and plays no part
00152   Jacobian jac_locked;
00153   JntArray qdot_out_reduced_locked, qdot_out_locked;
00154 
00155   SVD_HH svd;
00156   double eps;
00157   int maxiter;
00158 
00159   // Mimic joint specific
00160   std::vector<lma_kinematics_plugin::JointMimic> mimic_joints_;
00161   int num_mimic_joints;
00162 
00163   bool position_ik;
00164 
00165   // This is the set of variable used when solving for inverse kinematics
00166   // for the case where the redundant joint is "locked" and plays no part
00167   Eigen::MatrixXd U_locked;
00168   Eigen::VectorXd S_locked;
00169   Eigen::MatrixXd V_locked;
00170   Eigen::VectorXd tmp_locked;
00171 
00172   // This is the set of variable used when solving for position only inverse kinematics
00173   // for the case where the redundant joint is "locked" and plays no part
00174   Eigen::MatrixXd U_translate_locked;
00175   Eigen::VectorXd S_translate_locked;
00176   Eigen::MatrixXd V_translate_locked;
00177   Eigen::VectorXd tmp_translate_locked;
00178 
00179   // Internal storage for a map from the "locked" state to the full active state
00180   std::vector<unsigned int> locked_joints_map_index;
00181   unsigned int num_redundant_joints;
00182   bool redundant_joints_locked;
00183 };
00184 }
00185 #endif


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