chainiksolver_vel_pinv_mimic.hpp
Go to the documentation of this file.
1 // Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
2 
3 // Version: 1.0
4 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
5 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
6 // URL: http://www.orocos.org/kdl
7 
8 // This library is free software; you can redistribute it and/or
9 // modify it under the terms of the GNU Lesser General Public
10 // License as published by the Free Software Foundation; either
11 // version 2.1 of the License, or (at your option) any later version.
12 
13 // This library is distributed in the hope that it will be useful,
14 // but WITHOUT ANY WARRANTY; without even the implied warranty of
15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
16 // Lesser General Public License for more details.
17 
18 // You should have received a copy of the GNU Lesser General Public
19 // License along with this library; if not, write to the Free Software
20 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
21 
22 // Modified to account for "mimic" joints, i.e. joints whose motion has a
23 // linear relationship to that of another joint.
24 // Copyright (C) 2013 Sachin Chitta, Willow Garage
25 
26 #ifndef KDL_CHAIN_IKSOLVERVEL_PINV_Mimic_HPP
27 #define KDL_CHAIN_IKSOLVERVEL_PINV_Mimic_HPP
28 
29 #include "kdl/chainiksolver.hpp"
30 #include "kdl/chainjnttojacsolver.hpp"
31 #include "kdl/utilities/svd_HH.hpp"
32 #include "kdl/utilities/svd_eigen_HH.hpp"
33 
35 
36 namespace KDL
37 {
48 {
49 public:
67  bool position_ik = false, double eps = 0.00001, int maxiter = 150);
68 
70 
71  virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
72 
73  virtual int CartToJntRedundant(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
74 
79  virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out)
80  {
81  return -1;
82  };
83 
92  bool setMimicJoints(const std::vector<kdl_kinematics_plugin::JointMimic>& _mimic_joints);
93 
102  bool setRedundantJointsMapIndex(const std::vector<unsigned int>& redundant_joints_map_index);
103 
105  {
107  }
108 
110  {
111  redundant_joints_locked = false;
112  }
113 
114 private:
115  bool jacToJacReduced(const Jacobian& jac, Jacobian& jac_mimic);
116  bool jacToJacLocked(const Jacobian& jac, Jacobian& jac_locked);
117 
118  const Chain chain;
120 
121  // This set of variables are all used in the default case, i.e. where we are solving for the
122  // full end-effector pose
124  std::vector<JntArray> U;
126  std::vector<JntArray> V;
128 
129  // This is the "reduced" jacobian, i.e. the contributions of the mimic joints have been mapped onto
130  // the active DOFs here
133 
134  // This is the set of variable used when solving for position only inverse kinematics
135  Eigen::MatrixXd U_translate;
136  Eigen::VectorXd S_translate;
137  Eigen::MatrixXd V_translate;
138  Eigen::VectorXd tmp_translate;
139 
140  // This is the jacobian when the redundant joint is "locked" and plays no part
143 
145  double eps;
146  int maxiter;
147 
148  // Mimic joint specific
149  std::vector<kdl_kinematics_plugin::JointMimic> mimic_joints_;
151 
153 
154  // This is the set of variable used when solving for inverse kinematics
155  // for the case where the redundant joint is "locked" and plays no part
156  Eigen::MatrixXd U_locked;
157  Eigen::VectorXd S_locked;
158  Eigen::MatrixXd V_locked;
159  Eigen::VectorXd tmp_locked;
160 
161  // This is the set of variable used when solving for position only inverse kinematics
162  // for the case where the redundant joint is "locked" and plays no part
163  Eigen::MatrixXd U_translate_locked;
164  Eigen::VectorXd S_translate_locked;
165  Eigen::MatrixXd V_translate_locked;
166  Eigen::VectorXd tmp_translate_locked;
167 
168  // Internal storage for a map from the "locked" state to the full active state
169  std::vector<unsigned int> locked_joints_map_index;
170  unsigned int num_redundant_joints;
172 };
173 }
174 #endif
std::vector< kdl_kinematics_plugin::JointMimic > mimic_joints_
bool jacToJacReduced(const Jacobian &jac, Jacobian &jac_mimic)
std::vector< unsigned int > locked_joints_map_index
bool setRedundantJointsMapIndex(const std::vector< unsigned int > &redundant_joints_map_index)
Set a mapping between a reduced set of joints (numbering either 6 or 3) and the full set of active (i...
ChainIkSolverVel_pinv_mimic(const Chain &chain, int num_mimic_joints=0, int num_redundant_joints=0, bool position_ik=false, double eps=0.00001, int maxiter=150)
virtual int CartToJnt(const JntArray &q_init, const FrameVel &v_in, JntArrayVel &q_out)
virtual int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
virtual int CartToJntRedundant(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
bool setMimicJoints(const std::vector< kdl_kinematics_plugin::JointMimic > &_mimic_joints)
Set a vector of indices that map each (and every) joint in the chain onto the corresponding joint in ...
bool jacToJacLocked(const Jacobian &jac, Jacobian &jac_locked)


moveit_kinematics
Author(s): Dave Coleman , Ioan Sucan , Sachin Chitta
autogenerated on Sun Oct 18 2020 13:17:53