kdl_kinematics_plugin/src/chainiksolver_vel_pinv_mimic.cpp
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 
27 #include <ros/console.h>
28 
29 namespace KDL
30 {
32  int _num_redundant_joints, bool _position_ik, double _eps,
33  int _maxiter)
34  : chain(_chain)
35  , jnt2jac(chain)
36  , jac(chain.getNrOfJoints())
37  , U(6, JntArray(chain.getNrOfJoints() - _num_mimic_joints))
38  , S(chain.getNrOfJoints() - _num_mimic_joints)
39  , V(chain.getNrOfJoints() - _num_mimic_joints, JntArray(chain.getNrOfJoints() - _num_mimic_joints))
40  , tmp(chain.getNrOfJoints() - _num_mimic_joints)
41  , jac_reduced(chain.getNrOfJoints() - _num_mimic_joints)
42  , qdot_out_reduced(chain.getNrOfJoints() - _num_mimic_joints)
43  , U_translate(MatrixXd::Zero(3, chain.getNrOfJoints() - _num_mimic_joints))
44  , S_translate(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints))
45  , V_translate(MatrixXd::Zero(chain.getNrOfJoints() - _num_mimic_joints, chain.getNrOfJoints() - _num_mimic_joints))
46  , tmp_translate(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints))
47  , jac_locked(chain.getNrOfJoints() - _num_redundant_joints - _num_mimic_joints)
48  , qdot_out_reduced_locked(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints)
49  , qdot_out_locked(chain.getNrOfJoints() - _num_redundant_joints)
50  , svd(jac_reduced)
51  , eps(_eps)
52  , maxiter(_maxiter)
53  , num_mimic_joints(_num_mimic_joints)
54  , position_ik(_position_ik)
55  , U_locked(MatrixXd::Zero(6, chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
56  , S_locked(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
57  , V_locked(MatrixXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints,
58  chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
59  , tmp_locked(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
60  , U_translate_locked(MatrixXd::Zero(3, chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
61  , S_translate_locked(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
62  , V_translate_locked(MatrixXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints,
63  chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
64  , tmp_translate_locked(VectorXd::Zero(chain.getNrOfJoints() - _num_mimic_joints - _num_redundant_joints))
65  , num_redundant_joints(_num_redundant_joints)
66  , redundant_joints_locked(false)
67 {
69  for (std::size_t i = 0; i < mimic_joints_.size(); ++i)
70  mimic_joints_[i].reset(i);
71 }
72 
74 {
75 }
76 
77 bool ChainIkSolverVel_pinv_mimic::setMimicJoints(const std::vector<kdl_kinematics_plugin::JointMimic>& mimic_joints)
78 {
79  if (mimic_joints.size() != chain.getNrOfJoints())
80  return false;
81 
82  for (std::size_t i = 0; i < mimic_joints.size(); ++i)
83  {
84  if (mimic_joints[i].map_index >= chain.getNrOfJoints())
85  return false;
86  }
87  mimic_joints_ = mimic_joints;
88  return true;
89 }
90 
92  const std::vector<unsigned int>& redundant_joints_map_index)
93 {
94  if (redundant_joints_map_index.size() != chain.getNrOfJoints() - num_mimic_joints - num_redundant_joints)
95  {
96  ROS_ERROR("Map index size: %d does not match expected size. No. of joints: %d, num_mimic_joints: %d, "
97  "num_redundant_joints: %d",
98  (int)redundant_joints_map_index.size(), (int)chain.getNrOfJoints(), (int)num_mimic_joints,
100  return false;
101  }
102 
103  for (std::size_t i = 0; i < redundant_joints_map_index.size(); ++i)
104  {
105  if (redundant_joints_map_index[i] >= chain.getNrOfJoints() - num_mimic_joints)
106  return false;
107  }
108  locked_joints_map_index = redundant_joints_map_index;
109  return true;
110 }
111 
113 {
114  jac_reduced_l.data.setZero();
115  for (std::size_t i = 0; i < chain.getNrOfJoints(); ++i)
116  {
117  Twist vel1 = jac_reduced_l.getColumn(mimic_joints_[i].map_index);
118  Twist vel2 = jac.getColumn(i);
119  Twist result = vel1 + (mimic_joints_[i].multiplier * vel2);
120  jac_reduced_l.setColumn(mimic_joints_[i].map_index, result);
121  }
122  return true;
123 }
124 
126 {
127  jac_locked.data.setZero();
128  for (std::size_t i = 0; i < chain.getNrOfJoints() - num_mimic_joints - num_redundant_joints; ++i)
129  {
130  jac_locked.setColumn(i, jac.getColumn(locked_joints_map_index[i]));
131  }
132  return true;
133 }
134 
136 {
137  qdot_out.data.setZero();
138  // Let the ChainJntToJacSolver calculate the jacobian "jac" for
139  // the current joint positions "q_in". This will include the mimic joints
140  if (num_mimic_joints > 0)
141  {
142  jnt2jac.JntToJac(q_in, jac);
143  // Now compute the actual jacobian that involves only the active DOFs
145  }
146  else
148 
149  // Now compute the jacobian with redundant joints locked
151 
152  // Do a singular value decomposition of "jac" with maximum
153  // iterations "maxiter", put the results in "U", "S" and "V"
154  // jac = U*S*Vt
155 
156  int ret;
157  if (!position_ik)
159  else
160  ret =
163 
164  double sum;
165  unsigned int i, j;
166 
167  // We have to calculate qdot_out = jac_pinv*v_in
168  // Using the svd decomposition this becomes(jac_pinv=V*S_pinv*Ut):
169  // qdot_out = V*S_pinv*Ut*v_in
170 
171  unsigned int rows;
172  if (!position_ik)
173  rows = jac_locked.rows();
174  else
175  rows = 3;
176 
177  // first we calculate Ut*v_in
178  for (i = 0; i < jac_locked.columns(); i++)
179  {
180  sum = 0.0;
181  for (j = 0; j < rows; j++)
182  {
183  if (!position_ik)
184  sum += U_locked(j, i) * v_in(j);
185  else
186  sum += U_translate_locked(j, i) * v_in(j);
187  }
188  // If the singular value is too small (<eps), don't invert it but
189  // set the inverted singular value to zero (truncated svd)
190  if (!position_ik)
191  tmp(i) = sum * (fabs(S_locked(i)) < eps ? 0.0 : 1.0 / S_locked(i));
192  else
193  tmp(i) = sum * (fabs(S_translate_locked(i)) < eps ? 0.0 : 1.0 / S_translate_locked(i));
194  }
195  // tmp is now: tmp=S_pinv*Ut*v_in, we still have to premultiply
196  // it with V to get qdot_out
197  for (i = 0; i < jac_locked.columns(); i++)
198  {
199  sum = 0.0;
200  for (j = 0; j < jac_locked.columns(); j++)
201  {
202  if (!position_ik)
203  sum += V_locked(i, j) * tmp(j);
204  else
205  sum += V_translate_locked(i, j) * tmp(j);
206  }
207  // Put the result in qdot_out_reduced if mimic joints exist, otherwise in qdot_out
208  if (num_mimic_joints > 0)
209  qdot_out_reduced_locked(i) = sum;
210  else
211  qdot_out_locked(i) = sum;
212  }
213  ROS_DEBUG_STREAM_NAMED("kdl", "Solution:");
214 
215  if (num_mimic_joints > 0)
216  {
217  for (i = 0; i < chain.getNrOfJoints() - num_mimic_joints - num_redundant_joints; ++i)
218  {
220  }
221  for (i = 0; i < chain.getNrOfJoints(); ++i)
222  {
223  qdot_out(i) = qdot_out_reduced(mimic_joints_[i].map_index) * mimic_joints_[i].multiplier;
224  }
225  }
226  else
227  {
228  for (i = 0; i < chain.getNrOfJoints() - num_redundant_joints; ++i)
229  {
230  qdot_out(locked_joints_map_index[i]) = qdot_out_locked(i);
231  }
232  }
233  // Reset the flag
234  // redundant_joints_locked = false;
235  // return the return value of the svd decomposition
236  return ret;
237 }
238 
239 int ChainIkSolverVel_pinv_mimic::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
240 {
242  return CartToJntRedundant(q_in, v_in, qdot_out);
243 
244  // Let the ChainJntToJacSolver calculate the jacobian "jac" for
245  // the current joint positions "q_in". This will include the mimic joints
246  if (num_mimic_joints > 0)
247  {
248  jnt2jac.JntToJac(q_in, jac);
249  // Now compute the actual jacobian that involves only the active DOFs
251  }
252  else
254 
255  // Do a singular value decomposition of "jac" with maximum
256  // iterations "maxiter", put the results in "U", "S" and "V"
257  // jac = U*S*Vt
258 
259  int ret;
260  if (!position_ik)
261  ret = svd.calculate(jac_reduced, U, S, V, maxiter);
262  else
265 
266  double sum;
267  unsigned int i, j;
268 
269  // We have to calculate qdot_out = jac_pinv*v_in
270  // Using the svd decomposition this becomes(jac_pinv=V*S_pinv*Ut):
271  // qdot_out = V*S_pinv*Ut*v_in
272 
273  unsigned int rows;
274  if (!position_ik)
275  rows = jac_reduced.rows();
276  else
277  rows = 3;
278 
279  // first we calculate Ut*v_in
280  for (i = 0; i < jac_reduced.columns(); i++)
281  {
282  sum = 0.0;
283  for (j = 0; j < rows; j++)
284  {
285  if (!position_ik)
286  sum += U[j](i) * v_in(j);
287  else
288  sum += U_translate(j, i) * v_in(j);
289  }
290  // If the singular value is too small (<eps), don't invert it but
291  // set the inverted singular value to zero (truncated svd)
292  if (!position_ik)
293  tmp(i) = sum * (fabs(S(i)) < eps ? 0.0 : 1.0 / S(i));
294  else
295  tmp(i) = sum * (fabs(S_translate(i)) < eps ? 0.0 : 1.0 / S_translate(i));
296  }
297  // tmp is now: tmp=S_pinv*Ut*v_in, we still have to premultiply
298  // it with V to get qdot_out
299  for (i = 0; i < jac_reduced.columns(); i++)
300  {
301  sum = 0.0;
302  for (j = 0; j < jac_reduced.columns(); j++)
303  {
304  if (!position_ik)
305  sum += V[i](j) * tmp(j);
306  else
307  sum += V_translate(i, j) * tmp(j);
308  }
309  // Put the result in qdot_out_reduced if mimic joints exist, otherwise in qdot_out
310  if (num_mimic_joints > 0)
311  qdot_out_reduced(i) = sum;
312  else
313  qdot_out(i) = sum;
314  }
315  ROS_DEBUG_STREAM_NAMED("kdl", "Solution:");
316  if (num_mimic_joints > 0)
317  {
318  for (i = 0; i < chain.getNrOfJoints(); ++i)
319  {
320  qdot_out(i) = qdot_out_reduced(mimic_joints_[i].map_index) * mimic_joints_[i].multiplier;
321  }
322  }
323  // return the return value of the svd decomposition
324  return ret;
325 }
326 }
#define ROS_DEBUG_STREAM_NAMED(name, args)
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...
unsigned int columns() const
int svd_eigen_HH(const MatrixXd &A, MatrixXd &U, VectorXd &S, MatrixXd &V, VectorXd &tmp, int maxiter, double epsilon)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Matrix< double, 6, Eigen::Dynamic > data
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 JntToJac(const JntArray &q_in, Jacobian &jac, int segmentNR=-1)
Eigen::VectorXd data
void setColumn(unsigned int i, const Twist &t)
virtual int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
unsigned int getNrOfJoints() const
Twist getColumn(unsigned int i) const
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 ...
int calculate(const Jacobian &jac, std::vector< JntArray > &U, JntArray &w, std::vector< JntArray > &v, int maxiter)
bool jacToJacLocked(const Jacobian &jac, Jacobian &jac_locked)
#define ROS_ERROR(...)
unsigned int rows() const


moveit_kinematics
Author(s): Dave Coleman , Ioan Sucan , Sachin Chitta
autogenerated on Wed Jul 18 2018 02:49:10