00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048 #include <ros/ros.h>
00049 #include <moveit/kinematics_base/kinematics_base.h>
00050 #include <urdf/model.h>
00051 #include <tf_conversions/tf_kdl.h>
00052
00053
00054 const double LIMIT_TOLERANCE = .0000001;
00056 enum SEARCH_MODE { OPTIMIZE_FREE_JOINT=1, OPTIMIZE_MAX_JOINT=2 };
00057
00058 namespace ikfast_kinematics_plugin
00059 {
00060
00061 #define IKFAST_NO_MAIN // Don't include main() from IKFast
00062
00068 enum IkParameterizationType {
00069 IKP_None=0,
00070 IKP_Transform6D=0x67000001,
00071 IKP_Rotation3D=0x34000002,
00072 IKP_Translation3D=0x33000003,
00073 IKP_Direction3D=0x23000004,
00074 IKP_Ray4D=0x46000005,
00075 IKP_Lookat3D=0x23000006,
00076 IKP_TranslationDirection5D=0x56000007,
00077 IKP_TranslationXY2D=0x22000008,
00078 IKP_TranslationXYOrientation3D=0x33000009,
00079 IKP_TranslationLocalGlobal6D=0x3600000a,
00080
00081 IKP_TranslationXAxisAngle4D=0x4400000b,
00082 IKP_TranslationYAxisAngle4D=0x4400000c,
00083 IKP_TranslationZAxisAngle4D=0x4400000d,
00084
00085 IKP_TranslationXAxisAngleZNorm4D=0x4400000e,
00086 IKP_TranslationYAxisAngleXNorm4D=0x4400000f,
00087 IKP_TranslationZAxisAngleYNorm4D=0x44000010,
00088
00089 IKP_NumberOfParameterizations=16,
00090
00091 IKP_VelocityDataBit = 0x00008000,
00092 IKP_Transform6DVelocity = IKP_Transform6D|IKP_VelocityDataBit,
00093 IKP_Rotation3DVelocity = IKP_Rotation3D|IKP_VelocityDataBit,
00094 IKP_Translation3DVelocity = IKP_Translation3D|IKP_VelocityDataBit,
00095 IKP_Direction3DVelocity = IKP_Direction3D|IKP_VelocityDataBit,
00096 IKP_Ray4DVelocity = IKP_Ray4D|IKP_VelocityDataBit,
00097 IKP_Lookat3DVelocity = IKP_Lookat3D|IKP_VelocityDataBit,
00098 IKP_TranslationDirection5DVelocity = IKP_TranslationDirection5D|IKP_VelocityDataBit,
00099 IKP_TranslationXY2DVelocity = IKP_TranslationXY2D|IKP_VelocityDataBit,
00100 IKP_TranslationXYOrientation3DVelocity = IKP_TranslationXYOrientation3D|IKP_VelocityDataBit,
00101 IKP_TranslationLocalGlobal6DVelocity = IKP_TranslationLocalGlobal6D|IKP_VelocityDataBit,
00102 IKP_TranslationXAxisAngle4DVelocity = IKP_TranslationXAxisAngle4D|IKP_VelocityDataBit,
00103 IKP_TranslationYAxisAngle4DVelocity = IKP_TranslationYAxisAngle4D|IKP_VelocityDataBit,
00104 IKP_TranslationZAxisAngle4DVelocity = IKP_TranslationZAxisAngle4D|IKP_VelocityDataBit,
00105 IKP_TranslationXAxisAngleZNorm4DVelocity = IKP_TranslationXAxisAngleZNorm4D|IKP_VelocityDataBit,
00106 IKP_TranslationYAxisAngleXNorm4DVelocity = IKP_TranslationYAxisAngleXNorm4D|IKP_VelocityDataBit,
00107 IKP_TranslationZAxisAngleYNorm4DVelocity = IKP_TranslationZAxisAngleYNorm4D|IKP_VelocityDataBit,
00108
00109 IKP_UniqueIdMask = 0x0000ffff,
00110 IKP_CustomDataBit = 0x00010000,
00111 };
00112
00113
00114 #include "fanuc_m6ib_manipulator_ikfast_solver.cpp"
00115
00116 class IKFastKinematicsPlugin : public kinematics::KinematicsBase
00117 {
00118 std::vector<std::string> joint_names_;
00119 std::vector<double> joint_min_vector_;
00120 std::vector<double> joint_max_vector_;
00121 std::vector<bool> joint_has_limits_vector_;
00122 std::vector<std::string> link_names_;
00123 size_t num_joints_;
00124 std::vector<int> free_params_;
00125 bool active_;
00126
00127 const std::vector<std::string>& getJointNames() const { return joint_names_; }
00128 const std::vector<std::string>& getLinkNames() const { return link_names_; }
00129
00130 public:
00131
00135 IKFastKinematicsPlugin():active_(false){}
00136
00146
00147 bool getPositionIK(const geometry_msgs::Pose &ik_pose,
00148 const std::vector<double> &ik_seed_state,
00149 std::vector<double> &solution,
00150 moveit_msgs::MoveItErrorCodes &error_code,
00151 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00152
00161 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00162 const std::vector<double> &ik_seed_state,
00163 double timeout,
00164 std::vector<double> &solution,
00165 moveit_msgs::MoveItErrorCodes &error_code,
00166 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00167
00177 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00178 const std::vector<double> &ik_seed_state,
00179 double timeout,
00180 const std::vector<double> &consistency_limits,
00181 std::vector<double> &solution,
00182 moveit_msgs::MoveItErrorCodes &error_code,
00183 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00184
00193 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00194 const std::vector<double> &ik_seed_state,
00195 double timeout,
00196 std::vector<double> &solution,
00197 const IKCallbackFn &solution_callback,
00198 moveit_msgs::MoveItErrorCodes &error_code,
00199 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00200
00211 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00212 const std::vector<double> &ik_seed_state,
00213 double timeout,
00214 const std::vector<double> &consistency_limits,
00215 std::vector<double> &solution,
00216 const IKCallbackFn &solution_callback,
00217 moveit_msgs::MoveItErrorCodes &error_code,
00218 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00219
00228 bool getPositionFK(const std::vector<std::string> &link_names,
00229 const std::vector<double> &joint_angles,
00230 std::vector<geometry_msgs::Pose> &poses) const;
00231
00232 private:
00233
00234 bool initialize(const std::string &robot_description,
00235 const std::string& group_name,
00236 const std::string& base_name,
00237 const std::string& tip_name,
00238 double search_discretization);
00239
00244 int solve(KDL::Frame &pose_frame, const std::vector<double> &vfree, IkSolutionList<IkReal> &solutions) const;
00245
00249 void getSolution(const IkSolutionList<IkReal> &solutions, int i, std::vector<double>& solution) const;
00250
00251 double harmonize(const std::vector<double> &ik_seed_state, std::vector<double> &solution) const;
00252
00253 void getClosestSolution(const IkSolutionList<IkReal> &solutions, const std::vector<double> &ik_seed_state, std::vector<double> &solution) const;
00254 void fillFreeParams(int count, int *array);
00255 bool getCount(int &count, const int &max_count, const int &min_count) const;
00256
00257 };
00258
00259 bool IKFastKinematicsPlugin::initialize(const std::string &robot_description,
00260 const std::string& group_name,
00261 const std::string& base_name,
00262 const std::string& tip_name,
00263 double search_discretization)
00264 {
00265 setValues(robot_description, group_name, base_name, tip_name, search_discretization);
00266
00267 ros::NodeHandle node_handle("~/"+group_name);
00268
00269 std::string robot;
00270 node_handle.param("robot",robot,std::string());
00271
00272
00273 fillFreeParams( GetNumFreeParameters(), GetFreeParameters() );
00274 num_joints_ = GetNumJoints();
00275
00276 if(free_params_.size() > 1)
00277 {
00278 ROS_FATAL("Only one free joint paramter supported!");
00279 return false;
00280 }
00281
00282 urdf::Model robot_model;
00283 std::string xml_string;
00284
00285 std::string urdf_xml,full_urdf_xml;
00286 node_handle.param("urdf_xml",urdf_xml,robot_description);
00287 node_handle.searchParam(urdf_xml,full_urdf_xml);
00288
00289 ROS_DEBUG_NAMED("ikfast","Reading xml file from parameter server");
00290 if (!node_handle.getParam(full_urdf_xml, xml_string))
00291 {
00292 ROS_FATAL_NAMED("ikfast","Could not load the xml from parameter server: %s", urdf_xml.c_str());
00293 return false;
00294 }
00295
00296 node_handle.param(full_urdf_xml,xml_string,std::string());
00297 robot_model.initString(xml_string);
00298
00299 ROS_DEBUG_STREAM_NAMED("ikfast","Reading joints and links from URDF");
00300
00301 boost::shared_ptr<urdf::Link> link = boost::const_pointer_cast<urdf::Link>(robot_model.getLink(getTipFrame()));
00302 while(link->name != base_frame_ && joint_names_.size() <= num_joints_)
00303 {
00304 ROS_DEBUG_NAMED("ikfast","Link %s",link->name.c_str());
00305 link_names_.push_back(link->name);
00306 boost::shared_ptr<urdf::Joint> joint = link->parent_joint;
00307 if(joint)
00308 {
00309 if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
00310 {
00311 ROS_DEBUG_STREAM_NAMED("ikfast","Adding joint " << joint->name );
00312
00313 joint_names_.push_back(joint->name);
00314 float lower, upper;
00315 int hasLimits;
00316 if ( joint->type != urdf::Joint::CONTINUOUS )
00317 {
00318 if(joint->safety)
00319 {
00320 lower = joint->safety->soft_lower_limit;
00321 upper = joint->safety->soft_upper_limit;
00322 } else {
00323 lower = joint->limits->lower;
00324 upper = joint->limits->upper;
00325 }
00326 hasLimits = 1;
00327 }
00328 else
00329 {
00330 lower = -M_PI;
00331 upper = M_PI;
00332 hasLimits = 0;
00333 }
00334 if(hasLimits)
00335 {
00336 joint_has_limits_vector_.push_back(true);
00337 joint_min_vector_.push_back(lower);
00338 joint_max_vector_.push_back(upper);
00339 }
00340 else
00341 {
00342 joint_has_limits_vector_.push_back(false);
00343 joint_min_vector_.push_back(-M_PI);
00344 joint_max_vector_.push_back(M_PI);
00345 }
00346 }
00347 } else
00348 {
00349 ROS_WARN_NAMED("ikfast","no joint corresponding to %s",link->name.c_str());
00350 }
00351 link = link->getParent();
00352 }
00353
00354 if(joint_names_.size() != num_joints_)
00355 {
00356 ROS_FATAL_STREAM_NAMED("ikfast","Joint numbers mismatch: URDF has " << joint_names_.size() << " and IKFast has " << num_joints_);
00357 return false;
00358 }
00359
00360 std::reverse(link_names_.begin(),link_names_.end());
00361 std::reverse(joint_names_.begin(),joint_names_.end());
00362 std::reverse(joint_min_vector_.begin(),joint_min_vector_.end());
00363 std::reverse(joint_max_vector_.begin(),joint_max_vector_.end());
00364 std::reverse(joint_has_limits_vector_.begin(), joint_has_limits_vector_.end());
00365
00366 for(size_t i=0; i <num_joints_; ++i)
00367 ROS_DEBUG_STREAM_NAMED("ikfast",joint_names_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i] << " " << joint_has_limits_vector_[i]);
00368
00369 active_ = true;
00370 return true;
00371 }
00372
00373 int IKFastKinematicsPlugin::solve(KDL::Frame &pose_frame, const std::vector<double> &vfree, IkSolutionList<IkReal> &solutions) const
00374 {
00375
00376 solutions.Clear();
00377
00378 double trans[3];
00379 trans[0] = pose_frame.p[0];
00380 trans[1] = pose_frame.p[1];
00381 trans[2] = pose_frame.p[2];
00382
00383 KDL::Rotation mult;
00384 KDL::Vector direction;
00385
00386 switch (GetIkType())
00387 {
00388 case IKP_Transform6D:
00389 case IKP_Translation3D:
00390
00391
00392 mult = pose_frame.M;
00393
00394 double vals[9];
00395 vals[0] = mult(0,0);
00396 vals[1] = mult(0,1);
00397 vals[2] = mult(0,2);
00398 vals[3] = mult(1,0);
00399 vals[4] = mult(1,1);
00400 vals[5] = mult(1,2);
00401 vals[6] = mult(2,0);
00402 vals[7] = mult(2,1);
00403 vals[8] = mult(2,2);
00404
00405
00406 ComputeIk(trans, vals, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
00407 return solutions.GetNumSolutions();
00408
00409 case IKP_Direction3D:
00410 case IKP_Ray4D:
00411 case IKP_TranslationDirection5D:
00412
00413
00414 direction = pose_frame.M * KDL::Vector(0, 0, 1);
00415 ComputeIk(trans, direction.data, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
00416 return solutions.GetNumSolutions();
00417
00418 case IKP_TranslationXAxisAngle4D:
00419 case IKP_TranslationYAxisAngle4D:
00420 case IKP_TranslationZAxisAngle4D:
00421
00422 ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet.");
00423 return 0;
00424
00425 case IKP_TranslationLocalGlobal6D:
00426
00427 ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet.");
00428 return 0;
00429
00430 case IKP_Rotation3D:
00431 case IKP_Lookat3D:
00432 case IKP_TranslationXY2D:
00433 case IKP_TranslationXYOrientation3D:
00434 case IKP_TranslationXAxisAngleZNorm4D:
00435 case IKP_TranslationYAxisAngleXNorm4D:
00436 case IKP_TranslationZAxisAngleYNorm4D:
00437 ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet.");
00438 return 0;
00439
00440 default:
00441 ROS_ERROR_NAMED("ikfast", "Unknown IkParameterizationType! Was the solver generated with an incompatible version of Openrave?");
00442 return 0;
00443 }
00444 }
00445
00446 void IKFastKinematicsPlugin::getSolution(const IkSolutionList<IkReal> &solutions, int i, std::vector<double>& solution) const
00447 {
00448 solution.clear();
00449 solution.resize(num_joints_);
00450
00451
00452 const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
00453 std::vector<IkReal> vsolfree( sol.GetFree().size() );
00454 sol.GetSolution(&solution[0],vsolfree.size()>0?&vsolfree[0]:NULL);
00455
00456
00457
00458
00459
00460
00461
00462 }
00463
00464 double IKFastKinematicsPlugin::harmonize(const std::vector<double> &ik_seed_state, std::vector<double> &solution) const
00465 {
00466 double dist_sqr = 0;
00467 std::vector<double> ss = ik_seed_state;
00468 for(size_t i=0; i< ik_seed_state.size(); ++i)
00469 {
00470 while(ss[i] > 2*M_PI) {
00471 ss[i] -= 2*M_PI;
00472 }
00473 while(ss[i] < 2*M_PI) {
00474 ss[i] += 2*M_PI;
00475 }
00476 while(solution[i] > 2*M_PI) {
00477 solution[i] -= 2*M_PI;
00478 }
00479 while(solution[i] < 2*M_PI) {
00480 solution[i] += 2*M_PI;
00481 }
00482 dist_sqr += fabs(ik_seed_state[i] - solution[i]);
00483 }
00484 return dist_sqr;
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 void IKFastKinematicsPlugin::getClosestSolution(const IkSolutionList<IkReal> &solutions, const std::vector<double> &ik_seed_state, std::vector<double> &solution) const
00511 {
00512 double mindist = DBL_MAX;
00513 int minindex = -1;
00514 std::vector<double> sol;
00515
00516
00517 for(size_t i=0; i < solutions.GetNumSolutions(); ++i)
00518 {
00519 getSolution(solutions, i,sol);
00520 double dist = harmonize(ik_seed_state, sol);
00521 ROS_INFO_STREAM_NAMED("ikfast","Dist " << i << " dist " << dist);
00522
00523 if(minindex == -1 || dist<mindist){
00524 minindex = i;
00525 mindist = dist;
00526 }
00527 }
00528 if(minindex >= 0){
00529 getSolution(solutions, minindex,solution);
00530 harmonize(ik_seed_state, solution);
00531 }
00532 }
00533
00534 void IKFastKinematicsPlugin::fillFreeParams(int count, int *array)
00535 {
00536 free_params_.clear();
00537 for(int i=0; i<count;++i) free_params_.push_back(array[i]);
00538 }
00539
00540 bool IKFastKinematicsPlugin::getCount(int &count, const int &max_count, const int &min_count) const
00541 {
00542 if(count > 0)
00543 {
00544 if(-count >= min_count)
00545 {
00546 count = -count;
00547 return true;
00548 }
00549 else if(count+1 <= max_count)
00550 {
00551 count = count+1;
00552 return true;
00553 }
00554 else
00555 {
00556 return false;
00557 }
00558 }
00559 else
00560 {
00561 if(1-count <= max_count)
00562 {
00563 count = 1-count;
00564 return true;
00565 }
00566 else if(count-1 >= min_count)
00567 {
00568 count = count -1;
00569 return true;
00570 }
00571 else
00572 return false;
00573 }
00574 }
00575
00576 bool IKFastKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
00577 const std::vector<double> &joint_angles,
00578 std::vector<geometry_msgs::Pose> &poses) const
00579 {
00580 if (GetIkType() != IKP_Transform6D) {
00581
00582
00583
00584
00585 ROS_ERROR_NAMED("ikfast", "Can only compute FK for Transform6D IK type!");
00586 return false;
00587 }
00588
00589 KDL::Frame p_out;
00590 if(link_names.size() == 0) {
00591 ROS_WARN_STREAM_NAMED("ikfast","Link names with nothing");
00592 return false;
00593 }
00594
00595 if(link_names.size()!=1 || link_names[0]!=getTipFrame()){
00596 ROS_ERROR_NAMED("ikfast","Can compute FK for %s only",getTipFrame().c_str());
00597 return false;
00598 }
00599
00600 bool valid = true;
00601
00602 IkReal eerot[9],eetrans[3];
00603 IkReal angles[joint_angles.size()];
00604 for (unsigned char i=0; i < joint_angles.size(); i++)
00605 angles[i] = joint_angles[i];
00606
00607
00608 ComputeFk(angles,eetrans,eerot);
00609
00610 for(int i=0; i<3;++i)
00611 p_out.p.data[i] = eetrans[i];
00612
00613 for(int i=0; i<9;++i)
00614 p_out.M.data[i] = eerot[i];
00615
00616 poses.resize(1);
00617 tf::poseKDLToMsg(p_out,poses[0]);
00618
00619 return valid;
00620 }
00621
00622 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00623 const std::vector<double> &ik_seed_state,
00624 double timeout,
00625 std::vector<double> &solution,
00626 moveit_msgs::MoveItErrorCodes &error_code,
00627 const kinematics::KinematicsQueryOptions &options) const
00628 {
00629 const IKCallbackFn solution_callback = 0;
00630 std::vector<double> consistency_limits;
00631
00632 return searchPositionIK(ik_pose,
00633 ik_seed_state,
00634 timeout,
00635 consistency_limits,
00636 solution,
00637 solution_callback,
00638 error_code,
00639 options);
00640 }
00641
00642 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00643 const std::vector<double> &ik_seed_state,
00644 double timeout,
00645 const std::vector<double> &consistency_limits,
00646 std::vector<double> &solution,
00647 moveit_msgs::MoveItErrorCodes &error_code,
00648 const kinematics::KinematicsQueryOptions &options) const
00649 {
00650 const IKCallbackFn solution_callback = 0;
00651 return searchPositionIK(ik_pose,
00652 ik_seed_state,
00653 timeout,
00654 consistency_limits,
00655 solution,
00656 solution_callback,
00657 error_code,
00658 options);
00659 }
00660
00661 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00662 const std::vector<double> &ik_seed_state,
00663 double timeout,
00664 std::vector<double> &solution,
00665 const IKCallbackFn &solution_callback,
00666 moveit_msgs::MoveItErrorCodes &error_code,
00667 const kinematics::KinematicsQueryOptions &options) const
00668 {
00669 std::vector<double> consistency_limits;
00670 return searchPositionIK(ik_pose,
00671 ik_seed_state,
00672 timeout,
00673 consistency_limits,
00674 solution,
00675 solution_callback,
00676 error_code,
00677 options);
00678 }
00679
00680 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00681 const std::vector<double> &ik_seed_state,
00682 double timeout,
00683 const std::vector<double> &consistency_limits,
00684 std::vector<double> &solution,
00685 const IKCallbackFn &solution_callback,
00686 moveit_msgs::MoveItErrorCodes &error_code,
00687 const kinematics::KinematicsQueryOptions &options) const
00688 {
00689 ROS_DEBUG_STREAM_NAMED("ikfast","searchPositionIK");
00690
00692 SEARCH_MODE search_mode = OPTIMIZE_MAX_JOINT;
00693
00694
00695 if(free_params_.size()==0)
00696 {
00697 ROS_DEBUG_STREAM_NAMED("ikfast","No need to search since no free params/redundant joints");
00698
00699
00700 if(!getPositionIK(ik_pose, ik_seed_state, solution, error_code))
00701 {
00702 ROS_DEBUG_STREAM_NAMED("ikfast","No solution whatsoever");
00703 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00704 return false;
00705 }
00706
00707
00708 if( !solution_callback.empty() )
00709 {
00710 solution_callback(ik_pose, solution, error_code);
00711 if(error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
00712 {
00713 ROS_DEBUG_STREAM_NAMED("ikfast","Solution passes callback");
00714 return true;
00715 }
00716 else
00717 {
00718 ROS_DEBUG_STREAM_NAMED("ikfast","Solution has error code " << error_code);
00719 return false;
00720 }
00721 }
00722 else
00723 {
00724 return true;
00725 }
00726 }
00727
00728
00729
00730 if(!active_)
00731 {
00732 ROS_ERROR_STREAM_NAMED("ikfast","Kinematics not active");
00733 error_code.val = error_code.NO_IK_SOLUTION;
00734 return false;
00735 }
00736
00737 if(ik_seed_state.size() != num_joints_)
00738 {
00739 ROS_ERROR_STREAM_NAMED("ikfast","Seed state must have size " << num_joints_ << " instead of size " << ik_seed_state.size());
00740 error_code.val = error_code.NO_IK_SOLUTION;
00741 return false;
00742 }
00743
00744 if(!consistency_limits.empty() && consistency_limits.size() != num_joints_)
00745 {
00746 ROS_ERROR_STREAM_NAMED("ikfast","Consistency limits be empty or must have size " << num_joints_ << " instead of size " << consistency_limits.size());
00747 error_code.val = error_code.NO_IK_SOLUTION;
00748 return false;
00749 }
00750
00751
00752
00753
00754
00755 KDL::Frame frame;
00756 tf::poseMsgToKDL(ik_pose,frame);
00757
00758 std::vector<double> vfree(free_params_.size());
00759
00760 ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
00761 int counter = 0;
00762
00763 double initial_guess = ik_seed_state[free_params_[0]];
00764 vfree[0] = initial_guess;
00765
00766
00767
00768 int num_positive_increments;
00769 int num_negative_increments;
00770
00771 if(!consistency_limits.empty())
00772 {
00773
00774
00775 double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess+consistency_limits[free_params_[0]]);
00776 double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess-consistency_limits[free_params_[0]]);
00777
00778 num_positive_increments = (int)((max_limit-initial_guess)/search_discretization_);
00779 num_negative_increments = (int)((initial_guess-min_limit)/search_discretization_);
00780 }
00781 else
00782 {
00783 num_positive_increments = (joint_max_vector_[free_params_[0]]-initial_guess)/search_discretization_;
00784 num_negative_increments = (initial_guess-joint_min_vector_[free_params_[0]])/search_discretization_;
00785 }
00786
00787
00788
00789
00790 ROS_DEBUG_STREAM_NAMED("ikfast","Free param is " << free_params_[0] << " initial guess is " << initial_guess << ", # positive increments: " << num_positive_increments << ", # negative increments: " << num_negative_increments);
00791 if ((search_mode & OPTIMIZE_MAX_JOINT) && (num_positive_increments + num_negative_increments) > 1000)
00792 ROS_WARN_STREAM_ONCE_NAMED("ikfast", "Large search space, consider increasing the search discretization");
00793
00794 double best_costs = -1.0;
00795 std::vector<double> best_solution;
00796 int nattempts = 0, nvalid = 0;
00797
00798 while(true)
00799 {
00800 IkSolutionList<IkReal> solutions;
00801 int numsol = solve(frame,vfree, solutions);
00802
00803 ROS_DEBUG_STREAM_NAMED("ikfast","Found " << numsol << " solutions from IKFast");
00804
00805
00806
00807 if( numsol > 0 )
00808 {
00809 for(int s = 0; s < numsol; ++s)
00810 {
00811 nattempts++;
00812 std::vector<double> sol;
00813 getSolution(solutions,s,sol);
00814
00815 bool obeys_limits = true;
00816 for(unsigned int i = 0; i < sol.size(); i++)
00817 {
00818 if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i]))
00819 {
00820 obeys_limits = false;
00821 break;
00822 }
00823
00824 }
00825 if(obeys_limits)
00826 {
00827 getSolution(solutions,s,solution);
00828
00829
00830 if(!solution_callback.empty())
00831 {
00832 solution_callback(ik_pose, solution, error_code);
00833 }
00834 else
00835 {
00836 error_code.val = error_code.SUCCESS;
00837 }
00838
00839 if(error_code.val == error_code.SUCCESS)
00840 {
00841 nvalid++;
00842 if (search_mode & OPTIMIZE_MAX_JOINT)
00843 {
00844
00845 double costs = 0.0;
00846 for(unsigned int i = 0; i < solution.size(); i++)
00847 {
00848 double d = fabs(ik_seed_state[i] - solution[i]);
00849 if (d > costs)
00850 costs = d;
00851 }
00852 if (costs < best_costs || best_costs == -1.0)
00853 {
00854 best_costs = costs;
00855 best_solution = solution;
00856 }
00857 }
00858 else
00859
00860 return true;
00861 }
00862 }
00863 }
00864 }
00865
00866 if(!getCount(counter, num_positive_increments, -num_negative_increments))
00867 {
00868
00869 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00870 break;
00871 }
00872
00873 vfree[0] = initial_guess+search_discretization_*counter;
00874
00875 }
00876
00877 ROS_DEBUG_STREAM_NAMED("ikfast", "Valid solutions: " << nvalid << "/" << nattempts);
00878
00879 if ((search_mode & OPTIMIZE_MAX_JOINT) && best_costs != -1.0)
00880 {
00881 solution = best_solution;
00882 error_code.val = error_code.SUCCESS;
00883 return true;
00884 }
00885
00886
00887 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00888 return false;
00889 }
00890
00891
00892 bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose,
00893 const std::vector<double> &ik_seed_state,
00894 std::vector<double> &solution,
00895 moveit_msgs::MoveItErrorCodes &error_code,
00896 const kinematics::KinematicsQueryOptions &options) const
00897 {
00898 ROS_DEBUG_STREAM_NAMED("ikfast","getPositionIK");
00899
00900 if(!active_)
00901 {
00902 ROS_ERROR("kinematics not active");
00903 return false;
00904 }
00905
00906 std::vector<double> vfree(free_params_.size());
00907 for(std::size_t i = 0; i < free_params_.size(); ++i)
00908 {
00909 int p = free_params_[i];
00910 ROS_ERROR("%u is %f",p,ik_seed_state[p]);
00911 vfree[i] = ik_seed_state[p];
00912 }
00913
00914 KDL::Frame frame;
00915 tf::poseMsgToKDL(ik_pose,frame);
00916
00917 IkSolutionList<IkReal> solutions;
00918 int numsol = solve(frame,vfree,solutions);
00919
00920 ROS_DEBUG_STREAM_NAMED("ikfast","Found " << numsol << " solutions from IKFast");
00921
00922 if(numsol)
00923 {
00924 for(int s = 0; s < numsol; ++s)
00925 {
00926 std::vector<double> sol;
00927 getSolution(solutions,s,sol);
00928 ROS_DEBUG_NAMED("ikfast","Sol %d: %e %e %e %e %e %e", s, sol[0], sol[1], sol[2], sol[3], sol[4], sol[5]);
00929
00930 bool obeys_limits = true;
00931 for(unsigned int i = 0; i < sol.size(); i++)
00932 {
00933
00934 if(joint_has_limits_vector_[i] && ( (sol[i] < (joint_min_vector_[i]-LIMIT_TOLERANCE)) ||
00935 (sol[i] > (joint_max_vector_[i]+LIMIT_TOLERANCE)) ) )
00936 {
00937
00938 obeys_limits = false;
00939 ROS_DEBUG_STREAM_NAMED("ikfast","Not in limits! " << i << " value " << sol[i] << " has limit: " << joint_has_limits_vector_[i] << " being " << joint_min_vector_[i] << " to " << joint_max_vector_[i]);
00940 break;
00941 }
00942 }
00943 if(obeys_limits)
00944 {
00945
00946 getSolution(solutions,s,solution);
00947 error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00948 return true;
00949 }
00950 }
00951 }
00952 else
00953 {
00954 ROS_DEBUG_STREAM_NAMED("ikfast","No IK solution");
00955 }
00956
00957 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00958 return false;
00959 }
00960
00961
00962
00963 }
00964
00965
00966 #include <pluginlib/class_list_macros.h>
00967 PLUGINLIB_EXPORT_CLASS(ikfast_kinematics_plugin::IKFastKinematicsPlugin, kinematics::KinematicsBase);