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