00001 #include <ros/ros.h>
00002 #include <kinematics_base/kinematics_base.h>
00003 #include <urdf/model.h>
00004 #include <arm_kinematics_constraint_aware/ik_fast_solver.h>
00005
00006 namespace SIA20D_Mesh_manipulator_kinematics
00007 {
00008
00009
00010 #include "SIA20D_Mesh_manipulator_ikfast_output.cpp"
00011
00012 class IKFastKinematicsPlugin : public kinematics::KinematicsBase
00013 {
00014 std::vector<std::string> joint_names_;
00015 std::vector<double> joint_min_vector_;
00016 std::vector<double> joint_max_vector_;
00017 std::vector<bool> joint_has_limits_vector_;
00018 std::vector<std::string> link_names_;
00019 arm_kinematics_constraint_aware::ik_solver_base* ik_solver_;
00020 size_t num_joints_;
00021 std::vector<int> free_params_;
00022 void (*fk)(const IKReal* j, IKReal* eetrans, IKReal* eerot);
00023
00024 public:
00025
00026 IKFastKinematicsPlugin():ik_solver_(0) {}
00027 ~IKFastKinematicsPlugin(){ if(ik_solver_) delete ik_solver_;}
00028
00029 void fillFreeParams(int count, int *array) { free_params_.clear(); for(int i=0; i<count;++i) free_params_.push_back(array[i]); }
00030
00031 bool initialize(const std::string& group_name,
00032 const std::string& base_name,
00033 const std::string& tip_name,
00034 const double& search_discretization) {
00035 setValues(group_name, base_name, tip_name,search_discretization);
00036
00037 ros::NodeHandle node_handle("~/"+group_name);
00038
00039 std::string robot;
00040 node_handle.param("robot",robot,std::string());
00041
00042 fillFreeParams(getNumFreeParameters(),getFreeParameters());
00043 num_joints_ = getNumJoints();
00044 ik_solver_ = new arm_kinematics_constraint_aware::ikfast_solver<IKSolution>(ik, num_joints_);
00045 fk=fk;
00046
00047 if(free_params_.size()>1){
00048 ROS_FATAL("Only one free joint paramter supported!");
00049 return false;
00050 }
00051
00052 urdf::Model robot_model;
00053 std::string xml_string;
00054
00055 std::string urdf_xml,full_urdf_xml;
00056 node_handle.param("urdf_xml",urdf_xml,std::string("robot_description"));
00057 node_handle.searchParam(urdf_xml,full_urdf_xml);
00058
00059 ROS_DEBUG("Reading xml file from parameter server\n");
00060 if (!node_handle.getParam(full_urdf_xml, xml_string))
00061 {
00062 ROS_FATAL("Could not load the xml from parameter server: %s\n", urdf_xml.c_str());
00063 return false;
00064 }
00065
00066 node_handle.param(full_urdf_xml,xml_string,std::string());
00067 robot_model.initString(xml_string);
00068
00069 boost::shared_ptr<urdf::Link> link = boost::const_pointer_cast<urdf::Link>(robot_model.getLink(tip_name_));
00070 while(link->name != base_name_ && joint_names_.size() <= num_joints_){
00071
00072 link_names_.push_back(link->name);
00073 boost::shared_ptr<urdf::Joint> joint = link->parent_joint;
00074 if(joint){
00075 if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
00076 joint_names_.push_back(joint->name);
00077 float lower, upper;
00078 int hasLimits;
00079 if ( joint->type != urdf::Joint::CONTINUOUS ) {
00080 if(joint->safety) {
00081 lower = joint->safety->soft_lower_limit;
00082 upper = joint->safety->soft_upper_limit;
00083 } else {
00084 lower = joint->limits->lower;
00085 upper = joint->limits->upper;
00086 }
00087 hasLimits = 1;
00088 } else {
00089 lower = -M_PI;
00090 upper = M_PI;
00091 hasLimits = 0;
00092 }
00093 if(hasLimits) {
00094 joint_has_limits_vector_.push_back(true);
00095 joint_min_vector_.push_back(lower);
00096 joint_max_vector_.push_back(upper);
00097 } else {
00098 joint_has_limits_vector_.push_back(false);
00099 joint_min_vector_.push_back(-M_PI);
00100 joint_max_vector_.push_back(M_PI);
00101 }
00102 }
00103 } else{
00104 ROS_WARN("no joint corresponding to %s",link->name.c_str());
00105 }
00106 link = link->getParent();
00107 }
00108
00109 if(joint_names_.size() != num_joints_){
00110 ROS_FATAL("Joints number mismatch.");
00111 return false;
00112 }
00113
00114 std::reverse(link_names_.begin(),link_names_.end());
00115 std::reverse(joint_names_.begin(),joint_names_.end());
00116 std::reverse(joint_min_vector_.begin(),joint_min_vector_.end());
00117 std::reverse(joint_max_vector_.begin(),joint_max_vector_.end());
00118 std::reverse(joint_has_limits_vector_.begin(), joint_has_limits_vector_.end());
00119
00120 for(size_t i=0; i <num_joints_; ++i)
00121 ROS_INFO_STREAM(joint_names_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i] << " " << joint_has_limits_vector_[i]);
00122
00123 return true;
00124 }
00125
00126 bool getCount(int &count,
00127 const int &max_count,
00128 const int &min_count)
00129 {
00130 if(count > 0)
00131 {
00132 if(-count >= min_count)
00133 {
00134 count = -count;
00135 return true;
00136 }
00137 else if(count+1 <= max_count)
00138 {
00139 count = count+1;
00140 return true;
00141 }
00142 else
00143 {
00144 return false;
00145 }
00146 }
00147 else
00148 {
00149 if(1-count <= max_count)
00150 {
00151 count = 1-count;
00152 return true;
00153 }
00154 else if(count-1 >= min_count)
00155 {
00156 count = count -1;
00157 return true;
00158 }
00159 else
00160 return false;
00161 }
00162 }
00163
00164 bool getPositionIK(const geometry_msgs::Pose &ik_pose,
00165 const std::vector<double> &ik_seed_state,
00166 std::vector<double> &solution,
00167 int &error_code) {
00168
00169 std::vector<double> vfree(free_params_.size());
00170 for(std::size_t i = 0; i < free_params_.size(); ++i){
00171 int p = free_params_[i];
00172
00173 vfree[i] = ik_seed_state[p];
00174 }
00175
00176 KDL::Frame frame;
00177 tf::PoseMsgToKDL(ik_pose,frame);
00178
00179 int numsol = ik_solver_->solve(frame,vfree);
00180
00181
00182 if(numsol){
00183 for(int s = 0; s < numsol; ++s){
00184 std::vector<double> sol;
00185 ik_solver_->getSolution(s,sol);
00186 bool obeys_limits = true;
00187 for(unsigned int i = 0; i < sol.size(); i++) {
00188 if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i])) {
00189 obeys_limits = false;
00190 break;
00191 }
00192
00193 }
00194 if(obeys_limits) {
00195 ik_solver_->getSolution(s,solution);
00196 error_code = kinematics::SUCCESS;
00197 return true;
00198 }
00199 }
00200 }
00201
00202 error_code = kinematics::NO_IK_SOLUTION;
00203 return false;
00204 }
00205 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00206 const std::vector<double> &ik_seed_state,
00207 const double &timeout,
00208 std::vector<double> &solution,
00209 int &error_code) {
00210
00211 if(free_params_.size()==0){
00212 return getPositionIK(ik_pose, ik_seed_state,solution, error_code);
00213 }
00214
00215 KDL::Frame frame;
00216 tf::PoseMsgToKDL(ik_pose,frame);
00217
00218 std::vector<double> vfree(free_params_.size());
00219
00220 ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
00221 int counter = 0;
00222
00223 double initial_guess = ik_seed_state[free_params_[0]];
00224 vfree[0] = initial_guess;
00225
00226 int num_positive_increments = (joint_max_vector_[free_params_[0]]-initial_guess)/search_discretization_;
00227 int num_negative_increments = (initial_guess-joint_min_vector_[free_params_[0]])/search_discretization_;
00228
00229 ROS_INFO_STREAM("Free param is " << free_params_[0] << " initial guess is " << initial_guess << " " << num_positive_increments << " " << num_negative_increments);
00230
00231 while(1) {
00232 int numsol = ik_solver_->solve(frame,vfree);
00233
00234
00235
00236
00237
00238 if(numsol > 0){
00239 for(int s = 0; s < numsol; ++s){
00240 std::vector<double> sol;
00241 ik_solver_->getSolution(s,sol);
00242 bool obeys_limits = true;
00243 for(unsigned int i = 0; i < sol.size(); i++) {
00244 if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i])) {
00245 obeys_limits = false;
00246 break;
00247 }
00248
00249 }
00250 if(obeys_limits) {
00251 ik_solver_->getSolution(s,solution);
00252 error_code = kinematics::SUCCESS;
00253 return true;
00254 }
00255 }
00256 }
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266
00267
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279 if(!getCount(counter, num_positive_increments, num_negative_increments)) {
00280 error_code = kinematics::NO_IK_SOLUTION;
00281 return false;
00282 }
00283
00284 vfree[0] = initial_guess+search_discretization_*counter;
00285 ROS_DEBUG_STREAM(counter << " " << vfree[0]);
00286 }
00287
00288 error_code = kinematics::NO_IK_SOLUTION;
00289 return false;
00290 }
00291
00292 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00293 const std::vector<double> &ik_seed_state,
00294 const double &timeout,
00295 const unsigned int& redundancy,
00296 const double &consistency_limit,
00297 std::vector<double> &solution,
00298 int &error_code) {
00299
00300 if(free_params_.size()==0){
00301
00302 return getPositionIK(ik_pose, ik_seed_state,solution, error_code);
00303 ROS_WARN_STREAM("No free parameters, so can't search");
00304 }
00305
00306 if(redundancy != (unsigned int)free_params_[0]) {
00307 ROS_WARN_STREAM("Calling consistency search with wrong free param");
00308 return false;
00309 }
00310
00311 KDL::Frame frame;
00312 tf::PoseMsgToKDL(ik_pose,frame);
00313
00314 std::vector<double> vfree(free_params_.size());
00315
00316 ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
00317 int counter = 0;
00318
00319 double initial_guess = ik_seed_state[free_params_[0]];
00320 vfree[0] = initial_guess;
00321
00322 double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess+consistency_limit);
00323 double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess-consistency_limit);
00324
00325 int num_positive_increments = (int)((max_limit-initial_guess)/search_discretization_);
00326 int num_negative_increments = (int)((initial_guess-min_limit)/search_discretization_);
00327
00328 ROS_DEBUG_STREAM("Free param is " << free_params_[0] << " initial guess is " << initial_guess << " " << num_positive_increments << " " << num_negative_increments);
00329
00330 while(1) {
00331 int numsol = ik_solver_->solve(frame,vfree);
00332
00333
00334
00335
00336
00337 if(numsol > 0){
00338 for(int s = 0; s < numsol; ++s){
00339 std::vector<double> sol;
00340 ik_solver_->getSolution(s,sol);
00341 bool obeys_limits = true;
00342 for(unsigned int i = 0; i < sol.size(); i++) {
00343 if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i])) {
00344 obeys_limits = false;
00345 break;
00346 }
00347
00348 }
00349 if(obeys_limits) {
00350 ik_solver_->getSolution(s,solution);
00351 error_code = kinematics::SUCCESS;
00352 return true;
00353 }
00354 }
00355 }
00356
00357
00358
00359
00360
00361
00362
00363
00364
00365
00366
00367
00368
00369
00370
00371
00372
00373
00374
00375
00376
00377
00378 if(!getCount(counter, num_positive_increments, num_negative_increments)) {
00379 error_code = kinematics::NO_IK_SOLUTION;
00380 return false;
00381 }
00382
00383 vfree[0] = initial_guess+search_discretization_*counter;
00384 ROS_DEBUG_STREAM(counter << " " << vfree[0]);
00385 }
00386
00387 error_code = kinematics::NO_IK_SOLUTION;
00388 return false;
00389 }
00390
00391 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00392 const std::vector<double> &ik_seed_state,
00393 const double &timeout,
00394 std::vector<double> &solution,
00395 const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> &desired_pose_callback,
00396 const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> &solution_callback,
00397 int &error_code){
00398
00399 if(free_params_.size()==0){
00400 if(!getPositionIK(ik_pose, ik_seed_state,solution, error_code)) {
00401 ROS_DEBUG_STREAM("No solution whatsoever");
00402 error_code = kinematics::NO_IK_SOLUTION;
00403 return false;
00404 }
00405 solution_callback(ik_pose,solution,error_code);
00406 if(error_code == kinematics::SUCCESS) {
00407 ROS_DEBUG_STREAM("Solution passes");
00408 return true;
00409 } else {
00410 ROS_DEBUG_STREAM("Solution has error code " << error_code);
00411 return false;
00412 }
00413 }
00414
00415 if(!desired_pose_callback.empty())
00416 desired_pose_callback(ik_pose,ik_seed_state,error_code);
00417 if(error_code < 0)
00418 {
00419 ROS_ERROR("Could not find inverse kinematics for desired end-effector pose since the pose may be in collision");
00420 return false;
00421 }
00422
00423 KDL::Frame frame;
00424 tf::PoseMsgToKDL(ik_pose,frame);
00425
00426 std::vector<double> vfree(free_params_.size());
00427
00428 ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
00429 int counter = 0;
00430
00431 double initial_guess = ik_seed_state[free_params_[0]];
00432 vfree[0] = initial_guess;
00433
00434 int num_positive_increments = (joint_max_vector_[free_params_[0]]-initial_guess)/search_discretization_;
00435 int num_negative_increments = (initial_guess - joint_min_vector_[free_params_[0]])/search_discretization_;
00436
00437 ROS_DEBUG_STREAM("Free param is " << free_params_[0] << " initial guess is " << initial_guess << " " << num_positive_increments << " " << num_negative_increments);
00438
00439 unsigned int solvecount = 0;
00440 unsigned int countsol = 0;
00441
00442 ros::WallTime start = ros::WallTime::now();
00443
00444 std::vector<double> sol;
00445 while(1) {
00446 int numsol = ik_solver_->solve(frame,vfree);
00447 if(solvecount == 0) {
00448 if(numsol == 0) {
00449 ROS_DEBUG_STREAM("Bad solve time is " << ros::WallTime::now()-start);
00450 } else {
00451 ROS_DEBUG_STREAM("Good solve time is " << ros::WallTime::now()-start);
00452 }
00453 }
00454 solvecount++;
00455 if(numsol > 0){
00456 if(solution_callback.empty()){
00457 ik_solver_->getClosestSolution(ik_seed_state,solution);
00458 error_code = kinematics::SUCCESS;
00459 return true;
00460 }
00461
00462 for(int s = 0; s < numsol; ++s){
00463 ik_solver_->getSolution(s,sol);
00464 countsol++;
00465 bool obeys_limits = true;
00466 for(unsigned int i = 0; i < sol.size(); i++) {
00467 if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i])) {
00468 obeys_limits = false;
00469 break;
00470 }
00471 }
00472 if(obeys_limits) {
00473 solution_callback(ik_pose,sol,error_code);
00474 if(error_code == kinematics::SUCCESS){
00475 solution = sol;
00476 ROS_DEBUG_STREAM("Took " << (ros::WallTime::now() - start) << " to return true " << countsol << " " << solvecount);
00477 return true;
00478 }
00479 }
00480 }
00481 }
00482 if(!getCount(counter, num_positive_increments, num_negative_increments)) {
00483 error_code = kinematics::NO_IK_SOLUTION;
00484 ROS_DEBUG_STREAM("Took " << (ros::WallTime::now() - start) << " to return false " << countsol << " " << solvecount);
00485 return false;
00486 }
00487 vfree[0] = initial_guess+search_discretization_*counter;
00488 ROS_DEBUG_STREAM(counter << " " << vfree[0]);
00489 }
00490 error_code = kinematics::NO_IK_SOLUTION;
00491 return false;
00492 }
00493
00494 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00495 const std::vector<double> &ik_seed_state,
00496 const double &timeout,
00497 const unsigned int& redundancy,
00498 const double &consistency_limit,
00499 std::vector<double> &solution,
00500 const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> &desired_pose_callback,
00501 const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> &solution_callback,
00502 int &error_code){
00503
00504 if(free_params_.size()==0){
00505 if(!getPositionIK(ik_pose, ik_seed_state,solution, error_code)) {
00506 ROS_DEBUG_STREAM("No solution whatsoever");
00507 error_code = kinematics::NO_IK_SOLUTION;
00508 return false;
00509 }
00510 solution_callback(ik_pose,solution,error_code);
00511 if(error_code == kinematics::SUCCESS) {
00512 ROS_DEBUG_STREAM("Solution passes");
00513 return true;
00514 } else {
00515 ROS_DEBUG_STREAM("Solution has error code " << error_code);
00516 return false;
00517 }
00518 }
00519
00520 if(redundancy != (unsigned int) free_params_[0]) {
00521 ROS_WARN_STREAM("Calling consistency search with wrong free param");
00522 return false;
00523 }
00524
00525 if(!desired_pose_callback.empty())
00526 desired_pose_callback(ik_pose,ik_seed_state,error_code);
00527 if(error_code < 0)
00528 {
00529 ROS_ERROR("Could not find inverse kinematics for desired end-effector pose since the pose may be in collision");
00530 return false;
00531 }
00532
00533 KDL::Frame frame;
00534 tf::PoseMsgToKDL(ik_pose,frame);
00535
00536 std::vector<double> vfree(free_params_.size());
00537
00538 ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
00539 int counter = 0;
00540
00541 double initial_guess = ik_seed_state[free_params_[0]];
00542 vfree[0] = initial_guess;
00543
00544 double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess+consistency_limit);
00545 double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess-consistency_limit);
00546
00547 int num_positive_increments = (int)((max_limit-initial_guess)/search_discretization_);
00548 int num_negative_increments = (int)((initial_guess-min_limit)/search_discretization_);
00549
00550 ROS_DEBUG_STREAM("Free param is " << free_params_[0] << " initial guess is " << initial_guess << " " << num_positive_increments << " " << num_negative_increments);
00551
00552 unsigned int solvecount = 0;
00553 unsigned int countsol = 0;
00554
00555 ros::WallTime start = ros::WallTime::now();
00556
00557 std::vector<double> sol;
00558 while(1) {
00559 int numsol = ik_solver_->solve(frame,vfree);
00560 if(solvecount == 0) {
00561 if(numsol == 0) {
00562 ROS_DEBUG_STREAM("Bad solve time is " << ros::WallTime::now()-start);
00563 } else {
00564 ROS_DEBUG_STREAM("Good solve time is " << ros::WallTime::now()-start);
00565 }
00566 }
00567 solvecount++;
00568 if(numsol > 0){
00569 if(solution_callback.empty()){
00570 ik_solver_->getClosestSolution(ik_seed_state,solution);
00571 error_code = kinematics::SUCCESS;
00572 return true;
00573 }
00574
00575 for(int s = 0; s < numsol; ++s){
00576 ik_solver_->getSolution(s,sol);
00577 countsol++;
00578 bool obeys_limits = true;
00579 for(unsigned int i = 0; i < sol.size(); i++) {
00580 if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i])) {
00581 obeys_limits = false;
00582 break;
00583 }
00584 }
00585 if(obeys_limits) {
00586 solution_callback(ik_pose,sol,error_code);
00587 if(error_code == kinematics::SUCCESS){
00588 solution = sol;
00589 ROS_DEBUG_STREAM("Took " << (ros::WallTime::now() - start) << " to return true " << countsol << " " << solvecount);
00590 return true;
00591 }
00592 }
00593 }
00594 }
00595 if(!getCount(counter, num_positive_increments, num_negative_increments)) {
00596 error_code = kinematics::NO_IK_SOLUTION;
00597 ROS_DEBUG_STREAM("Took " << (ros::WallTime::now() - start) << " to return false " << countsol << " " << solvecount);
00598 return false;
00599 }
00600 vfree[0] = initial_guess+search_discretization_*counter;
00601 ROS_DEBUG_STREAM(counter << " " << vfree[0]);
00602 }
00603 error_code = kinematics::NO_IK_SOLUTION;
00604 return false;
00605 }
00606
00607 bool getPositionFK(const std::vector<std::string> &link_names,
00608 const std::vector<double> &joint_angles,
00609 std::vector<geometry_msgs::Pose> &poses){
00610 return false;
00611
00612 KDL::Frame p_out;
00613
00614 if(link_names.size() == 0) {
00615 ROS_WARN_STREAM("Link names with nothing");
00616 return false;
00617 }
00618
00619 if(link_names.size()!=1 || link_names[0]!=tip_name_){
00620 ROS_ERROR("Can compute FK for %s only",tip_name_.c_str());
00621 return false;
00622 }
00623
00624 bool valid = true;
00625
00626 double eerot[9], eetrans[3];
00627 fk(&joint_angles[0],eetrans,eerot);
00628 for(int i=0; i<3;++i) p_out.p.data[i] = eetrans[i];
00629 for(int i=0; i<9;++i) p_out.M.data[i] = eerot[i];
00630
00631 poses.resize(1);
00632 tf::PoseKDLToMsg(p_out,poses[0]);
00633
00634 return valid;
00635 }
00636 const std::vector<std::string>& getJointNames() const { return joint_names_; }
00637 const std::vector<std::string>& getLinkNames() const { return link_names_; }
00638 };
00639 }
00640
00641 #include <pluginlib/class_list_macros.h>
00642 PLUGINLIB_DECLARE_CLASS(SIA20D_Mesh_manipulator_kinematics, IKFastKinematicsPlugin, SIA20D_Mesh_manipulator_kinematics::IKFastKinematicsPlugin, kinematics::KinematicsBase);
00643