fanuc_m20ia_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;
55 
57 {
58 
59 #define IKFAST_NO_MAIN // Don't include main() from IKFast
60 
67  IKP_None=0,
68  IKP_Transform6D=0x67000001,
69  IKP_Rotation3D=0x34000002,
70  IKP_Translation3D=0x33000003,
71  IKP_Direction3D=0x23000004,
72  IKP_Ray4D=0x46000005,
73  IKP_Lookat3D=0x23000006,
74  IKP_TranslationDirection5D=0x56000007,
75  IKP_TranslationXY2D=0x22000008,
77  IKP_TranslationLocalGlobal6D=0x3600000a,
78 
79  IKP_TranslationXAxisAngle4D=0x4400000b,
80  IKP_TranslationYAxisAngle4D=0x4400000c,
81  IKP_TranslationZAxisAngle4D=0x4400000d,
82 
86 
88 
89  IKP_VelocityDataBit = 0x00008000,
106 
107  IKP_UniqueIdMask = 0x0000ffff,
108  IKP_CustomDataBit = 0x00010000,
109 };
110 
111 // Code generated by IKFast56/61
113 
115 {
116  std::vector<std::string> joint_names_;
117  std::vector<double> joint_min_vector_;
118  std::vector<double> joint_max_vector_;
119  std::vector<bool> joint_has_limits_vector_;
120  std::vector<std::string> link_names_;
121  size_t num_joints_;
122  std::vector<int> free_params_;
123  bool active_; // Internal variable that indicates whether solvers are configured and ready
124 
125  const std::vector<std::string>& getJointNames() const { return joint_names_; }
126  const std::vector<std::string>& getLinkNames() const { return link_names_; }
127 
128 public:
129 
133  IKFastKinematicsPlugin():active_(false){}
134 
144  // Returns the first IK solution that is within joint limits, this is called by get_ik() service
145  bool getPositionIK(const geometry_msgs::Pose &ik_pose,
146  const std::vector<double> &ik_seed_state,
147  std::vector<double> &solution,
148  moveit_msgs::MoveItErrorCodes &error_code,
150 
159  bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
160  const std::vector<double> &ik_seed_state,
161  double timeout,
162  std::vector<double> &solution,
163  moveit_msgs::MoveItErrorCodes &error_code,
165 
175  bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
176  const std::vector<double> &ik_seed_state,
177  double timeout,
178  const std::vector<double> &consistency_limits,
179  std::vector<double> &solution,
180  moveit_msgs::MoveItErrorCodes &error_code,
182 
191  bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
192  const std::vector<double> &ik_seed_state,
193  double timeout,
194  std::vector<double> &solution,
195  const IKCallbackFn &solution_callback,
196  moveit_msgs::MoveItErrorCodes &error_code,
198 
209  bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
210  const std::vector<double> &ik_seed_state,
211  double timeout,
212  const std::vector<double> &consistency_limits,
213  std::vector<double> &solution,
214  const IKCallbackFn &solution_callback,
215  moveit_msgs::MoveItErrorCodes &error_code,
217 
229  bool getPositionFK(const std::vector<std::string> &link_names,
230  const std::vector<double> &joint_angles,
231  std::vector<geometry_msgs::Pose> &poses) const;
232 
233 private:
234 
235  bool initialize(const std::string &robot_description,
236  const std::string& group_name,
237  const std::string& base_name,
238  const std::string& tip_name,
239  double search_discretization);
240 
245  int solve(KDL::Frame &pose_frame, const std::vector<double> &vfree, IkSolutionList<IkReal> &solutions) const;
246 
250  void getSolution(const IkSolutionList<IkReal> &solutions, int i, std::vector<double>& solution) const;
251 
252  double harmonize(const std::vector<double> &ik_seed_state, std::vector<double> &solution) const;
253  //void getOrderedSolutions(const std::vector<double> &ik_seed_state, std::vector<std::vector<double> >& solslist);
254  void getClosestSolution(const IkSolutionList<IkReal> &solutions, const std::vector<double> &ik_seed_state, std::vector<double> &solution) const;
255  void fillFreeParams(int count, int *array);
256  bool getCount(int &count, const int &max_count, const int &min_count) const;
257 
258 }; // end class
259 
260 bool IKFastKinematicsPlugin::initialize(const std::string &robot_description,
261  const std::string& group_name,
262  const std::string& base_name,
263  const std::string& tip_name,
264  double search_discretization)
265 {
266  setValues(robot_description, group_name, base_name, tip_name, search_discretization);
267 
268  ros::NodeHandle node_handle("~/"+group_name);
269 
270  std::string robot;
271  node_handle.param("robot",robot,std::string());
272 
273  // IKFast56/61
276 
277  if(free_params_.size() > 1)
278  {
279  ROS_FATAL("Only one free joint paramter supported!");
280  return false;
281  }
282 
283  urdf::Model robot_model;
284  std::string xml_string;
285 
286  std::string urdf_xml,full_urdf_xml;
287  node_handle.param("urdf_xml",urdf_xml,robot_description);
288  node_handle.searchParam(urdf_xml,full_urdf_xml);
290  ROS_DEBUG_NAMED("ikfast","Reading xml file from parameter server");
291  if (!node_handle.getParam(full_urdf_xml, xml_string))
292  {
293  ROS_FATAL_NAMED("ikfast","Could not load the xml from parameter server: %s", urdf_xml.c_str());
294  return false;
295  }
296 
297  node_handle.param(full_urdf_xml,xml_string,std::string());
298  robot_model.initString(xml_string);
299 
300  ROS_DEBUG_STREAM_NAMED("ikfast","Reading joints and links from URDF");
301 
302  urdf::LinkConstSharedPtr link = robot_model.getLink(tip_frame_);
303  while(link->name != base_frame_ && joint_names_.size() <= num_joints_)
304  {
305  ROS_DEBUG_NAMED("ikfast","Link %s",link->name.c_str());
306  link_names_.push_back(link->name);
307  urdf::JointSharedPtr joint = link->parent_joint;
308  if(joint)
309  {
310  if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
311  {
312  ROS_DEBUG_STREAM_NAMED("ikfast","Adding joint " << joint->name );
313 
314  joint_names_.push_back(joint->name);
315  float lower, upper;
316  int hasLimits;
317  if ( joint->type != urdf::Joint::CONTINUOUS )
318  {
319  if(joint->safety)
320  {
321  lower = joint->safety->soft_lower_limit;
322  upper = joint->safety->soft_upper_limit;
323  } else {
324  lower = joint->limits->lower;
325  upper = joint->limits->upper;
326  }
327  hasLimits = 1;
328  }
329  else
330  {
331  lower = -M_PI;
332  upper = M_PI;
333  hasLimits = 0;
334  }
335  if(hasLimits)
336  {
337  joint_has_limits_vector_.push_back(true);
338  joint_min_vector_.push_back(lower);
339  joint_max_vector_.push_back(upper);
340  }
341  else
342  {
343  joint_has_limits_vector_.push_back(false);
344  joint_min_vector_.push_back(-M_PI);
345  joint_max_vector_.push_back(M_PI);
346  }
347  }
348  } else
349  {
350  ROS_WARN_NAMED("ikfast","no joint corresponding to %s",link->name.c_str());
351  }
352  link = link->getParent();
353  }
354 
355  if(joint_names_.size() != num_joints_)
356  {
357  ROS_FATAL_STREAM_NAMED("ikfast","Joint numbers mismatch: URDF has " << joint_names_.size() << " and IKFast has " << num_joints_);
358  return false;
359  }
360 
361  std::reverse(link_names_.begin(),link_names_.end());
362  std::reverse(joint_names_.begin(),joint_names_.end());
363  std::reverse(joint_min_vector_.begin(),joint_min_vector_.end());
364  std::reverse(joint_max_vector_.begin(),joint_max_vector_.end());
365  std::reverse(joint_has_limits_vector_.begin(), joint_has_limits_vector_.end());
366 
367  for(size_t i=0; i <num_joints_; ++i)
368  ROS_INFO_STREAM_NAMED("ikfast",joint_names_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i] << " " << joint_has_limits_vector_[i]);
369 
370  active_ = true;
371  return true;
372 }
373 
374 int IKFastKinematicsPlugin::solve(KDL::Frame &pose_frame, const std::vector<double> &vfree, IkSolutionList<IkReal> &solutions) const
375 {
376  // IKFast56/61
377  solutions.Clear();
378 
379  double trans[3];
380  trans[0] = pose_frame.p[0];//-.18;
381  trans[1] = pose_frame.p[1];
382  trans[2] = pose_frame.p[2];
383 
384  KDL::Rotation mult;
385  KDL::Vector direction;
386 
387  switch (GetIkType())
388  {
389  case IKP_Transform6D:
390  // For **Transform6D**, eerot is 9 values for the 3x3 rotation matrix.
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_Translation3D:
432  case IKP_Lookat3D:
433  case IKP_TranslationXY2D:
438  ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet.");
439  return 0;
440 
441  default:
442  ROS_ERROR_NAMED("ikfast", "Unknown IkParameterizationType! Was the solver generated with an incompatible version of Openrave?");
443  return 0;
444  }
445 }
446 
447 void IKFastKinematicsPlugin::getSolution(const IkSolutionList<IkReal> &solutions, int i, std::vector<double>& solution) const
448 {
449  solution.clear();
450  solution.resize(num_joints_);
451 
452  // IKFast56/61
453  const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
454  std::vector<IkReal> vsolfree( sol.GetFree().size() );
455  sol.GetSolution(&solution[0],vsolfree.size()>0?&vsolfree[0]:NULL);
456 
457  // std::cout << "solution " << i << ":" ;
458  // for(int j=0;j<num_joints_; ++j)
459  // std::cout << " " << solution[j];
460  // std::cout << std::endl;
461 
462  //ROS_ERROR("%f %d",solution[2],vsolfree.size());
463 }
464 
465 double IKFastKinematicsPlugin::harmonize(const std::vector<double> &ik_seed_state, std::vector<double> &solution) const
466 {
467  double dist_sqr = 0;
468  std::vector<double> ss = ik_seed_state;
469  for(size_t i=0; i< ik_seed_state.size(); ++i)
470  {
471  while(ss[i] > 2*M_PI) {
472  ss[i] -= 2*M_PI;
473  }
474  while(ss[i] < 2*M_PI) {
475  ss[i] += 2*M_PI;
476  }
477  while(solution[i] > 2*M_PI) {
478  solution[i] -= 2*M_PI;
479  }
480  while(solution[i] < 2*M_PI) {
481  solution[i] += 2*M_PI;
482  }
483  dist_sqr += fabs(ik_seed_state[i] - solution[i]);
484  }
485  return dist_sqr;
486 }
487 
488 // void IKFastKinematicsPlugin::getOrderedSolutions(const std::vector<double> &ik_seed_state,
489 // std::vector<std::vector<double> >& solslist)
490 // {
491 // std::vector<double>
492 // double mindist = 0;
493 // int minindex = -1;
494 // std::vector<double> sol;
495 // for(size_t i=0;i<solslist.size();++i){
496 // getSolution(i,sol);
497 // double dist = harmonize(ik_seed_state, sol);
498 // //std::cout << "dist[" << i << "]= " << dist << std::endl;
499 // if(minindex == -1 || dist<mindist){
500 // minindex = i;
501 // mindist = dist;
502 // }
503 // }
504 // if(minindex >= 0){
505 // getSolution(minindex,solution);
506 // harmonize(ik_seed_state, solution);
507 // index = minindex;
508 // }
509 // }
510 
511 void IKFastKinematicsPlugin::getClosestSolution(const IkSolutionList<IkReal> &solutions, const std::vector<double> &ik_seed_state, std::vector<double> &solution) const
512 {
513  double mindist = DBL_MAX;
514  int minindex = -1;
515  std::vector<double> sol;
516 
517  // IKFast56/61
518  for(size_t i=0; i < solutions.GetNumSolutions(); ++i)
519  {
520  getSolution(solutions, i,sol);
521  double dist = harmonize(ik_seed_state, sol);
522  ROS_INFO_STREAM_NAMED("ikfast","Dist " << i << " dist " << dist);
523  //std::cout << "dist[" << i << "]= " << dist << std::endl;
524  if(minindex == -1 || dist<mindist){
525  minindex = i;
526  mindist = dist;
527  }
528  }
529  if(minindex >= 0){
530  getSolution(solutions, minindex,solution);
531  harmonize(ik_seed_state, solution);
532  }
533 }
534 
535 void IKFastKinematicsPlugin::fillFreeParams(int count, int *array)
536 {
537  free_params_.clear();
538  for(int i=0; i<count;++i) free_params_.push_back(array[i]);
539 }
540 
541 bool IKFastKinematicsPlugin::getCount(int &count, const int &max_count, const int &min_count) const
542 {
543  if(count > 0)
544  {
545  if(-count >= min_count)
546  {
547  count = -count;
548  return true;
549  }
550  else if(count+1 <= max_count)
551  {
552  count = count+1;
553  return true;
554  }
555  else
556  {
557  return false;
558  }
559  }
560  else
561  {
562  if(1-count <= max_count)
563  {
564  count = 1-count;
565  return true;
566  }
567  else if(count-1 >= min_count)
568  {
569  count = count -1;
570  return true;
571  }
572  else
573  return false;
574  }
575 }
576 
577 bool IKFastKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
578  const std::vector<double> &joint_angles,
579  std::vector<geometry_msgs::Pose> &poses) const
580 {
581 #ifndef IKTYPE_TRANSFORM_6D
582  ROS_ERROR_NAMED("ikfast", "Can only compute FK for IKTYPE_TRANSFORM_6D!");
583  return false;
584 #endif
585 
586  KDL::Frame p_out;
587  if(link_names.size() == 0) {
588  ROS_WARN_STREAM_NAMED("ikfast","Link names with nothing");
589  return false;
590  }
591 
592  if(link_names.size()!=1 || link_names[0]!=tip_frame_){
593  ROS_ERROR_NAMED("ikfast","Can compute FK for %s only",tip_frame_.c_str());
594  return false;
595  }
596 
597  bool valid = true;
598 
599  IkReal eerot[9],eetrans[3];
600  IkReal angles[joint_angles.size()];
601  for (unsigned char i=0; i < joint_angles.size(); i++)
602  angles[i] = joint_angles[i];
603 
604  // IKFast56/61
605  ComputeFk(angles,eetrans,eerot);
606 
607  for(int i=0; i<3;++i)
608  p_out.p.data[i] = eetrans[i];
609 
610  for(int i=0; i<9;++i)
611  p_out.M.data[i] = eerot[i];
612 
613  poses.resize(1);
614  tf::poseKDLToMsg(p_out,poses[0]);
615 
616  return valid;
617 }
618 
619 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
620  const std::vector<double> &ik_seed_state,
621  double timeout,
622  std::vector<double> &solution,
623  moveit_msgs::MoveItErrorCodes &error_code,
624  const kinematics::KinematicsQueryOptions &options) const
625 {
626  const IKCallbackFn solution_callback = 0;
627  std::vector<double> consistency_limits;
628 
629  return searchPositionIK(ik_pose,
630  ik_seed_state,
631  timeout,
632  consistency_limits,
633  solution,
634  solution_callback,
635  error_code,
636  options);
637 }
638 
639 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
640  const std::vector<double> &ik_seed_state,
641  double timeout,
642  const std::vector<double> &consistency_limits,
643  std::vector<double> &solution,
644  moveit_msgs::MoveItErrorCodes &error_code,
645  const kinematics::KinematicsQueryOptions &options) const
646 {
647  const IKCallbackFn solution_callback = 0;
648  return searchPositionIK(ik_pose,
649  ik_seed_state,
650  timeout,
651  consistency_limits,
652  solution,
653  solution_callback,
654  error_code,
655  options);
656 }
657 
658 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
659  const std::vector<double> &ik_seed_state,
660  double timeout,
661  std::vector<double> &solution,
662  const IKCallbackFn &solution_callback,
663  moveit_msgs::MoveItErrorCodes &error_code,
664  const kinematics::KinematicsQueryOptions &options) const
665 {
666  std::vector<double> consistency_limits;
667  return searchPositionIK(ik_pose,
668  ik_seed_state,
669  timeout,
670  consistency_limits,
671  solution,
672  solution_callback,
673  error_code,
674  options);
675 }
676 
677 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
678  const std::vector<double> &ik_seed_state,
679  double timeout,
680  const std::vector<double> &consistency_limits,
681  std::vector<double> &solution,
682  const IKCallbackFn &solution_callback,
683  moveit_msgs::MoveItErrorCodes &error_code,
684  const kinematics::KinematicsQueryOptions &options) const
685 {
686  ROS_DEBUG_STREAM_NAMED("ikfast","searchPositionIK");
687 
688  // Check if there are no redundant joints
689  if(free_params_.size()==0)
690  {
691  ROS_DEBUG_STREAM_NAMED("ikfast","No need to search since no free params/redundant joints");
692 
693  // Find first IK solution, within joint limits
694  if(!getPositionIK(ik_pose, ik_seed_state, solution, error_code))
695  {
696  ROS_DEBUG_STREAM_NAMED("ikfast","No solution whatsoever");
697  error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
698  return false;
699  }
700 
701  // check for collisions if a callback is provided
702  if( !solution_callback.empty() )
703  {
704  solution_callback(ik_pose, solution, error_code);
705  if(error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
706  {
707  ROS_DEBUG_STREAM_NAMED("ikfast","Solution passes callback");
708  return true;
709  }
710  else
711  {
712  ROS_DEBUG_STREAM_NAMED("ikfast","Solution has error code " << error_code);
713  return false;
714  }
715  }
716  else
717  {
718  return true; // no collision check callback provided
719  }
720  }
721 
722  // -------------------------------------------------------------------------------------------------
723  // Error Checking
724  if(!active_)
725  {
726  ROS_ERROR_STREAM_NAMED("ikfast","Kinematics not active");
727  error_code.val = error_code.NO_IK_SOLUTION;
728  return false;
729  }
730 
731  if(ik_seed_state.size() != num_joints_)
732  {
733  ROS_ERROR_STREAM_NAMED("ikfast","Seed state must have size " << num_joints_ << " instead of size " << ik_seed_state.size());
734  error_code.val = error_code.NO_IK_SOLUTION;
735  return false;
736  }
737 
738  if(!consistency_limits.empty() && consistency_limits.size() != num_joints_)
739  {
740  ROS_ERROR_STREAM_NAMED("ikfast","Consistency limits be empty or must have size " << num_joints_ << " instead of size " << consistency_limits.size());
741  error_code.val = error_code.NO_IK_SOLUTION;
742  return false;
743  }
744 
745 
746  // -------------------------------------------------------------------------------------------------
747  // Initialize
748 
749  KDL::Frame frame;
750  tf::poseMsgToKDL(ik_pose,frame);
751 
752  std::vector<double> vfree(free_params_.size());
753 
754  ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
755  int counter = 0;
756 
757  double initial_guess = ik_seed_state[free_params_[0]];
758  vfree[0] = initial_guess;
759 
760  // -------------------------------------------------------------------------------------------------
761  // Handle consitency limits if needed
762  int num_positive_increments;
763  int num_negative_increments;
764 
765  if(!consistency_limits.empty())
766  {
767  // moveit replaced consistency_limit (scalar) w/ consistency_limits (vector)
768  // Assume [0]th free_params element for now. Probably wrong.
769  double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess+consistency_limits[free_params_[0]]);
770  double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess-consistency_limits[free_params_[0]]);
771 
772  num_positive_increments = (int)((max_limit-initial_guess)/search_discretization_);
773  num_negative_increments = (int)((initial_guess-min_limit)/search_discretization_);
774  }
775  else // no consitency limits provided
776  {
777  num_positive_increments = (joint_max_vector_[free_params_[0]]-initial_guess)/search_discretization_;
778  num_negative_increments = (initial_guess-joint_min_vector_[free_params_[0]])/search_discretization_;
779  }
780 
781  // -------------------------------------------------------------------------------------------------
782  // Begin searching
783 
784  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);
785 
786  while(true)
787  {
788  IkSolutionList<IkReal> solutions;
789  int numsol = solve(frame,vfree, solutions);
790 
791  ROS_DEBUG_STREAM_NAMED("ikfast","Found " << numsol << " solutions from IKFast");
792 
793  //ROS_INFO("%f",vfree[0]);
794 
795  if( numsol > 0 )
796  {
797  for(int s = 0; s < numsol; ++s)
798  {
799  std::vector<double> sol;
800  getSolution(solutions,s,sol);
801 
802  bool obeys_limits = true;
803  for(unsigned int i = 0; i < sol.size(); i++)
804  {
805  if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i]))
806  {
807  obeys_limits = false;
808  break;
809  }
810  //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]);
811  }
812  if(obeys_limits)
813  {
814  getSolution(solutions,s,solution);
815 
816  // This solution is within joint limits, now check if in collision (if callback provided)
817  if(!solution_callback.empty())
818  {
819  solution_callback(ik_pose, solution, error_code);
820  }
821  else
822  {
823  error_code.val = error_code.SUCCESS;
824  }
825 
826  if(error_code.val == error_code.SUCCESS)
827  {
828  return true;
829  }
830  }
831  }
832  }
833 
834  if(!getCount(counter, num_positive_increments, num_negative_increments))
835  {
836  error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
837  return false;
838  }
839 
840  vfree[0] = initial_guess+search_discretization_*counter;
841  ROS_DEBUG_STREAM_NAMED("ikfast","Attempt " << counter << " with 0th free joint having value " << vfree[0]);
842  }
843 
844  // not really needed b/c shouldn't ever get here
845  error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
846  return false;
847 }
848 
849 // Used when there are no redundant joints - aka no free params
850 bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose,
851  const std::vector<double> &ik_seed_state,
852  std::vector<double> &solution,
853  moveit_msgs::MoveItErrorCodes &error_code,
854  const kinematics::KinematicsQueryOptions &options) const
855 {
856  ROS_DEBUG_STREAM_NAMED("ikfast","getPositionIK");
857 
858  if(!active_)
859  {
860  ROS_ERROR("kinematics not active");
861  return false;
862  }
863 
864  std::vector<double> vfree(free_params_.size());
865  for(std::size_t i = 0; i < free_params_.size(); ++i)
866  {
867  int p = free_params_[i];
868  ROS_ERROR("%u is %f",p,ik_seed_state[p]); // DTC
869  vfree[i] = ik_seed_state[p];
870  }
871 
872  KDL::Frame frame;
873  tf::poseMsgToKDL(ik_pose,frame);
874 
875  IkSolutionList<IkReal> solutions;
876  int numsol = solve(frame,vfree,solutions);
877 
878  ROS_DEBUG_STREAM_NAMED("ikfast","Found " << numsol << " solutions from IKFast");
879 
880  if(numsol)
881  {
882  for(int s = 0; s < numsol; ++s)
883  {
884  std::vector<double> sol;
885  getSolution(solutions,s,sol);
886  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]);
887 
888  bool obeys_limits = true;
889  for(unsigned int i = 0; i < sol.size(); i++)
890  {
891  // Add tolerance to limit check
892  if(joint_has_limits_vector_[i] && ( (sol[i] < (joint_min_vector_[i]-LIMIT_TOLERANCE)) ||
893  (sol[i] > (joint_max_vector_[i]+LIMIT_TOLERANCE)) ) )
894  {
895  // One element of solution is not within limits
896  obeys_limits = false;
897  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]);
898  break;
899  }
900  }
901  if(obeys_limits)
902  {
903  // All elements of solution obey limits
904  getSolution(solutions,s,solution);
905  error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
906  return true;
907  }
908  }
909  }
910  else
911  {
912  ROS_DEBUG_STREAM_NAMED("ikfast","No IK solution");
913  }
914 
915  error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
916  return false;
917 }
918 
919 
920 
921 } // end namespace
922 
923 //register IKFastKinematicsPlugin as a KinematicsBase implementation
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.
#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_FATAL_STREAM_NAMED(name, args)
options
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]
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...
PLUGINLIB_EXPORT_CLASS(ikfast_kinematics_plugin::IKFastKinematicsPlugin, kinematics::KinematicsBase)
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_m20ia_moveit_plugins
Author(s): G.A. vd. Hoorn (TU Delft Robotics Institute)
autogenerated on Sun Apr 4 2021 02:20:41