fanuc_lrmate200ib3l_manipulator_ikfast_moveit_plugin.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 IPA
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above copyright
15  * notice, this list of conditions and the following disclaimer in the
16  * documentation and/or other materials provided with the distribution.
17  * * Neither the name of the all of the author's companies nor the names of its
18  * contributors may be used to endorse or promote products derived from
19  * this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
25  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
26  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
27  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
28  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
29  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
30  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  *
33  *********************************************************************/
34 
35 /*
36  * IKFast Plugin Template for moveit
37  *
38  * AUTO-GENERATED by create_ikfast_moveit_plugin.py in arm_kinematics_tools
39  * You should run create_ikfast_moveit_plugin.py to generate your own plugin.
40  *
41  * Creates a kinematics plugin using the output of IKFast from OpenRAVE.
42  * This plugin and the move_group node can be used as a general
43  * kinematics service, from within the moveit planning environment, or in
44  * your own ROS node.
45  *
46  */
47 
48 #include <ros/ros.h>
50 #include <urdf/model.h>
51 #include <tf_conversions/tf_kdl.h>
52 
53 // Need a floating point tolerance when checking joint limits, in case the joint starts at limit
54 const double LIMIT_TOLERANCE = .0000001;
57 
59 {
60 
61 #define IKFAST_NO_MAIN // Don't include main() from IKFast
62 
70  IKP_Transform6D=0x67000001,
71  IKP_Rotation3D=0x34000002,
72  IKP_Translation3D=0x33000003,
73  IKP_Direction3D=0x23000004,
74  IKP_Ray4D=0x46000005,
75  IKP_Lookat3D=0x23000006,
77  IKP_TranslationXY2D=0x22000008,
80 
84 
88 
90 
91  IKP_VelocityDataBit = 0x00008000,
108 
109  IKP_UniqueIdMask = 0x0000ffff,
110  IKP_CustomDataBit = 0x00010000,
111 };
112 
113 // Code generated by IKFast56/61
115 
117 {
118  std::vector<std::string> joint_names_;
119  std::vector<double> joint_min_vector_;
120  std::vector<double> joint_max_vector_;
121  std::vector<bool> joint_has_limits_vector_;
122  std::vector<std::string> link_names_;
123  size_t num_joints_;
124  std::vector<int> free_params_;
125  bool active_; // Internal variable that indicates whether solvers are configured and ready
126 
127  const std::vector<std::string>& getJointNames() const { return joint_names_; }
128  const std::vector<std::string>& getLinkNames() const { return link_names_; }
130 public:
131 
135  IKFastKinematicsPlugin():active_(false){}
136 
146  // Returns the first IK solution that is within joint limits, this is called by get_ik() service
147  bool getPositionIK(const geometry_msgs::Pose &ik_pose,
148  const std::vector<double> &ik_seed_state,
149  std::vector<double> &solution,
150  moveit_msgs::MoveItErrorCodes &error_code,
152 
161  bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
162  const std::vector<double> &ik_seed_state,
163  double timeout,
164  std::vector<double> &solution,
165  moveit_msgs::MoveItErrorCodes &error_code,
177  bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
178  const std::vector<double> &ik_seed_state,
179  double timeout,
180  const std::vector<double> &consistency_limits,
181  std::vector<double> &solution,
182  moveit_msgs::MoveItErrorCodes &error_code,
184 
193  bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
194  const std::vector<double> &ik_seed_state,
195  double timeout,
196  std::vector<double> &solution,
197  const IKCallbackFn &solution_callback,
198  moveit_msgs::MoveItErrorCodes &error_code,
200 
211  bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
212  const std::vector<double> &ik_seed_state,
213  double timeout,
214  const std::vector<double> &consistency_limits,
215  std::vector<double> &solution,
216  const IKCallbackFn &solution_callback,
217  moveit_msgs::MoveItErrorCodes &error_code,
219 
228  bool getPositionFK(const std::vector<std::string> &link_names,
229  const std::vector<double> &joint_angles,
230  std::vector<geometry_msgs::Pose> &poses) const;
231 
232 private:
233 
234  bool initialize(const std::string &robot_description,
235  const std::string& group_name,
236  const std::string& base_name,
237  const std::string& tip_name,
238  double search_discretization);
239 
244  int solve(KDL::Frame &pose_frame, const std::vector<double> &vfree, IkSolutionList<IkReal> &solutions) const;
245 
249  void getSolution(const IkSolutionList<IkReal> &solutions, int i, std::vector<double>& solution) const;
250 
251  double harmonize(const std::vector<double> &ik_seed_state, std::vector<double> &solution) const;
252  //void getOrderedSolutions(const std::vector<double> &ik_seed_state, std::vector<std::vector<double> >& solslist);
253  void getClosestSolution(const IkSolutionList<IkReal> &solutions, const std::vector<double> &ik_seed_state, std::vector<double> &solution) const;
254  void fillFreeParams(int count, int *array);
255  bool getCount(int &count, const int &max_count, const int &min_count) const;
256 
257 }; // end class
258 
259 bool IKFastKinematicsPlugin::initialize(const std::string &robot_description,
260  const std::string& group_name,
261  const std::string& base_name,
262  const std::string& tip_name,
263  double search_discretization)
264 {
265  setValues(robot_description, group_name, base_name, tip_name, search_discretization);
266 
267  ros::NodeHandle node_handle("~/"+group_name);
268 
269  std::string robot;
270  node_handle.param("robot",robot,std::string());
271 
272  // IKFast56/61
274  num_joints_ = GetNumJoints();
276  if(free_params_.size() > 1)
277  {
278  ROS_FATAL("Only one free joint paramter supported!");
279  return false;
280  }
281 
282  urdf::Model robot_model;
283  std::string xml_string;
285  std::string urdf_xml,full_urdf_xml;
286  node_handle.param("urdf_xml",urdf_xml,robot_description);
287  node_handle.searchParam(urdf_xml,full_urdf_xml);
288 
289  ROS_DEBUG_NAMED("ikfast","Reading xml file from parameter server");
290  if (!node_handle.getParam(full_urdf_xml, xml_string))
291  {
292  ROS_FATAL_NAMED("ikfast","Could not load the xml from parameter server: %s", urdf_xml.c_str());
293  return false;
294  }
295 
296  node_handle.param(full_urdf_xml,xml_string,std::string());
297  robot_model.initString(xml_string);
298 
299  ROS_DEBUG_STREAM_NAMED("ikfast","Reading joints and links from URDF");
300 
301  urdf::LinkConstSharedPtr link = robot_model.getLink(getTipFrame());
302  while(link->name != base_frame_ && joint_names_.size() <= num_joints_)
303  {
304  ROS_DEBUG_NAMED("ikfast","Link %s",link->name.c_str());
305  link_names_.push_back(link->name);
306  urdf::JointSharedPtr joint = link->parent_joint;
307  if(joint)
308  {
309  if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
310  {
311  ROS_DEBUG_STREAM_NAMED("ikfast","Adding joint " << joint->name );
312 
313  joint_names_.push_back(joint->name);
314  float lower, upper;
315  int hasLimits;
316  if ( joint->type != urdf::Joint::CONTINUOUS )
317  {
318  if(joint->safety)
319  {
320  lower = joint->safety->soft_lower_limit;
321  upper = joint->safety->soft_upper_limit;
322  } else {
323  lower = joint->limits->lower;
324  upper = joint->limits->upper;
325  }
326  hasLimits = 1;
327  }
328  else
329  {
330  lower = -M_PI;
331  upper = M_PI;
332  hasLimits = 0;
333  }
334  if(hasLimits)
335  {
336  joint_has_limits_vector_.push_back(true);
337  joint_min_vector_.push_back(lower);
338  joint_max_vector_.push_back(upper);
339  }
340  else
341  {
342  joint_has_limits_vector_.push_back(false);
343  joint_min_vector_.push_back(-M_PI);
344  joint_max_vector_.push_back(M_PI);
345  }
346  }
347  } else
348  {
349  ROS_WARN_NAMED("ikfast","no joint corresponding to %s",link->name.c_str());
350  }
351  link = link->getParent();
352  }
353 
354  if(joint_names_.size() != num_joints_)
355  {
356  ROS_FATAL_STREAM_NAMED("ikfast","Joint numbers mismatch: URDF has " << joint_names_.size() << " and IKFast has " << num_joints_);
357  return false;
358  }
359 
360  std::reverse(link_names_.begin(),link_names_.end());
361  std::reverse(joint_names_.begin(),joint_names_.end());
362  std::reverse(joint_min_vector_.begin(),joint_min_vector_.end());
363  std::reverse(joint_max_vector_.begin(),joint_max_vector_.end());
364  std::reverse(joint_has_limits_vector_.begin(), joint_has_limits_vector_.end());
365 
366  for(size_t i=0; i <num_joints_; ++i)
367  ROS_DEBUG_STREAM_NAMED("ikfast",joint_names_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i] << " " << joint_has_limits_vector_[i]);
368 
369  active_ = true;
370  return true;
371 }
372 
373 int IKFastKinematicsPlugin::solve(KDL::Frame &pose_frame, const std::vector<double> &vfree, IkSolutionList<IkReal> &solutions) const
374 {
375  // IKFast56/61
376  solutions.Clear();
377 
378  double trans[3];
379  trans[0] = pose_frame.p[0];//-.18;
380  trans[1] = pose_frame.p[1];
381  trans[2] = pose_frame.p[2];
382 
383  KDL::Rotation mult;
384  KDL::Vector direction;
385 
386  switch (GetIkType())
387  {
388  case IKP_Transform6D:
389  case IKP_Translation3D:
390  // For **Transform6D**, eerot is 9 values for the 3x3 rotation matrix. For **Translation3D**, these are ignored.
391 
392  mult = pose_frame.M;
393 
394  double vals[9];
395  vals[0] = mult(0,0);
396  vals[1] = mult(0,1);
397  vals[2] = mult(0,2);
398  vals[3] = mult(1,0);
399  vals[4] = mult(1,1);
400  vals[5] = mult(1,2);
401  vals[6] = mult(2,0);
402  vals[7] = mult(2,1);
403  vals[8] = mult(2,2);
404 
405  // IKFast56/61
406  ComputeIk(trans, vals, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
407  return solutions.GetNumSolutions();
408 
409  case IKP_Direction3D:
410  case IKP_Ray4D:
412  // For **Direction3D**, **Ray4D**, and **TranslationDirection5D**, the first 3 values represent the target direction.
413 
414  direction = pose_frame.M * KDL::Vector(0, 0, 1);
415  ComputeIk(trans, direction.data, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
416  return solutions.GetNumSolutions();
417 
421  // For **TranslationXAxisAngle4D**, **TranslationYAxisAngle4D**, and **TranslationZAxisAngle4D**, the first value represents the angle.
422  ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet.");
423  return 0;
424 
426  // For **TranslationLocalGlobal6D**, the diagonal elements ([0],[4],[8]) are the local translation inside the end effector coordinate system.
427  ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet.");
428  return 0;
429 
430  case IKP_Rotation3D:
431  case IKP_Lookat3D:
432  case IKP_TranslationXY2D:
437  ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet.");
438  return 0;
439 
440  default:
441  ROS_ERROR_NAMED("ikfast", "Unknown IkParameterizationType! Was the solver generated with an incompatible version of Openrave?");
442  return 0;
443  }
444 }
445 
446 void IKFastKinematicsPlugin::getSolution(const IkSolutionList<IkReal> &solutions, int i, std::vector<double>& solution) const
447 {
448  solution.clear();
449  solution.resize(num_joints_);
450 
451  // IKFast56/61
452  const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
453  std::vector<IkReal> vsolfree( sol.GetFree().size() );
454  sol.GetSolution(&solution[0],vsolfree.size()>0?&vsolfree[0]:NULL);
455 
456  // std::cout << "solution " << i << ":" ;
457  // for(int j=0;j<num_joints_; ++j)
458  // std::cout << " " << solution[j];
459  // std::cout << std::endl;
460 
461  //ROS_ERROR("%f %d",solution[2],vsolfree.size());
462 }
463 
464 double IKFastKinematicsPlugin::harmonize(const std::vector<double> &ik_seed_state, std::vector<double> &solution) const
465 {
466  double dist_sqr = 0;
467  std::vector<double> ss = ik_seed_state;
468  for(size_t i=0; i< ik_seed_state.size(); ++i)
469  {
470  while(ss[i] > 2*M_PI) {
471  ss[i] -= 2*M_PI;
472  }
473  while(ss[i] < 2*M_PI) {
474  ss[i] += 2*M_PI;
475  }
476  while(solution[i] > 2*M_PI) {
477  solution[i] -= 2*M_PI;
478  }
479  while(solution[i] < 2*M_PI) {
480  solution[i] += 2*M_PI;
481  }
482  dist_sqr += fabs(ik_seed_state[i] - solution[i]);
483  }
484  return dist_sqr;
485 }
486 
487 // void IKFastKinematicsPlugin::getOrderedSolutions(const std::vector<double> &ik_seed_state,
488 // std::vector<std::vector<double> >& solslist)
489 // {
490 // std::vector<double>
491 // double mindist = 0;
492 // int minindex = -1;
493 // std::vector<double> sol;
494 // for(size_t i=0;i<solslist.size();++i){
495 // getSolution(i,sol);
496 // double dist = harmonize(ik_seed_state, sol);
497 // //std::cout << "dist[" << i << "]= " << dist << std::endl;
498 // if(minindex == -1 || dist<mindist){
499 // minindex = i;
500 // mindist = dist;
501 // }
502 // }
503 // if(minindex >= 0){
504 // getSolution(minindex,solution);
505 // harmonize(ik_seed_state, solution);
506 // index = minindex;
507 // }
508 // }
509 
510 void IKFastKinematicsPlugin::getClosestSolution(const IkSolutionList<IkReal> &solutions, const std::vector<double> &ik_seed_state, std::vector<double> &solution) const
511 {
512  double mindist = DBL_MAX;
513  int minindex = -1;
514  std::vector<double> sol;
515 
516  // IKFast56/61
517  for(size_t i=0; i < solutions.GetNumSolutions(); ++i)
518  {
519  getSolution(solutions, i,sol);
520  double dist = harmonize(ik_seed_state, sol);
521  ROS_INFO_STREAM_NAMED("ikfast","Dist " << i << " dist " << dist);
522  //std::cout << "dist[" << i << "]= " << dist << std::endl;
523  if(minindex == -1 || dist<mindist){
524  minindex = i;
525  mindist = dist;
526  }
527  }
528  if(minindex >= 0){
529  getSolution(solutions, minindex,solution);
530  harmonize(ik_seed_state, solution);
531  }
532 }
533 
534 void IKFastKinematicsPlugin::fillFreeParams(int count, int *array)
535 {
536  free_params_.clear();
537  for(int i=0; i<count;++i) free_params_.push_back(array[i]);
538 }
539 
540 bool IKFastKinematicsPlugin::getCount(int &count, const int &max_count, const int &min_count) const
541 {
542  if(count > 0)
543  {
544  if(-count >= min_count)
545  {
546  count = -count;
547  return true;
548  }
549  else if(count+1 <= max_count)
550  {
551  count = count+1;
552  return true;
553  }
554  else
555  {
556  return false;
557  }
558  }
559  else
560  {
561  if(1-count <= max_count)
562  {
563  count = 1-count;
564  return true;
565  }
566  else if(count-1 >= min_count)
567  {
568  count = count -1;
569  return true;
570  }
571  else
572  return false;
573  }
574 }
575 
576 bool IKFastKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
577  const std::vector<double> &joint_angles,
578  std::vector<geometry_msgs::Pose> &poses) const
579 {
580  if (GetIkType() != IKP_Transform6D) {
581  // ComputeFk() is the inverse function of ComputeIk(), so the format of
582  // eerot differs depending on IK type. The Transform6D IK type is the only
583  // one for which a 3x3 rotation matrix is returned, which means we can only
584  // compute FK for that IK type.
585  ROS_ERROR_NAMED("ikfast", "Can only compute FK for Transform6D IK type!");
586  return false;
587  }
588 
589  KDL::Frame p_out;
590  if(link_names.size() == 0) {
591  ROS_WARN_STREAM_NAMED("ikfast","Link names with nothing");
592  return false;
593  }
594 
595  if(link_names.size()!=1 || link_names[0]!=getTipFrame()){
596  ROS_ERROR_NAMED("ikfast","Can compute FK for %s only",getTipFrame().c_str());
597  return false;
598  }
599 
600  bool valid = true;
601 
602  IkReal eerot[9],eetrans[3];
603  IkReal angles[joint_angles.size()];
604  for (unsigned char i=0; i < joint_angles.size(); i++)
605  angles[i] = joint_angles[i];
606 
607  // IKFast56/61
608  ComputeFk(angles,eetrans,eerot);
609 
610  for(int i=0; i<3;++i)
611  p_out.p.data[i] = eetrans[i];
612 
613  for(int i=0; i<9;++i)
614  p_out.M.data[i] = eerot[i];
615 
616  poses.resize(1);
617  tf::poseKDLToMsg(p_out,poses[0]);
618 
619  return valid;
620 }
621 
622 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
623  const std::vector<double> &ik_seed_state,
624  double timeout,
625  std::vector<double> &solution,
626  moveit_msgs::MoveItErrorCodes &error_code,
627  const kinematics::KinematicsQueryOptions &options) const
628 {
629  const IKCallbackFn solution_callback = 0;
630  std::vector<double> consistency_limits;
631 
632  return searchPositionIK(ik_pose,
633  ik_seed_state,
634  timeout,
635  consistency_limits,
636  solution,
637  solution_callback,
638  error_code,
639  options);
640 }
641 
642 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
643  const std::vector<double> &ik_seed_state,
644  double timeout,
645  const std::vector<double> &consistency_limits,
646  std::vector<double> &solution,
647  moveit_msgs::MoveItErrorCodes &error_code,
648  const kinematics::KinematicsQueryOptions &options) const
649 {
650  const IKCallbackFn solution_callback = 0;
651  return searchPositionIK(ik_pose,
652  ik_seed_state,
653  timeout,
654  consistency_limits,
655  solution,
656  solution_callback,
657  error_code,
658  options);
659 }
660 
661 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
662  const std::vector<double> &ik_seed_state,
663  double timeout,
664  std::vector<double> &solution,
665  const IKCallbackFn &solution_callback,
666  moveit_msgs::MoveItErrorCodes &error_code,
667  const kinematics::KinematicsQueryOptions &options) const
668 {
669  std::vector<double> consistency_limits;
670  return searchPositionIK(ik_pose,
671  ik_seed_state,
672  timeout,
673  consistency_limits,
674  solution,
675  solution_callback,
676  error_code,
677  options);
678 }
679 
680 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
681  const std::vector<double> &ik_seed_state,
682  double timeout,
683  const std::vector<double> &consistency_limits,
684  std::vector<double> &solution,
685  const IKCallbackFn &solution_callback,
686  moveit_msgs::MoveItErrorCodes &error_code,
687  const kinematics::KinematicsQueryOptions &options) const
688 {
689  ROS_DEBUG_STREAM_NAMED("ikfast","searchPositionIK");
690 
692  SEARCH_MODE search_mode = OPTIMIZE_MAX_JOINT;
693 
694  // Check if there are no redundant joints
695  if(free_params_.size()==0)
696  {
697  ROS_DEBUG_STREAM_NAMED("ikfast","No need to search since no free params/redundant joints");
698 
699  // Find first IK solution, within joint limits
700  if(!getPositionIK(ik_pose, ik_seed_state, solution, error_code))
701  {
702  ROS_DEBUG_STREAM_NAMED("ikfast","No solution whatsoever");
703  error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
704  return false;
705  }
706 
707  // check for collisions if a callback is provided
708  if( !solution_callback.empty() )
709  {
710  solution_callback(ik_pose, solution, error_code);
711  if(error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
712  {
713  ROS_DEBUG_STREAM_NAMED("ikfast","Solution passes callback");
714  return true;
715  }
716  else
717  {
718  ROS_DEBUG_STREAM_NAMED("ikfast","Solution has error code " << error_code);
719  return false;
720  }
721  }
722  else
723  {
724  return true; // no collision check callback provided
725  }
726  }
727 
728  // -------------------------------------------------------------------------------------------------
729  // Error Checking
730  if(!active_)
731  {
732  ROS_ERROR_STREAM_NAMED("ikfast","Kinematics not active");
733  error_code.val = error_code.NO_IK_SOLUTION;
734  return false;
735  }
736 
737  if(ik_seed_state.size() != num_joints_)
738  {
739  ROS_ERROR_STREAM_NAMED("ikfast","Seed state must have size " << num_joints_ << " instead of size " << ik_seed_state.size());
740  error_code.val = error_code.NO_IK_SOLUTION;
741  return false;
742  }
743 
744  if(!consistency_limits.empty() && consistency_limits.size() != num_joints_)
745  {
746  ROS_ERROR_STREAM_NAMED("ikfast","Consistency limits be empty or must have size " << num_joints_ << " instead of size " << consistency_limits.size());
747  error_code.val = error_code.NO_IK_SOLUTION;
748  return false;
749  }
750 
751 
752  // -------------------------------------------------------------------------------------------------
753  // Initialize
754 
755  KDL::Frame frame;
756  tf::poseMsgToKDL(ik_pose,frame);
757 
758  std::vector<double> vfree(free_params_.size());
759 
760  ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
761  int counter = 0;
762 
763  double initial_guess = ik_seed_state[free_params_[0]];
764  vfree[0] = initial_guess;
765 
766  // -------------------------------------------------------------------------------------------------
767  // Handle consitency limits if needed
768  int num_positive_increments;
769  int num_negative_increments;
770 
771  if(!consistency_limits.empty())
772  {
773  // moveit replaced consistency_limit (scalar) w/ consistency_limits (vector)
774  // Assume [0]th free_params element for now. Probably wrong.
775  double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess+consistency_limits[free_params_[0]]);
776  double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess-consistency_limits[free_params_[0]]);
777 
778  num_positive_increments = (int)((max_limit-initial_guess)/search_discretization_);
779  num_negative_increments = (int)((initial_guess-min_limit)/search_discretization_);
780  }
781  else // no consitency limits provided
782  {
783  num_positive_increments = (joint_max_vector_[free_params_[0]]-initial_guess)/search_discretization_;
784  num_negative_increments = (initial_guess-joint_min_vector_[free_params_[0]])/search_discretization_;
785  }
786 
787  // -------------------------------------------------------------------------------------------------
788  // Begin searching
789 
790  ROS_DEBUG_STREAM_NAMED("ikfast","Free param is " << free_params_[0] << " initial guess is " << initial_guess << ", # positive increments: " << num_positive_increments << ", # negative increments: " << num_negative_increments);
791  if ((search_mode & OPTIMIZE_MAX_JOINT) && (num_positive_increments + num_negative_increments) > 1000)
792  ROS_WARN_STREAM_ONCE_NAMED("ikfast", "Large search space, consider increasing the search discretization");
793 
794  double best_costs = -1.0;
795  std::vector<double> best_solution;
796  int nattempts = 0, nvalid = 0;
797 
798  while(true)
799  {
800  IkSolutionList<IkReal> solutions;
801  int numsol = solve(frame,vfree, solutions);
802 
803  ROS_DEBUG_STREAM_NAMED("ikfast","Found " << numsol << " solutions from IKFast");
804 
805  //ROS_INFO("%f",vfree[0]);
806 
807  if( numsol > 0 )
808  {
809  for(int s = 0; s < numsol; ++s)
810  {
811  nattempts++;
812  std::vector<double> sol;
813  getSolution(solutions,s,sol);
814 
815  bool obeys_limits = true;
816  for(unsigned int i = 0; i < sol.size(); i++)
817  {
818  if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i]))
819  {
820  obeys_limits = false;
821  break;
822  }
823  //ROS_INFO_STREAM_NAMED("ikfast","Num " << i << " value " << sol[i] << " has limits " << joint_has_limits_vector_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i]);
824  }
825  if(obeys_limits)
826  {
827  getSolution(solutions,s,solution);
828 
829  // This solution is within joint limits, now check if in collision (if callback provided)
830  if(!solution_callback.empty())
831  {
832  solution_callback(ik_pose, solution, error_code);
833  }
834  else
835  {
836  error_code.val = error_code.SUCCESS;
837  }
838 
839  if(error_code.val == error_code.SUCCESS)
840  {
841  nvalid++;
842  if (search_mode & OPTIMIZE_MAX_JOINT)
843  {
844  // Costs for solution: Largest joint motion
845  double costs = 0.0;
846  for(unsigned int i = 0; i < solution.size(); i++)
847  {
848  double d = fabs(ik_seed_state[i] - solution[i]);
849  if (d > costs)
850  costs = d;
851  }
852  if (costs < best_costs || best_costs == -1.0)
853  {
854  best_costs = costs;
855  best_solution = solution;
856  }
857  }
858  else
859  // Return first feasible solution
860  return true;
861  }
862  }
863  }
864  }
865 
866  if(!getCount(counter, num_positive_increments, -num_negative_increments))
867  {
868  // Everything searched
869  error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
870  break;
871  }
872 
873  vfree[0] = initial_guess+search_discretization_*counter;
874  //ROS_DEBUG_STREAM_NAMED("ikfast","Attempt " << counter << " with 0th free joint having value " << vfree[0]);
875  }
876 
877  ROS_DEBUG_STREAM_NAMED("ikfast", "Valid solutions: " << nvalid << "/" << nattempts);
878 
879  if ((search_mode & OPTIMIZE_MAX_JOINT) && best_costs != -1.0)
880  {
881  solution = best_solution;
882  error_code.val = error_code.SUCCESS;
883  return true;
884  }
885 
886  // No solution found
887  error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
888  return false;
889 }
890 
891 // Used when there are no redundant joints - aka no free params
892 bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose,
893  const std::vector<double> &ik_seed_state,
894  std::vector<double> &solution,
895  moveit_msgs::MoveItErrorCodes &error_code,
896  const kinematics::KinematicsQueryOptions &options) const
897 {
898  ROS_DEBUG_STREAM_NAMED("ikfast","getPositionIK");
899 
900  if(!active_)
901  {
902  ROS_ERROR("kinematics not active");
903  return false;
904  }
905 
906  std::vector<double> vfree(free_params_.size());
907  for(std::size_t i = 0; i < free_params_.size(); ++i)
908  {
909  int p = free_params_[i];
910  ROS_ERROR("%u is %f",p,ik_seed_state[p]); // DTC
911  vfree[i] = ik_seed_state[p];
912  }
913 
914  KDL::Frame frame;
915  tf::poseMsgToKDL(ik_pose,frame);
916 
917  IkSolutionList<IkReal> solutions;
918  int numsol = solve(frame,vfree,solutions);
919 
920  ROS_DEBUG_STREAM_NAMED("ikfast","Found " << numsol << " solutions from IKFast");
921 
922  if(numsol)
923  {
924  for(int s = 0; s < numsol; ++s)
925  {
926  std::vector<double> sol;
927  getSolution(solutions,s,sol);
928  ROS_DEBUG_NAMED("ikfast","Sol %d: %e %e %e %e %e %e", s, sol[0], sol[1], sol[2], sol[3], sol[4], sol[5]);
929 
930  bool obeys_limits = true;
931  for(unsigned int i = 0; i < sol.size(); i++)
932  {
933  // Add tolerance to limit check
934  if(joint_has_limits_vector_[i] && ( (sol[i] < (joint_min_vector_[i]-LIMIT_TOLERANCE)) ||
935  (sol[i] > (joint_max_vector_[i]+LIMIT_TOLERANCE)) ) )
936  {
937  // One element of solution is not within limits
938  obeys_limits = false;
939  ROS_DEBUG_STREAM_NAMED("ikfast","Not in limits! " << i << " value " << sol[i] << " has limit: " << joint_has_limits_vector_[i] << " being " << joint_min_vector_[i] << " to " << joint_max_vector_[i]);
940  break;
941  }
942  }
943  if(obeys_limits)
944  {
945  // All elements of solution obey limits
946  getSolution(solutions,s,solution);
947  error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
948  return true;
949  }
950  }
951  }
952  else
953  {
954  ROS_DEBUG_STREAM_NAMED("ikfast","No IK solution");
955  }
956 
957  error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
958  return false;
959 }
960 
961 
962 
963 } // end namespace
964 
965 //register IKFastKinematicsPlugin as a KinematicsBase implementation
d
virtual const IkSolutionBase< T > & GetSolution(size_t index) const
returns the solution pointer
end effector origin reaches desired 3D translation, manipulator direction makes a specific angle with...
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(...)
#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,...)
bit is set if the ikparameterization contains custom data, this is only used when serializing the ik ...
bool getCount(int &count, const int &max_count, const int &min_count) const
virtual size_t GetNumSolutions() const
returns the number of solutions stored
XmlRpcServer s
void getClosestSolution(const IkSolutionList< IkReal > &solutions, const std::vector< double > &ik_seed_state, std::vector< double > &solution) const
IkParameterizationType
The types of inverse kinematics parameterizations supported.
The discrete solutions are returned in this structure.
SEARCH_MODE
Search modes for searchPositionIK(), see there.
#define ROS_INFO_STREAM_NAMED(name, args)
#define M_PI
virtual void Clear()
clears all current solutions, note that any memory addresses returned from GetSolution will be invali...
virtual const std::vector< int > & GetFree() const =0
Gets the indices of the configuration space that have to be preset before a full solution can be retu...
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...
local point on end effector origin reaches desired 3D global point
direction on end effector coordinate system points to desired 3D position
Rotation M
#define ROS_DEBUG_NAMED(name,...)
bit is set if the data represents the time-derivate velocity of an IkParameterization ...
#define ROS_WARN_STREAM_ONCE_NAMED(name, args)
virtual const std::string & getTipFrame() const
#define ROS_FATAL_STREAM_NAMED(name, args)
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
bool searchParam(const std::string &key, std::string &result) const
bool param(const std::string &param_name, T &param_val, const T &default_val) const
double data[3]
PLUGINLIB_EXPORT_CLASS(ikfast_kinematics_plugin::IKFastKinematicsPlugin, kinematics::KinematicsBase)
void poseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k)
2D translation along XY plane and 1D rotation around Z axis. The offset of the rotation is measured s...
end effector origin reaches desired 3D translation, manipulator direction makes a specific angle with...
end effector origin reaches desired 3D translation, manipulator direction makes a specific angle with...
double harmonize(const std::vector< double > &ik_seed_state, std::vector< double > &solution) const
virtual void setValues(const std::string &robot_description, const std::string &group_name, const std::string &base_frame, const std::string &tip_frame, double search_discretization)
def xml_string(rootXml, addHeader=True)
end effector origin and direction reaches desired 3D translation and direction. Can be thought of as ...
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn
end effector origin reaches desired 3D translation, manipulator direction needs to be orthogonal to y...
virtual void GetSolution(T *solution, const T *freevalues) const =0
gets a concrete solution
end effector origin reaches desired 3D translation, manipulator direction needs to be orthogonal to x...
void poseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m)
Default implementation of IkSolutionListBase.
ray on end effector coordinate system reaches desired global ray
bool getParam(const std::string &key, std::string &s) const
end effector origin reaches desired 3D translation, manipulator direction needs to be orthogonal to z...
#define ROS_FATAL_NAMED(name,...)
#define ROS_ERROR_NAMED(name,...)
static Time now()
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.
direction on end effector coordinate system reaches desired direction
#define ROS_ERROR(...)
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
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]


fanuc_lrmate200ib_moveit_plugins
Author(s): G.A. vd. Hoorn (TU Delft Robotics Institute)
autogenerated on Sun Apr 4 2021 02:20:29