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 ADEPT_VIPER_S650_arm_1_kinematics
00007 {
00008
00009
00010 #include "ADEPT_VIPER_S650_arm_1_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 return getPositionIK(ik_pose, ik_seed_state,solution, error_code);
00302 }
00303
00304 if(redundancy != (unsigned int)free_params_[0]) {
00305 ROS_WARN_STREAM("Calling consistency search with wrong free param");
00306 return false;
00307 }
00308
00309 KDL::Frame frame;
00310 tf::PoseMsgToKDL(ik_pose,frame);
00311
00312 std::vector<double> vfree(free_params_.size());
00313
00314 ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
00315 int counter = 0;
00316
00317 double initial_guess = ik_seed_state[free_params_[0]];
00318 vfree[0] = initial_guess;
00319
00320 double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess+consistency_limit);
00321 double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess-consistency_limit);
00322
00323 int num_positive_increments = (int)((max_limit-initial_guess)/search_discretization_);
00324 int num_negative_increments = (int)((initial_guess-min_limit)/search_discretization_);
00325
00326 ROS_DEBUG_STREAM("Free param is " << free_params_[0] << " initial guess is " << initial_guess << " " << num_positive_increments << " " << num_negative_increments);
00327
00328 while(1) {
00329 int numsol = ik_solver_->solve(frame,vfree);
00330
00331
00332
00333
00334
00335 if(numsol > 0){
00336 for(int s = 0; s < numsol; ++s){
00337 std::vector<double> sol;
00338 ik_solver_->getSolution(s,sol);
00339 bool obeys_limits = true;
00340 for(unsigned int i = 0; i < sol.size(); i++) {
00341 if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i])) {
00342 obeys_limits = false;
00343 break;
00344 }
00345
00346 }
00347 if(obeys_limits) {
00348 ik_solver_->getSolution(s,solution);
00349 error_code = kinematics::SUCCESS;
00350 return true;
00351 }
00352 }
00353 }
00354
00355
00356
00357
00358
00359
00360
00361
00362
00363
00364
00365
00366
00367
00368
00369
00370
00371
00372
00373
00374
00375
00376 if(!getCount(counter, num_positive_increments, num_negative_increments)) {
00377 error_code = kinematics::NO_IK_SOLUTION;
00378 return false;
00379 }
00380
00381 vfree[0] = initial_guess+search_discretization_*counter;
00382 ROS_DEBUG_STREAM(counter << " " << vfree[0]);
00383 }
00384
00385 error_code = kinematics::NO_IK_SOLUTION;
00386 return false;
00387 }
00388
00389 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00390 const std::vector<double> &ik_seed_state,
00391 const double &timeout,
00392 std::vector<double> &solution,
00393 const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> &desired_pose_callback,
00394 const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> &solution_callback,
00395 int &error_code){
00396
00397 if(free_params_.size()==0){
00398 if(!getPositionIK(ik_pose, ik_seed_state,solution, error_code)) {
00399 ROS_DEBUG_STREAM("No solution whatsoever");
00400 error_code = kinematics::NO_IK_SOLUTION;
00401 return false;
00402 }
00403 solution_callback(ik_pose,solution,error_code);
00404 if(error_code == kinematics::SUCCESS) {
00405 ROS_DEBUG_STREAM("Solution passes");
00406 return true;
00407 } else {
00408 ROS_DEBUG_STREAM("Solution has error code " << error_code);
00409 return false;
00410 }
00411 }
00412
00413 if(!desired_pose_callback.empty())
00414 desired_pose_callback(ik_pose,ik_seed_state,error_code);
00415 if(error_code < 0)
00416 {
00417 ROS_ERROR("Could not find inverse kinematics for desired end-effector pose since the pose may be in collision");
00418 return false;
00419 }
00420
00421 KDL::Frame frame;
00422 tf::PoseMsgToKDL(ik_pose,frame);
00423
00424 std::vector<double> vfree(free_params_.size());
00425
00426 ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
00427 int counter = 0;
00428
00429 double initial_guess = ik_seed_state[free_params_[0]];
00430 vfree[0] = initial_guess;
00431
00432 int num_positive_increments = (joint_max_vector_[free_params_[0]]-initial_guess)/search_discretization_;
00433 int num_negative_increments = (initial_guess - joint_min_vector_[free_params_[0]])/search_discretization_;
00434
00435 ROS_DEBUG_STREAM("Free param is " << free_params_[0] << " initial guess is " << initial_guess << " " << num_positive_increments << " " << num_negative_increments);
00436
00437 unsigned int solvecount = 0;
00438 unsigned int countsol = 0;
00439
00440 ros::WallTime start = ros::WallTime::now();
00441
00442 std::vector<double> sol;
00443 while(1) {
00444 int numsol = ik_solver_->solve(frame,vfree);
00445 if(solvecount == 0) {
00446 if(numsol == 0) {
00447 ROS_DEBUG_STREAM("Bad solve time is " << ros::WallTime::now()-start);
00448 } else {
00449 ROS_DEBUG_STREAM("Good solve time is " << ros::WallTime::now()-start);
00450 }
00451 }
00452 solvecount++;
00453 if(numsol > 0){
00454 if(solution_callback.empty()){
00455 ik_solver_->getClosestSolution(ik_seed_state,solution);
00456 error_code = kinematics::SUCCESS;
00457 return true;
00458 }
00459
00460 for(int s = 0; s < numsol; ++s){
00461 ik_solver_->getSolution(s,sol);
00462 countsol++;
00463 bool obeys_limits = true;
00464 for(unsigned int i = 0; i < sol.size(); i++) {
00465 if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i])) {
00466 obeys_limits = false;
00467 break;
00468 }
00469 }
00470 if(obeys_limits) {
00471 solution_callback(ik_pose,sol,error_code);
00472 if(error_code == kinematics::SUCCESS){
00473 solution = sol;
00474 ROS_DEBUG_STREAM("Took " << (ros::WallTime::now() - start) << " to return true " << countsol << " " << solvecount);
00475 return true;
00476 }
00477 }
00478 }
00479 }
00480 if(!getCount(counter, num_positive_increments, num_negative_increments)) {
00481 error_code = kinematics::NO_IK_SOLUTION;
00482 ROS_DEBUG_STREAM("Took " << (ros::WallTime::now() - start) << " to return false " << countsol << " " << solvecount);
00483 return false;
00484 }
00485 vfree[0] = initial_guess+search_discretization_*counter;
00486 ROS_DEBUG_STREAM(counter << " " << vfree[0]);
00487 }
00488 error_code = kinematics::NO_IK_SOLUTION;
00489 return false;
00490 }
00491
00492 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00493 const std::vector<double> &ik_seed_state,
00494 const double &timeout,
00495 const unsigned int& redundancy,
00496 const double &consistency_limit,
00497 std::vector<double> &solution,
00498 const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> &desired_pose_callback,
00499 const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> &solution_callback,
00500 int &error_code){
00501
00502 if(free_params_.size()==0){
00503 if(!getPositionIK(ik_pose, ik_seed_state,solution, error_code)) {
00504 ROS_DEBUG_STREAM("No solution whatsoever");
00505 error_code = kinematics::NO_IK_SOLUTION;
00506 return false;
00507 }
00508 solution_callback(ik_pose,solution,error_code);
00509 if(error_code == kinematics::SUCCESS) {
00510 ROS_DEBUG_STREAM("Solution passes");
00511 return true;
00512 } else {
00513 ROS_DEBUG_STREAM("Solution has error code " << error_code);
00514 return false;
00515 }
00516 }
00517
00518 if(redundancy != (unsigned int) free_params_[0]) {
00519 ROS_WARN_STREAM("Calling consistency search with wrong free param");
00520 return false;
00521 }
00522
00523 if(!desired_pose_callback.empty())
00524 desired_pose_callback(ik_pose,ik_seed_state,error_code);
00525 if(error_code < 0)
00526 {
00527 ROS_ERROR("Could not find inverse kinematics for desired end-effector pose since the pose may be in collision");
00528 return false;
00529 }
00530
00531 KDL::Frame frame;
00532 tf::PoseMsgToKDL(ik_pose,frame);
00533
00534 std::vector<double> vfree(free_params_.size());
00535
00536 ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
00537 int counter = 0;
00538
00539 double initial_guess = ik_seed_state[free_params_[0]];
00540 vfree[0] = initial_guess;
00541
00542 double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess+consistency_limit);
00543 double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess-consistency_limit);
00544
00545 int num_positive_increments = (int)((max_limit-initial_guess)/search_discretization_);
00546 int num_negative_increments = (int)((initial_guess-min_limit)/search_discretization_);
00547
00548 ROS_DEBUG_STREAM("Free param is " << free_params_[0] << " initial guess is " << initial_guess << " " << num_positive_increments << " " << num_negative_increments);
00549
00550 unsigned int solvecount = 0;
00551 unsigned int countsol = 0;
00552
00553 ros::WallTime start = ros::WallTime::now();
00554
00555 std::vector<double> sol;
00556 while(1) {
00557 int numsol = ik_solver_->solve(frame,vfree);
00558 if(solvecount == 0) {
00559 if(numsol == 0) {
00560 ROS_DEBUG_STREAM("Bad solve time is " << ros::WallTime::now()-start);
00561 } else {
00562 ROS_DEBUG_STREAM("Good solve time is " << ros::WallTime::now()-start);
00563 }
00564 }
00565 solvecount++;
00566 if(numsol > 0){
00567 if(solution_callback.empty()){
00568 ik_solver_->getClosestSolution(ik_seed_state,solution);
00569 error_code = kinematics::SUCCESS;
00570 return true;
00571 }
00572
00573 for(int s = 0; s < numsol; ++s){
00574 ik_solver_->getSolution(s,sol);
00575 countsol++;
00576 bool obeys_limits = true;
00577 for(unsigned int i = 0; i < sol.size(); i++) {
00578 if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i])) {
00579 obeys_limits = false;
00580 break;
00581 }
00582 }
00583 if(obeys_limits) {
00584 solution_callback(ik_pose,sol,error_code);
00585 if(error_code == kinematics::SUCCESS){
00586 solution = sol;
00587 ROS_DEBUG_STREAM("Took " << (ros::WallTime::now() - start) << " to return true " << countsol << " " << solvecount);
00588 return true;
00589 }
00590 }
00591 }
00592 }
00593 if(!getCount(counter, num_positive_increments, num_negative_increments)) {
00594 error_code = kinematics::NO_IK_SOLUTION;
00595 ROS_DEBUG_STREAM("Took " << (ros::WallTime::now() - start) << " to return false " << countsol << " " << solvecount);
00596 return false;
00597 }
00598 vfree[0] = initial_guess+search_discretization_*counter;
00599 ROS_DEBUG_STREAM(counter << " " << vfree[0]);
00600 }
00601 error_code = kinematics::NO_IK_SOLUTION;
00602 return false;
00603 }
00604
00605 bool getPositionFK(const std::vector<std::string> &link_names,
00606 const std::vector<double> &joint_angles,
00607 std::vector<geometry_msgs::Pose> &poses){
00608 return false;
00609
00610 KDL::Frame p_out;
00611
00612 if(link_names.size() == 0) {
00613 ROS_WARN_STREAM("Link names with nothing");
00614 return false;
00615 }
00616
00617 if(link_names.size()!=1 || link_names[0]!=tip_name_){
00618 ROS_ERROR("Can compute FK for %s only",tip_name_.c_str());
00619 return false;
00620 }
00621
00622 bool valid = true;
00623
00624 double eerot[9], eetrans[3];
00625 fk(&joint_angles[0],eetrans,eerot);
00626 for(int i=0; i<3;++i) p_out.p.data[i] = eetrans[i];
00627 for(int i=0; i<9;++i) p_out.M.data[i] = eerot[i];
00628
00629 poses.resize(1);
00630 tf::PoseKDLToMsg(p_out,poses[0]);
00631
00632 return valid;
00633 }
00634 const std::vector<std::string>& getJointNames() const { return joint_names_; }
00635 const std::vector<std::string>& getLinkNames() const { return link_names_; }
00636 };
00637 }
00638
00639 #include <pluginlib/class_list_macros.h>
00640 PLUGINLIB_DECLARE_CLASS(ADEPT_VIPER_S650_arm_1_kinematics, IKFastKinematicsPlugin, ADEPT_VIPER_S650_arm_1_kinematics::IKFastKinematicsPlugin, kinematics::KinematicsBase);
00641