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;
00055
00056 namespace katana_450_6m90a_kinematics
00057 {
00058
00059 #define IKFAST_NO_MAIN // Don't include main() from IKFast
00060
00066 enum IkParameterizationType {
00067 IKP_None=0,
00068 IKP_Transform6D=0x67000001,
00069 IKP_Rotation3D=0x34000002,
00070 IKP_Translation3D=0x33000003,
00071 IKP_Direction3D=0x23000004,
00072 IKP_Ray4D=0x46000005,
00073 IKP_Lookat3D=0x23000006,
00074 IKP_TranslationDirection5D=0x56000007,
00075 IKP_TranslationXY2D=0x22000008,
00076 IKP_TranslationXYOrientation3D=0x33000009,
00077 IKP_TranslationLocalGlobal6D=0x3600000a,
00078
00079 IKP_TranslationXAxisAngle4D=0x4400000b,
00080 IKP_TranslationYAxisAngle4D=0x4400000c,
00081 IKP_TranslationZAxisAngle4D=0x4400000d,
00082
00083 IKP_TranslationXAxisAngleZNorm4D=0x4400000e,
00084 IKP_TranslationYAxisAngleXNorm4D=0x4400000f,
00085 IKP_TranslationZAxisAngleYNorm4D=0x44000010,
00086
00087 IKP_NumberOfParameterizations=16,
00088
00089 IKP_VelocityDataBit = 0x00008000,
00090 IKP_Transform6DVelocity = IKP_Transform6D|IKP_VelocityDataBit,
00091 IKP_Rotation3DVelocity = IKP_Rotation3D|IKP_VelocityDataBit,
00092 IKP_Translation3DVelocity = IKP_Translation3D|IKP_VelocityDataBit,
00093 IKP_Direction3DVelocity = IKP_Direction3D|IKP_VelocityDataBit,
00094 IKP_Ray4DVelocity = IKP_Ray4D|IKP_VelocityDataBit,
00095 IKP_Lookat3DVelocity = IKP_Lookat3D|IKP_VelocityDataBit,
00096 IKP_TranslationDirection5DVelocity = IKP_TranslationDirection5D|IKP_VelocityDataBit,
00097 IKP_TranslationXY2DVelocity = IKP_TranslationXY2D|IKP_VelocityDataBit,
00098 IKP_TranslationXYOrientation3DVelocity = IKP_TranslationXYOrientation3D|IKP_VelocityDataBit,
00099 IKP_TranslationLocalGlobal6DVelocity = IKP_TranslationLocalGlobal6D|IKP_VelocityDataBit,
00100 IKP_TranslationXAxisAngle4DVelocity = IKP_TranslationXAxisAngle4D|IKP_VelocityDataBit,
00101 IKP_TranslationYAxisAngle4DVelocity = IKP_TranslationYAxisAngle4D|IKP_VelocityDataBit,
00102 IKP_TranslationZAxisAngle4DVelocity = IKP_TranslationZAxisAngle4D|IKP_VelocityDataBit,
00103 IKP_TranslationXAxisAngleZNorm4DVelocity = IKP_TranslationXAxisAngleZNorm4D|IKP_VelocityDataBit,
00104 IKP_TranslationYAxisAngleXNorm4DVelocity = IKP_TranslationYAxisAngleXNorm4D|IKP_VelocityDataBit,
00105 IKP_TranslationZAxisAngleYNorm4DVelocity = IKP_TranslationZAxisAngleYNorm4D|IKP_VelocityDataBit,
00106
00107 IKP_UniqueIdMask = 0x0000ffff,
00108 IKP_CustomDataBit = 0x00010000,
00109 };
00110
00111
00112 #include "katana_450_6m90a_ikfast_solver.cpp"
00113
00114 class IKFastKinematicsPlugin : public kinematics::KinematicsBase
00115 {
00116 std::vector<std::string> joint_names_;
00117 std::vector<double> joint_min_vector_;
00118 std::vector<double> joint_max_vector_;
00119 std::vector<bool> joint_has_limits_vector_;
00120 std::vector<std::string> link_names_;
00121 size_t num_joints_;
00122 std::vector<int> free_params_;
00123 bool active_;
00124
00125 const std::vector<std::string>& getJointNames() const { return joint_names_; }
00126 const std::vector<std::string>& getLinkNames() const { return link_names_; }
00127
00128 public:
00129
00133 IKFastKinematicsPlugin():active_(false){}
00134
00144
00145 bool getPositionIK(const geometry_msgs::Pose &ik_pose,
00146 const std::vector<double> &ik_seed_state,
00147 std::vector<double> &solution,
00148 moveit_msgs::MoveItErrorCodes &error_code,
00149 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00150
00159 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00160 const std::vector<double> &ik_seed_state,
00161 double timeout,
00162 std::vector<double> &solution,
00163 moveit_msgs::MoveItErrorCodes &error_code,
00164 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00165
00175 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00176 const std::vector<double> &ik_seed_state,
00177 double timeout,
00178 const std::vector<double> &consistency_limits,
00179 std::vector<double> &solution,
00180 moveit_msgs::MoveItErrorCodes &error_code,
00181 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00182
00191 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00192 const std::vector<double> &ik_seed_state,
00193 double timeout,
00194 std::vector<double> &solution,
00195 const IKCallbackFn &solution_callback,
00196 moveit_msgs::MoveItErrorCodes &error_code,
00197 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00198
00209 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00210 const std::vector<double> &ik_seed_state,
00211 double timeout,
00212 const std::vector<double> &consistency_limits,
00213 std::vector<double> &solution,
00214 const IKCallbackFn &solution_callback,
00215 moveit_msgs::MoveItErrorCodes &error_code,
00216 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00217
00229 bool getPositionFK(const std::vector<std::string> &link_names,
00230 const std::vector<double> &joint_angles,
00231 std::vector<geometry_msgs::Pose> &poses) const;
00232
00233 private:
00234
00235 bool initialize(const std::string &robot_description,
00236 const std::string& group_name,
00237 const std::string& base_name,
00238 const std::string& tip_name,
00239 double search_discretization);
00240
00245 int solve(KDL::Frame &pose_frame, const std::vector<double> &vfree, IkSolutionList<IkReal> &solutions) const;
00246
00250 void getSolution(const IkSolutionList<IkReal> &solutions, int i, std::vector<double>& solution) const;
00251
00252 double harmonize(const std::vector<double> &ik_seed_state, std::vector<double> &solution) const;
00253
00254 void getClosestSolution(const IkSolutionList<IkReal> &solutions, const std::vector<double> &ik_seed_state, std::vector<double> &solution) const;
00255 void fillFreeParams(int count, int *array);
00256 bool getCount(int &count, const int &max_count, const int &min_count) const;
00257
00258 };
00259
00260 bool IKFastKinematicsPlugin::initialize(const std::string &robot_description,
00261 const std::string& group_name,
00262 const std::string& base_name,
00263 const std::string& tip_name,
00264 double search_discretization)
00265 {
00266 setValues(robot_description, group_name, base_name, tip_name, search_discretization);
00267
00268 ros::NodeHandle node_handle("~/"+group_name);
00269
00270 std::string robot;
00271 node_handle.param("robot",robot,std::string());
00272
00273
00274 fillFreeParams( GetNumFreeParameters(), GetFreeParameters() );
00275 num_joints_ = GetNumJoints();
00276
00277 if(free_params_.size() > 1)
00278 {
00279 ROS_FATAL("Only one free joint paramter supported!");
00280 return false;
00281 }
00282
00283 urdf::Model robot_model;
00284 std::string xml_string;
00285
00286 std::string urdf_xml,full_urdf_xml;
00287 node_handle.param("urdf_xml",urdf_xml,robot_description);
00288 node_handle.searchParam(urdf_xml,full_urdf_xml);
00289
00290 ROS_DEBUG_NAMED("ikfast","Reading xml file from parameter server");
00291 if (!node_handle.getParam(full_urdf_xml, xml_string))
00292 {
00293 ROS_FATAL_NAMED("ikfast","Could not load the xml from parameter server: %s", urdf_xml.c_str());
00294 return false;
00295 }
00296
00297 node_handle.param(full_urdf_xml,xml_string,std::string());
00298 robot_model.initString(xml_string);
00299
00300 ROS_DEBUG_STREAM_NAMED("ikfast","Reading joints and links from URDF");
00301
00302 boost::shared_ptr<urdf::Link> link = boost::const_pointer_cast<urdf::Link>(robot_model.getLink(getTipFrame()));
00303 while(link->name != base_frame_ && joint_names_.size() <= num_joints_)
00304 {
00305 ROS_DEBUG_NAMED("ikfast","Link %s",link->name.c_str());
00306 link_names_.push_back(link->name);
00307 boost::shared_ptr<urdf::Joint> joint = link->parent_joint;
00308 if(joint)
00309 {
00310 if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
00311 {
00312 ROS_DEBUG_STREAM_NAMED("ikfast","Adding joint " << joint->name );
00313
00314 joint_names_.push_back(joint->name);
00315 float lower, upper;
00316 int hasLimits;
00317 if ( joint->type != urdf::Joint::CONTINUOUS )
00318 {
00319 if(joint->safety)
00320 {
00321 lower = joint->safety->soft_lower_limit;
00322 upper = joint->safety->soft_upper_limit;
00323 } else {
00324 lower = joint->limits->lower;
00325 upper = joint->limits->upper;
00326 }
00327 hasLimits = 1;
00328 }
00329 else
00330 {
00331 lower = -M_PI;
00332 upper = M_PI;
00333 hasLimits = 0;
00334 }
00335 if(hasLimits)
00336 {
00337 joint_has_limits_vector_.push_back(true);
00338 joint_min_vector_.push_back(lower);
00339 joint_max_vector_.push_back(upper);
00340 }
00341 else
00342 {
00343 joint_has_limits_vector_.push_back(false);
00344 joint_min_vector_.push_back(-M_PI);
00345 joint_max_vector_.push_back(M_PI);
00346 }
00347 }
00348 } else
00349 {
00350 ROS_WARN_NAMED("ikfast","no joint corresponding to %s",link->name.c_str());
00351 }
00352 link = link->getParent();
00353 }
00354
00355 if(joint_names_.size() != num_joints_)
00356 {
00357 ROS_FATAL_STREAM_NAMED("ikfast","Joint numbers mismatch: URDF has " << joint_names_.size() << " and IKFast has " << num_joints_);
00358 return false;
00359 }
00360
00361 std::reverse(link_names_.begin(),link_names_.end());
00362 std::reverse(joint_names_.begin(),joint_names_.end());
00363 std::reverse(joint_min_vector_.begin(),joint_min_vector_.end());
00364 std::reverse(joint_max_vector_.begin(),joint_max_vector_.end());
00365 std::reverse(joint_has_limits_vector_.begin(), joint_has_limits_vector_.end());
00366
00367 for(size_t i=0; i <num_joints_; ++i)
00368 ROS_INFO_STREAM_NAMED("ikfast",joint_names_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i] << " " << joint_has_limits_vector_[i]);
00369
00370 active_ = true;
00371 return true;
00372 }
00373
00374 int IKFastKinematicsPlugin::solve(KDL::Frame &pose_frame, const std::vector<double> &vfree, IkSolutionList<IkReal> &solutions) const
00375 {
00376
00377 solutions.Clear();
00378
00379 double trans[3];
00380 trans[0] = pose_frame.p[0];
00381 trans[1] = pose_frame.p[1];
00382 trans[2] = pose_frame.p[2];
00383
00384 KDL::Rotation mult;
00385 KDL::Vector direction;
00386
00387 switch (GetIkType())
00388 {
00389 case IKP_Transform6D:
00390 case IKP_Translation3D:
00391
00392
00393 mult = pose_frame.M;
00394
00395 double vals[9];
00396 vals[0] = mult(0,0);
00397 vals[1] = mult(0,1);
00398 vals[2] = mult(0,2);
00399 vals[3] = mult(1,0);
00400 vals[4] = mult(1,1);
00401 vals[5] = mult(1,2);
00402 vals[6] = mult(2,0);
00403 vals[7] = mult(2,1);
00404 vals[8] = mult(2,2);
00405
00406
00407 ComputeIk(trans, vals, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
00408 return solutions.GetNumSolutions();
00409
00410 case IKP_Direction3D:
00411 case IKP_Ray4D:
00412 case IKP_TranslationDirection5D:
00413
00414
00415 direction = pose_frame.M * KDL::Vector(0, 0, 1);
00416 ComputeIk(trans, direction.data, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
00417 return solutions.GetNumSolutions();
00418
00419 case IKP_TranslationXAxisAngle4D:
00420 case IKP_TranslationYAxisAngle4D:
00421 case IKP_TranslationZAxisAngle4D:
00422
00423 ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet.");
00424 return 0;
00425
00426 case IKP_TranslationLocalGlobal6D:
00427
00428 ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet.");
00429 return 0;
00430
00431 case IKP_Rotation3D:
00432 case IKP_Lookat3D:
00433 case IKP_TranslationXY2D:
00434 case IKP_TranslationXYOrientation3D:
00435 case IKP_TranslationXAxisAngleZNorm4D:
00436 case IKP_TranslationYAxisAngleXNorm4D:
00437 case IKP_TranslationZAxisAngleYNorm4D:
00438 ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet.");
00439 return 0;
00440
00441 default:
00442 ROS_ERROR_NAMED("ikfast", "Unknown IkParameterizationType! Was the solver generated with an incompatible version of Openrave?");
00443 return 0;
00444 }
00445 }
00446
00447 void IKFastKinematicsPlugin::getSolution(const IkSolutionList<IkReal> &solutions, int i, std::vector<double>& solution) const
00448 {
00449 solution.clear();
00450 solution.resize(num_joints_);
00451
00452
00453 const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
00454 std::vector<IkReal> vsolfree( sol.GetFree().size() );
00455 sol.GetSolution(&solution[0],vsolfree.size()>0?&vsolfree[0]:NULL);
00456
00457
00458
00459
00460
00461
00462
00463 }
00464
00465 double IKFastKinematicsPlugin::harmonize(const std::vector<double> &ik_seed_state, std::vector<double> &solution) const
00466 {
00467 double dist_sqr = 0;
00468 std::vector<double> ss = ik_seed_state;
00469 for(size_t i=0; i< ik_seed_state.size(); ++i)
00470 {
00471 while(ss[i] > 2*M_PI) {
00472 ss[i] -= 2*M_PI;
00473 }
00474 while(ss[i] < 2*M_PI) {
00475 ss[i] += 2*M_PI;
00476 }
00477 while(solution[i] > 2*M_PI) {
00478 solution[i] -= 2*M_PI;
00479 }
00480 while(solution[i] < 2*M_PI) {
00481 solution[i] += 2*M_PI;
00482 }
00483 dist_sqr += fabs(ik_seed_state[i] - solution[i]);
00484 }
00485 return dist_sqr;
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 void IKFastKinematicsPlugin::getClosestSolution(const IkSolutionList<IkReal> &solutions, const std::vector<double> &ik_seed_state, std::vector<double> &solution) const
00512 {
00513 double mindist = DBL_MAX;
00514 int minindex = -1;
00515 std::vector<double> sol;
00516
00517
00518 for(size_t i=0; i < solutions.GetNumSolutions(); ++i)
00519 {
00520 getSolution(solutions, i,sol);
00521 double dist = harmonize(ik_seed_state, sol);
00522 ROS_INFO_STREAM_NAMED("ikfast","Dist " << i << " dist " << dist);
00523
00524 if(minindex == -1 || dist<mindist){
00525 minindex = i;
00526 mindist = dist;
00527 }
00528 }
00529 if(minindex >= 0){
00530 getSolution(solutions, minindex,solution);
00531 harmonize(ik_seed_state, solution);
00532 }
00533 }
00534
00535 void IKFastKinematicsPlugin::fillFreeParams(int count, int *array)
00536 {
00537 free_params_.clear();
00538 for(int i=0; i<count;++i) free_params_.push_back(array[i]);
00539 }
00540
00541 bool IKFastKinematicsPlugin::getCount(int &count, const int &max_count, const int &min_count) const
00542 {
00543 if(count > 0)
00544 {
00545 if(-count >= min_count)
00546 {
00547 count = -count;
00548 return true;
00549 }
00550 else if(count+1 <= max_count)
00551 {
00552 count = count+1;
00553 return true;
00554 }
00555 else
00556 {
00557 return false;
00558 }
00559 }
00560 else
00561 {
00562 if(1-count <= max_count)
00563 {
00564 count = 1-count;
00565 return true;
00566 }
00567 else if(count-1 >= min_count)
00568 {
00569 count = count -1;
00570 return true;
00571 }
00572 else
00573 return false;
00574 }
00575 }
00576
00577 bool IKFastKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
00578 const std::vector<double> &joint_angles,
00579 std::vector<geometry_msgs::Pose> &poses) const
00580 {
00581 if (GetIkType() != IKP_Transform6D) {
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
00691
00692 if(free_params_.size()==0)
00693 {
00694 ROS_DEBUG_STREAM_NAMED("ikfast","No need to search since no free params/redundant joints");
00695
00696
00697 if(!getPositionIK(ik_pose, ik_seed_state, solution, error_code))
00698 {
00699 ROS_DEBUG_STREAM_NAMED("ikfast","No solution whatsoever");
00700 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00701 return false;
00702 }
00703
00704
00705 if( !solution_callback.empty() )
00706 {
00707 solution_callback(ik_pose, solution, error_code);
00708 if(error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
00709 {
00710 ROS_DEBUG_STREAM_NAMED("ikfast","Solution passes callback");
00711 return true;
00712 }
00713 else
00714 {
00715 ROS_DEBUG_STREAM_NAMED("ikfast","Solution has error code " << error_code);
00716 return false;
00717 }
00718 }
00719 else
00720 {
00721 return true;
00722 }
00723 }
00724
00725
00726
00727 if(!active_)
00728 {
00729 ROS_ERROR_STREAM_NAMED("ikfast","Kinematics not active");
00730 error_code.val = error_code.NO_IK_SOLUTION;
00731 return false;
00732 }
00733
00734 if(ik_seed_state.size() != num_joints_)
00735 {
00736 ROS_ERROR_STREAM_NAMED("ikfast","Seed state must have size " << num_joints_ << " instead of size " << ik_seed_state.size());
00737 error_code.val = error_code.NO_IK_SOLUTION;
00738 return false;
00739 }
00740
00741 if(!consistency_limits.empty() && consistency_limits.size() != num_joints_)
00742 {
00743 ROS_ERROR_STREAM_NAMED("ikfast","Consistency limits be empty or must have size " << num_joints_ << " instead of size " << consistency_limits.size());
00744 error_code.val = error_code.NO_IK_SOLUTION;
00745 return false;
00746 }
00747
00748
00749
00750
00751
00752 KDL::Frame frame;
00753 tf::poseMsgToKDL(ik_pose,frame);
00754
00755 std::vector<double> vfree(free_params_.size());
00756
00757 ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
00758 int counter = 0;
00759
00760 double initial_guess = ik_seed_state[free_params_[0]];
00761 vfree[0] = initial_guess;
00762
00763
00764
00765 int num_positive_increments;
00766 int num_negative_increments;
00767
00768 if(!consistency_limits.empty())
00769 {
00770
00771
00772 double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess+consistency_limits[free_params_[0]]);
00773 double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess-consistency_limits[free_params_[0]]);
00774
00775 num_positive_increments = (int)((max_limit-initial_guess)/search_discretization_);
00776 num_negative_increments = (int)((initial_guess-min_limit)/search_discretization_);
00777 }
00778 else
00779 {
00780 num_positive_increments = (joint_max_vector_[free_params_[0]]-initial_guess)/search_discretization_;
00781 num_negative_increments = (initial_guess-joint_min_vector_[free_params_[0]])/search_discretization_;
00782 }
00783
00784
00785
00786
00787 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);
00788
00789 while(true)
00790 {
00791 IkSolutionList<IkReal> solutions;
00792 int numsol = solve(frame,vfree, solutions);
00793
00794 ROS_DEBUG_STREAM_NAMED("ikfast","Found " << numsol << " solutions from IKFast");
00795
00796
00797
00798 if( numsol > 0 )
00799 {
00800 for(int s = 0; s < numsol; ++s)
00801 {
00802 std::vector<double> sol;
00803 getSolution(solutions,s,sol);
00804
00805 bool obeys_limits = true;
00806 for(unsigned int i = 0; i < sol.size(); i++)
00807 {
00808 if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i]))
00809 {
00810 obeys_limits = false;
00811 break;
00812 }
00813
00814 }
00815 if(obeys_limits)
00816 {
00817 getSolution(solutions,s,solution);
00818
00819
00820 if(!solution_callback.empty())
00821 {
00822 solution_callback(ik_pose, solution, error_code);
00823 }
00824 else
00825 {
00826 error_code.val = error_code.SUCCESS;
00827 }
00828
00829 if(error_code.val == error_code.SUCCESS)
00830 {
00831 return true;
00832 }
00833 }
00834 }
00835 }
00836
00837 if(!getCount(counter, num_positive_increments, num_negative_increments))
00838 {
00839 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00840 return false;
00841 }
00842
00843 vfree[0] = initial_guess+search_discretization_*counter;
00844 ROS_DEBUG_STREAM_NAMED("ikfast","Attempt " << counter << " with 0th free joint having value " << vfree[0]);
00845 }
00846
00847
00848 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00849 return false;
00850 }
00851
00852
00853 bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose,
00854 const std::vector<double> &ik_seed_state,
00855 std::vector<double> &solution,
00856 moveit_msgs::MoveItErrorCodes &error_code,
00857 const kinematics::KinematicsQueryOptions &options) const
00858 {
00859 ROS_DEBUG_STREAM_NAMED("ikfast","getPositionIK");
00860
00861 if(!active_)
00862 {
00863 ROS_ERROR("kinematics not active");
00864 return false;
00865 }
00866
00867 std::vector<double> vfree(free_params_.size());
00868 for(std::size_t i = 0; i < free_params_.size(); ++i)
00869 {
00870 int p = free_params_[i];
00871 ROS_ERROR("%u is %f",p,ik_seed_state[p]);
00872 vfree[i] = ik_seed_state[p];
00873 }
00874
00875 KDL::Frame frame;
00876 tf::poseMsgToKDL(ik_pose,frame);
00877
00878 IkSolutionList<IkReal> solutions;
00879 int numsol = solve(frame,vfree,solutions);
00880
00881 ROS_DEBUG_STREAM_NAMED("ikfast","Found " << numsol << " solutions from IKFast");
00882
00883 if(numsol)
00884 {
00885 for(int s = 0; s < numsol; ++s)
00886 {
00887 std::vector<double> sol;
00888 getSolution(solutions,s,sol);
00889 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]);
00890
00891 bool obeys_limits = true;
00892 for(unsigned int i = 0; i < sol.size(); i++)
00893 {
00894
00895 if(joint_has_limits_vector_[i] && ( (sol[i] < (joint_min_vector_[i]-LIMIT_TOLERANCE)) ||
00896 (sol[i] > (joint_max_vector_[i]+LIMIT_TOLERANCE)) ) )
00897 {
00898
00899 obeys_limits = false;
00900 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]);
00901 break;
00902 }
00903 }
00904 if(obeys_limits)
00905 {
00906
00907 getSolution(solutions,s,solution);
00908 error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00909 return true;
00910 }
00911 }
00912 }
00913 else
00914 {
00915 ROS_DEBUG_STREAM_NAMED("ikfast","No IK solution");
00916 }
00917
00918 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00919 return false;
00920 }
00921
00922
00923
00924 }
00925
00926
00927 #include <pluginlib/class_list_macros.h>
00928 PLUGINLIB_EXPORT_CLASS(katana_450_6m90a_kinematics::IKFastKinematicsPlugin, kinematics::KinematicsBase);