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 katana_450_6m90a_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 "katana_450_6m90a_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 KDL::Vector direction = mult * KDL::Vector(0, 0, 1);
00321
00322 double trans[3];
00323 trans[0] = pose_frame.p[0];
00324 trans[1] = pose_frame.p[1];
00325 trans[2] = pose_frame.p[2];
00326
00327
00328 ComputeIk(trans, direction.data, vfree.size() > 0 ? &vfree[0] : NULL, solutions_);
00329 return solutions_.GetNumSolutions();
00330 }
00331
00332 void IKFastKinematicsPlugin::getSolution(int i, std::vector<double>& solution)
00333 {
00334 solution.clear();
00335 solution.resize(num_joints_);
00336
00337
00338 const IkSolutionBase<IkReal>& sol = solutions_.GetSolution(i);
00339 std::vector<IkReal> vsolfree( sol.GetFree().size() );
00340 sol.GetSolution(&solution[0],vsolfree.size()>0?&vsolfree[0]:NULL);
00341
00342
00343
00344
00345
00346
00347
00348 }
00349
00350 double IKFastKinematicsPlugin::harmonize(const std::vector<double> &ik_seed_state, std::vector<double> &solution)
00351 {
00352 double dist_sqr = 0;
00353 std::vector<double> ss = ik_seed_state;
00354 for(size_t i=0; i< ik_seed_state.size(); ++i)
00355 {
00356 while(ss[i] > 2*M_PI) {
00357 ss[i] -= 2*M_PI;
00358 }
00359 while(ss[i] < 2*M_PI) {
00360 ss[i] += 2*M_PI;
00361 }
00362 while(solution[i] > 2*M_PI) {
00363 solution[i] -= 2*M_PI;
00364 }
00365 while(solution[i] < 2*M_PI) {
00366 solution[i] += 2*M_PI;
00367 }
00368 dist_sqr += fabs(ik_seed_state[i] - solution[i]);
00369 }
00370 return dist_sqr;
00371 }
00372
00373 double IKFastKinematicsPlugin::harmonize_old(const std::vector<double> &ik_seed_state, std::vector<double> &solution)
00374 {
00375 double dist_sqr = 0;
00376 for(size_t i=0; i< ik_seed_state.size(); ++i){
00377 double diff = ik_seed_state[i] - solution[i];
00378 if( diff > M_PI ) solution[i]+=2*M_PI;
00379 else if (diff < -M_PI) solution[i]-=2*M_PI;
00380 diff = ik_seed_state[i] - solution[i];
00381 dist_sqr += fabs(diff);
00382 }
00383 return dist_sqr;
00384 }
00385
00386
00387
00388
00389
00390
00391
00392
00393
00394
00395
00396
00397
00398
00399
00400
00401
00402
00403
00404
00405
00406
00407
00408
00409 void IKFastKinematicsPlugin::getClosestSolution(const std::vector<double> &ik_seed_state, std::vector<double> &solution)
00410 {
00411 double mindist = DBL_MAX;
00412 int minindex = -1;
00413 std::vector<double> sol;
00414
00415
00416 for(size_t i=0; i < solutions_.GetNumSolutions(); ++i)
00417 {
00418 getSolution(i,sol);
00419 double dist = harmonize(ik_seed_state, sol);
00420 ROS_INFO_STREAM("Dist " << i << " dist " << dist);
00421
00422 if(minindex == -1 || dist<mindist){
00423 minindex = i;
00424 mindist = dist;
00425 }
00426 }
00427 if(minindex >= 0){
00428 getSolution(minindex,solution);
00429 harmonize(ik_seed_state, solution);
00430 }
00431 }
00432
00433 void IKFastKinematicsPlugin::fillFreeParams(int count, int *array)
00434 {
00435 free_params_.clear();
00436 for(int i=0; i<count;++i) free_params_.push_back(array[i]);
00437 }
00438
00439 bool IKFastKinematicsPlugin::getCount(int &count, const int &max_count, const int &min_count)
00440 {
00441 if(count > 0)
00442 {
00443 if(-count >= min_count)
00444 {
00445 count = -count;
00446 return true;
00447 }
00448 else if(count+1 <= max_count)
00449 {
00450 count = count+1;
00451 return true;
00452 }
00453 else
00454 {
00455 return false;
00456 }
00457 }
00458 else
00459 {
00460 if(1-count <= max_count)
00461 {
00462 count = 1-count;
00463 return true;
00464 }
00465 else if(count-1 >= min_count)
00466 {
00467 count = count -1;
00468 return true;
00469 }
00470 else
00471 return false;
00472 }
00473 }
00474
00475 bool IKFastKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
00476 const std::vector<double> &joint_angles,
00477 std::vector<geometry_msgs::Pose> &poses)
00478 {
00479
00480
00481
00482 ROS_ERROR("Cannot compute FK when using TranslationDirection5D!");
00483 return false;
00484
00485
00486
00487
00488
00489
00490
00491
00492
00493
00494
00495
00496
00497
00498
00499
00500
00501
00502
00503
00504
00505
00506
00507
00508
00509
00510
00511 }
00512
00513 bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose,
00514 const std::vector<double> &ik_seed_state,
00515 std::vector<double> &solution,
00516 int &error_code)
00517 {
00518 std::vector<double> vfree(free_params_.size());
00519 for(std::size_t i = 0; i < free_params_.size(); ++i){
00520 int p = free_params_[i];
00521
00522 vfree[i] = ik_seed_state[p];
00523 }
00524
00525 KDL::Frame frame;
00526 tf::PoseMsgToKDL(ik_pose,frame);
00527
00528
00529 tf::Quaternion q;
00530 double roll, pitch, yaw;
00531 tf::quaternionMsgToTF(ik_pose.orientation, q);
00532 tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
00533 ROS_DEBUG("IK input - x:% 1.4f y:% 1.4f z:% 1.4f roll:% 1.4f pitch:% 1.4f yaw:% 1.4f (as quaternion: x:% 1.4f y:% 1.4f z:% 1.4f w:% 1.4f) ", ik_pose.position.x, ik_pose.position.y, ik_pose.position.z, roll, pitch, yaw, q.getX(), q.getY(), q.getZ(), q.getW());
00534
00535 int numsol = solve(frame,vfree);
00536
00537 if(numsol){
00538 for(int s = 0; s < numsol; ++s)
00539 {
00540 std::vector<double> sol;
00541 getSolution(s,sol);
00542
00543
00544 bool obeys_limits = true;
00545 for(unsigned int i = 0; i < sol.size(); i++) {
00546
00547 if(joint_has_limits_vector_[i] && ( (sol[i] < (joint_min_vector_[i]-LIMIT_TOLERANCE)) ||
00548 (sol[i] > (joint_max_vector_[i]+LIMIT_TOLERANCE)) ) )
00549 {
00550
00551 obeys_limits = false;
00552
00553 break;
00554 }
00555 }
00556 if(obeys_limits) {
00557
00558 getSolution(s,solution);
00559 error_code = kinematics::SUCCESS;
00560
00561 return true;
00562 }
00563 }
00564 }else{
00565
00566 }
00567
00568 error_code = kinematics::NO_IK_SOLUTION;
00569 return false;
00570 }
00571
00572
00573 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00574 const std::vector<double> &ik_seed_state,
00575 const double &timeout,
00576 std::vector<double> &solution,
00577 int &error_code)
00578 {
00579 if(free_params_.size()==0){
00580 return getPositionIK(ik_pose, ik_seed_state,solution, error_code);
00581 }
00582
00583 KDL::Frame frame;
00584 tf::PoseMsgToKDL(ik_pose,frame);
00585
00586 std::vector<double> vfree(free_params_.size());
00587
00588 ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
00589 int counter = 0;
00590
00591 double initial_guess = ik_seed_state[free_params_[0]];
00592 vfree[0] = initial_guess;
00593
00594 int num_positive_increments = (joint_max_vector_[free_params_[0]]-initial_guess)/search_discretization_;
00595 int num_negative_increments = (initial_guess-joint_min_vector_[free_params_[0]])/search_discretization_;
00596
00597
00598
00599 while(1) {
00600 int numsol = solve(frame,vfree);
00601
00602
00603
00604 if(numsol > 0){
00605 for(int s = 0; s < numsol; ++s){
00606 std::vector<double> sol;
00607 getSolution(s,sol);
00608
00609 bool obeys_limits = true;
00610 for(unsigned int i = 0; i < sol.size(); i++) {
00611 if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i])) {
00612 obeys_limits = false;
00613 break;
00614 }
00615
00616 }
00617 if(obeys_limits) {
00618 getSolution(s,solution);
00619 error_code = kinematics::SUCCESS;
00620 return true;
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
00646
00647
00648 if(!getCount(counter, num_positive_increments, num_negative_increments)) {
00649 error_code = kinematics::NO_IK_SOLUTION;
00650 return false;
00651 }
00652
00653 vfree[0] = initial_guess+search_discretization_*counter;
00654 ROS_DEBUG_STREAM(counter << " " << vfree[0]);
00655 }
00656
00657 error_code = kinematics::NO_IK_SOLUTION;
00658 return false;
00659 }
00660
00661
00662 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00663 const std::vector<double> &ik_seed_state,
00664 const double &timeout,
00665 const unsigned int& redundancy,
00666 const double &consistency_limit,
00667 std::vector<double> &solution,
00668 int &error_code)
00669 {
00670 if(free_params_.size()==0){
00671
00672 return getPositionIK(ik_pose, ik_seed_state,solution, error_code);
00673 ROS_WARN_STREAM("No free parameters, so can't search");
00674 }
00675
00676 if(redundancy != (unsigned int)free_params_[0]) {
00677 ROS_WARN_STREAM("Calling consistency search with wrong free param");
00678 return false;
00679 }
00680
00681 KDL::Frame frame;
00682 tf::PoseMsgToKDL(ik_pose,frame);
00683
00684 std::vector<double> vfree(free_params_.size());
00685
00686 ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
00687 int counter = 0;
00688
00689 double initial_guess = ik_seed_state[free_params_[0]];
00690 vfree[0] = initial_guess;
00691
00692 double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess+consistency_limit);
00693 double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess-consistency_limit);
00694
00695 int num_positive_increments = (int)((max_limit-initial_guess)/search_discretization_);
00696 int num_negative_increments = (int)((initial_guess-min_limit)/search_discretization_);
00697
00698 ROS_DEBUG_STREAM("Free param is " << free_params_[0] << " initial guess is " << initial_guess << " " << num_positive_increments << " " << num_negative_increments);
00699
00700 while(1) {
00701 int numsol = solve(frame,vfree);
00702
00703
00704
00705 if(numsol > 0){
00706 for(int s = 0; s < numsol; ++s){
00707 std::vector<double> sol;
00708
00709 getSolution(s,sol);
00710 bool obeys_limits = true;
00711 for(unsigned int i = 0; i < sol.size(); i++) {
00712 if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i])) {
00713 obeys_limits = false;
00714 break;
00715 }
00716
00717 }
00718 if(obeys_limits) {
00719
00720 getSolution(s,solution);
00721 error_code = kinematics::SUCCESS;
00722 return true;
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
00748
00749
00750 if(!getCount(counter, num_positive_increments, num_negative_increments)) {
00751 error_code = kinematics::NO_IK_SOLUTION;
00752 return false;
00753 }
00754
00755 vfree[0] = initial_guess+search_discretization_*counter;
00756 ROS_DEBUG_STREAM(counter << " " << vfree[0]);
00757 }
00758
00759 error_code = kinematics::NO_IK_SOLUTION;
00760 return false;
00761 }
00762
00763
00764
00765 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00766 const std::vector<double> &ik_seed_state,
00767 const double &timeout,
00768 std::vector<double> &solution,
00769 const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,
00770 int &error_code)> &desired_pose_callback,
00771 const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,
00772 int &error_code)> &solution_callback,
00773 int &error_code)
00774 {
00775
00776 if(free_params_.size()==0){
00777
00778 if(!getPositionIK(ik_pose, ik_seed_state,solution, error_code)) {
00779 ROS_DEBUG_STREAM("No solution whatsoever");
00780 error_code = kinematics::NO_IK_SOLUTION;
00781 return false;
00782 }
00783
00784 solution_callback(ik_pose,solution,error_code);
00785 if(error_code == kinematics::SUCCESS) {
00786 ROS_DEBUG_STREAM("Solution passes");
00787 return true;
00788 } else {
00789 ROS_DEBUG_STREAM("Solution has error code " << error_code);
00790 return false;
00791 }
00792 }
00793
00794 if(!desired_pose_callback.empty())
00795 desired_pose_callback(ik_pose,ik_seed_state,error_code);
00796 if(error_code < 0)
00797 {
00798 ROS_ERROR("Could not find inverse kinematics for desired end-effector pose since the pose may be in collision");
00799 return false;
00800 }
00801
00802 KDL::Frame frame;
00803 tf::PoseMsgToKDL(ik_pose,frame);
00804
00805 std::vector<double> vfree(free_params_.size());
00806
00807 ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
00808 int counter = 0;
00809
00810 double initial_guess = ik_seed_state[free_params_[0]];
00811 vfree[0] = initial_guess;
00812
00813 int num_positive_increments = (joint_max_vector_[free_params_[0]]-initial_guess)/search_discretization_;
00814 int num_negative_increments = (initial_guess - joint_min_vector_[free_params_[0]])/search_discretization_;
00815
00816 ROS_DEBUG_STREAM("Free param is " << free_params_[0] << " initial guess is " << initial_guess << " " << num_positive_increments << " " << num_negative_increments);
00817
00818 unsigned int solvecount = 0;
00819 unsigned int countsol = 0;
00820
00821 ros::WallTime start = ros::WallTime::now();
00822
00823 std::vector<double> sol;
00824 while(1) {
00825 int numsol = solve(frame,vfree);
00826
00827 if(solvecount == 0) {
00828 if(numsol == 0) {
00829 ROS_DEBUG_STREAM("Bad solve time is " << ros::WallTime::now()-start);
00830 } else {
00831 ROS_DEBUG_STREAM("Good solve time is " << ros::WallTime::now()-start);
00832 }
00833 }
00834 solvecount++;
00835 if(numsol > 0){
00836 if(solution_callback.empty()){
00837 getClosestSolution(ik_seed_state,solution);
00838 error_code = kinematics::SUCCESS;
00839 return true;
00840 }
00841
00842 for(int s = 0; s < numsol; ++s){
00843 getSolution(s,sol);
00844 countsol++;
00845 bool obeys_limits = true;
00846 for(unsigned int i = 0; i < sol.size(); i++) {
00847 if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i])) {
00848 obeys_limits = false;
00849 break;
00850 }
00851 }
00852 if(obeys_limits) {
00853 solution_callback(ik_pose,sol,error_code);
00854 if(error_code == kinematics::SUCCESS){
00855 solution = sol;
00856 ROS_DEBUG_STREAM("Took " << (ros::WallTime::now() - start) << " to return true " << countsol << " " << solvecount);
00857 return true;
00858 }
00859 }
00860 }
00861 }
00862 if(!getCount(counter, num_positive_increments, num_negative_increments)) {
00863 error_code = kinematics::NO_IK_SOLUTION;
00864 ROS_DEBUG_STREAM("Took " << (ros::WallTime::now() - start) << " to return false " << countsol << " " << solvecount);
00865 return false;
00866 }
00867 vfree[0] = initial_guess+search_discretization_*counter;
00868 ROS_DEBUG_STREAM(counter << " " << vfree[0]);
00869 }
00870 error_code = kinematics::NO_IK_SOLUTION;
00871 return false;
00872 }
00873
00874
00875 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00876 const std::vector<double> &ik_seed_state,
00877 const double &timeout,
00878 const unsigned int& redundancy,
00879 const double &consistency_limit,
00880 std::vector<double> &solution,
00881 const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,
00882 int &error_code)> &desired_pose_callback,
00883 const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,
00884 int &error_code)> &solution_callback,
00885 int &error_code)
00886 {
00887 if(free_params_.size()==0){
00888 if(!getPositionIK(ik_pose, ik_seed_state,solution, error_code)) {
00889 ROS_DEBUG_STREAM("No solution whatsoever");
00890 error_code = kinematics::NO_IK_SOLUTION;
00891 return false;
00892 }
00893 solution_callback(ik_pose,solution,error_code);
00894 if(error_code == kinematics::SUCCESS) {
00895 ROS_DEBUG_STREAM("Solution passes");
00896 return true;
00897 } else {
00898 ROS_DEBUG_STREAM("Solution has error code " << error_code);
00899 return false;
00900 }
00901 }
00902
00903 if(redundancy != (unsigned int) free_params_[0]) {
00904 ROS_WARN_STREAM("Calling consistency search with wrong free param");
00905 return false;
00906 }
00907
00908 if(!desired_pose_callback.empty())
00909 desired_pose_callback(ik_pose,ik_seed_state,error_code);
00910 if(error_code < 0)
00911 {
00912 ROS_ERROR("Could not find inverse kinematics for desired end-effector pose since the pose may be in collision");
00913 return false;
00914 }
00915
00916 KDL::Frame frame;
00917 tf::PoseMsgToKDL(ik_pose,frame);
00918
00919 std::vector<double> vfree(free_params_.size());
00920
00921 ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
00922 int counter = 0;
00923
00924 double initial_guess = ik_seed_state[free_params_[0]];
00925 vfree[0] = initial_guess;
00926
00927 double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess+consistency_limit);
00928 double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess-consistency_limit);
00929
00930 int num_positive_increments = (int)((max_limit-initial_guess)/search_discretization_);
00931 int num_negative_increments = (int)((initial_guess-min_limit)/search_discretization_);
00932
00933 ROS_DEBUG_STREAM("Free param is " << free_params_[0] << " initial guess is " << initial_guess << " " << num_positive_increments << " " << num_negative_increments);
00934
00935 unsigned int solvecount = 0;
00936 unsigned int countsol = 0;
00937
00938 ros::WallTime start = ros::WallTime::now();
00939
00940 std::vector<double> sol;
00941 while(1) {
00942 int numsol = solve(frame,vfree);
00943 if(solvecount == 0) {
00944 if(numsol == 0) {
00945 ROS_DEBUG_STREAM("Bad solve time is " << ros::WallTime::now()-start);
00946 } else {
00947 ROS_DEBUG_STREAM("Good solve time is " << ros::WallTime::now()-start);
00948 }
00949 }
00950 solvecount++;
00951 if(numsol > 0){
00952 if(solution_callback.empty()){
00953 getClosestSolution(ik_seed_state,solution);
00954 error_code = kinematics::SUCCESS;
00955 return true;
00956 }
00957
00958 for(int s = 0; s < numsol; ++s){
00959 getSolution(s,sol);
00960 countsol++;
00961 bool obeys_limits = true;
00962 for(unsigned int i = 0; i < sol.size(); i++) {
00963 if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i])) {
00964 obeys_limits = false;
00965 break;
00966 }
00967 }
00968 if(obeys_limits) {
00969 solution_callback(ik_pose,sol,error_code);
00970 if(error_code == kinematics::SUCCESS){
00971 solution = sol;
00972 ROS_DEBUG_STREAM("Took " << (ros::WallTime::now() - start) << " to return true " << countsol << " " << solvecount);
00973 return true;
00974 }
00975 }
00976 }
00977 }
00978 if(!getCount(counter, num_positive_increments, num_negative_increments)) {
00979 error_code = kinematics::NO_IK_SOLUTION;
00980 ROS_DEBUG_STREAM("Took " << (ros::WallTime::now() - start) << " to return false " << countsol << " " << solvecount);
00981 return false;
00982 }
00983 vfree[0] = initial_guess+search_discretization_*counter;
00984 ROS_DEBUG_STREAM(counter << " " << vfree[0]);
00985 }
00986 error_code = kinematics::NO_IK_SOLUTION;
00987 return false;
00988 }
00989
00990 }
00991
00992 #include <pluginlib/class_list_macros.h>
00993 PLUGINLIB_DECLARE_CLASS(katana_450_6m90a_kinematics, IKFastKinematicsPlugin, katana_450_6m90a_kinematics::IKFastKinematicsPlugin, kinematics::KinematicsBase);