chainiksolver_vel_pinv_mimic.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2016, CRI group, NTU, Singapore
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of CRI group nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Francisco Suarez-Ruiz */
36 
37 #ifndef KDL_CHAIN_IKSOLVERVEL_PINV_MIMIC_H
38 #define KDL_CHAIN_IKSOLVERVEL_PINV_MIMIC_H
39 
40 #include "kdl/chainiksolver.hpp"
41 #include "kdl/chainjnttojacsolver.hpp"
42 #include "kdl/utilities/svd_HH.hpp"
43 #include "kdl/utilities/svd_eigen_HH.hpp"
44 
46 
47 namespace KDL
48 {
58 class ChainIkSolverVel_pinv_mimic : public ChainIkSolverVel
59 {
60 public:
77  explicit ChainIkSolverVel_pinv_mimic(const Chain& chain, int num_mimic_joints = 0, int num_redundant_joints = 0,
78  bool position_ik = false, double eps = 0.00001, int maxiter = 150);
79 
81 
82  virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
83 
84  virtual int CartToJntRedundant(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
85 
90  virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out)
91  {
92  return -1;
93  };
94 
103  bool setMimicJoints(const std::vector<lma_kinematics_plugin::JointMimic>& _mimic_joints);
104 
113  bool setRedundantJointsMapIndex(const std::vector<unsigned int>& redundant_joints_map_index);
114 
116  {
118  }
119 
121  {
122  redundant_joints_locked = false;
123  }
124 
125 private:
126  bool jacToJacReduced(const Jacobian& jac, Jacobian& jac_mimic);
127  bool jacToJacLocked(const Jacobian& jac, Jacobian& jac_locked);
128 
129  const Chain chain;
131 
132  // This set of variables are all used in the default case, i.e. where we are solving for the
133  // full end-effector pose
134  Jacobian jac;
135  std::vector<JntArray> U;
136  JntArray S;
137  std::vector<JntArray> V;
138  JntArray tmp;
139 
140  // This is the "reduced" jacobian, i.e. the contributions of the mimic joints have been mapped onto
141  // the active DOFs here
144 
145  // This is the set of variable used when solving for position only inverse kinematics
146  Eigen::MatrixXd U_translate;
147  Eigen::VectorXd S_translate;
148  Eigen::MatrixXd V_translate;
149  Eigen::VectorXd tmp_translate;
150 
151  // This is the jacobian when the redundant joint is "locked" and plays no part
154 
155  SVD_HH svd;
156  double eps;
157  int maxiter;
158 
159  // Mimic joint specific
160  std::vector<lma_kinematics_plugin::JointMimic> mimic_joints_;
161  int num_mimic_joints;
162 
163  bool position_ik;
164 
165  // This is the set of variable used when solving for inverse kinematics
166  // for the case where the redundant joint is "locked" and plays no part
167  Eigen::MatrixXd U_locked;
168  Eigen::VectorXd S_locked;
169  Eigen::MatrixXd V_locked;
170  Eigen::VectorXd tmp_locked;
171 
172  // This is the set of variable used when solving for position only inverse kinematics
173  // for the case where the redundant joint is "locked" and plays no part
174  Eigen::MatrixXd U_translate_locked;
175  Eigen::VectorXd S_translate_locked;
176  Eigen::MatrixXd V_translate_locked;
177  Eigen::VectorXd tmp_translate_locked;
178 
179  // Internal storage for a map from the "locked" state to the full active state
180  std::vector<unsigned int> locked_joints_map_index;
181  unsigned int num_redundant_joints;
183 };
184 }
185 #endif
std::vector< lma_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