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 "robotican_manipulator_h_arm_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():
00136 active_(false)
00137 {
00138 srand( time(NULL) );
00139 supported_methods_.push_back(kinematics::DiscretizationMethods::NO_DISCRETIZATION);
00140 supported_methods_.push_back(kinematics::DiscretizationMethods::ALL_DISCRETIZED);
00141 supported_methods_.push_back(kinematics::DiscretizationMethods::ALL_RANDOM_SAMPLED);
00142 }
00143
00153
00154 bool getPositionIK(const geometry_msgs::Pose &ik_pose,
00155 const std::vector<double> &ik_seed_state,
00156 std::vector<double> &solution,
00157 moveit_msgs::MoveItErrorCodes &error_code,
00158 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00159
00175 bool getPositionIK(const std::vector<geometry_msgs::Pose> &ik_poses,
00176 const std::vector<double> &ik_seed_state,
00177 std::vector< std::vector<double> >& solutions,
00178 kinematics::KinematicsResult& result,
00179 const kinematics::KinematicsQueryOptions &options) const;
00180
00189 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00190 const std::vector<double> &ik_seed_state,
00191 double timeout,
00192 std::vector<double> &solution,
00193 moveit_msgs::MoveItErrorCodes &error_code,
00194 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00195
00205 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00206 const std::vector<double> &ik_seed_state,
00207 double timeout,
00208 const std::vector<double> &consistency_limits,
00209 std::vector<double> &solution,
00210 moveit_msgs::MoveItErrorCodes &error_code,
00211 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00212
00221 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00222 const std::vector<double> &ik_seed_state,
00223 double timeout,
00224 std::vector<double> &solution,
00225 const IKCallbackFn &solution_callback,
00226 moveit_msgs::MoveItErrorCodes &error_code,
00227 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00228
00239 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00240 const std::vector<double> &ik_seed_state,
00241 double timeout,
00242 const std::vector<double> &consistency_limits,
00243 std::vector<double> &solution,
00244 const IKCallbackFn &solution_callback,
00245 moveit_msgs::MoveItErrorCodes &error_code,
00246 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00247
00256 bool getPositionFK(const std::vector<std::string> &link_names,
00257 const std::vector<double> &joint_angles,
00258 std::vector<geometry_msgs::Pose> &poses) const;
00259
00260
00269 void setSearchDiscretization(const std::map<int,double>& discretization);
00270
00274 bool setRedundantJoints(const std::vector<unsigned int> &redundant_joint_indices);
00275
00276
00277 private:
00278
00279 bool initialize(const std::string &robot_description,
00280 const std::string& group_name,
00281 const std::string& base_name,
00282 const std::string& tip_name,
00283 double search_discretization);
00284
00289 int solve(KDL::Frame &pose_frame, const std::vector<double> &vfree, IkSolutionList<IkReal> &solutions) const;
00290
00294 void getSolution(const IkSolutionList<IkReal> &solutions, int i, std::vector<double>& solution) const;
00295
00296 double harmonize(const std::vector<double> &ik_seed_state, std::vector<double> &solution) const;
00297
00298 void getClosestSolution(const IkSolutionList<IkReal> &solutions, const std::vector<double> &ik_seed_state, std::vector<double> &solution) const;
00299 void fillFreeParams(int count, int *array);
00300 bool getCount(int &count, const int &max_count, const int &min_count) const;
00301
00308 bool sampleRedundantJoint(kinematics::DiscretizationMethod method, std::vector<double>& sampled_joint_vals) const;
00309
00310 };
00311
00312 bool IKFastKinematicsPlugin::initialize(const std::string &robot_description,
00313 const std::string& group_name,
00314 const std::string& base_name,
00315 const std::string& tip_name,
00316 double search_discretization)
00317 {
00318 setValues(robot_description, group_name, base_name, tip_name, search_discretization);
00319
00320 ros::NodeHandle node_handle("~/"+group_name);
00321
00322 std::string robot;
00323 node_handle.param("robot",robot,std::string());
00324
00325
00326 fillFreeParams( GetNumFreeParameters(), GetFreeParameters() );
00327 num_joints_ = GetNumJoints();
00328
00329 if(free_params_.size() > 1)
00330 {
00331 ROS_FATAL("Only one free joint parameter supported!");
00332 return false;
00333 }
00334 else if(free_params_.size() == 1)
00335 {
00336 redundant_joint_indices_.clear();
00337 redundant_joint_indices_.push_back(free_params_[0]);
00338 KinematicsBase::setSearchDiscretization(DEFAULT_SEARCH_DISCRETIZATION);
00339 }
00340
00341 urdf::Model robot_model;
00342 std::string xml_string;
00343
00344 std::string urdf_xml,full_urdf_xml;
00345 node_handle.param("urdf_xml",urdf_xml,robot_description);
00346 node_handle.searchParam(urdf_xml,full_urdf_xml);
00347
00348 ROS_DEBUG_NAMED("ikfast","Reading xml file from parameter server");
00349 if (!node_handle.getParam(full_urdf_xml, xml_string))
00350 {
00351 ROS_FATAL_NAMED("ikfast","Could not load the xml from parameter server: %s", urdf_xml.c_str());
00352 return false;
00353 }
00354
00355 node_handle.param(full_urdf_xml,xml_string,std::string());
00356 robot_model.initString(xml_string);
00357
00358 ROS_DEBUG_STREAM_NAMED("ikfast","Reading joints and links from URDF");
00359
00360 boost::shared_ptr<urdf::Link> link = boost::const_pointer_cast<urdf::Link>(robot_model.getLink(getTipFrame()));
00361 while(link->name != base_frame_ && joint_names_.size() <= num_joints_)
00362 {
00363 ROS_DEBUG_NAMED("ikfast","Link %s",link->name.c_str());
00364 link_names_.push_back(link->name);
00365 boost::shared_ptr<urdf::Joint> joint = link->parent_joint;
00366 if(joint)
00367 {
00368 if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
00369 {
00370 ROS_DEBUG_STREAM_NAMED("ikfast","Adding joint " << joint->name );
00371
00372 joint_names_.push_back(joint->name);
00373 float lower, upper;
00374 int hasLimits;
00375 if ( joint->type != urdf::Joint::CONTINUOUS )
00376 {
00377 if(joint->safety)
00378 {
00379 lower = joint->safety->soft_lower_limit;
00380 upper = joint->safety->soft_upper_limit;
00381 } else {
00382 lower = joint->limits->lower;
00383 upper = joint->limits->upper;
00384 }
00385 hasLimits = 1;
00386 }
00387 else
00388 {
00389 lower = -M_PI;
00390 upper = M_PI;
00391 hasLimits = 0;
00392 }
00393 if(hasLimits)
00394 {
00395 joint_has_limits_vector_.push_back(true);
00396 joint_min_vector_.push_back(lower);
00397 joint_max_vector_.push_back(upper);
00398 }
00399 else
00400 {
00401 joint_has_limits_vector_.push_back(false);
00402 joint_min_vector_.push_back(-M_PI);
00403 joint_max_vector_.push_back(M_PI);
00404 }
00405 }
00406 } else
00407 {
00408 ROS_WARN_NAMED("ikfast","no joint corresponding to %s",link->name.c_str());
00409 }
00410 link = link->getParent();
00411 }
00412
00413 if(joint_names_.size() != num_joints_)
00414 {
00415 ROS_FATAL_STREAM_NAMED("ikfast","Joint numbers mismatch: URDF has " << joint_names_.size() << " and IKFast has " << num_joints_);
00416 return false;
00417 }
00418
00419 std::reverse(link_names_.begin(),link_names_.end());
00420 std::reverse(joint_names_.begin(),joint_names_.end());
00421 std::reverse(joint_min_vector_.begin(),joint_min_vector_.end());
00422 std::reverse(joint_max_vector_.begin(),joint_max_vector_.end());
00423 std::reverse(joint_has_limits_vector_.begin(), joint_has_limits_vector_.end());
00424
00425 for(size_t i=0; i <num_joints_; ++i)
00426 ROS_DEBUG_STREAM_NAMED("ikfast",joint_names_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i] << " " << joint_has_limits_vector_[i]);
00427
00428 active_ = true;
00429 return true;
00430 }
00431
00432 void IKFastKinematicsPlugin::setSearchDiscretization(const std::map<int,double>& discretization)
00433 {
00434
00435 if(discretization.empty())
00436 {
00437 ROS_ERROR("The 'discretization' map is empty");
00438 return;
00439 }
00440
00441 if(redundant_joint_indices_.empty())
00442 {
00443 ROS_ERROR_STREAM("This group's solver doesn't support redundant joints");
00444 return;
00445 }
00446
00447 if(discretization.begin()->first != redundant_joint_indices_[0])
00448 {
00449 std::string redundant_joint = joint_names_[free_params_[0]];
00450 ROS_ERROR_STREAM("Attempted to discretize a non-redundant joint "<<discretization.begin()->first<<", only joint '"<<
00451 redundant_joint<<"' with index " <<redundant_joint_indices_[0]<<" is redundant.");
00452 return;
00453 }
00454
00455 if(discretization.begin()->second <= 0.0)
00456 {
00457 ROS_ERROR_STREAM("Discretization can not takes values that are <= 0");
00458 return;
00459 }
00460
00461
00462 redundant_joint_discretization_.clear();
00463 redundant_joint_discretization_[redundant_joint_indices_[0]] = discretization.begin()->second;
00464 }
00465
00466 bool IKFastKinematicsPlugin::setRedundantJoints(const std::vector<unsigned int> &redundant_joint_indices)
00467 {
00468
00469 ROS_ERROR_STREAM("Changing the redundant joints isn't permitted by this group's solver ");
00470 return false;
00471 }
00472
00473 int IKFastKinematicsPlugin::solve(KDL::Frame &pose_frame, const std::vector<double> &vfree, IkSolutionList<IkReal> &solutions) const
00474 {
00475
00476 solutions.Clear();
00477
00478 double trans[3];
00479 trans[0] = pose_frame.p[0];
00480 trans[1] = pose_frame.p[1];
00481 trans[2] = pose_frame.p[2];
00482
00483 KDL::Rotation mult;
00484 KDL::Vector direction;
00485
00486 switch (GetIkType())
00487 {
00488 case IKP_Transform6D:
00489 case IKP_Translation3D:
00490
00491
00492 mult = pose_frame.M;
00493
00494 double vals[9];
00495 vals[0] = mult(0,0);
00496 vals[1] = mult(0,1);
00497 vals[2] = mult(0,2);
00498 vals[3] = mult(1,0);
00499 vals[4] = mult(1,1);
00500 vals[5] = mult(1,2);
00501 vals[6] = mult(2,0);
00502 vals[7] = mult(2,1);
00503 vals[8] = mult(2,2);
00504
00505
00506 ComputeIk(trans, vals, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
00507 return solutions.GetNumSolutions();
00508
00509 case IKP_Direction3D:
00510 case IKP_Ray4D:
00511 case IKP_TranslationDirection5D:
00512
00513
00514 direction = pose_frame.M * KDL::Vector(0, 0, 1);
00515 ComputeIk(trans, direction.data, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
00516 return solutions.GetNumSolutions();
00517
00518 case IKP_TranslationXAxisAngle4D:
00519 case IKP_TranslationYAxisAngle4D:
00520 case IKP_TranslationZAxisAngle4D:
00521
00522 ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet.");
00523 return 0;
00524
00525 case IKP_TranslationLocalGlobal6D:
00526
00527 ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet.");
00528 return 0;
00529
00530 case IKP_Rotation3D:
00531 case IKP_Lookat3D:
00532 case IKP_TranslationXY2D:
00533 case IKP_TranslationXYOrientation3D:
00534 case IKP_TranslationXAxisAngleZNorm4D:
00535 case IKP_TranslationYAxisAngleXNorm4D:
00536 case IKP_TranslationZAxisAngleYNorm4D:
00537 ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet.");
00538 return 0;
00539
00540 default:
00541 ROS_ERROR_NAMED("ikfast", "Unknown IkParameterizationType! Was the solver generated with an incompatible version of Openrave?");
00542 return 0;
00543 }
00544 }
00545
00546 void IKFastKinematicsPlugin::getSolution(const IkSolutionList<IkReal> &solutions, int i, std::vector<double>& solution) const
00547 {
00548 solution.clear();
00549 solution.resize(num_joints_);
00550
00551
00552 const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
00553 std::vector<IkReal> vsolfree( sol.GetFree().size() );
00554 sol.GetSolution(&solution[0],vsolfree.size()>0?&vsolfree[0]:NULL);
00555
00556
00557
00558
00559
00560
00561
00562 }
00563
00564 double IKFastKinematicsPlugin::harmonize(const std::vector<double> &ik_seed_state, std::vector<double> &solution) const
00565 {
00566 double dist_sqr = 0;
00567 std::vector<double> ss = ik_seed_state;
00568 for(size_t i=0; i< ik_seed_state.size(); ++i)
00569 {
00570 while(ss[i] > 2*M_PI) {
00571 ss[i] -= 2*M_PI;
00572 }
00573 while(ss[i] < 2*M_PI) {
00574 ss[i] += 2*M_PI;
00575 }
00576 while(solution[i] > 2*M_PI) {
00577 solution[i] -= 2*M_PI;
00578 }
00579 while(solution[i] < 2*M_PI) {
00580 solution[i] += 2*M_PI;
00581 }
00582 dist_sqr += fabs(ik_seed_state[i] - solution[i]);
00583 }
00584 return dist_sqr;
00585 }
00586
00587
00588
00589
00590
00591
00592
00593
00594
00595
00596
00597
00598
00599
00600
00601
00602
00603
00604
00605
00606
00607
00608
00609
00610 void IKFastKinematicsPlugin::getClosestSolution(const IkSolutionList<IkReal> &solutions, const std::vector<double> &ik_seed_state, std::vector<double> &solution) const
00611 {
00612 double mindist = DBL_MAX;
00613 int minindex = -1;
00614 std::vector<double> sol;
00615
00616
00617 for(size_t i=0; i < solutions.GetNumSolutions(); ++i)
00618 {
00619 getSolution(solutions, i,sol);
00620 double dist = harmonize(ik_seed_state, sol);
00621 ROS_INFO_STREAM_NAMED("ikfast","Dist " << i << " dist " << dist);
00622
00623 if(minindex == -1 || dist<mindist){
00624 minindex = i;
00625 mindist = dist;
00626 }
00627 }
00628 if(minindex >= 0){
00629 getSolution(solutions, minindex,solution);
00630 harmonize(ik_seed_state, solution);
00631 }
00632 }
00633
00634 void IKFastKinematicsPlugin::fillFreeParams(int count, int *array)
00635 {
00636 free_params_.clear();
00637 for(int i=0; i<count;++i) free_params_.push_back(array[i]);
00638 }
00639
00640 bool IKFastKinematicsPlugin::getCount(int &count, const int &max_count, const int &min_count) const
00641 {
00642 if(count > 0)
00643 {
00644 if(-count >= min_count)
00645 {
00646 count = -count;
00647 return true;
00648 }
00649 else if(count+1 <= max_count)
00650 {
00651 count = count+1;
00652 return true;
00653 }
00654 else
00655 {
00656 return false;
00657 }
00658 }
00659 else
00660 {
00661 if(1-count <= max_count)
00662 {
00663 count = 1-count;
00664 return true;
00665 }
00666 else if(count-1 >= min_count)
00667 {
00668 count = count -1;
00669 return true;
00670 }
00671 else
00672 return false;
00673 }
00674 }
00675
00676 bool IKFastKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
00677 const std::vector<double> &joint_angles,
00678 std::vector<geometry_msgs::Pose> &poses) const
00679 {
00680 if (GetIkType() != IKP_Transform6D) {
00681
00682
00683
00684
00685 ROS_ERROR_NAMED("ikfast", "Can only compute FK for Transform6D IK type!");
00686 return false;
00687 }
00688
00689 KDL::Frame p_out;
00690 if(link_names.size() == 0) {
00691 ROS_WARN_STREAM_NAMED("ikfast","Link names with nothing");
00692 return false;
00693 }
00694
00695 if(link_names.size()!=1 || link_names[0]!=getTipFrame()){
00696 ROS_ERROR_NAMED("ikfast","Can compute FK for %s only",getTipFrame().c_str());
00697 return false;
00698 }
00699
00700 bool valid = true;
00701
00702 IkReal eerot[9],eetrans[3];
00703 IkReal angles[joint_angles.size()];
00704 for (unsigned char i=0; i < joint_angles.size(); i++)
00705 angles[i] = joint_angles[i];
00706
00707
00708 ComputeFk(angles,eetrans,eerot);
00709
00710 for(int i=0; i<3;++i)
00711 p_out.p.data[i] = eetrans[i];
00712
00713 for(int i=0; i<9;++i)
00714 p_out.M.data[i] = eerot[i];
00715
00716 poses.resize(1);
00717 tf::poseKDLToMsg(p_out,poses[0]);
00718
00719 return valid;
00720 }
00721
00722 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00723 const std::vector<double> &ik_seed_state,
00724 double timeout,
00725 std::vector<double> &solution,
00726 moveit_msgs::MoveItErrorCodes &error_code,
00727 const kinematics::KinematicsQueryOptions &options) const
00728 {
00729 const IKCallbackFn solution_callback = 0;
00730 std::vector<double> consistency_limits;
00731
00732 return searchPositionIK(ik_pose,
00733 ik_seed_state,
00734 timeout,
00735 consistency_limits,
00736 solution,
00737 solution_callback,
00738 error_code,
00739 options);
00740 }
00741
00742 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00743 const std::vector<double> &ik_seed_state,
00744 double timeout,
00745 const std::vector<double> &consistency_limits,
00746 std::vector<double> &solution,
00747 moveit_msgs::MoveItErrorCodes &error_code,
00748 const kinematics::KinematicsQueryOptions &options) const
00749 {
00750 const IKCallbackFn solution_callback = 0;
00751 return searchPositionIK(ik_pose,
00752 ik_seed_state,
00753 timeout,
00754 consistency_limits,
00755 solution,
00756 solution_callback,
00757 error_code,
00758 options);
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 IKCallbackFn &solution_callback,
00766 moveit_msgs::MoveItErrorCodes &error_code,
00767 const kinematics::KinematicsQueryOptions &options) const
00768 {
00769 std::vector<double> consistency_limits;
00770 return searchPositionIK(ik_pose,
00771 ik_seed_state,
00772 timeout,
00773 consistency_limits,
00774 solution,
00775 solution_callback,
00776 error_code,
00777 options);
00778 }
00779
00780 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00781 const std::vector<double> &ik_seed_state,
00782 double timeout,
00783 const std::vector<double> &consistency_limits,
00784 std::vector<double> &solution,
00785 const IKCallbackFn &solution_callback,
00786 moveit_msgs::MoveItErrorCodes &error_code,
00787 const kinematics::KinematicsQueryOptions &options) const
00788 {
00789 ROS_DEBUG_STREAM_NAMED("ikfast","searchPositionIK");
00790
00792 SEARCH_MODE search_mode = OPTIMIZE_MAX_JOINT;
00793
00794
00795 if(free_params_.size()==0)
00796 {
00797 ROS_DEBUG_STREAM_NAMED("ikfast","No need to search since no free params/redundant joints");
00798
00799
00800 if(!getPositionIK(ik_pose, ik_seed_state, solution, error_code))
00801 {
00802 ROS_DEBUG_STREAM_NAMED("ikfast","No solution whatsoever");
00803 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00804 return false;
00805 }
00806
00807
00808 if( !solution_callback.empty() )
00809 {
00810 solution_callback(ik_pose, solution, error_code);
00811 if(error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
00812 {
00813 ROS_DEBUG_STREAM_NAMED("ikfast","Solution passes callback");
00814 return true;
00815 }
00816 else
00817 {
00818 ROS_DEBUG_STREAM_NAMED("ikfast","Solution has error code " << error_code);
00819 return false;
00820 }
00821 }
00822 else
00823 {
00824 return true;
00825 }
00826 }
00827
00828
00829
00830 if(!active_)
00831 {
00832 ROS_ERROR_STREAM_NAMED("ikfast","Kinematics not active");
00833 error_code.val = error_code.NO_IK_SOLUTION;
00834 return false;
00835 }
00836
00837 if(ik_seed_state.size() != num_joints_)
00838 {
00839 ROS_ERROR_STREAM_NAMED("ikfast","Seed state must have size " << num_joints_ << " instead of size " << ik_seed_state.size());
00840 error_code.val = error_code.NO_IK_SOLUTION;
00841 return false;
00842 }
00843
00844 if(!consistency_limits.empty() && consistency_limits.size() != num_joints_)
00845 {
00846 ROS_ERROR_STREAM_NAMED("ikfast","Consistency limits be empty or must have size " << num_joints_ << " instead of size " << consistency_limits.size());
00847 error_code.val = error_code.NO_IK_SOLUTION;
00848 return false;
00849 }
00850
00851
00852
00853
00854
00855 KDL::Frame frame;
00856 tf::poseMsgToKDL(ik_pose,frame);
00857
00858 std::vector<double> vfree(free_params_.size());
00859
00860 ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
00861 int counter = 0;
00862
00863 double initial_guess = ik_seed_state[free_params_[0]];
00864 vfree[0] = initial_guess;
00865
00866
00867
00868 int num_positive_increments;
00869 int num_negative_increments;
00870
00871 if(!consistency_limits.empty())
00872 {
00873
00874
00875 double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess+consistency_limits[free_params_[0]]);
00876 double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess-consistency_limits[free_params_[0]]);
00877
00878 num_positive_increments = (int)((max_limit-initial_guess)/search_discretization_);
00879 num_negative_increments = (int)((initial_guess-min_limit)/search_discretization_);
00880 }
00881 else
00882 {
00883 num_positive_increments = (joint_max_vector_[free_params_[0]]-initial_guess)/search_discretization_;
00884 num_negative_increments = (initial_guess-joint_min_vector_[free_params_[0]])/search_discretization_;
00885 }
00886
00887
00888
00889
00890 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);
00891 if ((search_mode & OPTIMIZE_MAX_JOINT) && (num_positive_increments + num_negative_increments) > 1000)
00892 ROS_WARN_STREAM_ONCE_NAMED("ikfast", "Large search space, consider increasing the search discretization");
00893
00894 double best_costs = -1.0;
00895 std::vector<double> best_solution;
00896 int nattempts = 0, nvalid = 0;
00897
00898 while(true)
00899 {
00900 IkSolutionList<IkReal> solutions;
00901 int numsol = solve(frame,vfree, solutions);
00902
00903 ROS_DEBUG_STREAM_NAMED("ikfast","Found " << numsol << " solutions from IKFast");
00904
00905
00906
00907 if( numsol > 0 )
00908 {
00909 for(int s = 0; s < numsol; ++s)
00910 {
00911 nattempts++;
00912 std::vector<double> sol;
00913 getSolution(solutions,s,sol);
00914
00915 bool obeys_limits = true;
00916 for(unsigned int i = 0; i < sol.size(); i++)
00917 {
00918 if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i]))
00919 {
00920 obeys_limits = false;
00921 break;
00922 }
00923
00924 }
00925 if(obeys_limits)
00926 {
00927 getSolution(solutions,s,solution);
00928
00929
00930 if(!solution_callback.empty())
00931 {
00932 solution_callback(ik_pose, solution, error_code);
00933 }
00934 else
00935 {
00936 error_code.val = error_code.SUCCESS;
00937 }
00938
00939 if(error_code.val == error_code.SUCCESS)
00940 {
00941 nvalid++;
00942 if (search_mode & OPTIMIZE_MAX_JOINT)
00943 {
00944
00945 double costs = 0.0;
00946 for(unsigned int i = 0; i < solution.size(); i++)
00947 {
00948 double d = fabs(ik_seed_state[i] - solution[i]);
00949 if (d > costs)
00950 costs = d;
00951 }
00952 if (costs < best_costs || best_costs == -1.0)
00953 {
00954 best_costs = costs;
00955 best_solution = solution;
00956 }
00957 }
00958 else
00959
00960 return true;
00961 }
00962 }
00963 }
00964 }
00965
00966 if(!getCount(counter, num_positive_increments, -num_negative_increments))
00967 {
00968
00969 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00970 break;
00971 }
00972
00973 vfree[0] = initial_guess+search_discretization_*counter;
00974
00975 }
00976
00977 ROS_DEBUG_STREAM_NAMED("ikfast", "Valid solutions: " << nvalid << "/" << nattempts);
00978
00979 if ((search_mode & OPTIMIZE_MAX_JOINT) && best_costs != -1.0)
00980 {
00981 solution = best_solution;
00982 error_code.val = error_code.SUCCESS;
00983 return true;
00984 }
00985
00986
00987 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00988 return false;
00989 }
00990
00991
00992 bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose,
00993 const std::vector<double> &ik_seed_state,
00994 std::vector<double> &solution,
00995 moveit_msgs::MoveItErrorCodes &error_code,
00996 const kinematics::KinematicsQueryOptions &options) const
00997 {
00998 ROS_DEBUG_STREAM_NAMED("ikfast","getPositionIK");
00999
01000 if(!active_)
01001 {
01002 ROS_ERROR("kinematics not active");
01003 return false;
01004 }
01005
01006 std::vector<double> vfree(free_params_.size());
01007 for(std::size_t i = 0; i < free_params_.size(); ++i)
01008 {
01009 int p = free_params_[i];
01010 ROS_ERROR("%u is %f",p,ik_seed_state[p]);
01011 vfree[i] = ik_seed_state[p];
01012 }
01013
01014 KDL::Frame frame;
01015 tf::poseMsgToKDL(ik_pose,frame);
01016
01017 IkSolutionList<IkReal> solutions;
01018 int numsol = solve(frame,vfree,solutions);
01019
01020 ROS_DEBUG_STREAM_NAMED("ikfast","Found " << numsol << " solutions from IKFast");
01021
01022 if(numsol)
01023 {
01024 for(int s = 0; s < numsol; ++s)
01025 {
01026 std::vector<double> sol;
01027 getSolution(solutions,s,sol);
01028 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]);
01029
01030 bool obeys_limits = true;
01031 for(unsigned int i = 0; i < sol.size(); i++)
01032 {
01033
01034 if(joint_has_limits_vector_[i] && ( (sol[i] < (joint_min_vector_[i]-LIMIT_TOLERANCE)) ||
01035 (sol[i] > (joint_max_vector_[i]+LIMIT_TOLERANCE)) ) )
01036 {
01037
01038 obeys_limits = false;
01039 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]);
01040 break;
01041 }
01042 }
01043 if(obeys_limits)
01044 {
01045
01046 getSolution(solutions,s,solution);
01047 error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
01048 return true;
01049 }
01050 }
01051 }
01052 else
01053 {
01054 ROS_DEBUG_STREAM_NAMED("ikfast","No IK solution");
01055 }
01056
01057 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
01058 return false;
01059 }
01060
01061 bool IKFastKinematicsPlugin::getPositionIK(const std::vector<geometry_msgs::Pose> &ik_poses,
01062 const std::vector<double> &ik_seed_state,
01063 std::vector< std::vector<double> >& solutions,
01064 kinematics::KinematicsResult& result,
01065 const kinematics::KinematicsQueryOptions &options) const
01066 {
01067 ROS_DEBUG_STREAM_NAMED("ikfast","getPositionIK with multiple solutions");
01068
01069 if(!active_)
01070 {
01071 ROS_ERROR("kinematics not active");
01072 result.kinematic_error = kinematics::KinematicErrors::SOLVER_NOT_ACTIVE;
01073 return false;
01074 }
01075
01076 if(ik_poses.empty())
01077 {
01078 ROS_ERROR("ik_poses is empty");
01079 result.kinematic_error = kinematics::KinematicErrors::EMPTY_TIP_POSES;
01080 return false;
01081 }
01082
01083 if(ik_poses.size() > 1)
01084 {
01085 ROS_ERROR("ik_poses contains multiple entries, only one is allowed");
01086 result.kinematic_error = kinematics::KinematicErrors::MULTIPLE_TIPS_NOT_SUPPORTED;
01087 return false;
01088 }
01089
01090 if(ik_seed_state.size() < num_joints_)
01091 {
01092 ROS_ERROR_STREAM("ik_seed_state only has "<<ik_seed_state.size()<<" entries, this ikfast solver requires "<<num_joints_);
01093 return false;
01094 }
01095
01096 KDL::Frame frame;
01097 tf::poseMsgToKDL(ik_poses[0],frame);
01098
01099
01100 std::vector< IkSolutionList<IkReal> > solution_set;
01101 IkSolutionList<IkReal> ik_solutions;
01102 std::vector<double> vfree;
01103 int numsol = 0;
01104 std::vector<double> sampled_joint_vals;
01105 if(!redundant_joint_indices_.empty())
01106 {
01107
01108 sampled_joint_vals.push_back(ik_seed_state[redundant_joint_indices_[0]]);
01109
01110
01111 if(options.discretization_method == kinematics::DiscretizationMethods::NO_DISCRETIZATION &&
01112 joint_has_limits_vector_[redundant_joint_indices_.front()])
01113 {
01114 double joint_min = joint_min_vector_[redundant_joint_indices_.front()];
01115 double joint_max = joint_max_vector_[redundant_joint_indices_.front()];
01116
01117 double jv = sampled_joint_vals[0];
01118 if(!( (jv > (joint_min - LIMIT_TOLERANCE)) && (jv < (joint_max + LIMIT_TOLERANCE)) ))
01119 {
01120 result.kinematic_error = kinematics::KinematicErrors::IK_SEED_OUTSIDE_LIMITS;
01121 ROS_ERROR_STREAM("ik seed is out of bounds");
01122 return false;
01123 }
01124
01125 }
01126
01127
01128
01129 if(!sampleRedundantJoint(options.discretization_method,sampled_joint_vals))
01130 {
01131 result.kinematic_error = kinematics::KinematicErrors::UNSUPORTED_DISCRETIZATION_REQUESTED;
01132 return false;
01133 }
01134
01135 for(unsigned int i = 0; i < sampled_joint_vals.size(); i++)
01136 {
01137 vfree.clear();
01138 vfree.push_back(sampled_joint_vals[i]);
01139 numsol += solve(frame,vfree,ik_solutions);
01140 solution_set.push_back(ik_solutions);
01141 }
01142 }
01143 else
01144 {
01145
01146 numsol = solve(frame,vfree,ik_solutions);
01147 solution_set.push_back(ik_solutions);
01148 }
01149
01150 ROS_DEBUG_STREAM_NAMED("ikfast","Found " << numsol << " solutions from IKFast");
01151 bool solutions_found = false;
01152 if( numsol > 0 )
01153 {
01154
01155
01156
01157 for(unsigned int r = 0; r < solution_set.size() ; r++)
01158 {
01159
01160 ik_solutions = solution_set[r];
01161 numsol = ik_solutions.GetNumSolutions();
01162 for(int s = 0; s < numsol; ++s)
01163 {
01164 std::vector<double> sol;
01165 getSolution(ik_solutions,s,sol);
01166
01167 bool obeys_limits = true;
01168 for(unsigned int i = 0; i < sol.size(); i++)
01169 {
01170
01171 if(joint_has_limits_vector_[i] && ( (sol[i] < (joint_min_vector_[i]-LIMIT_TOLERANCE)) ||
01172 (sol[i] > (joint_max_vector_[i]+LIMIT_TOLERANCE)) ) )
01173 {
01174
01175 obeys_limits = false;
01176 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]);
01177 break;
01178 }
01179 }
01180 if(obeys_limits)
01181 {
01182
01183 solutions_found = true;
01184 solutions.push_back(sol);
01185 }
01186 }
01187 }
01188
01189 if(solutions_found)
01190 {
01191 result.kinematic_error = kinematics::KinematicErrors::OK;
01192 return true;
01193 }
01194 }
01195 else
01196 {
01197 ROS_DEBUG_STREAM_NAMED("ikfast","No IK solution");
01198 }
01199
01200 result.kinematic_error = kinematics::KinematicErrors::NO_SOLUTION;
01201 return false;
01202 }
01203
01204 bool IKFastKinematicsPlugin::sampleRedundantJoint(kinematics::DiscretizationMethod method, std::vector<double>& sampled_joint_vals) const
01205 {
01206 double joint_min = -M_PI;
01207 double joint_max = M_PI;
01208 int index = redundant_joint_indices_.front();
01209 double joint_dscrt = redundant_joint_discretization_.at(index);
01210
01211 if(joint_has_limits_vector_[redundant_joint_indices_.front()])
01212 {
01213 joint_min = joint_min_vector_[index];
01214 joint_max = joint_max_vector_[index];
01215 }
01216
01217
01218 switch(method)
01219 {
01220 case kinematics::DiscretizationMethods::ALL_DISCRETIZED:
01221 {
01222 int steps = std::ceil((joint_max - joint_min)/joint_dscrt);
01223 for(unsigned int i = 0; i < steps;i++)
01224 {
01225 sampled_joint_vals.push_back(joint_min + joint_dscrt*i);
01226 }
01227 sampled_joint_vals.push_back(joint_max);
01228 }
01229 break;
01230 case kinematics::DiscretizationMethods::ALL_RANDOM_SAMPLED:
01231 {
01232 int steps = std::ceil((joint_max - joint_min)/joint_dscrt);
01233 steps = steps > 0 ? steps : 1;
01234 double diff = joint_max - joint_min;
01235 for(int i = 0; i < steps; i++)
01236 {
01237 sampled_joint_vals.push_back( ((diff*std::rand())/(static_cast<double>(RAND_MAX))) + joint_min );
01238 }
01239 }
01240
01241 break;
01242 case kinematics::DiscretizationMethods::NO_DISCRETIZATION:
01243
01244 break;
01245 default:
01246 ROS_ERROR_STREAM("Discretization method "<<method<<" is not supported");
01247 return false;
01248 }
01249
01250 return true;
01251 }
01252
01253
01254 }
01255
01256
01257 #include <pluginlib/class_list_macros.h>
01258 PLUGINLIB_EXPORT_CLASS(ikfast_kinematics_plugin::IKFastKinematicsPlugin, kinematics::KinematicsBase);