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*, *TranslationZAxisAngle4D* - end effector origin
550  // reaches desired 3D translation, manipulator direction makes a specific angle with x/y/z-axis (defined in the
551  // manipulator base link’s coordinate system)
552  ROS_ERROR_NAMED(name_, "IK for this IkParameterizationType not implemented yet.");
553  return 0;
554 
556  // For **TranslationLocalGlobal6D**, the diagonal elements ([0],[4],[8]) are the local translation inside the end
557  // effector coordinate system.
558  ROS_ERROR_NAMED(name_, "IK for this IkParameterizationType not implemented yet.");
559  return 0;
560 
561  case IKP_Rotation3D:
562  case IKP_Lookat3D:
563  case IKP_TranslationXY2D:
565  ROS_ERROR_NAMED(name_, "IK for this IkParameterizationType not implemented yet.");
566  return 0;
567 
569  double roll, pitch, yaw;
570  // For **TranslationXAxisAngleZNorm4D** - end effector origin reaches desired 3D translation, manipulator
571  // direction needs to be orthogonal to z axis and be rotated at a certain angle starting from the x axis (defined
572  // in the manipulator base link’s coordinate system)
573  pose_frame.M.GetRPY(roll, pitch, yaw);
574  ComputeIk(trans, &yaw, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
575  return solutions.GetNumSolutions();
576 
578  // For **TranslationYAxisAngleXNorm4D** - end effector origin reaches desired 3D translation, manipulator
579  // direction needs to be orthogonal to x axis and be rotated at a certain angle starting from the y axis (defined
580  // in the manipulator base link’s coordinate system)
581  pose_frame.M.GetRPY(roll, pitch, yaw);
582  ComputeIk(trans, &roll, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
583  return solutions.GetNumSolutions();
584 
586  // For **TranslationZAxisAngleYNorm4D** - end effector origin reaches desired 3D translation, manipulator
587  // direction needs to be orthogonal to y axis and be rotated at a certain angle starting from the z axis (defined
588  // in the manipulator base link’s coordinate system)
589  pose_frame.M.GetRPY(roll, pitch, yaw);
590  ComputeIk(trans, &pitch, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
591  return solutions.GetNumSolutions();
592 
593  default:
594  ROS_ERROR_NAMED(name_, "Unknown IkParameterizationType! "
595  "Was the solver generated with an incompatible version of Openrave?");
596  return 0;
597  }
598 }
599 
600 void IKFastKinematicsPlugin::getSolution(const IkSolutionList<IkReal>& solutions, int i,
601  std::vector<double>& solution) const
602 {
603  solution.clear();
604  solution.resize(num_joints_);
605 
606  // IKFast56/61
607  const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
608  std::vector<IkReal> vsolfree(sol.GetFree().size());
609  sol.GetSolution(&solution[0], vsolfree.size() > 0 ? &vsolfree[0] : NULL);
610 
611  // std::cout << "solution " << i << ":" ;
612  // for(int j=0;j<num_joints_; ++j)
613  // std::cout << " " << solution[j];
614  // std::cout << std::endl;
615 
616  // ROS_ERROR("%f %d",solution[2],vsolfree.size());
617 }
618 
619 void IKFastKinematicsPlugin::getSolution(const IkSolutionList<IkReal>& solutions,
620  const std::vector<double>& ik_seed_state, int i,
621  std::vector<double>& solution) const
622 {
623  solution.clear();
624  solution.resize(num_joints_);
625 
626  // IKFast56/61
627  const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
628  std::vector<IkReal> vsolfree(sol.GetFree().size());
629  sol.GetSolution(&solution[0], vsolfree.size() > 0 ? &vsolfree[0] : NULL);
630 
631  // rotate joints by +/-360° where it is possible and useful
632  for (std::size_t i = 0; i < num_joints_; ++i)
633  {
634  if (joint_has_limits_vector_[i])
635  {
636  double signed_distance = solution[i] - ik_seed_state[i];
637  while (signed_distance > M_PI && solution[i] - 2 * M_PI > (joint_min_vector_[i] - LIMIT_TOLERANCE))
638  {
639  signed_distance -= 2 * M_PI;
640  solution[i] -= 2 * M_PI;
641  }
642  while (signed_distance < -M_PI && solution[i] + 2 * M_PI < (joint_max_vector_[i] + LIMIT_TOLERANCE))
643  {
644  signed_distance += 2 * M_PI;
645  solution[i] += 2 * M_PI;
646  }
647  }
648  }
649 }
650 
651 double IKFastKinematicsPlugin::harmonize(const std::vector<double>& ik_seed_state, std::vector<double>& solution) const
652 {
653  double dist_sqr = 0;
654  std::vector<double> ss = ik_seed_state;
655  for (size_t i = 0; i < ik_seed_state.size(); ++i)
656  {
657  while (ss[i] > 2 * M_PI)
658  {
659  ss[i] -= 2 * M_PI;
660  }
661  while (ss[i] < 2 * M_PI)
662  {
663  ss[i] += 2 * M_PI;
664  }
665  while (solution[i] > 2 * M_PI)
666  {
667  solution[i] -= 2 * M_PI;
668  }
669  while (solution[i] < 2 * M_PI)
670  {
671  solution[i] += 2 * M_PI;
672  }
673  dist_sqr += fabs(ik_seed_state[i] - solution[i]);
674  }
675  return dist_sqr;
676 }
677 
678 // void IKFastKinematicsPlugin::getOrderedSolutions(const std::vector<double> &ik_seed_state,
679 // std::vector<std::vector<double> >& solslist)
680 // {
681 // std::vector<double>
682 // double mindist = 0;
683 // int minindex = -1;
684 // std::vector<double> sol;
685 // for(size_t i=0;i<solslist.size();++i){
686 // getSolution(i,sol);
687 // double dist = harmonize(ik_seed_state, sol);
688 // //std::cout << "dist[" << i << "]= " << dist << std::endl;
689 // if(minindex == -1 || dist<mindist){
690 // minindex = i;
691 // mindist = dist;
692 // }
693 // }
694 // if(minindex >= 0){
695 // getSolution(minindex,solution);
696 // harmonize(ik_seed_state, solution);
697 // index = minindex;
698 // }
699 // }
700 
701 void IKFastKinematicsPlugin::getClosestSolution(const IkSolutionList<IkReal>& solutions,
702  const std::vector<double>& ik_seed_state,
703  std::vector<double>& solution) const
704 {
705  double mindist = DBL_MAX;
706  int minindex = -1;
707  std::vector<double> sol;
708 
709  // IKFast56/61
710  for (size_t i = 0; i < solutions.GetNumSolutions(); ++i)
711  {
712  getSolution(solutions, i, sol);
713  double dist = harmonize(ik_seed_state, sol);
714  ROS_INFO_STREAM_NAMED(name_, "Dist " << i << " dist " << dist);
715  // std::cout << "dist[" << i << "]= " << dist << std::endl;
716  if (minindex == -1 || dist < mindist)
717  {
718  minindex = i;
719  mindist = dist;
720  }
721  }
722  if (minindex >= 0)
723  {
724  getSolution(solutions, minindex, solution);
725  harmonize(ik_seed_state, solution);
726  }
727 }
728 
729 void IKFastKinematicsPlugin::fillFreeParams(int count, int* array)
730 {
731  free_params_.clear();
732  for (int i = 0; i < count; ++i)
733  free_params_.push_back(array[i]);
734 }
735 
736 bool IKFastKinematicsPlugin::getCount(int& count, const int& max_count, const int& min_count) const
737 {
738  if (count > 0)
739  {
740  if (-count >= min_count)
741  {
742  count = -count;
743  return true;
744  }
745  else if (count + 1 <= max_count)
746  {
747  count = count + 1;
748  return true;
749  }
750  else
751  {
752  return false;
753  }
754  }
755  else
756  {
757  if (1 - count <= max_count)
758  {
759  count = 1 - count;
760  return true;
761  }
762  else if (count - 1 >= min_count)
763  {
764  count = count - 1;
765  return true;
766  }
767  else
768  return false;
769  }
770 }
771 
772 bool IKFastKinematicsPlugin::getPositionFK(const std::vector<std::string>& link_names,
773  const std::vector<double>& joint_angles,
774  std::vector<geometry_msgs::Pose>& poses) const
775 {
776  if (GetIkType() != IKP_Transform6D)
777  {
778  // ComputeFk() is the inverse function of ComputeIk(), so the format of
779  // eerot differs depending on IK type. The Transform6D IK type is the only
780  // one for which a 3x3 rotation matrix is returned, which means we can only
781  // compute FK for that IK type.
782  ROS_ERROR_NAMED(name_, "Can only compute FK for Transform6D IK type!");
783  return false;
784  }
785 
786  KDL::Frame p_out;
787  if (link_names.size() == 0)
788  {
789  ROS_WARN_STREAM_NAMED(name_, "Link names with nothing");
790  return false;
791  }
792 
793  if (link_names.size() != 1 || link_names[0] != getTipFrame())
794  {
795  ROS_ERROR_NAMED(name_, "Can compute FK for %s only", getTipFrame().c_str());
796  return false;
797  }
798 
799  bool valid = true;
800 
801  IkReal eerot[9], eetrans[3];
802 
803  if (joint_angles.size() != num_joints_)
804  {
805  ROS_ERROR_NAMED(name_, "Unexpected number of joint angles");
806  return false;
807  }
808 
809  IkReal angles[num_joints_];
810  for (unsigned char i = 0; i < num_joints_; i++)
811  angles[i] = joint_angles[i];
812 
813  // IKFast56/61
814  ComputeFk(angles, eetrans, eerot);
815 
816  for (int i = 0; i < 3; ++i)
817  p_out.p.data[i] = eetrans[i];
818 
819  for (int i = 0; i < 9; ++i)
820  p_out.M.data[i] = eerot[i];
821 
822  poses.resize(1);
823  tf::poseKDLToMsg(p_out, poses[0]);
824 
825  return valid;
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, moveit_msgs::MoveItErrorCodes& error_code,
831  const kinematics::KinematicsQueryOptions& options) const
832 {
833  const IKCallbackFn solution_callback = 0;
834  std::vector<double> consistency_limits;
835 
836  return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
837  options);
838 }
839 
840 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
841  const std::vector<double>& ik_seed_state, double timeout,
842  const std::vector<double>& consistency_limits,
843  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
844  const kinematics::KinematicsQueryOptions& options) const
845 {
846  const IKCallbackFn solution_callback = 0;
847  return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
848  options);
849 }
850 
851 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
852  const std::vector<double>& ik_seed_state, double timeout,
853  std::vector<double>& solution, const IKCallbackFn& solution_callback,
854  moveit_msgs::MoveItErrorCodes& error_code,
855  const kinematics::KinematicsQueryOptions& options) const
856 {
857  std::vector<double> consistency_limits;
858  return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
859  options);
860 }
861 
862 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
863  const std::vector<double>& ik_seed_state, double timeout,
864  const std::vector<double>& consistency_limits,
865  std::vector<double>& solution, const IKCallbackFn& solution_callback,
866  moveit_msgs::MoveItErrorCodes& error_code,
867  const kinematics::KinematicsQueryOptions& options) const
868 {
869  ROS_DEBUG_STREAM_NAMED(name_, "searchPositionIK");
870 
872  SEARCH_MODE search_mode = _SEARCH_MODE_;
873 
874  // Check if there are no redundant joints
875  if (free_params_.size() == 0)
876  {
877  ROS_DEBUG_STREAM_NAMED(name_, "No need to search since no free params/redundant joints");
878 
879  std::vector<geometry_msgs::Pose> ik_poses(1, ik_pose);
880  std::vector<std::vector<double>> solutions;
881  kinematics::KinematicsResult kinematic_result;
882  // Find all IK solution within joint limits
883  if (!getPositionIK(ik_poses, ik_seed_state, solutions, kinematic_result, options))
884  {
885  ROS_DEBUG_STREAM_NAMED(name_, "No solution whatsoever");
886  error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
887  return false;
888  }
889 
890  // sort solutions by their distance to the seed
891  std::vector<LimitObeyingSol> solutions_obey_limits;
892  for (std::size_t i = 0; i < solutions.size(); ++i)
893  {
894  double dist_from_seed = 0.0;
895  for (std::size_t j = 0; j < ik_seed_state.size(); ++j)
896  {
897  dist_from_seed += fabs(ik_seed_state[j] - solutions[i][j]);
898  }
899 
900  solutions_obey_limits.push_back({ solutions[i], dist_from_seed });
901  }
902  std::sort(solutions_obey_limits.begin(), solutions_obey_limits.end());
903 
904  // check for collisions if a callback is provided
905  if (!solution_callback.empty())
906  {
907  for (std::size_t i = 0; i < solutions_obey_limits.size(); ++i)
908  {
909  solution_callback(ik_pose, solutions_obey_limits[i].value, error_code);
910  if (error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
911  {
912  solution = solutions_obey_limits[i].value;
913  ROS_DEBUG_STREAM_NAMED(name_, "Solution passes callback");
914  return true;
915  }
916  }
917 
918  ROS_DEBUG_STREAM_NAMED(name_, "Solution has error code " << error_code);
919  return false;
920  }
921  else
922  {
923  solution = solutions_obey_limits[0].value;
924  error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
925  return true; // no collision check callback provided
926  }
927  }
928 
929  // -------------------------------------------------------------------------------------------------
930  // Error Checking
931  if (!active_)
932  {
933  ROS_ERROR_STREAM_NAMED(name_, "Kinematics not active");
934  error_code.val = error_code.NO_IK_SOLUTION;
935  return false;
936  }
937 
938  if (ik_seed_state.size() != num_joints_)
939  {
940  ROS_ERROR_STREAM_NAMED(name_, "Seed state must have size " << num_joints_ << " instead of size "
941  << ik_seed_state.size());
942  error_code.val = error_code.NO_IK_SOLUTION;
943  return false;
944  }
945 
946  if (!consistency_limits.empty() && consistency_limits.size() != num_joints_)
947  {
948  ROS_ERROR_STREAM_NAMED(name_, "Consistency limits be empty or must have size " << num_joints_ << " instead of size "
949  << consistency_limits.size());
950  error_code.val = error_code.NO_IK_SOLUTION;
951  return false;
952  }
953 
954  // -------------------------------------------------------------------------------------------------
955  // Initialize
956 
957  KDL::Frame frame;
958  tf::poseMsgToKDL(ik_pose, frame);
959 
960  std::vector<double> vfree(free_params_.size());
961 
962  ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
963  int counter = 0;
964 
965  double initial_guess = ik_seed_state[free_params_[0]];
966  vfree[0] = initial_guess;
967 
968  // -------------------------------------------------------------------------------------------------
969  // Handle consitency limits if needed
970  int num_positive_increments;
971  int num_negative_increments;
972 
973  if (!consistency_limits.empty())
974  {
975  // moveit replaced consistency_limit (scalar) w/ consistency_limits (vector)
976  // Assume [0]th free_params element for now. Probably wrong.
977  double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess + consistency_limits[free_params_[0]]);
978  double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess - consistency_limits[free_params_[0]]);
979 
980  num_positive_increments = (int)((max_limit - initial_guess) / search_discretization_);
981  num_negative_increments = (int)((initial_guess - min_limit) / search_discretization_);
982  }
983  else // no consitency limits provided
984  {
985  num_positive_increments = (joint_max_vector_[free_params_[0]] - initial_guess) / search_discretization_;
986  num_negative_increments = (initial_guess - joint_min_vector_[free_params_[0]]) / search_discretization_;
987  }
988 
989  // -------------------------------------------------------------------------------------------------
990  // Begin searching
991 
992  ROS_DEBUG_STREAM_NAMED(name_, "Free param is " << free_params_[0] << " initial guess is " << initial_guess
993  << ", # positive increments: " << num_positive_increments
994  << ", # negative increments: " << num_negative_increments);
995  if ((search_mode & OPTIMIZE_MAX_JOINT) && (num_positive_increments + num_negative_increments) > 1000)
996  ROS_WARN_STREAM_ONCE_NAMED(name_, "Large search space, consider increasing the search discretization");
997 
998  double best_costs = -1.0;
999  std::vector<double> best_solution;
1000  int nattempts = 0, nvalid = 0;
1001 
1002  while (true)
1003  {
1004  IkSolutionList<IkReal> solutions;
1005  int numsol = solve(frame, vfree, solutions);
1006 
1007  ROS_DEBUG_STREAM_NAMED(name_, "Found " << numsol << " solutions from IKFast");
1008 
1009  // ROS_INFO("%f",vfree[0]);
1010 
1011  if (numsol > 0)
1012  {
1013  for (int s = 0; s < numsol; ++s)
1014  {
1015  nattempts++;
1016  std::vector<double> sol;
1017  getSolution(solutions, ik_seed_state, s, sol);
1018 
1019  bool obeys_limits = true;
1020  for (unsigned int i = 0; i < sol.size(); i++)
1021  {
1022  if (joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i]))
1023  {
1024  obeys_limits = false;
1025  break;
1026  }
1027  // ROS_INFO_STREAM_NAMED(name_,"Num " << i << " value " << sol[i] << " has limits " <<
1028  // joint_has_limits_vector_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i]);
1029  }
1030  if (obeys_limits)
1031  {
1032  getSolution(solutions, ik_seed_state, s, solution);
1033 
1034  // This solution is within joint limits, now check if in collision (if callback provided)
1035  if (!solution_callback.empty())
1036  {
1037  solution_callback(ik_pose, solution, error_code);
1038  }
1039  else
1040  {
1041  error_code.val = error_code.SUCCESS;
1042  }
1043 
1044  if (error_code.val == error_code.SUCCESS)
1045  {
1046  nvalid++;
1047  if (search_mode & OPTIMIZE_MAX_JOINT)
1048  {
1049  // Costs for solution: Largest joint motion
1050  double costs = 0.0;
1051  for (unsigned int i = 0; i < solution.size(); i++)
1052  {
1053  double d = fabs(ik_seed_state[i] - solution[i]);
1054  if (d > costs)
1055  costs = d;
1056  }
1057  if (costs < best_costs || best_costs == -1.0)
1058  {
1059  best_costs = costs;
1060  best_solution = solution;
1061  }
1062  }
1063  else
1064  // Return first feasible solution
1065  return true;
1066  }
1067  }
1068  }
1069  }
1070 
1071  if (!getCount(counter, num_positive_increments, -num_negative_increments))
1072  {
1073  // Everything searched
1074  error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
1075  break;
1076  }
1077 
1078  vfree[0] = initial_guess + search_discretization_ * counter;
1079  // ROS_DEBUG_STREAM_NAMED(name_,"Attempt " << counter << " with 0th free joint having value " << vfree[0]);
1080  }
1081 
1082  ROS_DEBUG_STREAM_NAMED(name_, "Valid solutions: " << nvalid << "/" << nattempts);
1083 
1084  if ((search_mode & OPTIMIZE_MAX_JOINT) && best_costs != -1.0)
1085  {
1086  solution = best_solution;
1087  error_code.val = error_code.SUCCESS;
1088  return true;
1089  }
1090 
1091  // No solution found
1092  error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
1093  return false;
1094 }
1095 
1096 // Used when there are no redundant joints - aka no free params
1097 bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
1098  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
1099  const kinematics::KinematicsQueryOptions& options) const
1100 {
1101  ROS_DEBUG_STREAM_NAMED(name_, "getPositionIK");
1102 
1103  if (!active_)
1104  {
1105  ROS_ERROR("kinematics not active");
1106  return false;
1107  }
1108 
1109  if (ik_seed_state.size() < num_joints_)
1110  {
1111  ROS_ERROR_STREAM("ik_seed_state only has " << ik_seed_state.size() << " entries, this ikfast solver requires "
1112  << num_joints_);
1113  return false;
1114  }
1115 
1116  // Check if seed is in bound
1117  for (std::size_t i = 0; i < ik_seed_state.size(); i++)
1118  {
1119  // Add tolerance to limit check
1120  if (joint_has_limits_vector_[i] && ((ik_seed_state[i] < (joint_min_vector_[i] - LIMIT_TOLERANCE)) ||
1121  (ik_seed_state[i] > (joint_max_vector_[i] + LIMIT_TOLERANCE))))
1122  {
1123  ROS_DEBUG_STREAM_NAMED("ikseed", "Not in limits! " << (int)i << " value " << ik_seed_state[i]
1124  << " has limit: " << joint_has_limits_vector_[i] << " being "
1125  << joint_min_vector_[i] << " to " << joint_max_vector_[i]);
1126  return false;
1127  }
1128  }
1129 
1130  std::vector<double> vfree(free_params_.size());
1131  for (std::size_t i = 0; i < free_params_.size(); ++i)
1132  {
1133  int p = free_params_[i];
1134  ROS_ERROR("%u is %f", p, ik_seed_state[p]); // DTC
1135  vfree[i] = ik_seed_state[p];
1136  }
1137 
1138  KDL::Frame frame;
1139  tf::poseMsgToKDL(ik_pose, frame);
1140 
1141  IkSolutionList<IkReal> solutions;
1142  int numsol = solve(frame, vfree, solutions);
1143  ROS_DEBUG_STREAM_NAMED(name_, "Found " << numsol << " solutions from IKFast");
1144 
1145  std::vector<LimitObeyingSol> solutions_obey_limits;
1146 
1147  if (numsol)
1148  {
1149  std::vector<double> solution_obey_limits;
1150  for (std::size_t s = 0; s < numsol; ++s)
1151  {
1152  std::vector<double> sol;
1153  getSolution(solutions, ik_seed_state, s, sol);
1154  ROS_DEBUG_NAMED(name_, "Sol %d: %e %e %e %e %e %e", (int)s, sol[0], sol[1], sol[2], sol[3], sol[4],
1155  sol[5]);
1156 
1157  bool obeys_limits = true;
1158  for (std::size_t i = 0; i < sol.size(); i++)
1159  {
1160  // Add tolerance to limit check
1161  if (joint_has_limits_vector_[i] && ((sol[i] < (joint_min_vector_[i] - LIMIT_TOLERANCE)) ||
1162  (sol[i] > (joint_max_vector_[i] + LIMIT_TOLERANCE))))
1163  {
1164  // One element of solution is not within limits
1165  obeys_limits = false;
1166  ROS_DEBUG_STREAM_NAMED(name_, "Not in limits! " << (int)i << " value " << sol[i] << " has limit: "
1167  << joint_has_limits_vector_[i] << " being "
1168  << joint_min_vector_[i] << " to " << joint_max_vector_[i]);
1169  break;
1170  }
1171  }
1172  if (obeys_limits)
1173  {
1174  // All elements of this solution obey limits
1175  getSolution(solutions, ik_seed_state, s, solution_obey_limits);
1176  double dist_from_seed = 0.0;
1177  for (std::size_t i = 0; i < ik_seed_state.size(); ++i)
1178  {
1179  dist_from_seed += fabs(ik_seed_state[i] - solution_obey_limits[i]);
1180  }
1181 
1182  solutions_obey_limits.push_back({ solution_obey_limits, dist_from_seed });
1183  }
1184  }
1185  }
1186  else
1187  {
1188  ROS_DEBUG_STREAM_NAMED(name_, "No IK solution");
1189  }
1190 
1191  // Sort the solutions under limits and find the one that is closest to ik_seed_state
1192  if (!solutions_obey_limits.empty())
1193  {
1194  std::sort(solutions_obey_limits.begin(), solutions_obey_limits.end());
1195  solution = solutions_obey_limits[0].value;
1196  error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
1197  return true;
1198  }
1199 
1200  error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
1201  return false;
1202 }
1203 
1204 bool IKFastKinematicsPlugin::getPositionIK(const std::vector<geometry_msgs::Pose>& ik_poses,
1205  const std::vector<double>& ik_seed_state,
1206  std::vector<std::vector<double>>& solutions,
1208  const kinematics::KinematicsQueryOptions& options) const
1209 {
1210  ROS_DEBUG_STREAM_NAMED(name_, "getPositionIK with multiple solutions");
1211 
1212  if (!active_)
1213  {
1214  ROS_ERROR("kinematics not active");
1216  return false;
1217  }
1218 
1219  if (ik_poses.empty())
1220  {
1221  ROS_ERROR("ik_poses is empty");
1223  return false;
1224  }
1225 
1226  if (ik_poses.size() > 1)
1227  {
1228  ROS_ERROR("ik_poses contains multiple entries, only one is allowed");
1230  return false;
1231  }
1232 
1233  if (ik_seed_state.size() < num_joints_)
1234  {
1235  ROS_ERROR_STREAM("ik_seed_state only has " << ik_seed_state.size() << " entries, this ikfast solver requires "
1236  << num_joints_);
1237  return false;
1238  }
1239 
1240  KDL::Frame frame;
1241  tf::poseMsgToKDL(ik_poses[0], frame);
1242 
1243  // solving ik
1244  std::vector<IkSolutionList<IkReal>> solution_set;
1245  IkSolutionList<IkReal> ik_solutions;
1246  std::vector<double> vfree;
1247  int numsol = 0;
1248  std::vector<double> sampled_joint_vals;
1249  if (!redundant_joint_indices_.empty())
1250  {
1251  // initializing from seed
1252  sampled_joint_vals.push_back(ik_seed_state[redundant_joint_indices_[0]]);
1253 
1254  // checking joint limits when using no discretization
1256  joint_has_limits_vector_[redundant_joint_indices_.front()])
1257  {
1258  double joint_min = joint_min_vector_[redundant_joint_indices_.front()];
1259  double joint_max = joint_max_vector_[redundant_joint_indices_.front()];
1260 
1261  double jv = sampled_joint_vals[0];
1262  if (!((jv > (joint_min - LIMIT_TOLERANCE)) && (jv < (joint_max + LIMIT_TOLERANCE))))
1263  {
1265  ROS_ERROR_STREAM("ik seed is out of bounds");
1266  return false;
1267  }
1268  }
1269 
1270  // computing all solutions sets for each sampled value of the redundant joint
1271  if (!sampleRedundantJoint(options.discretization_method, sampled_joint_vals))
1272  {
1274  return false;
1275  }
1276 
1277  for (unsigned int i = 0; i < sampled_joint_vals.size(); i++)
1278  {
1279  vfree.clear();
1280  vfree.push_back(sampled_joint_vals[i]);
1281  numsol += solve(frame, vfree, ik_solutions);
1282  solution_set.push_back(ik_solutions);
1283  }
1284  }
1285  else
1286  {
1287  // computing for single solution set
1288  numsol = solve(frame, vfree, ik_solutions);
1289  solution_set.push_back(ik_solutions);
1290  }
1291 
1292  ROS_DEBUG_STREAM_NAMED(name_, "Found " << numsol << " solutions from IKFast");
1293  bool solutions_found = false;
1294  if (numsol > 0)
1295  {
1296  /*
1297  Iterating through all solution sets and storing those that do not exceed joint limits.
1298  */
1299  for (unsigned int r = 0; r < solution_set.size(); r++)
1300  {
1301  ik_solutions = solution_set[r];
1302  numsol = ik_solutions.GetNumSolutions();
1303  for (int s = 0; s < numsol; ++s)
1304  {
1305  std::vector<double> sol;
1306  getSolution(ik_solutions, ik_seed_state, s, sol);
1307 
1308  bool obeys_limits = true;
1309  for (unsigned int i = 0; i < sol.size(); i++)
1310  {
1311  // Add tolerance to limit check
1312  if (joint_has_limits_vector_[i] && ((sol[i] < (joint_min_vector_[i] - LIMIT_TOLERANCE)) ||
1313  (sol[i] > (joint_max_vector_[i] + LIMIT_TOLERANCE))))
1314  {
1315  // One element of solution is not within limits
1316  obeys_limits = false;
1317  ROS_DEBUG_STREAM_NAMED(name_, "Not in limits! " << i << " value " << sol[i] << " has limit: "
1318  << joint_has_limits_vector_[i] << " being "
1319  << joint_min_vector_[i] << " to " << joint_max_vector_[i]);
1320  break;
1321  }
1322  }
1323  if (obeys_limits)
1324  {
1325  // All elements of solution obey limits
1326  solutions_found = true;
1327  solutions.push_back(sol);
1328  }
1329  }
1330  }
1331 
1332  if (solutions_found)
1333  {
1335  return true;
1336  }
1337  }
1338  else
1339  {
1340  ROS_DEBUG_STREAM_NAMED(name_, "No IK solution");
1341  }
1342 
1344  return false;
1345 }
1346 
1348  std::vector<double>& sampled_joint_vals) const
1349 {
1350  double joint_min = -M_PI;
1351  double joint_max = M_PI;
1352  int index = redundant_joint_indices_.front();
1353  double joint_dscrt = redundant_joint_discretization_.at(index);
1354 
1355  if (joint_has_limits_vector_[redundant_joint_indices_.front()])
1356  {
1357  joint_min = joint_min_vector_[index];
1358  joint_max = joint_max_vector_[index];
1359  }
1360 
1361  switch (method)
1362  {
1364  {
1365  int steps = std::ceil((joint_max - joint_min) / joint_dscrt);
1366  for (unsigned int i = 0; i < steps; i++)
1367  {
1368  sampled_joint_vals.push_back(joint_min + joint_dscrt * i);
1369  }
1370  sampled_joint_vals.push_back(joint_max);
1371  }
1372  break;
1374  {
1375  int steps = std::ceil((joint_max - joint_min) / joint_dscrt);
1376  steps = steps > 0 ? steps : 1;
1377  double diff = joint_max - joint_min;
1378  for (int i = 0; i < steps; i++)
1379  {
1380  sampled_joint_vals.push_back(((diff * std::rand()) / (static_cast<double>(RAND_MAX))) + joint_min);
1381  }
1382  }
1383 
1384  break;
1386 
1387  break;
1388  default:
1389  ROS_ERROR_STREAM("Discretization method " << method << " is not supported");
1390  return false;
1391  }
1392 
1393  return true;
1394 }
1395 
1396 } // end namespace
1397 
1398 // 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
void GetRPY(double &roll, double &pitch, double &yaw) const
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 Wed Jul 10 2019 04:03:41