trac_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 
32 #include <trac_ik/trac_ik.hpp>
33 #include <boost/date_time.hpp>
34 #include <Eigen/Geometry>
35 #include <ros/ros.h>
36 #include <limits>
38 #include <urdf/model.h>
39 
40 namespace TRAC_IK
41 {
42 
43 TRAC_IK::TRAC_IK(const std::string& base_link, const std::string& tip_link, const std::string& URDF_param, double _maxtime, double _eps, SolveType _type) :
44  initialized(false),
45  eps(_eps),
46  maxtime(_maxtime),
47  solvetype(_type)
48 {
49 
50  ros::NodeHandle node_handle("~");
51 
52  urdf::Model robot_model;
53  std::string xml_string;
54 
55  std::string urdf_xml, full_urdf_xml;
56  node_handle.param("urdf_xml", urdf_xml, URDF_param);
57  node_handle.searchParam(urdf_xml, full_urdf_xml);
58 
59  ROS_DEBUG_NAMED("trac_ik", "Reading xml file from parameter server");
60  if (!node_handle.getParam(full_urdf_xml, xml_string))
61  {
62  ROS_FATAL_NAMED("trac_ik", "Could not load the xml from parameter server: %s", urdf_xml.c_str());
63  return;
64  }
65 
66  node_handle.param(full_urdf_xml, xml_string, std::string());
67  robot_model.initString(xml_string);
68 
69  ROS_DEBUG_STREAM_NAMED("trac_ik", "Reading joints and links from URDF");
70 
71  KDL::Tree tree;
72 
73  if (!kdl_parser::treeFromUrdfModel(robot_model, tree))
74  ROS_FATAL("Failed to extract kdl tree from xml robot description");
75 
76  if (!tree.getChain(base_link, tip_link, chain))
77  ROS_FATAL("Couldn't find chain %s to %s", base_link.c_str(), tip_link.c_str());
78 
79  std::vector<KDL::Segment> chain_segs = chain.segments;
80 
81  urdf::JointConstSharedPtr joint;
82 
83  std::vector<double> l_bounds, u_bounds;
84 
85  lb.resize(chain.getNrOfJoints());
86  ub.resize(chain.getNrOfJoints());
87 
88  uint joint_num = 0;
89  for (unsigned int i = 0; i < chain_segs.size(); ++i)
90  {
91  joint = robot_model.getJoint(chain_segs[i].getJoint().getName());
92  if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
93  {
94  joint_num++;
95  float lower, upper;
96  int hasLimits;
97  if (joint->type != urdf::Joint::CONTINUOUS)
98  {
99  if (joint->safety)
100  {
101  lower = std::max(joint->limits->lower, joint->safety->soft_lower_limit);
102  upper = std::min(joint->limits->upper, joint->safety->soft_upper_limit);
103  }
104  else
105  {
106  lower = joint->limits->lower;
107  upper = joint->limits->upper;
108  }
109  hasLimits = 1;
110  }
111  else
112  {
113  hasLimits = 0;
114  }
115  if (hasLimits)
116  {
117  lb(joint_num - 1) = lower;
118  ub(joint_num - 1) = upper;
119  }
120  else
121  {
122  lb(joint_num - 1) = std::numeric_limits<float>::lowest();
123  ub(joint_num - 1) = std::numeric_limits<float>::max();
124  }
125  ROS_DEBUG_STREAM_NAMED("trac_ik", "IK Using joint " << joint->name << " " << lb(joint_num - 1) << " " << ub(joint_num - 1));
126  }
127  }
128 
129  initialize();
130 }
131 
132 
133 TRAC_IK::TRAC_IK(const KDL::Chain& _chain, const KDL::JntArray& _q_min, const KDL::JntArray& _q_max, double _maxtime, double _eps, SolveType _type):
134  initialized(false),
135  chain(_chain),
136  lb(_q_min),
137  ub(_q_max),
138  eps(_eps),
139  maxtime(_maxtime),
140  solvetype(_type)
141 {
142  initialize();
143 }
144 
145 void TRAC_IK::initialize()
146 {
147 
148  assert(chain.getNrOfJoints() == lb.data.size());
149  assert(chain.getNrOfJoints() == ub.data.size());
150 
151  jacsolver.reset(new KDL::ChainJntToJacSolver(chain));
152  nl_solver.reset(new NLOPT_IK::NLOPT_IK(chain, lb, ub, maxtime, eps, NLOPT_IK::SumSq));
153  iksolver.reset(new KDL::ChainIkSolverPos_TL(chain, lb, ub, maxtime, eps, true, true));
154 
155  for (uint i = 0; i < chain.segments.size(); i++)
156  {
157  std::string type = chain.segments[i].getJoint().getTypeName();
158  if (type.find("Rot") != std::string::npos)
159  {
160  if (ub(types.size()) >= std::numeric_limits<float>::max() &&
161  lb(types.size()) <= std::numeric_limits<float>::lowest())
162  types.push_back(KDL::BasicJointType::Continuous);
163  else
164  types.push_back(KDL::BasicJointType::RotJoint);
165  }
166  else if (type.find("Trans") != std::string::npos)
167  types.push_back(KDL::BasicJointType::TransJoint);
168  }
169 
170  assert(types.size() == lb.data.size());
171 
172  initialized = true;
173 }
174 
175 bool TRAC_IK::unique_solution(const KDL::JntArray& sol)
176 {
177 
178  for (uint i = 0; i < solutions.size(); i++)
179  if (myEqual(sol, solutions[i]))
180  return false;
181  return true;
182 
183 }
184 
185 inline void normalizeAngle(double& val, const double& min, const double& max)
186 {
187  if (val > max)
188  {
189  //Find actual angle offset
190  double diffangle = fmod(val - max, 2 * M_PI);
191  // Add that to upper bound and go back a full rotation
192  val = max + diffangle - 2 * M_PI;
193  }
194 
195  if (val < min)
196  {
197  //Find actual angle offset
198  double diffangle = fmod(min - val, 2 * M_PI);
199  // Add that to upper bound and go back a full rotation
200  val = min - diffangle + 2 * M_PI;
201  }
202 }
203 
204 inline void normalizeAngle(double& val, const double& target)
205 {
206  double new_target = target + M_PI;
207  if (val > new_target)
208  {
209  //Find actual angle offset
210  double diffangle = fmod(val - new_target, 2 * M_PI);
211  // Add that to upper bound and go back a full rotation
212  val = new_target + diffangle - 2 * M_PI;
213  }
214 
215  new_target = target - M_PI;
216  if (val < new_target)
217  {
218  //Find actual angle offset
219  double diffangle = fmod(new_target - val, 2 * M_PI);
220  // Add that to upper bound and go back a full rotation
221  val = new_target - diffangle + 2 * M_PI;
222  }
223 }
224 
225 
226 template<typename T1, typename T2>
227 bool TRAC_IK::runSolver(T1& solver, T2& other_solver,
228  const KDL::JntArray &q_init,
229  const KDL::Frame &p_in)
230 {
231  KDL::JntArray q_out;
232 
233  double fulltime = maxtime;
234  KDL::JntArray seed = q_init;
235 
236  boost::posix_time::time_duration timediff;
237  double time_left;
238 
239  while (true)
240  {
241  timediff = boost::posix_time::microsec_clock::local_time() - start_time;
242  time_left = fulltime - timediff.total_nanoseconds() / 1000000000.0;
243 
244  if (time_left <= 0)
245  break;
246 
247  solver.setMaxtime(time_left);
248 
249  int RC = solver.CartToJnt(seed, p_in, q_out, bounds);
250  if (RC >= 0)
251  {
252  switch (solvetype)
253  {
254  case Manip1:
255  case Manip2:
256  normalize_limits(q_init, q_out);
257  break;
258  default:
259  normalize_seed(q_init, q_out);
260  break;
261  }
262  mtx_.lock();
263  if (unique_solution(q_out))
264  {
265  solutions.push_back(q_out);
266  uint curr_size = solutions.size();
267  errors.resize(curr_size);
268  mtx_.unlock();
269  double err, penalty;
270  switch (solvetype)
271  {
272  case Manip1:
273  penalty = manipPenalty(q_out);
274  err = penalty * TRAC_IK::ManipValue1(q_out);
275  break;
276  case Manip2:
277  penalty = manipPenalty(q_out);
278  err = penalty * TRAC_IK::ManipValue2(q_out);
279  break;
280  default:
281  err = TRAC_IK::JointErr(q_init, q_out);
282  break;
283  }
284  mtx_.lock();
285  errors[curr_size - 1] = std::make_pair(err, curr_size - 1);
286  }
287  mtx_.unlock();
288  }
289 
290  if (!solutions.empty() && solvetype == Speed)
291  break;
292 
293  for (unsigned int j = 0; j < seed.data.size(); j++)
294  if (types[j] == KDL::BasicJointType::Continuous)
295  seed(j) = fRand(q_init(j) - 2 * M_PI, q_init(j) + 2 * M_PI);
296  else
297  seed(j) = fRand(lb(j), ub(j));
298  }
299  other_solver.abort();
300 
301  solver.setMaxtime(fulltime);
302 
303  return true;
304 }
305 
306 
307 void TRAC_IK::normalize_seed(const KDL::JntArray& seed, KDL::JntArray& solution)
308 {
309  // Make sure rotational joint values are within 1 revolution of seed; then
310  // ensure joint limits are met.
311 
312  bool improved = false;
313 
314  for (uint i = 0; i < lb.data.size(); i++)
315  {
316 
317  if (types[i] == KDL::BasicJointType::TransJoint)
318  continue;
319 
320  double target = seed(i);
321  double val = solution(i);
322 
323  normalizeAngle(val, target);
324 
325  if (types[i] == KDL::BasicJointType::Continuous)
326  {
327  solution(i) = val;
328  continue;
329  }
330 
331  normalizeAngle(val, lb(i), ub(i));
332 
333  solution(i) = val;
334  }
335 }
336 
337 void TRAC_IK::normalize_limits(const KDL::JntArray& seed, KDL::JntArray& solution)
338 {
339  // Make sure rotational joint values are within 1 revolution of middle of
340  // limits; then ensure joint limits are met.
341 
342  bool improved = false;
343 
344  for (uint i = 0; i < lb.data.size(); i++)
345  {
346 
347  if (types[i] == KDL::BasicJointType::TransJoint)
348  continue;
349 
350  double target = seed(i);
351 
352  if (types[i] == KDL::BasicJointType::RotJoint && types[i] != KDL::BasicJointType::Continuous)
353  target = (ub(i) + lb(i)) / 2.0;
354 
355  double val = solution(i);
356 
357  normalizeAngle(val, target);
358 
359  if (types[i] == KDL::BasicJointType::Continuous)
360  {
361  solution(i) = val;
362  continue;
363  }
364 
365  normalizeAngle(val, lb(i), ub(i));
366 
367  solution(i) = val;
368  }
369 
370 }
371 
372 
373 double TRAC_IK::manipPenalty(const KDL::JntArray& arr)
374 {
375  double penalty = 1.0;
376  for (uint i = 0; i < arr.data.size(); i++)
377  {
378  if (types[i] == KDL::BasicJointType::Continuous)
379  continue;
380  double range = ub(i) - lb(i);
381  penalty *= ((arr(i) - lb(i)) * (ub(i) - arr(i)) / (range * range));
382  }
383  return std::max(0.0, 1.0 - exp(-1 * penalty));
384 }
385 
386 
387 double TRAC_IK::ManipValue1(const KDL::JntArray& arr)
388 {
389  KDL::Jacobian jac(arr.data.size());
390 
391  jacsolver->JntToJac(arr, jac);
392 
393  Eigen::JacobiSVD<Eigen::MatrixXd> svdsolver(jac.data);
394  Eigen::MatrixXd singular_values = svdsolver.singularValues();
395 
396  double error = 1.0;
397  for (unsigned int i = 0; i < singular_values.rows(); ++i)
398  error *= singular_values(i, 0);
399  return error;
400 }
401 
402 double TRAC_IK::ManipValue2(const KDL::JntArray& arr)
403 {
404  KDL::Jacobian jac(arr.data.size());
405 
406  jacsolver->JntToJac(arr, jac);
407 
408  Eigen::JacobiSVD<Eigen::MatrixXd> svdsolver(jac.data);
409  Eigen::MatrixXd singular_values = svdsolver.singularValues();
410 
411  return singular_values.minCoeff() / singular_values.maxCoeff();
412 }
413 
414 
415 int TRAC_IK::CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &p_in, KDL::JntArray &q_out, const KDL::Twist& _bounds)
416 {
417 
418  if (!initialized)
419  {
420  ROS_ERROR("TRAC-IK was not properly initialized with a valid chain or limits. IK cannot proceed");
421  return -1;
422  }
423 
424 
425  start_time = boost::posix_time::microsec_clock::local_time();
426 
427  nl_solver->reset();
428  iksolver->reset();
429 
430  solutions.clear();
431  errors.clear();
432 
433  bounds = _bounds;
434 
435  task1 = std::thread(&TRAC_IK::runKDL, this, q_init, p_in);
436  task2 = std::thread(&TRAC_IK::runNLOPT, this, q_init, p_in);
437 
438  task1.join();
439  task2.join();
440 
441  if (solutions.empty())
442  {
443  q_out = q_init;
444  return -3;
445  }
446 
447  switch (solvetype)
448  {
449  case Manip1:
450  case Manip2:
451  std::sort(errors.rbegin(), errors.rend()); // rbegin/rend to sort by max
452  break;
453  default:
454  std::sort(errors.begin(), errors.end());
455  break;
456  }
457 
458  q_out = solutions[errors[0].second];
459 
460  return solutions.size();
461 }
462 
463 
464 TRAC_IK::~TRAC_IK()
465 {
466  if (task1.joinable())
467  task1.join();
468  if (task2.joinable())
469  task2.join();
470 }
471 }
#define ROS_FATAL(...)
#define ROS_DEBUG_STREAM_NAMED(name, args)
URDF_EXPORT bool initString(const std::string &xmlstring)
KDL_PARSER_PUBLIC bool treeFromUrdfModel(const urdf::ModelInterface &robot_model, KDL::Tree &tree)
ROSCONSOLE_DECL void initialize()
#define M_PI
Definition: math3d.h:42
std::string getName(void *handle)
#define ROS_DEBUG_NAMED(name,...)
bool searchParam(const std::string &key, std::string &result) const
bool param(const std::string &param_name, T &param_val, const T &default_val) const
Eigen::VectorXd data
bool getChain(const std::string &chain_root, const std::string &chain_tip, Chain &chain) const
bool getParam(const std::string &key, std::string &s) const
#define ROS_FATAL_NAMED(name,...)
#define ROS_ERROR(...)
void normalizeAngle(double &val, const double &min, const double &max)
Definition: trac_ik.cpp:185
dual_quaternion exp(dual_quaternion a)


trac_ik_lib
Author(s): Patrick Beeson, Barrett Ames
autogenerated on Sat Sep 19 2020 03:40:57