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
00049
00050
00051
00052
00053
00054
00055
00056
00057 #include <ros/ros.h>
00058 #include <moveit/kinematics_base/kinematics_base.h>
00059 #include <urdf/model.h>
00060 #include <tf_conversions/tf_kdl.h>
00061
00062
00063 const double LIMIT_TOLERANCE = .0000001;
00064
00065 namespace irb_2400_manipulator_kinematics
00066 {
00067 #define IKFAST_HAS_LIBRARY // Build IKFast with API functions
00068 #define IKFAST_NO_MAIN // Don't include main() from IKFast
00069
00070 #include "irb_2400_manipulator_ikfast_solver.cpp"
00071
00072 class IKFastKinematicsPlugin : public kinematics::KinematicsBase
00073 {
00074 std::vector<std::string> joint_names_;
00075 std::vector<double> joint_min_vector_;
00076 std::vector<double> joint_max_vector_;
00077 std::vector<bool> joint_has_limits_vector_;
00078 std::vector<std::string> link_names_;
00079 size_t num_joints_;
00080 std::vector<int> free_params_;
00081
00082 const std::vector<std::string>& getJointNames() const { return joint_names_; }
00083 const std::vector<std::string>& getLinkNames() const { return link_names_; }
00084
00085 public:
00086
00090 IKFastKinematicsPlugin() {}
00091
00099
00100
00101 bool getPositionFK(const std::vector<std::string> &link_names,
00102 const std::vector<double> &joint_angles,
00103 std::vector<geometry_msgs::Pose> &poses) const;
00104
00111
00112
00113 bool getPositionIK(const geometry_msgs::Pose &ik_pose,
00114 const std::vector<double> &ik_seed_state,
00115 std::vector<double> &solution,
00116 moveit_msgs::MoveItErrorCodes &error_code) const;
00117
00126 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00127 const std::vector<double> &ik_seed_state,
00128 double timeout,
00129 std::vector<double> &solution,
00130 moveit_msgs::MoveItErrorCodes &error_code) const;
00131
00141 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00142 const std::vector<double> &ik_seed_state,
00143 double timeout,
00144 const std::vector<double> &consistency_limits,
00145 std::vector<double> &solution,
00146 moveit_msgs::MoveItErrorCodes &error_code) const;
00147
00156
00157 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00158 const std::vector<double> &ik_seed_state,
00159 double timeout,
00160 std::vector<double> &solution,
00161 const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,
00162 moveit_msgs::MoveItErrorCodes &error_code)> &solution_callback,
00163 moveit_msgs::MoveItErrorCodes &error_code) const;
00164
00175 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00176 const std::vector<double> &ik_seed_state,
00177 double timeout,
00178 const std::vector<double> &consistency_limits,
00179 std::vector<double> &solution,
00180 const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,
00181 moveit_msgs::MoveItErrorCodes &error_code)> &solution_callback,
00182 moveit_msgs::MoveItErrorCodes &error_code) const;
00183
00184 private:
00185
00186 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);
00187
00192 int solve(KDL::Frame &pose_frame, const std::vector<double> &vfree, IkSolutionList<IkReal> &solutions) const;
00193
00197 void getSolution(const IkSolutionList<IkReal> &solutions, int i, std::vector<double>& solution) const;
00198
00199 double harmonize(const std::vector<double> &ik_seed_state, std::vector<double> &solution) const;
00200 double harmonize_old(const std::vector<double> &ik_seed_state, std::vector<double> &solution) const;
00201
00202 void getClosestSolution(const IkSolutionList<IkReal> &solutions, const std::vector<double> &ik_seed_state, std::vector<double> &solution) const;
00203 void fillFreeParams(int count, int *array);
00204 bool getCount(int &count, const int &max_count, const int &min_count) const;
00205
00206 };
00207
00208 bool IKFastKinematicsPlugin::initialize(const std::string& robot_description,
00209 const std::string& group_name,
00210 const std::string& base_name,
00211 const std::string& tip_name,
00212 double search_discretization)
00213 {
00214 setValues(robot_description, group_name, base_name, tip_name,search_discretization);
00215
00216 ros::NodeHandle node_handle("~/"+group_name);
00217
00218 std::string robot;
00219 node_handle.param("robot",robot,std::string());
00220
00221
00222 fillFreeParams( GetNumFreeParameters(), GetFreeParameters() );
00223 num_joints_ = GetNumJoints();
00224
00225 if(free_params_.size()>1){
00226 ROS_FATAL("Only one free joint paramter supported!");
00227 return false;
00228 }
00229
00230 urdf::Model robot_model;
00231 std::string xml_string;
00232
00233 std::string urdf_xml,full_urdf_xml;
00234 node_handle.param("urdf_xml",urdf_xml,std::string("robot_description"));
00235 node_handle.searchParam(urdf_xml,full_urdf_xml);
00236
00237 ROS_DEBUG("Reading xml file from parameter server\n");
00238 if (!node_handle.getParam(full_urdf_xml, xml_string))
00239 {
00240 ROS_FATAL("Could not load the xml from parameter server: %s\n", urdf_xml.c_str());
00241 return false;
00242 }
00243
00244 node_handle.param(full_urdf_xml,xml_string,std::string());
00245 robot_model.initString(xml_string);
00246
00247 boost::shared_ptr<urdf::Link> link = boost::const_pointer_cast<urdf::Link>(robot_model.getLink(tip_frame_));
00248 while(link->name != base_frame_ && joint_names_.size() <= num_joints_){
00249
00250 link_names_.push_back(link->name);
00251 boost::shared_ptr<urdf::Joint> joint = link->parent_joint;
00252 if(joint){
00253 if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
00254 joint_names_.push_back(joint->name);
00255 float lower, upper;
00256 int hasLimits;
00257 if ( joint->type != urdf::Joint::CONTINUOUS ) {
00258 if(joint->safety) {
00259 lower = joint->safety->soft_lower_limit;
00260 upper = joint->safety->soft_upper_limit;
00261 } else {
00262 lower = joint->limits->lower;
00263 upper = joint->limits->upper;
00264 }
00265 hasLimits = 1;
00266 } else {
00267 lower = -M_PI;
00268 upper = M_PI;
00269 hasLimits = 0;
00270 }
00271 if(hasLimits) {
00272 joint_has_limits_vector_.push_back(true);
00273 joint_min_vector_.push_back(lower);
00274 joint_max_vector_.push_back(upper);
00275 } else {
00276 joint_has_limits_vector_.push_back(false);
00277 joint_min_vector_.push_back(-M_PI);
00278 joint_max_vector_.push_back(M_PI);
00279 }
00280 }
00281 } else{
00282 ROS_WARN("no joint corresponding to %s",link->name.c_str());
00283 }
00284 link = link->getParent();
00285 }
00286
00287 if(joint_names_.size() != num_joints_){
00288 ROS_FATAL("Joints number mismatch.");
00289 return false;
00290 }
00291
00292 std::reverse(link_names_.begin(),link_names_.end());
00293 std::reverse(joint_names_.begin(),joint_names_.end());
00294 std::reverse(joint_min_vector_.begin(),joint_min_vector_.end());
00295 std::reverse(joint_max_vector_.begin(),joint_max_vector_.end());
00296 std::reverse(joint_has_limits_vector_.begin(), joint_has_limits_vector_.end());
00297
00298 for(size_t i=0; i <num_joints_; ++i)
00299 ROS_INFO_STREAM(joint_names_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i] << " " << joint_has_limits_vector_[i]);
00300
00301 return true;
00302 }
00303
00304 int IKFastKinematicsPlugin::solve(KDL::Frame &pose_frame, const std::vector<double> &vfree, IkSolutionList<IkReal> &solutions) const
00305 {
00306
00307 solutions.Clear();
00308
00309
00310 KDL::Rotation orig = pose_frame.M;
00311 KDL::Rotation mult = orig;
00312
00313 double vals[9];
00314 vals[0] = mult(0,0);
00315 vals[1] = mult(0,1);
00316 vals[2] = mult(0,2);
00317 vals[3] = mult(1,0);
00318 vals[4] = mult(1,1);
00319 vals[5] = mult(1,2);
00320 vals[6] = mult(2,0);
00321 vals[7] = mult(2,1);
00322 vals[8] = mult(2,2);
00323
00324 double trans[3];
00325 trans[0] = pose_frame.p[0];
00326 trans[1] = pose_frame.p[1];
00327 trans[2] = pose_frame.p[2];
00328
00329
00330 ComputeIk(trans, vals, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
00331 return solutions.GetNumSolutions();
00332 }
00333
00334 void IKFastKinematicsPlugin::getSolution(const IkSolutionList<IkReal> &solutions, int i, std::vector<double>& solution) const
00335 {
00336 solution.clear();
00337 solution.resize(num_joints_);
00338
00339
00340 const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
00341 std::vector<IkReal> vsolfree( sol.GetFree().size() );
00342 sol.GetSolution(&solution[0],vsolfree.size()>0?&vsolfree[0]:NULL);
00343
00344
00345
00346
00347
00348
00349
00350 }
00351
00352 double IKFastKinematicsPlugin::harmonize(const std::vector<double> &ik_seed_state, std::vector<double> &solution) const
00353 {
00354 double dist_sqr = 0;
00355 std::vector<double> ss = ik_seed_state;
00356 for(size_t i=0; i< ik_seed_state.size(); ++i)
00357 {
00358 while(ss[i] > 2*M_PI) {
00359 ss[i] -= 2*M_PI;
00360 }
00361 while(ss[i] < 2*M_PI) {
00362 ss[i] += 2*M_PI;
00363 }
00364 while(solution[i] > 2*M_PI) {
00365 solution[i] -= 2*M_PI;
00366 }
00367 while(solution[i] < 2*M_PI) {
00368 solution[i] += 2*M_PI;
00369 }
00370 dist_sqr += fabs(ik_seed_state[i] - solution[i]);
00371 }
00372 return dist_sqr;
00373 }
00374
00375 double IKFastKinematicsPlugin::harmonize_old(const std::vector<double> &ik_seed_state, std::vector<double> &solution) const
00376 {
00377 double dist_sqr = 0;
00378 for(size_t i=0; i< ik_seed_state.size(); ++i){
00379 double diff = ik_seed_state[i] - solution[i];
00380 if( diff > M_PI ) solution[i]+=2*M_PI;
00381 else if (diff < -M_PI) solution[i]-=2*M_PI;
00382 diff = ik_seed_state[i] - solution[i];
00383 dist_sqr += fabs(diff);
00384 }
00385 return dist_sqr;
00386 }
00387
00388
00389
00390
00391
00392
00393
00394
00395
00396
00397
00398
00399
00400
00401
00402
00403
00404
00405
00406
00407
00408
00409
00410
00411 void IKFastKinematicsPlugin::getClosestSolution(const IkSolutionList<IkReal> &solutions, const std::vector<double> &ik_seed_state, std::vector<double> &solution) const
00412 {
00413 double mindist = DBL_MAX;
00414 int minindex = -1;
00415 std::vector<double> sol;
00416
00417
00418 for(size_t i=0; i < solutions.GetNumSolutions(); ++i)
00419 {
00420 getSolution(solutions, i,sol);
00421 double dist = harmonize(ik_seed_state, sol);
00422 ROS_INFO_STREAM("Dist " << i << " dist " << dist);
00423
00424 if(minindex == -1 || dist<mindist){
00425 minindex = i;
00426 mindist = dist;
00427 }
00428 }
00429 if(minindex >= 0){
00430 getSolution(solutions, minindex,solution);
00431 harmonize(ik_seed_state, solution);
00432 }
00433 }
00434
00435 void IKFastKinematicsPlugin::fillFreeParams(int count, int *array)
00436 {
00437 free_params_.clear();
00438 for(int i=0; i<count;++i) free_params_.push_back(array[i]);
00439 }
00440
00441 bool IKFastKinematicsPlugin::getCount(int &count, const int &max_count, const int &min_count) const
00442 {
00443 if(count > 0)
00444 {
00445 if(-count >= min_count)
00446 {
00447 count = -count;
00448 return true;
00449 }
00450 else if(count+1 <= max_count)
00451 {
00452 count = count+1;
00453 return true;
00454 }
00455 else
00456 {
00457 return false;
00458 }
00459 }
00460 else
00461 {
00462 if(1-count <= max_count)
00463 {
00464 count = 1-count;
00465 return true;
00466 }
00467 else if(count-1 >= min_count)
00468 {
00469 count = count -1;
00470 return true;
00471 }
00472 else
00473 return false;
00474 }
00475 }
00476
00477 bool IKFastKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
00478 const std::vector<double> &joint_angles,
00479 std::vector<geometry_msgs::Pose> &poses) const
00480 {
00481 KDL::Frame p_out;
00482 if(link_names.size() == 0) {
00483 ROS_WARN_STREAM("Link names with nothing");
00484 return false;
00485 }
00486
00487 if(link_names.size()!=1 || link_names[0]!=tip_frame_){
00488 ROS_ERROR("Can compute FK for %s only",tip_frame_.c_str());
00489 return false;
00490 }
00491
00492 bool valid = true;
00493
00494 IkReal eerot[9],eetrans[3];
00495 IkReal angles[joint_angles.size()];
00496 for (unsigned char i=0; i < joint_angles.size(); i++) angles[i] = joint_angles[i];
00497
00498
00499 ComputeFk(angles,eetrans,eerot);
00500
00501 for(int i=0; i<3;++i) p_out.p.data[i] = eetrans[i];
00502 for(int i=0; i<9;++i) p_out.M.data[i] = eerot[i];
00503 poses.resize(1);
00504 tf::PoseKDLToMsg(p_out,poses[0]);
00505
00506 return valid;
00507 }
00508
00509 bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose,
00510 const std::vector<double> &ik_seed_state,
00511 std::vector<double> &solution,
00512 moveit_msgs::MoveItErrorCodes &error_code) const
00513 {
00514 std::vector<double> vfree(free_params_.size());
00515 for(std::size_t i = 0; i < free_params_.size(); ++i){
00516 int p = free_params_[i];
00517
00518 vfree[i] = ik_seed_state[p];
00519 }
00520
00521 KDL::Frame frame;
00522 tf::PoseMsgToKDL(ik_pose,frame);
00523
00524 IkSolutionList<IkReal> solutions;
00525 int numsol = solve(frame,vfree,solutions);
00526
00527 if(numsol){
00528 for(int s = 0; s < numsol; ++s)
00529 {
00530 std::vector<double> sol;
00531 getSolution(solutions,s,sol);
00532
00533
00534 bool obeys_limits = true;
00535 for(unsigned int i = 0; i < sol.size(); i++) {
00536
00537 if(joint_has_limits_vector_[i] && ( (sol[i] < (joint_min_vector_[i]-LIMIT_TOLERANCE)) ||
00538 (sol[i] > (joint_max_vector_[i]+LIMIT_TOLERANCE)) ) )
00539 {
00540
00541 obeys_limits = false;
00542
00543 break;
00544 }
00545 }
00546 if(obeys_limits) {
00547
00548 getSolution(solutions,s,solution);
00549 error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00550
00551 return true;
00552 }
00553 }
00554 }else{
00555
00556 }
00557
00558 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00559 return false;
00560 }
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 moveit_msgs::MoveItErrorCodes &error_code) const
00568 {
00569 if(free_params_.size()==0){
00570 return getPositionIK(ik_pose, ik_seed_state,solution, error_code);
00571 }
00572
00573 KDL::Frame frame;
00574 tf::PoseMsgToKDL(ik_pose,frame);
00575
00576 std::vector<double> vfree(free_params_.size());
00577
00578 ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
00579 int counter = 0;
00580
00581 double initial_guess = ik_seed_state[free_params_[0]];
00582 vfree[0] = initial_guess;
00583
00584 int num_positive_increments = (joint_max_vector_[free_params_[0]]-initial_guess)/search_discretization_;
00585 int num_negative_increments = (initial_guess-joint_min_vector_[free_params_[0]])/search_discretization_;
00586
00587
00588
00589 while(1) {
00590 IkSolutionList<IkReal> solutions;
00591 int numsol = solve(frame,vfree, solutions);
00592
00593
00594
00595 if(numsol > 0){
00596 for(int s = 0; s < numsol; ++s){
00597 std::vector<double> sol;
00598 getSolution(solutions,s,sol);
00599
00600 bool obeys_limits = true;
00601 for(unsigned int i = 0; i < sol.size(); i++) {
00602 if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i])) {
00603 obeys_limits = false;
00604 break;
00605 }
00606
00607 }
00608 if(obeys_limits) {
00609 getSolution(solutions,s,solution);
00610 error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00611 return true;
00612 }
00613 }
00614 }
00615
00616
00617
00618
00619
00620
00621
00622
00623
00624
00625
00626
00627
00628
00629
00630
00631
00632
00633
00634
00635
00636
00637
00638
00639 if(!getCount(counter, num_positive_increments, num_negative_increments)) {
00640 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00641 return false;
00642 }
00643
00644 vfree[0] = initial_guess+search_discretization_*counter;
00645 ROS_DEBUG_STREAM(counter << " " << vfree[0]);
00646 }
00647
00648 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00649 return false;
00650 }
00651
00652
00653 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00654 const std::vector<double> &ik_seed_state,
00655 double timeout,
00656 const std::vector<double> &consistency_limits,
00657 std::vector<double> &solution,
00658 moveit_msgs::MoveItErrorCodes &error_code) const
00659 {
00660 if(free_params_.size()==0){
00661
00662 return getPositionIK(ik_pose, ik_seed_state,solution, error_code);
00663 ROS_WARN_STREAM("No free parameters, so can't search");
00664 }
00665
00666 KDL::Frame frame;
00667 tf::PoseMsgToKDL(ik_pose,frame);
00668
00669 std::vector<double> vfree(free_params_.size());
00670
00671 ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
00672 int counter = 0;
00673
00674 double initial_guess = ik_seed_state[free_params_[0]];
00675 vfree[0] = initial_guess;
00676
00677
00678
00679 double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess+consistency_limits[free_params_[0]]);
00680 double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess-consistency_limits[free_params_[0]]);
00681
00682 int num_positive_increments = (int)((max_limit-initial_guess)/search_discretization_);
00683 int num_negative_increments = (int)((initial_guess-min_limit)/search_discretization_);
00684
00685 ROS_DEBUG_STREAM("Free param is " << free_params_[0] << " initial guess is " << initial_guess << " " << num_positive_increments << " " << num_negative_increments);
00686
00687 while(1) {
00688 IkSolutionList<IkReal> solutions;
00689 int numsol = solve(frame,vfree, solutions);
00690
00691
00692
00693 if(numsol > 0){
00694 for(int s = 0; s < numsol; ++s){
00695 std::vector<double> sol;
00696
00697 getSolution(solutions, s,sol);
00698 bool obeys_limits = true;
00699 for(unsigned int i = 0; i < sol.size(); i++) {
00700 if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i])) {
00701 obeys_limits = false;
00702 break;
00703 }
00704
00705 }
00706 if(obeys_limits) {
00707
00708 getSolution(solutions, s,solution);
00709 error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00710 return true;
00711 }
00712 }
00713 }
00714
00715
00716
00717
00718
00719
00720
00721
00722
00723
00724
00725
00726
00727
00728
00729
00730
00731
00732
00733
00734
00735
00736
00737
00738 if(!getCount(counter, num_positive_increments, num_negative_increments)) {
00739 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00740 return false;
00741 }
00742
00743 vfree[0] = initial_guess+search_discretization_*counter;
00744 ROS_DEBUG_STREAM(counter << " " << vfree[0]);
00745 }
00746
00747 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00748 return false;
00749 }
00750
00751
00752
00753 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00754 const std::vector<double> &ik_seed_state,
00755 double timeout,
00756 std::vector<double> &solution,
00757 const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,
00758 moveit_msgs::MoveItErrorCodes &error_code)> &solution_callback,
00759 moveit_msgs::MoveItErrorCodes &error_code) const
00760 {
00761
00762 if(free_params_.size()==0){
00763
00764 if(!getPositionIK(ik_pose, ik_seed_state,solution, error_code)) {
00765 ROS_DEBUG_STREAM("No solution whatsoever");
00766 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00767 return false;
00768 }
00769
00770 solution_callback(ik_pose,solution,error_code);
00771 if(error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS) {
00772 ROS_DEBUG_STREAM("Solution passes");
00773 return true;
00774 } else {
00775 ROS_DEBUG_STREAM("Solution has error code " << error_code);
00776 return false;
00777 }
00778 }
00779
00780 KDL::Frame frame;
00781 tf::PoseMsgToKDL(ik_pose,frame);
00782
00783 std::vector<double> vfree(free_params_.size());
00784
00785 ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
00786 int counter = 0;
00787
00788 double initial_guess = ik_seed_state[free_params_[0]];
00789 vfree[0] = initial_guess;
00790
00791 int num_positive_increments = (joint_max_vector_[free_params_[0]]-initial_guess)/search_discretization_;
00792 int num_negative_increments = (initial_guess - joint_min_vector_[free_params_[0]])/search_discretization_;
00793
00794 ROS_DEBUG_STREAM("Free param is " << free_params_[0] << " initial guess is " << initial_guess << " " << num_positive_increments << " " << num_negative_increments);
00795
00796 unsigned int solvecount = 0;
00797 unsigned int countsol = 0;
00798
00799 ros::WallTime start = ros::WallTime::now();
00800
00801 std::vector<double> sol;
00802 while(1) {
00803 IkSolutionList<IkReal> solutions;
00804 int numsol = solve(frame,vfree,solutions);
00805
00806 if(solvecount == 0) {
00807 if(numsol == 0) {
00808 ROS_DEBUG_STREAM("Bad solve time is " << ros::WallTime::now()-start);
00809 } else {
00810 ROS_DEBUG_STREAM("Good solve time is " << ros::WallTime::now()-start);
00811 }
00812 }
00813 solvecount++;
00814 if(numsol > 0){
00815 if(solution_callback.empty()){
00816 getClosestSolution(solutions, ik_seed_state,solution);
00817 error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00818 return true;
00819 }
00820
00821 for(int s = 0; s < numsol; ++s){
00822 getSolution(solutions, s,sol);
00823 countsol++;
00824 bool obeys_limits = true;
00825 for(unsigned int i = 0; i < sol.size(); i++) {
00826 if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i])) {
00827 obeys_limits = false;
00828 break;
00829 }
00830 }
00831 if(obeys_limits) {
00832 solution_callback(ik_pose,sol,error_code);
00833 if(error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS){
00834 solution = sol;
00835 ROS_DEBUG_STREAM("Took " << (ros::WallTime::now() - start) << " to return true " << countsol << " " << solvecount);
00836 return true;
00837 }
00838 }
00839 }
00840 }
00841 if(!getCount(counter, num_positive_increments, num_negative_increments)) {
00842 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00843 ROS_DEBUG_STREAM("Took " << (ros::WallTime::now() - start) << " to return false " << countsol << " " << solvecount);
00844 return false;
00845 }
00846 vfree[0] = initial_guess+search_discretization_*counter;
00847 ROS_DEBUG_STREAM(counter << " " << vfree[0]);
00848 }
00849 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00850 return false;
00851 }
00852
00853
00854 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00855 const std::vector<double> &ik_seed_state,
00856 double timeout,
00857 const std::vector<double> &consistency_limits,
00858 std::vector<double> &solution,
00859 const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,
00860 moveit_msgs::MoveItErrorCodes &error_code)> &solution_callback,
00861 moveit_msgs::MoveItErrorCodes &error_code) const
00862 {
00863 if(free_params_.size()==0){
00864 if(!getPositionIK(ik_pose, ik_seed_state,solution, error_code)) {
00865 ROS_DEBUG_STREAM("No solution whatsoever");
00866 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00867 return false;
00868 }
00869 solution_callback(ik_pose,solution,error_code);
00870 if(error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS) {
00871 ROS_DEBUG_STREAM("Solution passes");
00872 return true;
00873 } else {
00874 ROS_DEBUG_STREAM("Solution has error code " << error_code);
00875 return false;
00876 }
00877 }
00878
00879 KDL::Frame frame;
00880 tf::PoseMsgToKDL(ik_pose,frame);
00881
00882 std::vector<double> vfree(free_params_.size());
00883
00884 ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
00885 int counter = 0;
00886
00887 double initial_guess = ik_seed_state[free_params_[0]];
00888 vfree[0] = initial_guess;
00889
00890
00891
00892 double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess+consistency_limits[free_params_[0]]);
00893 double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess-consistency_limits[free_params_[0]]);
00894
00895 int num_positive_increments = (int)((max_limit-initial_guess)/search_discretization_);
00896 int num_negative_increments = (int)((initial_guess-min_limit)/search_discretization_);
00897
00898 ROS_DEBUG_STREAM("Free param is " << free_params_[0] << " initial guess is " << initial_guess << " " << num_positive_increments << " " << num_negative_increments);
00899
00900 unsigned int solvecount = 0;
00901 unsigned int countsol = 0;
00902
00903 ros::WallTime start = ros::WallTime::now();
00904
00905 std::vector<double> sol;
00906 while(1) {
00907 IkSolutionList<IkReal> solutions;
00908 int numsol = solve(frame,vfree,solutions);
00909 if(solvecount == 0) {
00910 if(numsol == 0) {
00911 ROS_DEBUG_STREAM("Bad solve time is " << ros::WallTime::now()-start);
00912 } else {
00913 ROS_DEBUG_STREAM("Good solve time is " << ros::WallTime::now()-start);
00914 }
00915 }
00916 solvecount++;
00917 if(numsol > 0){
00918 if(solution_callback.empty()){
00919 getClosestSolution(solutions, ik_seed_state,solution);
00920 error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00921 return true;
00922 }
00923
00924 for(int s = 0; s < numsol; ++s){
00925 getSolution(solutions, s,sol);
00926 countsol++;
00927 bool obeys_limits = true;
00928 for(unsigned int i = 0; i < sol.size(); i++) {
00929 if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i])) {
00930 obeys_limits = false;
00931 break;
00932 }
00933 }
00934 if(obeys_limits) {
00935 solution_callback(ik_pose,sol,error_code);
00936 if(error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS){
00937 solution = sol;
00938 ROS_DEBUG_STREAM("Took " << (ros::WallTime::now() - start) << " to return true " << countsol << " " << solvecount);
00939 return true;
00940 }
00941 }
00942 }
00943 }
00944 if(!getCount(counter, num_positive_increments, num_negative_increments)) {
00945 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00946 ROS_DEBUG_STREAM("Took " << (ros::WallTime::now() - start) << " to return false " << countsol << " " << solvecount);
00947 return false;
00948 }
00949 vfree[0] = initial_guess+search_discretization_*counter;
00950 ROS_DEBUG_STREAM(counter << " " << vfree[0]);
00951 }
00952 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00953 return false;
00954 }
00955
00956 }
00957
00958 #include <pluginlib/class_list_macros.h>
00959 PLUGINLIB_EXPORT_CLASS(irb_2400_manipulator_kinematics::IKFastKinematicsPlugin, kinematics::KinematicsBase);
00960