00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048 #include <ros/ros.h>
00049 #include <moveit/kinematics_base/kinematics_base.h>
00050 #include <urdf/model.h>
00051 #include <tf_conversions/tf_kdl.h>
00052
00053
00054 const double LIMIT_TOLERANCE = .0000001;
00055
00056 namespace ikfast_kinematics_plugin
00057 {
00058
00059 #define IKFAST_NO_MAIN // Don't include main() from IKFast
00060
00061
00062 #include "turtlebot_arm_arm_ikfast_solver.cpp"
00063
00064 class IKFastKinematicsPlugin : public kinematics::KinematicsBase
00065 {
00066 std::vector<std::string> joint_names_;
00067 std::vector<double> joint_min_vector_;
00068 std::vector<double> joint_max_vector_;
00069 std::vector<bool> joint_has_limits_vector_;
00070 std::vector<std::string> link_names_;
00071 size_t num_joints_;
00072 std::vector<int> free_params_;
00073 bool active_;
00074
00075 const std::vector<std::string>& getJointNames() const { return joint_names_; }
00076 const std::vector<std::string>& getLinkNames() const { return link_names_; }
00077
00078 public:
00079
00083 IKFastKinematicsPlugin():active_(false){}
00084
00095
00096 bool getPositionIK(const geometry_msgs::Pose &ik_pose,
00097 const std::vector<double> &ik_seed_state,
00098 std::vector<double> &solution,
00099 moveit_msgs::MoveItErrorCodes &error_code,
00100 bool lock_redundant_joints = false) const;
00101
00110 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00111 const std::vector<double> &ik_seed_state,
00112 double timeout,
00113 std::vector<double> &solution,
00114 moveit_msgs::MoveItErrorCodes &error_code,
00115 bool lock_redundant_joints = false) const;
00116
00126 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00127 const std::vector<double> &ik_seed_state,
00128 double timeout,
00129 const std::vector<double> &consistency_limits,
00130 std::vector<double> &solution,
00131 moveit_msgs::MoveItErrorCodes &error_code,
00132 bool lock_redundant_joints = false) const;
00133
00142 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00143 const std::vector<double> &ik_seed_state,
00144 double timeout,
00145 std::vector<double> &solution,
00146 const IKCallbackFn &solution_callback,
00147 moveit_msgs::MoveItErrorCodes &error_code,
00148 bool lock_redundant_joints = false) const;
00149
00160 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00161 const std::vector<double> &ik_seed_state,
00162 double timeout,
00163 const std::vector<double> &consistency_limits,
00164 std::vector<double> &solution,
00165 const IKCallbackFn &solution_callback,
00166 moveit_msgs::MoveItErrorCodes &error_code,
00167 bool lock_redundant_joints = false) const;
00168
00180 bool getPositionFK(const std::vector<std::string> &link_names,
00181 const std::vector<double> &joint_angles,
00182 std::vector<geometry_msgs::Pose> &poses) const;
00183
00184 private:
00185
00186 bool initialize(const std::string &robot_description,
00187 const std::string& group_name,
00188 const std::string& base_name,
00189 const std::string& tip_name,
00190 double search_discretization);
00191
00196 int solve(KDL::Frame &pose_frame, const std::vector<double> &vfree, IkSolutionList<IkReal> &solutions) const;
00197
00201 void getSolution(const IkSolutionList<IkReal> &solutions, int i, std::vector<double>& solution) const;
00202
00203 double harmonize(const std::vector<double> &ik_seed_state, std::vector<double> &solution) const;
00204
00205 void getClosestSolution(const IkSolutionList<IkReal> &solutions, const std::vector<double> &ik_seed_state, std::vector<double> &solution) const;
00206 void fillFreeParams(int count, int *array);
00207 bool getCount(int &count, const int &max_count, const int &min_count) const;
00208
00209 };
00210
00211 bool IKFastKinematicsPlugin::initialize(const std::string &robot_description,
00212 const std::string& group_name,
00213 const std::string& base_name,
00214 const std::string& tip_name,
00215 double search_discretization)
00216 {
00217 setValues(robot_description, group_name, base_name, tip_name, search_discretization);
00218 base_frame_ = "arm_base_link";
00219
00220 ros::NodeHandle node_handle("~/"+group_name);
00221
00222 std::string robot;
00223 node_handle.param("robot",robot,std::string());
00224
00225
00226 fillFreeParams( GetNumFreeParameters(), GetFreeParameters() );
00227 num_joints_ = GetNumJoints();
00228
00229 if(free_params_.size() > 1)
00230 {
00231 ROS_FATAL("Only one free joint paramter supported!");
00232 return false;
00233 }
00234
00235 urdf::Model robot_model;
00236 std::string xml_string;
00237
00238 std::string urdf_xml,full_urdf_xml;
00239 node_handle.param("urdf_xml",urdf_xml,robot_description);
00240 node_handle.searchParam(urdf_xml,full_urdf_xml);
00241
00242 ROS_DEBUG_NAMED("ikfast","Reading xml file from parameter server");
00243 if (!node_handle.getParam(full_urdf_xml, xml_string))
00244 {
00245 ROS_FATAL_NAMED("ikfast","Could not load the xml from parameter server: %s", urdf_xml.c_str());
00246 return false;
00247 }
00248
00249 node_handle.param(full_urdf_xml,xml_string,std::string());
00250 robot_model.initString(xml_string);
00251
00252 ROS_DEBUG_STREAM_NAMED("ikfast","Reading joints and links from URDF");
00253
00254 boost::shared_ptr<urdf::Link> link = boost::const_pointer_cast<urdf::Link>(robot_model.getLink(tip_frame_));
00255 while(link->name != base_frame_ && joint_names_.size() <= num_joints_)
00256 {
00257 ROS_DEBUG_NAMED("ikfast","Link %s",link->name.c_str());
00258 link_names_.push_back(link->name);
00259 boost::shared_ptr<urdf::Joint> joint = link->parent_joint;
00260 if(joint)
00261 {
00262 if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
00263 {
00264 ROS_DEBUG_STREAM_NAMED("ikfast","Adding joint " << joint->name );
00265
00266 joint_names_.push_back(joint->name);
00267 float lower, upper;
00268 int hasLimits;
00269 if ( joint->type != urdf::Joint::CONTINUOUS )
00270 {
00271 if(joint->safety)
00272 {
00273 lower = joint->safety->soft_lower_limit;
00274 upper = joint->safety->soft_upper_limit;
00275 } else {
00276 lower = joint->limits->lower;
00277 upper = joint->limits->upper;
00278 }
00279 hasLimits = 1;
00280 }
00281 else
00282 {
00283 lower = -M_PI;
00284 upper = M_PI;
00285 hasLimits = 0;
00286 }
00287 if(hasLimits)
00288 {
00289 joint_has_limits_vector_.push_back(true);
00290 joint_min_vector_.push_back(lower);
00291 joint_max_vector_.push_back(upper);
00292 }
00293 else
00294 {
00295 joint_has_limits_vector_.push_back(false);
00296 joint_min_vector_.push_back(-M_PI);
00297 joint_max_vector_.push_back(M_PI);
00298 }
00299 }
00300 } else
00301 {
00302 ROS_WARN_NAMED("ikfast","no joint corresponding to %s",link->name.c_str());
00303 }
00304 link = link->getParent();
00305 }
00306
00307 if(joint_names_.size() != num_joints_)
00308 {
00309 ROS_FATAL_STREAM_NAMED("ikfast","Joint numbers mismatch: URDF has " << joint_names_.size() << " and IKFast has " << num_joints_);
00310 return false;
00311 }
00312
00313 std::reverse(link_names_.begin(),link_names_.end());
00314 std::reverse(joint_names_.begin(),joint_names_.end());
00315 std::reverse(joint_min_vector_.begin(),joint_min_vector_.end());
00316 std::reverse(joint_max_vector_.begin(),joint_max_vector_.end());
00317 std::reverse(joint_has_limits_vector_.begin(), joint_has_limits_vector_.end());
00318
00319 for(size_t i=0; i <num_joints_; ++i)
00320 ROS_INFO_STREAM_NAMED("ikfast",joint_names_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i] << " " << joint_has_limits_vector_[i]);
00321
00322 active_ = true;
00323 return true;
00324 }
00325
00326 int IKFastKinematicsPlugin::solve(KDL::Frame &pose_frame, const std::vector<double> &vfree, IkSolutionList<IkReal> &solutions) const
00327 {
00328
00329 solutions.Clear();
00330
00331
00332 KDL::Rotation orig = pose_frame.M;
00333 KDL::Rotation mult = orig;
00334
00335
00336
00337
00338
00339
00340
00341
00342
00343
00344
00345 KDL::Vector direction = mult * KDL::Vector(0,0,1);
00346
00347 double trans[3];
00348 trans[0] = pose_frame.p[0];
00349 trans[1] = pose_frame.p[1];
00350 trans[2] = pose_frame.p[2];
00351
00352
00353 ComputeIk(trans, direction.data, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
00354
00355 return solutions.GetNumSolutions();
00356 }
00357
00358 void IKFastKinematicsPlugin::getSolution(const IkSolutionList<IkReal> &solutions, int i, std::vector<double>& solution) const
00359 {
00360 solution.clear();
00361 solution.resize(num_joints_);
00362
00363
00364 const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
00365 std::vector<IkReal> vsolfree( sol.GetFree().size() );
00366 sol.GetSolution(&solution[0],vsolfree.size()>0?&vsolfree[0]:NULL);
00367
00368
00369
00370
00371
00372
00373
00374 }
00375
00376 double IKFastKinematicsPlugin::harmonize(const std::vector<double> &ik_seed_state, std::vector<double> &solution) const
00377 {
00378 double dist_sqr = 0;
00379 std::vector<double> ss = ik_seed_state;
00380 for(size_t i=0; i< ik_seed_state.size(); ++i)
00381 {
00382 while(ss[i] > 2*M_PI) {
00383 ss[i] -= 2*M_PI;
00384 }
00385 while(ss[i] < 2*M_PI) {
00386 ss[i] += 2*M_PI;
00387 }
00388 while(solution[i] > 2*M_PI) {
00389 solution[i] -= 2*M_PI;
00390 }
00391 while(solution[i] < 2*M_PI) {
00392 solution[i] += 2*M_PI;
00393 }
00394 dist_sqr += fabs(ik_seed_state[i] - solution[i]);
00395 }
00396 return dist_sqr;
00397 }
00398
00399
00400
00401
00402
00403
00404
00405
00406
00407
00408
00409
00410
00411
00412
00413
00414
00415
00416
00417
00418
00419
00420
00421
00422 void IKFastKinematicsPlugin::getClosestSolution(const IkSolutionList<IkReal> &solutions, const std::vector<double> &ik_seed_state, std::vector<double> &solution) const
00423 {
00424 double mindist = DBL_MAX;
00425 int minindex = -1;
00426 std::vector<double> sol;
00427
00428
00429 for(size_t i=0; i < solutions.GetNumSolutions(); ++i)
00430 {
00431 getSolution(solutions, i,sol);
00432 double dist = harmonize(ik_seed_state, sol);
00433 ROS_INFO_STREAM_NAMED("ikfast","Dist " << i << " dist " << dist);
00434
00435 if(minindex == -1 || dist<mindist){
00436 minindex = i;
00437 mindist = dist;
00438 }
00439 }
00440 if(minindex >= 0){
00441 getSolution(solutions, minindex,solution);
00442 harmonize(ik_seed_state, solution);
00443 }
00444 }
00445
00446 void IKFastKinematicsPlugin::fillFreeParams(int count, int *array)
00447 {
00448 free_params_.clear();
00449 for(int i=0; i<count;++i) free_params_.push_back(array[i]);
00450 }
00451
00452 bool IKFastKinematicsPlugin::getCount(int &count, const int &max_count, const int &min_count) const
00453 {
00454 if(count > 0)
00455 {
00456 if(-count >= min_count)
00457 {
00458 count = -count;
00459 return true;
00460 }
00461 else if(count+1 <= max_count)
00462 {
00463 count = count+1;
00464 return true;
00465 }
00466 else
00467 {
00468 return false;
00469 }
00470 }
00471 else
00472 {
00473 if(1-count <= max_count)
00474 {
00475 count = 1-count;
00476 return true;
00477 }
00478 else if(count-1 >= min_count)
00479 {
00480 count = count -1;
00481 return true;
00482 }
00483 else
00484 return false;
00485 }
00486 }
00487
00488 bool IKFastKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
00489 const std::vector<double> &joint_angles,
00490 std::vector<geometry_msgs::Pose> &poses) const
00491 {
00492 KDL::Frame p_out;
00493 if(link_names.size() == 0) {
00494 ROS_WARN_STREAM_NAMED("ikfast","Link names with nothing");
00495 return false;
00496 }
00497
00498 if(link_names.size()!=1 || link_names[0]!=tip_frame_){
00499 ROS_ERROR_NAMED("ikfast","Can compute FK for %s only",tip_frame_.c_str());
00500 return false;
00501 }
00502
00503 bool valid = true;
00504
00505 IkReal eerot[9],eetrans[3];
00506 IkReal angles[joint_angles.size()];
00507 for (unsigned char i=0; i < joint_angles.size(); i++)
00508 angles[i] = joint_angles[i];
00509
00510
00511 ComputeFk(angles,eetrans,eerot);
00512
00513 for(int i=0; i<3;++i)
00514 p_out.p.data[i] = eetrans[i];
00515
00516 for(int i=0; i<9;++i)
00517 p_out.M.data[i] = eerot[i];
00518
00519 poses.resize(1);
00520 tf::poseKDLToMsg(p_out,poses[0]);
00521
00522 return valid;
00523 }
00524
00525 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00526 const std::vector<double> &ik_seed_state,
00527 double timeout,
00528 std::vector<double> &solution,
00529 moveit_msgs::MoveItErrorCodes &error_code,
00530 bool lock_redundancy) const
00531 {
00532 const IKCallbackFn solution_callback = 0;
00533 std::vector<double> consistency_limits;
00534
00535 return searchPositionIK(ik_pose,
00536 ik_seed_state,
00537 timeout,
00538 consistency_limits,
00539 solution,
00540 solution_callback,
00541 error_code,
00542 lock_redundancy);
00543 }
00544
00545 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00546 const std::vector<double> &ik_seed_state,
00547 double timeout,
00548 const std::vector<double> &consistency_limits,
00549 std::vector<double> &solution,
00550 moveit_msgs::MoveItErrorCodes &error_code,
00551 bool lock_redundancy) const
00552 {
00553 const IKCallbackFn solution_callback = 0;
00554 return searchPositionIK(ik_pose,
00555 ik_seed_state,
00556 timeout,
00557 consistency_limits,
00558 solution,
00559 solution_callback,
00560 error_code,
00561 lock_redundancy);
00562 }
00563
00564 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00565 const std::vector<double> &ik_seed_state,
00566 double timeout,
00567 std::vector<double> &solution,
00568 const IKCallbackFn &solution_callback,
00569 moveit_msgs::MoveItErrorCodes &error_code,
00570 bool lock_redundancy) const
00571 {
00572 std::vector<double> consistency_limits;
00573 return searchPositionIK(ik_pose,
00574 ik_seed_state,
00575 timeout,
00576 consistency_limits,
00577 solution,
00578 solution_callback,
00579 error_code,
00580 lock_redundancy);
00581 }
00582
00583 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00584 const std::vector<double> &ik_seed_state,
00585 double timeout,
00586 const std::vector<double> &consistency_limits,
00587 std::vector<double> &solution,
00588 const IKCallbackFn &solution_callback,
00589 moveit_msgs::MoveItErrorCodes &error_code,
00590 bool lock_redundant_joints) const
00591 {
00592 ROS_DEBUG_STREAM_NAMED("ikfast","searchPositionIK");
00593
00594
00595 if(free_params_.size()==0)
00596 {
00597 ROS_DEBUG_STREAM_NAMED("ikfast","No need to search since no free params/redundant joints");
00598
00599
00600 if(!getPositionIK(ik_pose, ik_seed_state, solution, error_code))
00601 {
00602 ROS_DEBUG_STREAM_NAMED("ikfast","No solution whatsoever");
00603 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00604 return false;
00605 }
00606
00607
00608 if( !solution_callback.empty() )
00609 {
00610 solution_callback(ik_pose, solution, error_code);
00611 if(error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
00612 {
00613 ROS_DEBUG_STREAM_NAMED("ikfast","Solution passes callback");
00614 return true;
00615 }
00616 else
00617 {
00618 ROS_DEBUG_STREAM_NAMED("ikfast","Solution has error code " << error_code);
00619 return false;
00620 }
00621 }
00622 else
00623 {
00624 return true;
00625 }
00626 }
00627
00628
00629
00630 if(!active_)
00631 {
00632 ROS_ERROR_STREAM_NAMED("ikfast","Kinematics not active");
00633 error_code.val = error_code.NO_IK_SOLUTION;
00634 return false;
00635 }
00636
00637 if(ik_seed_state.size() != num_joints_)
00638 {
00639 ROS_ERROR_STREAM_NAMED("ikfast","Seed state must have size " << num_joints_ << " instead of size " << ik_seed_state.size());
00640 error_code.val = error_code.NO_IK_SOLUTION;
00641 return false;
00642 }
00643
00644 if(!consistency_limits.empty() && consistency_limits.size() != num_joints_)
00645 {
00646 ROS_ERROR_STREAM_NAMED("ikfast","Consistency limits be empty or must have size " << num_joints_ << " instead of size " << consistency_limits.size());
00647 error_code.val = error_code.NO_IK_SOLUTION;
00648 return false;
00649 }
00650
00651
00652
00653
00654
00655 KDL::Frame frame;
00656 tf::poseMsgToKDL(ik_pose,frame);
00657
00658 std::vector<double> vfree(free_params_.size());
00659
00660 ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
00661 int counter = 0;
00662
00663 double initial_guess = ik_seed_state[free_params_[0]];
00664 vfree[0] = initial_guess;
00665
00666
00667
00668 int num_positive_increments;
00669 int num_negative_increments;
00670
00671 if(!consistency_limits.empty())
00672 {
00673
00674
00675 double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess+consistency_limits[free_params_[0]]);
00676 double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess-consistency_limits[free_params_[0]]);
00677
00678 num_positive_increments = (int)((max_limit-initial_guess)/search_discretization_);
00679 num_negative_increments = (int)((initial_guess-min_limit)/search_discretization_);
00680 }
00681 else
00682 {
00683 num_positive_increments = (joint_max_vector_[free_params_[0]]-initial_guess)/search_discretization_;
00684 num_negative_increments = (initial_guess-joint_min_vector_[free_params_[0]])/search_discretization_;
00685 }
00686
00687
00688
00689
00690 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);
00691
00692 while(true)
00693 {
00694 IkSolutionList<IkReal> solutions;
00695 int numsol = solve(frame,vfree, solutions);
00696
00697 ROS_DEBUG_STREAM_NAMED("ikfast","Found " << numsol << " solutions from IKFast");
00698
00699
00700
00701 if( numsol > 0 )
00702 {
00703 for(int s = 0; s < numsol; ++s)
00704 {
00705 std::vector<double> sol;
00706 getSolution(solutions,s,sol);
00707
00708 bool obeys_limits = true;
00709 for(unsigned int i = 0; i < sol.size(); i++)
00710 {
00711 if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i]))
00712 {
00713 obeys_limits = false;
00714 break;
00715 }
00716
00717 }
00718 if(obeys_limits)
00719 {
00720 getSolution(solutions,s,solution);
00721
00722
00723 if(!solution_callback.empty())
00724 {
00725 solution_callback(ik_pose, solution, error_code);
00726 }
00727 else
00728 {
00729 error_code.val = error_code.SUCCESS;
00730 }
00731
00732 if(error_code.val == error_code.SUCCESS)
00733 {
00734 return true;
00735 }
00736 }
00737 }
00738 }
00739
00740 if(!getCount(counter, num_positive_increments, num_negative_increments))
00741 {
00742 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00743 return false;
00744 }
00745
00746 vfree[0] = initial_guess+search_discretization_*counter;
00747 ROS_DEBUG_STREAM_NAMED("ikfast","Attempt " << counter << " with 0th free joint having value " << vfree[0]);
00748 }
00749
00750
00751 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00752 return false;
00753 }
00754
00755
00756 bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose,
00757 const std::vector<double> &ik_seed_state,
00758 std::vector<double> &solution,
00759 moveit_msgs::MoveItErrorCodes &error_code,
00760 bool lock_redundant_joints) const
00761 {
00762 ROS_DEBUG_STREAM_NAMED("ikfast","getPositionIK");
00763
00764 if(!active_)
00765 {
00766 ROS_ERROR("kinematics not active");
00767 return false;
00768 }
00769
00770 std::vector<double> vfree(free_params_.size());
00771 for(std::size_t i = 0; i < free_params_.size(); ++i)
00772 {
00773 int p = free_params_[i];
00774 ROS_ERROR("%u is %f",p,ik_seed_state[p]);
00775 vfree[i] = ik_seed_state[p];
00776 }
00777
00778 KDL::Frame frame;
00779 tf::poseMsgToKDL(ik_pose,frame);
00780
00781 IkSolutionList<IkReal> solutions;
00782 int numsol = solve(frame,vfree,solutions);
00783
00784 ROS_DEBUG_STREAM_NAMED("ikfast","Found " << numsol << " solutions from IKFast");
00785
00786 if(numsol)
00787 {
00788 for(int s = 0; s < numsol; ++s)
00789 {
00790 std::vector<double> sol;
00791 getSolution(solutions,s,sol);
00792 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]);
00793
00794 bool obeys_limits = true;
00795 for(unsigned int i = 0; i < sol.size(); i++)
00796 {
00797
00798 if(joint_has_limits_vector_[i] && ( (sol[i] < (joint_min_vector_[i]-LIMIT_TOLERANCE)) ||
00799 (sol[i] > (joint_max_vector_[i]+LIMIT_TOLERANCE)) ) )
00800 {
00801
00802 obeys_limits = false;
00803 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]);
00804 break;
00805 }
00806 }
00807 if(obeys_limits)
00808 {
00809
00810 getSolution(solutions,s,solution);
00811 error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00812 return true;
00813 }
00814 }
00815 }
00816 else
00817 {
00818 ROS_DEBUG_STREAM_NAMED("ikfast","No IK solution");
00819 }
00820
00821 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00822 return false;
00823 }
00824
00825
00826
00827 }
00828
00829
00830 #include <pluginlib/class_list_macros.h>
00831 PLUGINLIB_EXPORT_CLASS(ikfast_kinematics_plugin::IKFastKinematicsPlugin, kinematics::KinematicsBase);