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