nlopt_ik.cpp
Go to the documentation of this file.
1 /********************************************************************************
2 Copyright (c) 2015, TRACLabs, Inc.
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without modification,
6  are permitted provided that the following conditions are met:
7 
8  1. Redistributions of source code must retain the above copyright notice,
9  this list of conditions and the following disclaimer.
10 
11  2. Redistributions in binary form must reproduce the above copyright notice,
12  this list of conditions and the following disclaimer in the documentation
13  and/or other materials provided with the distribution.
14 
15  3. Neither the name of the copyright holder nor the names of its contributors
16  may be used to endorse or promote products derived from this software
17  without specific prior written permission.
18 
19 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
20 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
21 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
22 IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
23 INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
24 BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
25 DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
26 LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
27 OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
28 OF THE POSSIBILITY OF SUCH DAMAGE.
29 ********************************************************************************/
30 
31 #include <trac_ik/nlopt_ik.hpp>
32 #include <ros/ros.h>
33 #include <limits>
34 #include <boost/date_time.hpp>
36 #include <cmath>
37 
38 
39 
40 namespace NLOPT_IK
41 {
42 
44 
45 double minfunc(const std::vector<double>& x, std::vector<double>& grad, void* data)
46 {
47  // Auxilory function to minimize (Sum of Squared joint angle error
48  // from the requested configuration). Because we wanted a Class
49  // without static members, but NLOpt library does not support
50  // passing methods of Classes, we use these auxilary functions.
51 
52  NLOPT_IK *c = (NLOPT_IK *) data;
53 
54  return c->minJoints(x, grad);
55 }
56 
57 double minfuncDQ(const std::vector<double>& x, std::vector<double>& grad, void* data)
58 {
59  // Auxilory function to minimize (Sum of Squared joint angle error
60  // from the requested configuration). Because we wanted a Class
61  // without static members, but NLOpt library does not support
62  // passing methods of Classes, we use these auxilary functions.
63  NLOPT_IK *c = (NLOPT_IK *) data;
64 
65  std::vector<double> vals(x);
66 
67  double jump = boost::math::tools::epsilon<float>();
68  double result[1];
69  c->cartDQError(vals, result);
70 
71  if (!grad.empty())
72  {
73  double v1[1];
74  for (uint i = 0; i < x.size(); i++)
75  {
76  double original = vals[i];
77 
78  vals[i] = original + jump;
79  c->cartDQError(vals, v1);
80 
81  vals[i] = original;
82  grad[i] = (v1[0] - result[0]) / (2 * jump);
83  }
84  }
85 
86  return result[0];
87 }
88 
89 
90 double minfuncSumSquared(const std::vector<double>& x, std::vector<double>& grad, void* data)
91 {
92  // Auxilory function to minimize (Sum of Squared joint angle error
93  // from the requested configuration). Because we wanted a Class
94  // without static members, but NLOpt library does not support
95  // passing methods of Classes, we use these auxilary functions.
96 
97  NLOPT_IK *c = (NLOPT_IK *) data;
98 
99  std::vector<double> vals(x);
100 
101  double jump = boost::math::tools::epsilon<float>();
102  double result[1];
103  c->cartSumSquaredError(vals, result);
104 
105  if (!grad.empty())
106  {
107  double v1[1];
108  for (uint i = 0; i < x.size(); i++)
109  {
110  double original = vals[i];
111 
112  vals[i] = original + jump;
113  c->cartSumSquaredError(vals, v1);
114 
115  vals[i] = original;
116  grad[i] = (v1[0] - result[0]) / (2.0 * jump);
117  }
118  }
119 
120  return result[0];
121 }
122 
123 
124 double minfuncL2(const std::vector<double>& x, std::vector<double>& grad, void* data)
125 {
126  // Auxilory function to minimize (Sum of Squared joint angle error
127  // from the requested configuration). Because we wanted a Class
128  // without static members, but NLOpt library does not support
129  // passing methods of Classes, we use these auxilary functions.
130 
131  NLOPT_IK *c = (NLOPT_IK *) data;
132 
133  std::vector<double> vals(x);
134 
135  double jump = boost::math::tools::epsilon<float>();
136  double result[1];
137  c->cartL2NormError(vals, result);
138 
139  if (!grad.empty())
140  {
141  double v1[1];
142  for (uint i = 0; i < x.size(); i++)
143  {
144  double original = vals[i];
145 
146  vals[i] = original + jump;
147  c->cartL2NormError(vals, v1);
148 
149  vals[i] = original;
150  grad[i] = (v1[0] - result[0]) / (2.0 * jump);
151  }
152  }
153 
154  return result[0];
155 }
156 
157 
158 
159 void constrainfuncm(uint m, double* result, uint n, const double* x, double* grad, void* data)
160 {
161  //Equality constraint auxilary function for Euclidean distance .
162  //This also uses a small walk to approximate the gradient of the
163  //constraint function at the current joint angles.
164 
165  NLOPT_IK *c = (NLOPT_IK *) data;
166 
167  std::vector<double> vals(n);
168 
169  for (uint i = 0; i < n; i++)
170  {
171  vals[i] = x[i];
172  }
173 
174  double jump = boost::math::tools::epsilon<float>();
175 
176  c->cartSumSquaredError(vals, result);
177 
178  if (grad != NULL)
179  {
180  std::vector<double> v1(m);
181  for (uint i = 0; i < n; i++)
182  {
183  double o = vals[i];
184  vals[i] = o + jump;
185  c->cartSumSquaredError(vals, v1.data());
186  vals[i] = o;
187  for (uint j = 0; j < m; j++)
188  {
189  grad[j * n + i] = (v1[j] - result[j]) / (2 * jump);
190  }
191  }
192  }
193 }
194 
195 
196 NLOPT_IK::NLOPT_IK(const KDL::Chain& _chain, const KDL::JntArray& _q_min, const KDL::JntArray& _q_max, double _maxtime, double _eps, OptType _type):
197  chain(_chain), fksolver(chain), maxtime(_maxtime), eps(std::abs(_eps)), TYPE(_type)
198 {
199  assert(chain.getNrOfJoints() == _q_min.data.size());
200  assert(chain.getNrOfJoints() == _q_max.data.size());
201 
202  //Constructor for an IK Class. Takes in a Chain to operate on,
203  //the min and max joint limits, an (optional) maximum number of
204  //iterations, and an (optional) desired error.
205  reset();
206 
207  if (chain.getNrOfJoints() < 2)
208  {
209  ROS_WARN_THROTTLE(1.0, "NLOpt_IK can only be run for chains of length 2 or more");
210  return;
211  }
212  opt = nlopt::opt(nlopt::LD_SLSQP, _chain.getNrOfJoints());
213 
214  for (uint i = 0; i < chain.getNrOfJoints(); i++)
215  {
216  lb.push_back(_q_min(i));
217  ub.push_back(_q_max(i));
218  }
219 
220  for (uint i = 0; i < chain.segments.size(); i++)
221  {
222  std::string type = chain.segments[i].getJoint().getTypeName();
223  if (type.find("Rot") != std::string::npos)
224  {
225  if (_q_max(types.size()) >= std::numeric_limits<float>::max() &&
226  _q_min(types.size()) <= std::numeric_limits<float>::lowest())
227  types.push_back(KDL::BasicJointType::Continuous);
228  else
229  types.push_back(KDL::BasicJointType::RotJoint);
230  }
231  else if (type.find("Trans") != std::string::npos)
232  types.push_back(KDL::BasicJointType::TransJoint);
233  }
234 
235  assert(types.size() == lb.size());
236 
237  std::vector<double> tolerance(1, boost::math::tools::epsilon<float>());
238  opt.set_xtol_abs(tolerance[0]);
239 
240 
241  switch (TYPE)
242  {
243  case Joint:
244  opt.set_min_objective(minfunc, this);
245  opt.add_equality_mconstraint(constrainfuncm, this, tolerance);
246  break;
247  case DualQuat:
248  opt.set_min_objective(minfuncDQ, this);
249  break;
250  case SumSq:
251  opt.set_min_objective(minfuncSumSquared, this);
252  break;
253  case L2:
254  opt.set_min_objective(minfuncL2, this);
255  break;
256  }
257 }
258 
259 
260 double NLOPT_IK::minJoints(const std::vector<double>& x, std::vector<double>& grad)
261 {
262  // Actual function to compute the error between the current joint
263  // configuration and the desired. The SSE is easy to provide a
264  // closed form gradient for.
265 
266  bool gradient = !grad.empty();
267 
268  double err = 0;
269  for (uint i = 0; i < x.size(); i++)
270  {
271  err += pow(x[i] - des[i], 2);
272  if (gradient)
273  grad[i] = 2.0 * (x[i] - des[i]);
274  }
275 
276  return err;
277 
278 }
279 
280 
281 void NLOPT_IK::cartSumSquaredError(const std::vector<double>& x, double error[])
282 {
283  // Actual function to compute Euclidean distance error. This uses
284  // the KDL Forward Kinematics solver to compute the Cartesian pose
285  // of the current joint configuration and compares that to the
286  // desired Cartesian pose for the IK solve.
287 
288  if (aborted || progress != -3)
289  {
290  opt.force_stop();
291  return;
292  }
293 
294 
295  KDL::JntArray q(x.size());
296 
297  for (uint i = 0; i < x.size(); i++)
298  q(i) = x[i];
299 
300  int rc = fksolver.JntToCart(q, currentPose);
301 
302  if (rc < 0)
303  ROS_FATAL_STREAM("KDL FKSolver is failing: " << q.data);
304 
305  if (std::isnan(currentPose.p.x()))
306  {
307  ROS_ERROR("NaNs from NLOpt!!");
308  error[0] = std::numeric_limits<float>::max();
309  progress = -1;
310  return;
311  }
312 
313  KDL::Twist delta_twist = KDL::diffRelative(targetPose, currentPose);
314 
315  for (int i = 0; i < 6; i++)
316  {
317  if (std::abs(delta_twist[i]) <= std::abs(bounds[i]))
318  delta_twist[i] = 0.0;
319  }
320 
321  error[0] = KDL::dot(delta_twist.vel, delta_twist.vel) + KDL::dot(delta_twist.rot, delta_twist.rot);
322 
323  if (KDL::Equal(delta_twist, KDL::Twist::Zero(), eps))
324  {
325  progress = 1;
326  best_x = x;
327  return;
328  }
329 }
330 
331 
332 
333 void NLOPT_IK::cartL2NormError(const std::vector<double>& x, double error[])
334 {
335  // Actual function to compute Euclidean distance error. This uses
336  // the KDL Forward Kinematics solver to compute the Cartesian pose
337  // of the current joint configuration and compares that to the
338  // desired Cartesian pose for the IK solve.
339 
340  if (aborted || progress != -3)
341  {
342  opt.force_stop();
343  return;
344  }
345 
346  KDL::JntArray q(x.size());
347 
348  for (uint i = 0; i < x.size(); i++)
349  q(i) = x[i];
350 
351  int rc = fksolver.JntToCart(q, currentPose);
352 
353  if (rc < 0)
354  ROS_FATAL_STREAM("KDL FKSolver is failing: " << q.data);
355 
356 
357  if (std::isnan(currentPose.p.x()))
358  {
359  ROS_ERROR("NaNs from NLOpt!!");
360  error[0] = std::numeric_limits<float>::max();
361  progress = -1;
362  return;
363  }
364 
365  KDL::Twist delta_twist = KDL::diffRelative(targetPose, currentPose);
366 
367  for (int i = 0; i < 6; i++)
368  {
369  if (std::abs(delta_twist[i]) <= std::abs(bounds[i]))
370  delta_twist[i] = 0.0;
371  }
372 
373  error[0] = std::sqrt(KDL::dot(delta_twist.vel, delta_twist.vel) + KDL::dot(delta_twist.rot, delta_twist.rot));
374 
375  if (KDL::Equal(delta_twist, KDL::Twist::Zero(), eps))
376  {
377  progress = 1;
378  best_x = x;
379  return;
380  }
381 }
382 
383 
384 
385 
386 void NLOPT_IK::cartDQError(const std::vector<double>& x, double error[])
387 {
388  // Actual function to compute Euclidean distance error. This uses
389  // the KDL Forward Kinematics solver to compute the Cartesian pose
390  // of the current joint configuration and compares that to the
391  // desired Cartesian pose for the IK solve.
392 
393  if (aborted || progress != -3)
394  {
395  opt.force_stop();
396  return;
397  }
398 
399  KDL::JntArray q(x.size());
400 
401  for (uint i = 0; i < x.size(); i++)
402  q(i) = x[i];
403 
404  int rc = fksolver.JntToCart(q, currentPose);
405 
406  if (rc < 0)
407  ROS_FATAL_STREAM("KDL FKSolver is failing: " << q.data);
408 
409 
410  if (std::isnan(currentPose.p.x()))
411  {
412  ROS_ERROR("NaNs from NLOpt!!");
413  error[0] = std::numeric_limits<float>::max();
414  progress = -1;
415  return;
416  }
417 
418  KDL::Twist delta_twist = KDL::diffRelative(targetPose, currentPose);
419 
420  for (int i = 0; i < 6; i++)
421  {
422  if (std::abs(delta_twist[i]) <= std::abs(bounds[i]))
423  delta_twist[i] = 0.0;
424  }
425 
426  math3d::matrix3x3<double> currentRotationMatrix(currentPose.M.data);
427  math3d::quaternion<double> currentQuaternion = math3d::rot_matrix_to_quaternion<double>(currentRotationMatrix);
428  math3d::point3d currentTranslation(currentPose.p.data);
429  dual_quaternion currentDQ = dual_quaternion::rigid_transformation(currentQuaternion, currentTranslation);
430 
431  dual_quaternion errorDQ = (currentDQ * !targetDQ).normalize();
432  errorDQ.log();
433  error[0] = 4.0f * dot(errorDQ, errorDQ);
434 
435 
436  if (KDL::Equal(delta_twist, KDL::Twist::Zero(), eps))
437  {
438  progress = 1;
439  best_x = x;
440  return;
441  }
442 }
443 
444 
445 int NLOPT_IK::CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &p_in, KDL::JntArray &q_out, const KDL::Twist _bounds, const KDL::JntArray& q_desired)
446 {
447  // User command to start an IK solve. Takes in a seed
448  // configuration, a Cartesian pose, and (optional) a desired
449  // configuration. If the desired is not provided, the seed is
450  // used. Outputs the joint configuration found that solves the
451  // IK.
452 
453  // Returns -3 if a configuration could not be found within the eps
454  // set up in the constructor.
455 
456  boost::posix_time::ptime start_time = boost::posix_time::microsec_clock::local_time();
457  boost::posix_time::time_duration diff;
458 
459  bounds = _bounds;
460  q_out = q_init;
461 
462  if (chain.getNrOfJoints() < 2)
463  {
464  ROS_ERROR_THROTTLE(1.0, "NLOpt_IK can only be run for chains of length 2 or more");
465  return -3;
466  }
467 
468  if (q_init.data.size() != types.size())
469  {
470  ROS_ERROR_THROTTLE(1.0, "IK seeded with wrong number of joints. Expected %d but got %d", (int)types.size(), (int)q_init.data.size());
471  return -3;
472  }
473 
474  opt.set_maxtime(maxtime);
475 
476 
477  double minf; /* the minimum objective value, upon return */
478 
479  targetPose = p_in;
480 
481  if (TYPE == 1) // DQ
482  {
483  math3d::matrix3x3<double> targetRotationMatrix(targetPose.M.data);
484  math3d::quaternion<double> targetQuaternion = math3d::rot_matrix_to_quaternion<double>(targetRotationMatrix);
485  math3d::point3d targetTranslation(targetPose.p.data);
486  targetDQ = dual_quaternion::rigid_transformation(targetQuaternion, targetTranslation);
487  }
488  // else if (TYPE == 1)
489  // {
490  // z_target = targetPose*z_up;
491  // x_target = targetPose*x_out;
492  // y_target = targetPose*y_out;
493  // }
494 
495 
496  // fksolver.JntToCart(q_init,currentPose);
497 
498  std::vector<double> x(chain.getNrOfJoints());
499 
500  for (uint i = 0; i < x.size(); i++)
501  {
502  x[i] = q_init(i);
503 
504  if (types[i] == KDL::BasicJointType::Continuous)
505  continue;
506 
507  if (types[i] == KDL::BasicJointType::TransJoint)
508  {
509  x[i] = std::min(x[i], ub[i]);
510  x[i] = std::max(x[i], lb[i]);
511  }
512  else
513  {
514 
515  // Below is to handle bad seeds outside of limits
516 
517  if (x[i] > ub[i])
518  {
519  //Find actual angle offset
520  double diffangle = fmod(x[i] - ub[i], 2 * M_PI);
521  // Add that to upper bound and go back a full rotation
522  x[i] = ub[i] + diffangle - 2 * M_PI;
523  }
524 
525  if (x[i] < lb[i])
526  {
527  //Find actual angle offset
528  double diffangle = fmod(lb[i] - x[i], 2 * M_PI);
529  // Subtract that from lower bound and go forward a full rotation
530  x[i] = lb[i] - diffangle + 2 * M_PI;
531  }
532 
533  if (x[i] > ub[i])
534  x[i] = (ub[i] + lb[i]) / 2.0;
535  }
536  }
537 
538  best_x = x;
539  progress = -3;
540 
541  std::vector<double> artificial_lower_limits(lb.size());
542 
543  for (uint i = 0; i < lb.size(); i++)
544  if (types[i] == KDL::BasicJointType::Continuous)
545  artificial_lower_limits[i] = best_x[i] - 2 * M_PI;
546  else if (types[i] == KDL::BasicJointType::TransJoint)
547  artificial_lower_limits[i] = lb[i];
548  else
549  artificial_lower_limits[i] = std::max(lb[i], best_x[i] - 2 * M_PI);
550 
551  opt.set_lower_bounds(artificial_lower_limits);
552 
553  std::vector<double> artificial_upper_limits(lb.size());
554 
555  for (uint i = 0; i < ub.size(); i++)
556  if (types[i] == KDL::BasicJointType::Continuous)
557  artificial_upper_limits[i] = best_x[i] + 2 * M_PI;
558  else if (types[i] == KDL::BasicJointType::TransJoint)
559  artificial_upper_limits[i] = ub[i];
560  else
561  artificial_upper_limits[i] = std::min(ub[i], best_x[i] + 2 * M_PI);
562 
563  opt.set_upper_bounds(artificial_upper_limits);
564 
565  if (q_desired.data.size() == 0)
566  {
567  des = x;
568  }
569  else
570  {
571  des.resize(x.size());
572  for (uint i = 0; i < des.size(); i++)
573  des[i] = q_desired(i);
574  }
575 
576  try
577  {
578  opt.optimize(x, minf);
579  }
580  catch (...)
581  {
582  }
583 
584  if (progress == -1) // Got NaNs
585  progress = -3;
586 
587 
588  if (!aborted && progress < 0)
589  {
590 
591  double time_left;
592  diff = boost::posix_time::microsec_clock::local_time() - start_time;
593  time_left = maxtime - diff.total_nanoseconds() / 1000000000.0;
594 
595  while (time_left > 0 && !aborted && progress < 0)
596  {
597 
598  for (uint i = 0; i < x.size(); i++)
599  x[i] = fRand(artificial_lower_limits[i], artificial_upper_limits[i]);
600 
601  opt.set_maxtime(time_left);
602 
603  try
604  {
605  opt.optimize(x, minf);
606  }
607  catch (...) {}
608 
609  if (progress == -1) // Got NaNs
610  progress = -3;
611 
612  diff = boost::posix_time::microsec_clock::local_time() - start_time;
613  time_left = maxtime - diff.total_nanoseconds() / 1000000000.0;
614  }
615  }
616 
617 
618  for (uint i = 0; i < x.size(); i++)
619  {
620  q_out(i) = best_x[i];
621  }
622 
623  return progress;
624 
625 }
626 
627 
628 }
double dot(const dual_quaternion &a, const dual_quaternion &b)
static Twist Zero()
#define ROS_WARN_THROTTLE(rate,...)
double minfuncSumSquared(const std::vector< double > &x, std::vector< double > &grad, void *data)
Definition: nlopt_ik.cpp:90
Vector vel
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
#define M_PI
Definition: math3d.h:42
IMETHOD Twist diffRelative(const Frame &F_a_b1, const Frame &F_a_b2, double dt=1)
Definition: kdl_tl.hpp:118
static dual_quaternion rigid_transformation(const quaternion< double > &r, const point3d &t)
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
double minfunc(const std::vector< double > &x, std::vector< double > &grad, void *data)
Definition: nlopt_ik.cpp:45
dual_quaternion targetDQ
Definition: nlopt_ik.cpp:43
#define ROS_ERROR_THROTTLE(rate,...)
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
#define ROS_FATAL_STREAM(args)
Vector rot
Eigen::VectorXd data
unsigned int getNrOfJoints() const
void constrainfuncm(uint m, double *result, uint n, const double *x, double *grad, void *data)
Definition: nlopt_ik.cpp:159
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
dual_quaternion & log()
void normalize(quaternion< T > &q)
Definition: math3d.h:689
double minfuncDQ(const std::vector< double > &x, std::vector< double > &grad, void *data)
Definition: nlopt_ik.cpp:57
#define ROS_ERROR(...)
double minfuncL2(const std::vector< double > &x, std::vector< double > &grad, void *data)
Definition: nlopt_ik.cpp:124


trac_ik_lib
Author(s): Patrick Beeson, Barrett Ames
autogenerated on Tue Jun 1 2021 02:38:35