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  const size_t num_joints_;
162  std::vector<int> free_params_;
163  bool active_; // Internal variable that indicates whether solvers are configured and ready
164  const std::string name_{ "ikfast" };
165 
166  const std::vector<std::string>& getJointNames() const
167  {
168  return joint_names_;
169  }
170  const std::vector<std::string>& getLinkNames() const
171  {
172  return link_names_;
173  }
174 
175 public:
179  IKFastKinematicsPlugin() : num_joints_(GetNumJoints()), active_(false)
180  {
181  srand(time(NULL));
182  supported_methods_.push_back(kinematics::DiscretizationMethods::NO_DISCRETIZATION);
183  supported_methods_.push_back(kinematics::DiscretizationMethods::ALL_DISCRETIZED);
184  supported_methods_.push_back(kinematics::DiscretizationMethods::ALL_RANDOM_SAMPLED);
185  }
186 
196  // Returns the IK solution that is within joint limits closest to ik_seed_state
197  bool getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
198  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
200 
216  bool getPositionIK(const std::vector<geometry_msgs::Pose>& ik_poses, const std::vector<double>& ik_seed_state,
217  std::vector<std::vector<double>>& solutions, kinematics::KinematicsResult& result,
219 
228  bool searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
229  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
231 
241  bool searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
242  const std::vector<double>& consistency_limits, std::vector<double>& solution,
243  moveit_msgs::MoveItErrorCodes& error_code,
245 
254  bool searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
255  std::vector<double>& solution, const IKCallbackFn& solution_callback,
256  moveit_msgs::MoveItErrorCodes& error_code,
258 
269  bool searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
270  const std::vector<double>& consistency_limits, std::vector<double>& solution,
271  const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
273 
282  bool getPositionFK(const std::vector<std::string>& link_names, const std::vector<double>& joint_angles,
283  std::vector<geometry_msgs::Pose>& poses) const;
284 
294  void setSearchDiscretization(const std::map<int, double>& discretization);
295 
299  bool setRedundantJoints(const std::vector<unsigned int>& redundant_joint_indices);
300 
301 private:
302  bool initialize(const std::string& robot_description, const std::string& group_name, const std::string& base_name,
303  const std::string& tip_name, double search_discretization);
304 
309  int solve(KDL::Frame& pose_frame, const std::vector<double>& vfree, IkSolutionList<IkReal>& solutions) const;
310 
314  void getSolution(const IkSolutionList<IkReal>& solutions, int i, std::vector<double>& solution) const;
315 
319  void getSolution(const IkSolutionList<IkReal>& solutions, const std::vector<double>& ik_seed_state, int i,
320  std::vector<double>& solution) const;
321 
322  double harmonize(const std::vector<double>& ik_seed_state, std::vector<double>& solution) const;
323  // void getOrderedSolutions(const std::vector<double> &ik_seed_state, std::vector<std::vector<double> >& solslist);
324  void getClosestSolution(const IkSolutionList<IkReal>& solutions, const std::vector<double>& ik_seed_state,
325  std::vector<double>& solution) const;
326  void fillFreeParams(int count, int* array);
327  bool getCount(int& count, const int& max_count, const int& min_count) const;
328 
335  bool sampleRedundantJoint(kinematics::DiscretizationMethod method, std::vector<double>& sampled_joint_vals) const;
336 
337 }; // end class
338 
339 bool IKFastKinematicsPlugin::initialize(const std::string& robot_description, const std::string& group_name,
340  const std::string& base_name, const std::string& tip_name,
341  double search_discretization)
342 {
343  setValues(robot_description, group_name, base_name, tip_name, search_discretization);
344 
345  ros::NodeHandle node_handle("~/" + group_name);
346 
347  std::string robot;
348  lookupParam("robot", robot, std::string());
349 
350  // IKFast56/61
351  fillFreeParams(GetNumFreeParameters(), GetFreeParameters());
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(name_, "Reading xml file from parameter server");
373  if (!node_handle.getParam(full_urdf_xml, xml_string))
374  {
375  ROS_FATAL_NAMED(name_, "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(name_, "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(name_, "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(name_, "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(name_, "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(name_, "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(name_, 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(name_, "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(name_, "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(name_, "IK for this IkParameterizationType not implemented yet.");
568  return 0;
569 
570  default:
571  ROS_ERROR_NAMED(name_, "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(name_, "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(name_, "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(name_, "Link names with nothing");
767  return false;
768  }
769 
770  if (link_names.size() != 1 || link_names[0] != getTipFrame())
771  {
772  ROS_ERROR_NAMED(name_, "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 
780  if (joint_angles.size() != num_joints_)
781  {
782  ROS_ERROR_NAMED(name_, "Unexpected number of joint angles");
783  return false;
784  }
785 
786  IkReal angles[num_joints_];
787  for (unsigned char i = 0; i < num_joints_; i++)
788  angles[i] = joint_angles[i];
789 
790  // IKFast56/61
791  ComputeFk(angles, eetrans, eerot);
792 
793  for (int i = 0; i < 3; ++i)
794  p_out.p.data[i] = eetrans[i];
795 
796  for (int i = 0; i < 9; ++i)
797  p_out.M.data[i] = eerot[i];
798 
799  poses.resize(1);
800  tf::poseKDLToMsg(p_out, poses[0]);
801 
802  return valid;
803 }
804 
805 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
806  const std::vector<double>& ik_seed_state, double timeout,
807  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
808  const kinematics::KinematicsQueryOptions& options) const
809 {
810  const IKCallbackFn solution_callback = 0;
811  std::vector<double> consistency_limits;
812 
813  return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
814  options);
815 }
816 
817 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
818  const std::vector<double>& ik_seed_state, double timeout,
819  const std::vector<double>& consistency_limits,
820  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
821  const kinematics::KinematicsQueryOptions& options) const
822 {
823  const IKCallbackFn solution_callback = 0;
824  return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
825  options);
826 }
827 
828 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
829  const std::vector<double>& ik_seed_state, double timeout,
830  std::vector<double>& solution, const IKCallbackFn& solution_callback,
831  moveit_msgs::MoveItErrorCodes& error_code,
832  const kinematics::KinematicsQueryOptions& options) const
833 {
834  std::vector<double> consistency_limits;
835  return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
836  options);
837 }
838 
839 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
840  const std::vector<double>& ik_seed_state, double timeout,
841  const std::vector<double>& consistency_limits,
842  std::vector<double>& solution, const IKCallbackFn& solution_callback,
843  moveit_msgs::MoveItErrorCodes& error_code,
844  const kinematics::KinematicsQueryOptions& options) const
845 {
846  ROS_DEBUG_STREAM_NAMED(name_, "searchPositionIK");
847 
849  SEARCH_MODE search_mode = _SEARCH_MODE_;
850 
851  // Check if there are no redundant joints
852  if (free_params_.size() == 0)
853  {
854  ROS_DEBUG_STREAM_NAMED(name_, "No need to search since no free params/redundant joints");
855 
856  std::vector<geometry_msgs::Pose> ik_poses(1, ik_pose);
857  std::vector<std::vector<double>> solutions;
858  kinematics::KinematicsResult kinematic_result;
859  // Find all IK solution within joint limits
860  if (!getPositionIK(ik_poses, ik_seed_state, solutions, kinematic_result, options))
861  {
862  ROS_DEBUG_STREAM_NAMED(name_, "No solution whatsoever");
863  error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
864  return false;
865  }
866 
867  // sort solutions by their distance to the seed
868  std::vector<LimitObeyingSol> solutions_obey_limits;
869  for (std::size_t i = 0; i < solutions.size(); ++i)
870  {
871  double dist_from_seed = 0.0;
872  for (std::size_t j = 0; j < ik_seed_state.size(); ++j)
873  {
874  dist_from_seed += fabs(ik_seed_state[j] - solutions[i][j]);
875  }
876 
877  solutions_obey_limits.push_back({ solutions[i], dist_from_seed });
878  }
879  std::sort(solutions_obey_limits.begin(), solutions_obey_limits.end());
880 
881  // check for collisions if a callback is provided
882  if (!solution_callback.empty())
883  {
884  for (std::size_t i = 0; i < solutions_obey_limits.size(); ++i)
885  {
886  solution_callback(ik_pose, solutions_obey_limits[i].value, error_code);
887  if (error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
888  {
889  solution = solutions_obey_limits[i].value;
890  ROS_DEBUG_STREAM_NAMED(name_, "Solution passes callback");
891  return true;
892  }
893  }
894 
895  ROS_DEBUG_STREAM_NAMED(name_, "Solution has error code " << error_code);
896  return false;
897  }
898  else
899  {
900  solution = solutions_obey_limits[0].value;
901  error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
902  return true; // no collision check callback provided
903  }
904  }
905 
906  // -------------------------------------------------------------------------------------------------
907  // Error Checking
908  if (!active_)
909  {
910  ROS_ERROR_STREAM_NAMED(name_, "Kinematics not active");
911  error_code.val = error_code.NO_IK_SOLUTION;
912  return false;
913  }
914 
915  if (ik_seed_state.size() != num_joints_)
916  {
917  ROS_ERROR_STREAM_NAMED(name_, "Seed state must have size " << num_joints_ << " instead of size "
918  << ik_seed_state.size());
919  error_code.val = error_code.NO_IK_SOLUTION;
920  return false;
921  }
922 
923  if (!consistency_limits.empty() && consistency_limits.size() != num_joints_)
924  {
925  ROS_ERROR_STREAM_NAMED(name_, "Consistency limits be empty or must have size " << num_joints_ << " instead of size "
926  << consistency_limits.size());
927  error_code.val = error_code.NO_IK_SOLUTION;
928  return false;
929  }
930 
931  // -------------------------------------------------------------------------------------------------
932  // Initialize
933 
934  KDL::Frame frame;
935  tf::poseMsgToKDL(ik_pose, frame);
936 
937  std::vector<double> vfree(free_params_.size());
938 
939  ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
940  int counter = 0;
941 
942  double initial_guess = ik_seed_state[free_params_[0]];
943  vfree[0] = initial_guess;
944 
945  // -------------------------------------------------------------------------------------------------
946  // Handle consitency limits if needed
947  int num_positive_increments;
948  int num_negative_increments;
949 
950  if (!consistency_limits.empty())
951  {
952  // moveit replaced consistency_limit (scalar) w/ consistency_limits (vector)
953  // Assume [0]th free_params element for now. Probably wrong.
954  double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess + consistency_limits[free_params_[0]]);
955  double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess - consistency_limits[free_params_[0]]);
956 
957  num_positive_increments = (int)((max_limit - initial_guess) / search_discretization_);
958  num_negative_increments = (int)((initial_guess - min_limit) / search_discretization_);
959  }
960  else // no consitency limits provided
961  {
962  num_positive_increments = (joint_max_vector_[free_params_[0]] - initial_guess) / search_discretization_;
963  num_negative_increments = (initial_guess - joint_min_vector_[free_params_[0]]) / search_discretization_;
964  }
965 
966  // -------------------------------------------------------------------------------------------------
967  // Begin searching
968 
969  ROS_DEBUG_STREAM_NAMED(name_, "Free param is " << free_params_[0] << " initial guess is " << initial_guess
970  << ", # positive increments: " << num_positive_increments
971  << ", # negative increments: " << num_negative_increments);
972  if ((search_mode & OPTIMIZE_MAX_JOINT) && (num_positive_increments + num_negative_increments) > 1000)
973  ROS_WARN_STREAM_ONCE_NAMED(name_, "Large search space, consider increasing the search discretization");
974 
975  double best_costs = -1.0;
976  std::vector<double> best_solution;
977  int nattempts = 0, nvalid = 0;
978 
979  while (true)
980  {
981  IkSolutionList<IkReal> solutions;
982  int numsol = solve(frame, vfree, solutions);
983 
984  ROS_DEBUG_STREAM_NAMED(name_, "Found " << numsol << " solutions from IKFast");
985 
986  // ROS_INFO("%f",vfree[0]);
987 
988  if (numsol > 0)
989  {
990  for (int s = 0; s < numsol; ++s)
991  {
992  nattempts++;
993  std::vector<double> sol;
994  getSolution(solutions, ik_seed_state, s, sol);
995 
996  bool obeys_limits = true;
997  for (unsigned int i = 0; i < sol.size(); i++)
998  {
999  if (joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i]))
1000  {
1001  obeys_limits = false;
1002  break;
1003  }
1004  // ROS_INFO_STREAM_NAMED(name_,"Num " << i << " value " << sol[i] << " has limits " <<
1005  // joint_has_limits_vector_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i]);
1006  }
1007  if (obeys_limits)
1008  {
1009  getSolution(solutions, ik_seed_state, s, solution);
1010 
1011  // This solution is within joint limits, now check if in collision (if callback provided)
1012  if (!solution_callback.empty())
1013  {
1014  solution_callback(ik_pose, solution, error_code);
1015  }
1016  else
1017  {
1018  error_code.val = error_code.SUCCESS;
1019  }
1020 
1021  if (error_code.val == error_code.SUCCESS)
1022  {
1023  nvalid++;
1024  if (search_mode & OPTIMIZE_MAX_JOINT)
1025  {
1026  // Costs for solution: Largest joint motion
1027  double costs = 0.0;
1028  for (unsigned int i = 0; i < solution.size(); i++)
1029  {
1030  double d = fabs(ik_seed_state[i] - solution[i]);
1031  if (d > costs)
1032  costs = d;
1033  }
1034  if (costs < best_costs || best_costs == -1.0)
1035  {
1036  best_costs = costs;
1037  best_solution = solution;
1038  }
1039  }
1040  else
1041  // Return first feasible solution
1042  return true;
1043  }
1044  }
1045  }
1046  }
1047 
1048  if (!getCount(counter, num_positive_increments, -num_negative_increments))
1049  {
1050  // Everything searched
1051  error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
1052  break;
1053  }
1054 
1055  vfree[0] = initial_guess + search_discretization_ * counter;
1056  // ROS_DEBUG_STREAM_NAMED(name_,"Attempt " << counter << " with 0th free joint having value " << vfree[0]);
1057  }
1058 
1059  ROS_DEBUG_STREAM_NAMED(name_, "Valid solutions: " << nvalid << "/" << nattempts);
1060 
1061  if ((search_mode & OPTIMIZE_MAX_JOINT) && best_costs != -1.0)
1062  {
1063  solution = best_solution;
1064  error_code.val = error_code.SUCCESS;
1065  return true;
1066  }
1067 
1068  // No solution found
1069  error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
1070  return false;
1071 }
1072 
1073 // Used when there are no redundant joints - aka no free params
1074 bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
1075  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
1076  const kinematics::KinematicsQueryOptions& options) const
1077 {
1078  ROS_DEBUG_STREAM_NAMED(name_, "getPositionIK");
1079 
1080  if (!active_)
1081  {
1082  ROS_ERROR("kinematics not active");
1083  return false;
1084  }
1085 
1086  if (ik_seed_state.size() < num_joints_)
1087  {
1088  ROS_ERROR_STREAM("ik_seed_state only has " << ik_seed_state.size() << " entries, this ikfast solver requires "
1089  << num_joints_);
1090  return false;
1091  }
1092 
1093  // Check if seed is in bound
1094  for (std::size_t i = 0; i < ik_seed_state.size(); i++)
1095  {
1096  // Add tolerance to limit check
1097  if (joint_has_limits_vector_[i] && ((ik_seed_state[i] < (joint_min_vector_[i] - LIMIT_TOLERANCE)) ||
1098  (ik_seed_state[i] > (joint_max_vector_[i] + LIMIT_TOLERANCE))))
1099  {
1100  ROS_DEBUG_STREAM_NAMED("ikseed", "Not in limits! " << (int)i << " value " << ik_seed_state[i]
1101  << " has limit: " << joint_has_limits_vector_[i] << " being "
1102  << joint_min_vector_[i] << " to " << joint_max_vector_[i]);
1103  return false;
1104  }
1105  }
1106 
1107  std::vector<double> vfree(free_params_.size());
1108  for (std::size_t i = 0; i < free_params_.size(); ++i)
1109  {
1110  int p = free_params_[i];
1111  ROS_ERROR("%u is %f", p, ik_seed_state[p]); // DTC
1112  vfree[i] = ik_seed_state[p];
1113  }
1114 
1115  KDL::Frame frame;
1116  tf::poseMsgToKDL(ik_pose, frame);
1117 
1118  IkSolutionList<IkReal> solutions;
1119  int numsol = solve(frame, vfree, solutions);
1120  ROS_DEBUG_STREAM_NAMED(name_, "Found " << numsol << " solutions from IKFast");
1121 
1122  std::vector<LimitObeyingSol> solutions_obey_limits;
1123 
1124  if (numsol)
1125  {
1126  std::vector<double> solution_obey_limits;
1127  for (std::size_t s = 0; s < numsol; ++s)
1128  {
1129  std::vector<double> sol;
1130  getSolution(solutions, ik_seed_state, s, sol);
1131  ROS_DEBUG_NAMED(name_, "Sol %d: %e %e %e %e %e %e", (int)s, sol[0], sol[1], sol[2], sol[3], sol[4],
1132  sol[5]);
1133 
1134  bool obeys_limits = true;
1135  for (std::size_t i = 0; i < sol.size(); i++)
1136  {
1137  // Add tolerance to limit check
1138  if (joint_has_limits_vector_[i] && ((sol[i] < (joint_min_vector_[i] - LIMIT_TOLERANCE)) ||
1139  (sol[i] > (joint_max_vector_[i] + LIMIT_TOLERANCE))))
1140  {
1141  // One element of solution is not within limits
1142  obeys_limits = false;
1143  ROS_DEBUG_STREAM_NAMED(name_, "Not in limits! " << (int)i << " value " << sol[i] << " has limit: "
1144  << joint_has_limits_vector_[i] << " being "
1145  << joint_min_vector_[i] << " to " << joint_max_vector_[i]);
1146  break;
1147  }
1148  }
1149  if (obeys_limits)
1150  {
1151  // All elements of this solution obey limits
1152  getSolution(solutions, ik_seed_state, s, solution_obey_limits);
1153  double dist_from_seed = 0.0;
1154  for (std::size_t i = 0; i < ik_seed_state.size(); ++i)
1155  {
1156  dist_from_seed += fabs(ik_seed_state[i] - solution_obey_limits[i]);
1157  }
1158 
1159  solutions_obey_limits.push_back({ solution_obey_limits, dist_from_seed });
1160  }
1161  }
1162  }
1163  else
1164  {
1165  ROS_DEBUG_STREAM_NAMED(name_, "No IK solution");
1166  }
1167 
1168  // Sort the solutions under limits and find the one that is closest to ik_seed_state
1169  if (!solutions_obey_limits.empty())
1170  {
1171  std::sort(solutions_obey_limits.begin(), solutions_obey_limits.end());
1172  solution = solutions_obey_limits[0].value;
1173  error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
1174  return true;
1175  }
1176 
1177  error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
1178  return false;
1179 }
1180 
1181 bool IKFastKinematicsPlugin::getPositionIK(const std::vector<geometry_msgs::Pose>& ik_poses,
1182  const std::vector<double>& ik_seed_state,
1183  std::vector<std::vector<double>>& solutions,
1185  const kinematics::KinematicsQueryOptions& options) const
1186 {
1187  ROS_DEBUG_STREAM_NAMED(name_, "getPositionIK with multiple solutions");
1188 
1189  if (!active_)
1190  {
1191  ROS_ERROR("kinematics not active");
1193  return false;
1194  }
1195 
1196  if (ik_poses.empty())
1197  {
1198  ROS_ERROR("ik_poses is empty");
1200  return false;
1201  }
1202 
1203  if (ik_poses.size() > 1)
1204  {
1205  ROS_ERROR("ik_poses contains multiple entries, only one is allowed");
1207  return false;
1208  }
1209 
1210  if (ik_seed_state.size() < num_joints_)
1211  {
1212  ROS_ERROR_STREAM("ik_seed_state only has " << ik_seed_state.size() << " entries, this ikfast solver requires "
1213  << num_joints_);
1214  return false;
1215  }
1216 
1217  KDL::Frame frame;
1218  tf::poseMsgToKDL(ik_poses[0], frame);
1219 
1220  // solving ik
1221  std::vector<IkSolutionList<IkReal>> solution_set;
1222  IkSolutionList<IkReal> ik_solutions;
1223  std::vector<double> vfree;
1224  int numsol = 0;
1225  std::vector<double> sampled_joint_vals;
1226  if (!redundant_joint_indices_.empty())
1227  {
1228  // initializing from seed
1229  sampled_joint_vals.push_back(ik_seed_state[redundant_joint_indices_[0]]);
1230 
1231  // checking joint limits when using no discretization
1233  joint_has_limits_vector_[redundant_joint_indices_.front()])
1234  {
1235  double joint_min = joint_min_vector_[redundant_joint_indices_.front()];
1236  double joint_max = joint_max_vector_[redundant_joint_indices_.front()];
1237 
1238  double jv = sampled_joint_vals[0];
1239  if (!((jv > (joint_min - LIMIT_TOLERANCE)) && (jv < (joint_max + LIMIT_TOLERANCE))))
1240  {
1242  ROS_ERROR_STREAM("ik seed is out of bounds");
1243  return false;
1244  }
1245  }
1246 
1247  // computing all solutions sets for each sampled value of the redundant joint
1248  if (!sampleRedundantJoint(options.discretization_method, sampled_joint_vals))
1249  {
1251  return false;
1252  }
1253 
1254  for (unsigned int i = 0; i < sampled_joint_vals.size(); i++)
1255  {
1256  vfree.clear();
1257  vfree.push_back(sampled_joint_vals[i]);
1258  numsol += solve(frame, vfree, ik_solutions);
1259  solution_set.push_back(ik_solutions);
1260  }
1261  }
1262  else
1263  {
1264  // computing for single solution set
1265  numsol = solve(frame, vfree, ik_solutions);
1266  solution_set.push_back(ik_solutions);
1267  }
1268 
1269  ROS_DEBUG_STREAM_NAMED(name_, "Found " << numsol << " solutions from IKFast");
1270  bool solutions_found = false;
1271  if (numsol > 0)
1272  {
1273  /*
1274  Iterating through all solution sets and storing those that do not exceed joint limits.
1275  */
1276  for (unsigned int r = 0; r < solution_set.size(); r++)
1277  {
1278  ik_solutions = solution_set[r];
1279  numsol = ik_solutions.GetNumSolutions();
1280  for (int s = 0; s < numsol; ++s)
1281  {
1282  std::vector<double> sol;
1283  getSolution(ik_solutions, ik_seed_state, s, sol);
1284 
1285  bool obeys_limits = true;
1286  for (unsigned int i = 0; i < sol.size(); i++)
1287  {
1288  // Add tolerance to limit check
1289  if (joint_has_limits_vector_[i] && ((sol[i] < (joint_min_vector_[i] - LIMIT_TOLERANCE)) ||
1290  (sol[i] > (joint_max_vector_[i] + LIMIT_TOLERANCE))))
1291  {
1292  // One element of solution is not within limits
1293  obeys_limits = false;
1294  ROS_DEBUG_STREAM_NAMED(name_, "Not in limits! " << i << " value " << sol[i] << " has limit: "
1295  << joint_has_limits_vector_[i] << " being "
1296  << joint_min_vector_[i] << " to " << joint_max_vector_[i]);
1297  break;
1298  }
1299  }
1300  if (obeys_limits)
1301  {
1302  // All elements of solution obey limits
1303  solutions_found = true;
1304  solutions.push_back(sol);
1305  }
1306  }
1307  }
1308 
1309  if (solutions_found)
1310  {
1312  return true;
1313  }
1314  }
1315  else
1316  {
1317  ROS_DEBUG_STREAM_NAMED(name_, "No IK solution");
1318  }
1319 
1321  return false;
1322 }
1323 
1325  std::vector<double>& sampled_joint_vals) const
1326 {
1327  double joint_min = -M_PI;
1328  double joint_max = M_PI;
1329  int index = redundant_joint_indices_.front();
1330  double joint_dscrt = redundant_joint_discretization_.at(index);
1331 
1332  if (joint_has_limits_vector_[redundant_joint_indices_.front()])
1333  {
1334  joint_min = joint_min_vector_[index];
1335  joint_max = joint_max_vector_[index];
1336  }
1337 
1338  switch (method)
1339  {
1341  {
1342  int steps = std::ceil((joint_max - joint_min) / joint_dscrt);
1343  for (unsigned int i = 0; i < steps; i++)
1344  {
1345  sampled_joint_vals.push_back(joint_min + joint_dscrt * i);
1346  }
1347  sampled_joint_vals.push_back(joint_max);
1348  }
1349  break;
1351  {
1352  int steps = std::ceil((joint_max - joint_min) / joint_dscrt);
1353  steps = steps > 0 ? steps : 1;
1354  double diff = joint_max - joint_min;
1355  for (int i = 0; i < steps; i++)
1356  {
1357  sampled_joint_vals.push_back(((diff * std::rand()) / (static_cast<double>(RAND_MAX))) + joint_min);
1358  }
1359  }
1360 
1361  break;
1363 
1364  break;
1365  default:
1366  ROS_ERROR_STREAM("Discretization method " << method << " is not supported");
1367  return false;
1368  }
1369 
1370  return true;
1371 }
1372 
1373 } // end namespace
1374 
1375 // 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)
URDF_EXPORT 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 Thu Jul 12 2018 02:54:22