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