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