ikfast61_moveit_plugin_template.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2013, Dave Coleman, CU Boulder; Jeremy Zoss, SwRI; David Butterworth, KAIST; Mathias Lüdtke, Fraunhofer
6  *IPA
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above copyright
16  * notice, this list of conditions and the following disclaimer in the
17  * documentation and/or other materials provided with the distribution.
18  * * Neither the name of the all of the author's companies nor the names of its
19  * contributors may be used to endorse or promote products derived from
20  * this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
23  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
24  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
25  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
26  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
27  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
28  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
29  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
30  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
31  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  *********************************************************************/
35 
36 /*
37  * IKFast Plugin Template for moveit
38  *
39  * AUTO-GENERATED by create_ikfast_moveit_plugin.py in arm_kinematics_tools
40  * You should run create_ikfast_moveit_plugin.py to generate your own plugin.
41  *
42  * Creates a kinematics plugin using the output of IKFast from OpenRAVE.
43  * This plugin and the move_group node can be used as a general
44  * kinematics service, from within the moveit planning environment, or in
45  * your own ROS node.
46  *
47  */
48 
49 #include <ros/ros.h>
51 #include <urdf/model.h>
52 #include <tf_conversions/tf_kdl.h>
53 
54 // Need a floating point tolerance when checking joint limits, in case the joint starts at limit
55 const double LIMIT_TOLERANCE = .0000001;
58 {
61 };
62 
64 {
65 #define IKFAST_NO_MAIN // Don't include main() from IKFast
66 
73 {
74  IKP_None = 0,
75  IKP_Transform6D = 0x67000001,
76  IKP_Rotation3D = 0x34000002,
77  IKP_Translation3D = 0x33000003,
78  IKP_Direction3D = 0x23000004,
79  IKP_Ray4D = 0x46000005,
80  IKP_Lookat3D = 0x23000006,
82  IKP_TranslationXY2D = 0x22000008,
86  IKP_TranslationLocalGlobal6D = 0x3600000a,
89 
91  IKP_TranslationYAxisAngle4D = 0x4400000c,
94  IKP_TranslationZAxisAngle4D = 0x4400000d,
97 
101  IKP_TranslationYAxisAngleXNorm4D = 0x4400000f,
105  IKP_TranslationZAxisAngleYNorm4D = 0x44000010,
109 
114 
116  0x00008000,
133 
134  IKP_UniqueIdMask = 0x0000ffff,
135  IKP_CustomDataBit = 0x00010000,
136 };
138 
139 // struct for storing and sorting solutions
141 {
142  std::vector<double> value;
144 
145  bool operator<(const LimitObeyingSol& a) const
146  {
147  return dist_from_seed < a.dist_from_seed;
148  }
149 };
150 
151 // Code generated by IKFast56/61
152 #include "_ROBOT_NAME___GROUP_NAME__ikfast_solver.cpp"
153 
155 {
156  std::vector<std::string> joint_names_;
157  std::vector<double> joint_min_vector_;
158  std::vector<double> joint_max_vector_;
159  std::vector<bool> joint_has_limits_vector_;
160  std::vector<std::string> link_names_;
161  size_t num_joints_;
162  std::vector<int> free_params_;
163  bool active_; // Internal variable that indicates whether solvers are configured and ready
164 
165  const std::vector<std::string>& getJointNames() const
166  {
167  return joint_names_;
168  }
169  const std::vector<std::string>& getLinkNames() const
170  {
171  return link_names_;
172  }
173 
174 public:
178  IKFastKinematicsPlugin() : active_(false)
179  {
180  srand(time(NULL));
181  supported_methods_.push_back(kinematics::DiscretizationMethods::NO_DISCRETIZATION);
182  supported_methods_.push_back(kinematics::DiscretizationMethods::ALL_DISCRETIZED);
183  supported_methods_.push_back(kinematics::DiscretizationMethods::ALL_RANDOM_SAMPLED);
184  }
185 
195  // Returns the IK solution that is within joint limits closest to ik_seed_state
196  bool getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
197  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
199 
215  bool getPositionIK(const std::vector<geometry_msgs::Pose>& ik_poses, const std::vector<double>& ik_seed_state,
216  std::vector<std::vector<double>>& solutions, kinematics::KinematicsResult& result,
218 
227  bool searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
228  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
230 
240  bool searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
241  const std::vector<double>& consistency_limits, std::vector<double>& solution,
242  moveit_msgs::MoveItErrorCodes& error_code,
244 
253  bool searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
254  std::vector<double>& solution, const IKCallbackFn& solution_callback,
255  moveit_msgs::MoveItErrorCodes& error_code,
257 
268  bool searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
269  const std::vector<double>& consistency_limits, std::vector<double>& solution,
270  const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
272 
281  bool getPositionFK(const std::vector<std::string>& link_names, const std::vector<double>& joint_angles,
282  std::vector<geometry_msgs::Pose>& poses) const;
283 
293  void setSearchDiscretization(const std::map<int, double>& discretization);
294 
298  bool setRedundantJoints(const std::vector<unsigned int>& redundant_joint_indices);
299 
300 private:
301  bool initialize(const std::string& robot_description, const std::string& group_name, const std::string& base_name,
302  const std::string& tip_name, double search_discretization);
303 
308  int solve(KDL::Frame& pose_frame, const std::vector<double>& vfree, IkSolutionList<IkReal>& solutions) const;
309 
313  void getSolution(const IkSolutionList<IkReal>& solutions, int i, std::vector<double>& solution) const;
314 
318  void getSolution(const IkSolutionList<IkReal>& solutions, const std::vector<double>& ik_seed_state, int i,
319  std::vector<double>& solution) const;
320 
321  double harmonize(const std::vector<double>& ik_seed_state, std::vector<double>& solution) const;
322  // void getOrderedSolutions(const std::vector<double> &ik_seed_state, std::vector<std::vector<double> >& solslist);
323  void getClosestSolution(const IkSolutionList<IkReal>& solutions, const std::vector<double>& ik_seed_state,
324  std::vector<double>& solution) const;
325  void fillFreeParams(int count, int* array);
326  bool getCount(int& count, const int& max_count, const int& min_count) const;
327 
334  bool sampleRedundantJoint(kinematics::DiscretizationMethod method, std::vector<double>& sampled_joint_vals) const;
335 
336 }; // end class
337 
338 bool IKFastKinematicsPlugin::initialize(const std::string& robot_description, const std::string& group_name,
339  const std::string& base_name, const std::string& tip_name,
340  double search_discretization)
341 {
342  setValues(robot_description, group_name, base_name, tip_name, search_discretization);
343 
344  ros::NodeHandle node_handle("~/" + group_name);
345 
346  std::string robot;
347  lookupParam("robot", robot, std::string());
348 
349  // IKFast56/61
350  fillFreeParams(GetNumFreeParameters(), GetFreeParameters());
351  num_joints_ = GetNumJoints();
352 
353  if (free_params_.size() > 1)
354  {
355  ROS_FATAL("Only one free joint parameter supported!");
356  return false;
357  }
358  else if (free_params_.size() == 1)
359  {
360  redundant_joint_indices_.clear();
361  redundant_joint_indices_.push_back(free_params_[0]);
362  KinematicsBase::setSearchDiscretization(DEFAULT_SEARCH_DISCRETIZATION);
363  }
364 
365  urdf::Model robot_model;
366  std::string xml_string;
367 
368  std::string urdf_xml, full_urdf_xml;
369  lookupParam("urdf_xml", urdf_xml, robot_description);
370  node_handle.searchParam(urdf_xml, full_urdf_xml);
371 
372  ROS_DEBUG_NAMED("ikfast", "Reading xml file from parameter server");
373  if (!node_handle.getParam(full_urdf_xml, xml_string))
374  {
375  ROS_FATAL_NAMED("ikfast", "Could not load the xml from parameter server: %s", urdf_xml.c_str());
376  return false;
377  }
378 
379  robot_model.initString(xml_string);
380 
381  ROS_DEBUG_STREAM_NAMED("ikfast", "Reading joints and links from URDF");
382 
383  urdf::LinkConstSharedPtr link = robot_model.getLink(getTipFrame());
384  while (link->name != base_frame_ && joint_names_.size() <= num_joints_)
385  {
386  ROS_DEBUG_NAMED("ikfast", "Link %s", link->name.c_str());
387  link_names_.push_back(link->name);
388  urdf::JointSharedPtr joint = link->parent_joint;
389  if (joint)
390  {
391  if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
392  {
393  ROS_DEBUG_STREAM_NAMED("ikfast", "Adding joint " << joint->name);
394 
395  joint_names_.push_back(joint->name);
396  float lower, upper;
397  int hasLimits;
398  if (joint->type != urdf::Joint::CONTINUOUS)
399  {
400  if (joint->safety)
401  {
402  lower = joint->safety->soft_lower_limit;
403  upper = joint->safety->soft_upper_limit;
404  }
405  else
406  {
407  lower = joint->limits->lower;
408  upper = joint->limits->upper;
409  }
410  hasLimits = 1;
411  }
412  else
413  {
414  lower = -M_PI;
415  upper = M_PI;
416  hasLimits = 0;
417  }
418  if (hasLimits)
419  {
420  joint_has_limits_vector_.push_back(true);
421  joint_min_vector_.push_back(lower);
422  joint_max_vector_.push_back(upper);
423  }
424  else
425  {
426  joint_has_limits_vector_.push_back(false);
427  joint_min_vector_.push_back(-M_PI);
428  joint_max_vector_.push_back(M_PI);
429  }
430  }
431  }
432  else
433  {
434  ROS_WARN_NAMED("ikfast", "no joint corresponding to %s", link->name.c_str());
435  }
436  link = link->getParent();
437  }
438 
439  if (joint_names_.size() != num_joints_)
440  {
441  ROS_FATAL_STREAM_NAMED("ikfast", "Joint numbers mismatch: URDF has " << joint_names_.size() << " and IKFast has "
442  << num_joints_);
443  return false;
444  }
445 
446  std::reverse(link_names_.begin(), link_names_.end());
447  std::reverse(joint_names_.begin(), joint_names_.end());
448  std::reverse(joint_min_vector_.begin(), joint_min_vector_.end());
449  std::reverse(joint_max_vector_.begin(), joint_max_vector_.end());
450  std::reverse(joint_has_limits_vector_.begin(), joint_has_limits_vector_.end());
451 
452  for (size_t i = 0; i < num_joints_; ++i)
453  ROS_DEBUG_STREAM_NAMED("ikfast", joint_names_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i]
454  << " " << joint_has_limits_vector_[i]);
455 
456  active_ = true;
457  return true;
458 }
459 
460 void IKFastKinematicsPlugin::setSearchDiscretization(const std::map<int, double>& discretization)
461 {
462  if (discretization.empty())
463  {
464  ROS_ERROR("The 'discretization' map is empty");
465  return;
466  }
467 
468  if (redundant_joint_indices_.empty())
469  {
470  ROS_ERROR_STREAM("This group's solver doesn't support redundant joints");
471  return;
472  }
473 
474  if (discretization.begin()->first != redundant_joint_indices_[0])
475  {
476  std::string redundant_joint = joint_names_[free_params_[0]];
477  ROS_ERROR_STREAM("Attempted to discretize a non-redundant joint "
478  << discretization.begin()->first << ", only joint '" << redundant_joint << "' with index "
479  << redundant_joint_indices_[0] << " is redundant.");
480  return;
481  }
482 
483  if (discretization.begin()->second <= 0.0)
484  {
485  ROS_ERROR_STREAM("Discretization can not takes values that are <= 0");
486  return;
487  }
488 
489  redundant_joint_discretization_.clear();
490  redundant_joint_discretization_[redundant_joint_indices_[0]] = discretization.begin()->second;
491 }
492 
493 bool IKFastKinematicsPlugin::setRedundantJoints(const std::vector<unsigned int>& redundant_joint_indices)
494 {
495  ROS_ERROR_STREAM("Changing the redundant joints isn't permitted by this group's solver ");
496  return false;
497 }
498 
499 int IKFastKinematicsPlugin::solve(KDL::Frame& pose_frame, const std::vector<double>& vfree,
500  IkSolutionList<IkReal>& solutions) const
501 {
502  // IKFast56/61
503  solutions.Clear();
504 
505  double trans[3];
506  trans[0] = pose_frame.p[0]; //-.18;
507  trans[1] = pose_frame.p[1];
508  trans[2] = pose_frame.p[2];
509 
510  KDL::Rotation mult;
511  KDL::Vector direction;
512 
513  switch (GetIkType())
514  {
515  case IKP_Transform6D:
516  case IKP_Translation3D:
517  // For **Transform6D**, eerot is 9 values for the 3x3 rotation matrix. For **Translation3D**, these are ignored.
518 
519  mult = pose_frame.M;
520 
521  double vals[9];
522  vals[0] = mult(0, 0);
523  vals[1] = mult(0, 1);
524  vals[2] = mult(0, 2);
525  vals[3] = mult(1, 0);
526  vals[4] = mult(1, 1);
527  vals[5] = mult(1, 2);
528  vals[6] = mult(2, 0);
529  vals[7] = mult(2, 1);
530  vals[8] = mult(2, 2);
531 
532  // IKFast56/61
533  ComputeIk(trans, vals, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
534  return solutions.GetNumSolutions();
535 
536  case IKP_Direction3D:
537  case IKP_Ray4D:
539  // For **Direction3D**, **Ray4D**, and **TranslationDirection5D**, the first 3 values represent the target
540  // direction.
541 
542  direction = pose_frame.M * KDL::Vector(0, 0, 1);
543  ComputeIk(trans, direction.data, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
544  return solutions.GetNumSolutions();
545 
549  // For **TranslationXAxisAngle4D**, **TranslationYAxisAngle4D**, and **TranslationZAxisAngle4D**, the first value
550  // represents the angle.
551  ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet.");
552  return 0;
553 
555  // For **TranslationLocalGlobal6D**, the diagonal elements ([0],[4],[8]) are the local translation inside the end
556  // effector coordinate system.
557  ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet.");
558  return 0;
559 
560  case IKP_Rotation3D:
561  case IKP_Lookat3D:
562  case IKP_TranslationXY2D:
567  ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet.");
568  return 0;
569 
570  default:
571  ROS_ERROR_NAMED("ikfast", "Unknown IkParameterizationType! Was the solver generated with an incompatible version "
572  "of Openrave?");
573  return 0;
574  }
575 }
576 
577 void IKFastKinematicsPlugin::getSolution(const IkSolutionList<IkReal>& solutions, int i,
578  std::vector<double>& solution) const
579 {
580  solution.clear();
581  solution.resize(num_joints_);
582 
583  // IKFast56/61
584  const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
585  std::vector<IkReal> vsolfree(sol.GetFree().size());
586  sol.GetSolution(&solution[0], vsolfree.size() > 0 ? &vsolfree[0] : NULL);
587 
588  // std::cout << "solution " << i << ":" ;
589  // for(int j=0;j<num_joints_; ++j)
590  // std::cout << " " << solution[j];
591  // std::cout << std::endl;
592 
593  // ROS_ERROR("%f %d",solution[2],vsolfree.size());
594 }
595 
596 void IKFastKinematicsPlugin::getSolution(const IkSolutionList<IkReal>& solutions,
597  const std::vector<double>& ik_seed_state, int i,
598  std::vector<double>& solution) const
599 {
600  solution.clear();
601  solution.resize(num_joints_);
602 
603  // IKFast56/61
604  const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
605  std::vector<IkReal> vsolfree(sol.GetFree().size());
606  sol.GetSolution(&solution[0], vsolfree.size() > 0 ? &vsolfree[0] : NULL);
607 
608  // rotate joints by +/-360° where it is possible and useful
609  for (std::size_t i = 0; i < num_joints_; ++i)
610  {
611  if (joint_has_limits_vector_[i])
612  {
613  double signed_distance = solution[i] - ik_seed_state[i];
614  while (signed_distance > M_PI && solution[i] - 2 * M_PI > (joint_min_vector_[i] - LIMIT_TOLERANCE))
615  {
616  signed_distance -= 2 * M_PI;
617  solution[i] -= 2 * M_PI;
618  }
619  while (signed_distance < -M_PI && solution[i] + 2 * M_PI < (joint_max_vector_[i] + LIMIT_TOLERANCE))
620  {
621  signed_distance += 2 * M_PI;
622  solution[i] += 2 * M_PI;
623  }
624  }
625  }
626 }
627 
628 double IKFastKinematicsPlugin::harmonize(const std::vector<double>& ik_seed_state, std::vector<double>& solution) const
629 {
630  double dist_sqr = 0;
631  std::vector<double> ss = ik_seed_state;
632  for (size_t i = 0; i < ik_seed_state.size(); ++i)
633  {
634  while (ss[i] > 2 * M_PI)
635  {
636  ss[i] -= 2 * M_PI;
637  }
638  while (ss[i] < 2 * M_PI)
639  {
640  ss[i] += 2 * M_PI;
641  }
642  while (solution[i] > 2 * M_PI)
643  {
644  solution[i] -= 2 * M_PI;
645  }
646  while (solution[i] < 2 * M_PI)
647  {
648  solution[i] += 2 * M_PI;
649  }
650  dist_sqr += fabs(ik_seed_state[i] - solution[i]);
651  }
652  return dist_sqr;
653 }
654 
655 // void IKFastKinematicsPlugin::getOrderedSolutions(const std::vector<double> &ik_seed_state,
656 // std::vector<std::vector<double> >& solslist)
657 // {
658 // std::vector<double>
659 // double mindist = 0;
660 // int minindex = -1;
661 // std::vector<double> sol;
662 // for(size_t i=0;i<solslist.size();++i){
663 // getSolution(i,sol);
664 // double dist = harmonize(ik_seed_state, sol);
665 // //std::cout << "dist[" << i << "]= " << dist << std::endl;
666 // if(minindex == -1 || dist<mindist){
667 // minindex = i;
668 // mindist = dist;
669 // }
670 // }
671 // if(minindex >= 0){
672 // getSolution(minindex,solution);
673 // harmonize(ik_seed_state, solution);
674 // index = minindex;
675 // }
676 // }
677 
678 void IKFastKinematicsPlugin::getClosestSolution(const IkSolutionList<IkReal>& solutions,
679  const std::vector<double>& ik_seed_state,
680  std::vector<double>& solution) const
681 {
682  double mindist = DBL_MAX;
683  int minindex = -1;
684  std::vector<double> sol;
685 
686  // IKFast56/61
687  for (size_t i = 0; i < solutions.GetNumSolutions(); ++i)
688  {
689  getSolution(solutions, i, sol);
690  double dist = harmonize(ik_seed_state, sol);
691  ROS_INFO_STREAM_NAMED("ikfast", "Dist " << i << " dist " << dist);
692  // std::cout << "dist[" << i << "]= " << dist << std::endl;
693  if (minindex == -1 || dist < mindist)
694  {
695  minindex = i;
696  mindist = dist;
697  }
698  }
699  if (minindex >= 0)
700  {
701  getSolution(solutions, minindex, solution);
702  harmonize(ik_seed_state, solution);
703  }
704 }
705 
706 void IKFastKinematicsPlugin::fillFreeParams(int count, int* array)
707 {
708  free_params_.clear();
709  for (int i = 0; i < count; ++i)
710  free_params_.push_back(array[i]);
711 }
712 
713 bool IKFastKinematicsPlugin::getCount(int& count, const int& max_count, const int& min_count) const
714 {
715  if (count > 0)
716  {
717  if (-count >= min_count)
718  {
719  count = -count;
720  return true;
721  }
722  else if (count + 1 <= max_count)
723  {
724  count = count + 1;
725  return true;
726  }
727  else
728  {
729  return false;
730  }
731  }
732  else
733  {
734  if (1 - count <= max_count)
735  {
736  count = 1 - count;
737  return true;
738  }
739  else if (count - 1 >= min_count)
740  {
741  count = count - 1;
742  return true;
743  }
744  else
745  return false;
746  }
747 }
748 
749 bool IKFastKinematicsPlugin::getPositionFK(const std::vector<std::string>& link_names,
750  const std::vector<double>& joint_angles,
751  std::vector<geometry_msgs::Pose>& poses) const
752 {
753  if (GetIkType() != IKP_Transform6D)
754  {
755  // ComputeFk() is the inverse function of ComputeIk(), so the format of
756  // eerot differs depending on IK type. The Transform6D IK type is the only
757  // one for which a 3x3 rotation matrix is returned, which means we can only
758  // compute FK for that IK type.
759  ROS_ERROR_NAMED("ikfast", "Can only compute FK for Transform6D IK type!");
760  return false;
761  }
762 
763  KDL::Frame p_out;
764  if (link_names.size() == 0)
765  {
766  ROS_WARN_STREAM_NAMED("ikfast", "Link names with nothing");
767  return false;
768  }
769 
770  if (link_names.size() != 1 || link_names[0] != getTipFrame())
771  {
772  ROS_ERROR_NAMED("ikfast", "Can compute FK for %s only", getTipFrame().c_str());
773  return false;
774  }
775 
776  bool valid = true;
777 
778  IkReal eerot[9], eetrans[3];
779  IkReal angles[joint_angles.size()];
780  for (unsigned char i = 0; i < joint_angles.size(); i++)
781  angles[i] = joint_angles[i];
782 
783  // IKFast56/61
784  ComputeFk(angles, eetrans, eerot);
785 
786  for (int i = 0; i < 3; ++i)
787  p_out.p.data[i] = eetrans[i];
788 
789  for (int i = 0; i < 9; ++i)
790  p_out.M.data[i] = eerot[i];
791 
792  poses.resize(1);
793  tf::poseKDLToMsg(p_out, poses[0]);
794 
795  return valid;
796 }
797 
798 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
799  const std::vector<double>& ik_seed_state, double timeout,
800  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
801  const kinematics::KinematicsQueryOptions& options) const
802 {
803  const IKCallbackFn solution_callback = 0;
804  std::vector<double> consistency_limits;
805 
806  return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
807  options);
808 }
809 
810 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
811  const std::vector<double>& ik_seed_state, double timeout,
812  const std::vector<double>& consistency_limits,
813  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
814  const kinematics::KinematicsQueryOptions& options) const
815 {
816  const IKCallbackFn solution_callback = 0;
817  return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
818  options);
819 }
820 
821 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
822  const std::vector<double>& ik_seed_state, double timeout,
823  std::vector<double>& solution, const IKCallbackFn& solution_callback,
824  moveit_msgs::MoveItErrorCodes& error_code,
825  const kinematics::KinematicsQueryOptions& options) const
826 {
827  std::vector<double> consistency_limits;
828  return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
829  options);
830 }
831 
832 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
833  const std::vector<double>& ik_seed_state, double timeout,
834  const std::vector<double>& consistency_limits,
835  std::vector<double>& solution, const IKCallbackFn& solution_callback,
836  moveit_msgs::MoveItErrorCodes& error_code,
837  const kinematics::KinematicsQueryOptions& options) const
838 {
839  ROS_DEBUG_STREAM_NAMED("ikfast", "searchPositionIK");
840 
842  SEARCH_MODE search_mode = _SEARCH_MODE_;
843 
844  // Check if there are no redundant joints
845  if (free_params_.size() == 0)
846  {
847  ROS_DEBUG_STREAM_NAMED("ikfast", "No need to search since no free params/redundant joints");
848 
849  std::vector<geometry_msgs::Pose> ik_poses(1, ik_pose);
850  std::vector<std::vector<double>> solutions;
851  kinematics::KinematicsResult kinematic_result;
852  // Find all IK solution within joint limits
853  if (!getPositionIK(ik_poses, ik_seed_state, solutions, kinematic_result, options))
854  {
855  ROS_DEBUG_STREAM_NAMED("ikfast", "No solution whatsoever");
856  error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
857  return false;
858  }
859 
860  // sort solutions by their distance to the seed
861  std::vector<LimitObeyingSol> solutions_obey_limits;
862  for (std::size_t i = 0; i < solutions.size(); ++i)
863  {
864  double dist_from_seed = 0.0;
865  for (std::size_t j = 0; j < ik_seed_state.size(); ++j)
866  {
867  dist_from_seed += fabs(ik_seed_state[j] - solutions[i][j]);
868  }
869 
870  solutions_obey_limits.push_back({ solutions[i], dist_from_seed });
871  }
872  std::sort(solutions_obey_limits.begin(), solutions_obey_limits.end());
873 
874  // check for collisions if a callback is provided
875  if (!solution_callback.empty())
876  {
877  for (std::size_t i = 0; i < solutions_obey_limits.size(); ++i)
878  {
879  solution_callback(ik_pose, solutions_obey_limits[i].value, error_code);
880  if (error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
881  {
882  solution = solutions_obey_limits[i].value;
883  ROS_DEBUG_STREAM_NAMED("ikfast", "Solution passes callback");
884  return true;
885  }
886  }
887 
888  ROS_DEBUG_STREAM_NAMED("ikfast", "Solution has error code " << error_code);
889  return false;
890  }
891  else
892  {
893  solution = solutions_obey_limits[0].value;
894  error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
895  return true; // no collision check callback provided
896  }
897  }
898 
899  // -------------------------------------------------------------------------------------------------
900  // Error Checking
901  if (!active_)
902  {
903  ROS_ERROR_STREAM_NAMED("ikfast", "Kinematics not active");
904  error_code.val = error_code.NO_IK_SOLUTION;
905  return false;
906  }
907 
908  if (ik_seed_state.size() != num_joints_)
909  {
910  ROS_ERROR_STREAM_NAMED("ikfast", "Seed state must have size " << num_joints_ << " instead of size "
911  << ik_seed_state.size());
912  error_code.val = error_code.NO_IK_SOLUTION;
913  return false;
914  }
915 
916  if (!consistency_limits.empty() && consistency_limits.size() != num_joints_)
917  {
918  ROS_ERROR_STREAM_NAMED("ikfast", "Consistency limits be empty or must have size "
919  << num_joints_ << " instead of size " << consistency_limits.size());
920  error_code.val = error_code.NO_IK_SOLUTION;
921  return false;
922  }
923 
924  // -------------------------------------------------------------------------------------------------
925  // Initialize
926 
927  KDL::Frame frame;
928  tf::poseMsgToKDL(ik_pose, frame);
929 
930  std::vector<double> vfree(free_params_.size());
931 
932  ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
933  int counter = 0;
934 
935  double initial_guess = ik_seed_state[free_params_[0]];
936  vfree[0] = initial_guess;
937 
938  // -------------------------------------------------------------------------------------------------
939  // Handle consitency limits if needed
940  int num_positive_increments;
941  int num_negative_increments;
942 
943  if (!consistency_limits.empty())
944  {
945  // moveit replaced consistency_limit (scalar) w/ consistency_limits (vector)
946  // Assume [0]th free_params element for now. Probably wrong.
947  double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess + consistency_limits[free_params_[0]]);
948  double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess - consistency_limits[free_params_[0]]);
949 
950  num_positive_increments = (int)((max_limit - initial_guess) / search_discretization_);
951  num_negative_increments = (int)((initial_guess - min_limit) / search_discretization_);
952  }
953  else // no consitency limits provided
954  {
955  num_positive_increments = (joint_max_vector_[free_params_[0]] - initial_guess) / search_discretization_;
956  num_negative_increments = (initial_guess - joint_min_vector_[free_params_[0]]) / search_discretization_;
957  }
958 
959  // -------------------------------------------------------------------------------------------------
960  // Begin searching
961 
962  ROS_DEBUG_STREAM_NAMED("ikfast", "Free param is " << free_params_[0] << " initial guess is " << initial_guess
963  << ", # positive increments: " << num_positive_increments
964  << ", # negative increments: " << num_negative_increments);
965  if ((search_mode & OPTIMIZE_MAX_JOINT) && (num_positive_increments + num_negative_increments) > 1000)
966  ROS_WARN_STREAM_ONCE_NAMED("ikfast", "Large search space, consider increasing the search discretization");
967 
968  double best_costs = -1.0;
969  std::vector<double> best_solution;
970  int nattempts = 0, nvalid = 0;
971 
972  while (true)
973  {
974  IkSolutionList<IkReal> solutions;
975  int numsol = solve(frame, vfree, solutions);
976 
977  ROS_DEBUG_STREAM_NAMED("ikfast", "Found " << numsol << " solutions from IKFast");
978 
979  // ROS_INFO("%f",vfree[0]);
980 
981  if (numsol > 0)
982  {
983  for (int s = 0; s < numsol; ++s)
984  {
985  nattempts++;
986  std::vector<double> sol;
987  getSolution(solutions, ik_seed_state, s, sol);
988 
989  bool obeys_limits = true;
990  for (unsigned int i = 0; i < sol.size(); i++)
991  {
992  if (joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i]))
993  {
994  obeys_limits = false;
995  break;
996  }
997  // ROS_INFO_STREAM_NAMED("ikfast","Num " << i << " value " << sol[i] << " has limits " <<
998  // joint_has_limits_vector_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i]);
999  }
1000  if (obeys_limits)
1001  {
1002  getSolution(solutions, ik_seed_state, s, solution);
1003 
1004  // This solution is within joint limits, now check if in collision (if callback provided)
1005  if (!solution_callback.empty())
1006  {
1007  solution_callback(ik_pose, solution, error_code);
1008  }
1009  else
1010  {
1011  error_code.val = error_code.SUCCESS;
1012  }
1013 
1014  if (error_code.val == error_code.SUCCESS)
1015  {
1016  nvalid++;
1017  if (search_mode & OPTIMIZE_MAX_JOINT)
1018  {
1019  // Costs for solution: Largest joint motion
1020  double costs = 0.0;
1021  for (unsigned int i = 0; i < solution.size(); i++)
1022  {
1023  double d = fabs(ik_seed_state[i] - solution[i]);
1024  if (d > costs)
1025  costs = d;
1026  }
1027  if (costs < best_costs || best_costs == -1.0)
1028  {
1029  best_costs = costs;
1030  best_solution = solution;
1031  }
1032  }
1033  else
1034  // Return first feasible solution
1035  return true;
1036  }
1037  }
1038  }
1039  }
1040 
1041  if (!getCount(counter, num_positive_increments, -num_negative_increments))
1042  {
1043  // Everything searched
1044  error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
1045  break;
1046  }
1047 
1048  vfree[0] = initial_guess + search_discretization_ * counter;
1049  // ROS_DEBUG_STREAM_NAMED("ikfast","Attempt " << counter << " with 0th free joint having value " << vfree[0]);
1050  }
1051 
1052  ROS_DEBUG_STREAM_NAMED("ikfast", "Valid solutions: " << nvalid << "/" << nattempts);
1053 
1054  if ((search_mode & OPTIMIZE_MAX_JOINT) && best_costs != -1.0)
1055  {
1056  solution = best_solution;
1057  error_code.val = error_code.SUCCESS;
1058  return true;
1059  }
1060 
1061  // No solution found
1062  error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
1063  return false;
1064 }
1065 
1066 // Used when there are no redundant joints - aka no free params
1067 bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
1068  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
1069  const kinematics::KinematicsQueryOptions& options) const
1070 {
1071  ROS_DEBUG_STREAM_NAMED("ikfast", "getPositionIK");
1072 
1073  if (!active_)
1074  {
1075  ROS_ERROR("kinematics not active");
1076  return false;
1077  }
1078 
1079  if (ik_seed_state.size() < num_joints_)
1080  {
1081  ROS_ERROR_STREAM("ik_seed_state only has " << ik_seed_state.size() << " entries, this ikfast solver requires "
1082  << num_joints_);
1083  return false;
1084  }
1085 
1086  // Check if seed is in bound
1087  for (std::size_t i = 0; i < ik_seed_state.size(); i++)
1088  {
1089  // Add tolerance to limit check
1090  if (joint_has_limits_vector_[i] && ((ik_seed_state[i] < (joint_min_vector_[i] - LIMIT_TOLERANCE)) ||
1091  (ik_seed_state[i] > (joint_max_vector_[i] + LIMIT_TOLERANCE))))
1092  {
1093  ROS_DEBUG_STREAM_NAMED("ikseed", "Not in limits! " << (int)i << " value " << ik_seed_state[i]
1094  << " has limit: " << joint_has_limits_vector_[i] << " being "
1095  << joint_min_vector_[i] << " to " << joint_max_vector_[i]);
1096  return false;
1097  }
1098  }
1099 
1100  std::vector<double> vfree(free_params_.size());
1101  for (std::size_t i = 0; i < free_params_.size(); ++i)
1102  {
1103  int p = free_params_[i];
1104  ROS_ERROR("%u is %f", p, ik_seed_state[p]); // DTC
1105  vfree[i] = ik_seed_state[p];
1106  }
1107 
1108  KDL::Frame frame;
1109  tf::poseMsgToKDL(ik_pose, frame);
1110 
1111  IkSolutionList<IkReal> solutions;
1112  int numsol = solve(frame, vfree, solutions);
1113  ROS_DEBUG_STREAM_NAMED("ikfast", "Found " << numsol << " solutions from IKFast");
1114 
1115  std::vector<LimitObeyingSol> solutions_obey_limits;
1116 
1117  if (numsol)
1118  {
1119  std::vector<double> solution_obey_limits;
1120  for (std::size_t s = 0; s < numsol; ++s)
1121  {
1122  std::vector<double> sol;
1123  getSolution(solutions, ik_seed_state, s, sol);
1124  ROS_DEBUG_NAMED("ikfast", "Sol %d: %e %e %e %e %e %e", (int)s, sol[0], sol[1], sol[2], sol[3], sol[4],
1125  sol[5]);
1126 
1127  bool obeys_limits = true;
1128  for (std::size_t i = 0; i < sol.size(); i++)
1129  {
1130  // Add tolerance to limit check
1131  if (joint_has_limits_vector_[i] && ((sol[i] < (joint_min_vector_[i] - LIMIT_TOLERANCE)) ||
1132  (sol[i] > (joint_max_vector_[i] + LIMIT_TOLERANCE))))
1133  {
1134  // One element of solution is not within limits
1135  obeys_limits = false;
1136  ROS_DEBUG_STREAM_NAMED("ikfast", "Not in limits! " << (int)i << " value " << sol[i] << " has limit: "
1137  << joint_has_limits_vector_[i] << " being "
1138  << joint_min_vector_[i] << " to " << joint_max_vector_[i]);
1139  break;
1140  }
1141  }
1142  if (obeys_limits)
1143  {
1144  // All elements of this solution obey limits
1145  getSolution(solutions, ik_seed_state, s, solution_obey_limits);
1146  double dist_from_seed = 0.0;
1147  for (std::size_t i = 0; i < ik_seed_state.size(); ++i)
1148  {
1149  dist_from_seed += fabs(ik_seed_state[i] - solution_obey_limits[i]);
1150  }
1151 
1152  solutions_obey_limits.push_back({ solution_obey_limits, dist_from_seed });
1153  }
1154  }
1155  }
1156  else
1157  {
1158  ROS_DEBUG_STREAM_NAMED("ikfast", "No IK solution");
1159  }
1160 
1161  // Sort the solutions under limits and find the one that is closest to ik_seed_state
1162  if (!solutions_obey_limits.empty())
1163  {
1164  std::sort(solutions_obey_limits.begin(), solutions_obey_limits.end());
1165  solution = solutions_obey_limits[0].value;
1166  error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
1167  return true;
1168  }
1169 
1170  error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
1171  return false;
1172 }
1173 
1174 bool IKFastKinematicsPlugin::getPositionIK(const std::vector<geometry_msgs::Pose>& ik_poses,
1175  const std::vector<double>& ik_seed_state,
1176  std::vector<std::vector<double>>& solutions,
1178  const kinematics::KinematicsQueryOptions& options) const
1179 {
1180  ROS_DEBUG_STREAM_NAMED("ikfast", "getPositionIK with multiple solutions");
1181 
1182  if (!active_)
1183  {
1184  ROS_ERROR("kinematics not active");
1186  return false;
1187  }
1188 
1189  if (ik_poses.empty())
1190  {
1191  ROS_ERROR("ik_poses is empty");
1193  return false;
1194  }
1195 
1196  if (ik_poses.size() > 1)
1197  {
1198  ROS_ERROR("ik_poses contains multiple entries, only one is allowed");
1200  return false;
1201  }
1202 
1203  if (ik_seed_state.size() < num_joints_)
1204  {
1205  ROS_ERROR_STREAM("ik_seed_state only has " << ik_seed_state.size() << " entries, this ikfast solver requires "
1206  << num_joints_);
1207  return false;
1208  }
1209 
1210  KDL::Frame frame;
1211  tf::poseMsgToKDL(ik_poses[0], frame);
1212 
1213  // solving ik
1214  std::vector<IkSolutionList<IkReal>> solution_set;
1215  IkSolutionList<IkReal> ik_solutions;
1216  std::vector<double> vfree;
1217  int numsol = 0;
1218  std::vector<double> sampled_joint_vals;
1219  if (!redundant_joint_indices_.empty())
1220  {
1221  // initializing from seed
1222  sampled_joint_vals.push_back(ik_seed_state[redundant_joint_indices_[0]]);
1223 
1224  // checking joint limits when using no discretization
1226  joint_has_limits_vector_[redundant_joint_indices_.front()])
1227  {
1228  double joint_min = joint_min_vector_[redundant_joint_indices_.front()];
1229  double joint_max = joint_max_vector_[redundant_joint_indices_.front()];
1230 
1231  double jv = sampled_joint_vals[0];
1232  if (!((jv > (joint_min - LIMIT_TOLERANCE)) && (jv < (joint_max + LIMIT_TOLERANCE))))
1233  {
1235  ROS_ERROR_STREAM("ik seed is out of bounds");
1236  return false;
1237  }
1238  }
1239 
1240  // computing all solutions sets for each sampled value of the redundant joint
1241  if (!sampleRedundantJoint(options.discretization_method, sampled_joint_vals))
1242  {
1244  return false;
1245  }
1246 
1247  for (unsigned int i = 0; i < sampled_joint_vals.size(); i++)
1248  {
1249  vfree.clear();
1250  vfree.push_back(sampled_joint_vals[i]);
1251  numsol += solve(frame, vfree, ik_solutions);
1252  solution_set.push_back(ik_solutions);
1253  }
1254  }
1255  else
1256  {
1257  // computing for single solution set
1258  numsol = solve(frame, vfree, ik_solutions);
1259  solution_set.push_back(ik_solutions);
1260  }
1261 
1262  ROS_DEBUG_STREAM_NAMED("ikfast", "Found " << numsol << " solutions from IKFast");
1263  bool solutions_found = false;
1264  if (numsol > 0)
1265  {
1266  /*
1267  Iterating through all solution sets and storing those that do not exceed joint limits.
1268  */
1269  for (unsigned int r = 0; r < solution_set.size(); r++)
1270  {
1271  ik_solutions = solution_set[r];
1272  numsol = ik_solutions.GetNumSolutions();
1273  for (int s = 0; s < numsol; ++s)
1274  {
1275  std::vector<double> sol;
1276  getSolution(ik_solutions, ik_seed_state, s, sol);
1277 
1278  bool obeys_limits = true;
1279  for (unsigned int i = 0; i < sol.size(); i++)
1280  {
1281  // Add tolerance to limit check
1282  if (joint_has_limits_vector_[i] && ((sol[i] < (joint_min_vector_[i] - LIMIT_TOLERANCE)) ||
1283  (sol[i] > (joint_max_vector_[i] + LIMIT_TOLERANCE))))
1284  {
1285  // One element of solution is not within limits
1286  obeys_limits = false;
1288  "ikfast", "Not in limits! " << i << " value " << sol[i] << " has limit: " << joint_has_limits_vector_[i]
1289  << " being " << joint_min_vector_[i] << " to " << joint_max_vector_[i]);
1290  break;
1291  }
1292  }
1293  if (obeys_limits)
1294  {
1295  // All elements of solution obey limits
1296  solutions_found = true;
1297  solutions.push_back(sol);
1298  }
1299  }
1300  }
1301 
1302  if (solutions_found)
1303  {
1305  return true;
1306  }
1307  }
1308  else
1309  {
1310  ROS_DEBUG_STREAM_NAMED("ikfast", "No IK solution");
1311  }
1312 
1314  return false;
1315 }
1316 
1318  std::vector<double>& sampled_joint_vals) const
1319 {
1320  double joint_min = -M_PI;
1321  double joint_max = M_PI;
1322  int index = redundant_joint_indices_.front();
1323  double joint_dscrt = redundant_joint_discretization_.at(index);
1324 
1325  if (joint_has_limits_vector_[redundant_joint_indices_.front()])
1326  {
1327  joint_min = joint_min_vector_[index];
1328  joint_max = joint_max_vector_[index];
1329  }
1330 
1331  switch (method)
1332  {
1334  {
1335  int steps = std::ceil((joint_max - joint_min) / joint_dscrt);
1336  for (unsigned int i = 0; i < steps; i++)
1337  {
1338  sampled_joint_vals.push_back(joint_min + joint_dscrt * i);
1339  }
1340  sampled_joint_vals.push_back(joint_max);
1341  }
1342  break;
1344  {
1345  int steps = std::ceil((joint_max - joint_min) / joint_dscrt);
1346  steps = steps > 0 ? steps : 1;
1347  double diff = joint_max - joint_min;
1348  for (int i = 0; i < steps; i++)
1349  {
1350  sampled_joint_vals.push_back(((diff * std::rand()) / (static_cast<double>(RAND_MAX))) + joint_min);
1351  }
1352  }
1353 
1354  break;
1356 
1357  break;
1358  default:
1359  ROS_ERROR_STREAM("Discretization method " << method << " is not supported");
1360  return false;
1361  }
1362 
1363  return true;
1364 }
1365 
1366 } // end namespace
1367 
1368 // register IKFastKinematicsPlugin as a KinematicsBase implementation
d
KinematicError kinematic_error
bool initialize(const std::string &robot_description, const std::string &group_name, const std::string &base_name, const std::string &tip_name, double search_discretization)
#define ROS_FATAL(...)
const std::vector< std::string > & getLinkNames() const
#define ROS_DEBUG_STREAM_NAMED(name, args)
#define ROS_ERROR_STREAM_NAMED(name, args)
bool initString(const std::string &xmlstring)
#define ROS_WARN_NAMED(name,...)
bool getCount(int &count, const int &max_count, const int &min_count) const
ROSCONSOLE_DECL void initialize()
XmlRpcServer s
void getClosestSolution(const IkSolutionList< IkReal > &solutions, const std::vector< double > &ik_seed_state, std::vector< double > &solution) const
void setSearchDiscretization(const std::map< int, double > &discretization)
Sets the discretization value for the redundant joint.
ray on end effector coordinate system reaches desired global ray
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
IkParameterizationType
The types of inverse kinematics parameterizations supported.
bool sampleRedundantJoint(kinematics::DiscretizationMethod method, std::vector< double > &sampled_joint_vals) const
samples the designated redundant joint using the chosen discretization method
local point on end effector origin reaches desired 3D global point
#define ROS_INFO_STREAM_NAMED(name, args)
#define M_PI
PLUGINLIB_EXPORT_CLASS(ikfast_kinematics_plugin::IKFastKinematicsPlugin, kinematics::KinematicsBase)
bool searchPositionIK(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_seed_state, double timeout, std::vector< double > &solution, moveit_msgs::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const
Given a desired pose of the end-effector, search for the joint angles required to reach it...
const double LIMIT_TOLERANCE
Rotation M
number of parameterizations (does not count IKP_None)
#define ROS_DEBUG_NAMED(name,...)
unsigned int index
bool setRedundantJoints(const std::vector< unsigned int > &redundant_joint_indices)
Overrides the default method to prevent changing the redundant joints.
#define ROS_WARN_STREAM_ONCE_NAMED(name, args)
end effector reaches desired 6D transformation
end effector reaches desired 3D rotation
#define ROS_FATAL_STREAM_NAMED(name, args)
options
bool searchParam(const std::string &key, std::string &result) const
double data[3]
void poseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k)
direction on end effector coordinate system points to desired 3D position
DiscretizationMethod discretization_method
double harmonize(const std::vector< double > &ik_seed_state, std::vector< double > &solution) const
def xml_string(rootXml, addHeader=True)
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn
direction on end effector coordinate system reaches desired direction
const std::vector< std::string > & getJointNames() const
bit is set if the data represents the time-derivate velocity of an IkParameterization ...
void poseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m)
bool getParam(const std::string &key, std::string &s) const
#define ROS_FATAL_NAMED(name,...)
#define ROS_ERROR_NAMED(name,...)
static Time now()
#define ROS_ERROR_STREAM(args)
bool getPositionIK(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_seed_state, std::vector< double > &solution, moveit_msgs::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const
Given a desired pose of the end-effector, compute the joint angles to reach it.
int solve(KDL::Frame &pose_frame, const std::vector< double > &vfree, IkSolutionList< IkReal > &solutions) const
Calls the IK solver from IKFast.
r
#define ROS_ERROR(...)
end effector origin reaches desired 3D translation
SEARCH_MODE
Search modes for searchPositionIK(), see there.
bool getPositionFK(const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::Pose > &poses) const
Given a set of joint angles and a set of links, compute their pose.
void getSolution(const IkSolutionList< IkReal > &solutions, int i, std::vector< double > &solution) const
Gets a specific solution from the set.
#define ROS_WARN_STREAM_NAMED(name, args)
double data[9]


moveit_kinematics
Author(s): Dave Coleman , Ioan Sucan , Sachin Chitta
autogenerated on Mon Jan 15 2018 03:51:32