lwr_kinematics_plugin.cpp
Go to the documentation of this file.
00001 #include "ros/ros.h"
00002 #include <kinematics_base/kinematics_base.h>
00003 #include <kdl/tree.hpp>
00004 #include <urdf/model.h>
00005 #include <kdl_parser/kdl_parser.hpp>
00006 #include <kdl/chainfksolverpos_recursive.hpp>
00007 #include <tf/tf.h>
00008 #include <tf_conversions/tf_kdl.h>
00009 #include <map>
00010 #include <iostream>
00011 #define IKFAST_NO_MAIN
00012 #ifdef IKFAST_REAL
00013 typedef IKFAST_REAL IKReal;
00014 #else
00015 typedef double IKReal;
00016 #endif
00017 
00018 
00019 namespace lwr_arm_kinematics
00020 {
00021 
00022 class ik_solver_base {
00023 public:
00024     virtual int solve(KDL::Frame &pose_frame, const std::vector<double> &ik_seed_state) = 0;
00025     virtual void getSolution(int i, std::vector<double> &solution) = 0;
00026     virtual void getClosestSolution(const std::vector<double> &ik_seed_state, std::vector<double> &solution) = 0;
00027 };
00028 
00029 template <class T> class ikfast_solver: public ik_solver_base {
00030 public:
00031     typedef bool (*ik_type)(const IKReal* eetrans, const IKReal* eerot, const IKReal* pfree, std::vector<T>& vsolutions);
00032     ikfast_solver(ik_type ik,int numJoints):ik(ik),numJoints(numJoints) {}
00033     virtual int solve(KDL::Frame &pose_frame, const std::vector<double> &vfree) {
00034 
00035         solutions.clear();
00036         ik(pose_frame.p.data, pose_frame.M.data, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
00037 
00038         return solutions.size();
00039     }
00040     virtual void getSolution(int i, std::vector<double> &solution) {
00041         solution.clear();
00042         std::vector<IKReal> vsolfree(solutions[i].GetFree().size());
00043         solution.clear();
00044         solution.resize(numJoints);
00045         solutions[i].GetSolution(&solution[0],vsolfree.size()>0?&vsolfree[0]:NULL);
00046         std::cout << "solution " << i << ":" ;
00047         for(int j=0; j<numJoints; ++j)
00048             std::cout << " " << solution[j];
00049         std::cout << std::endl;
00050 
00051         //ROS_ERROR("%f %d",solution[2],vsolfree.size());
00052     }
00053 
00054     virtual void getClosestSolution(const std::vector<double> &ik_seed_state, std::vector<double> &solution) {
00055         double mindist = 0;
00056         int minindex = -1;
00057         std::vector<double> sol;
00058         for(size_t i=0; i<solutions.size(); ++i) {
00059             getSolution(i,sol);
00060             double dist = 0;
00061             for(int j=0; j< numJoints; ++j) {
00062                 double diff = ik_seed_state[j] - sol[j];
00063                 dist += diff*diff;
00064             }
00065             if(minindex == -1 || dist<mindist) {
00066                 minindex = i;
00067                 mindist = dist;
00068             }
00069         }
00070         if(minindex >= 0) getSolution(minindex,solution);
00071     }
00072 
00073 
00074 
00075 private:
00076     ik_type ik;
00077     std::vector<T> solutions;
00078     int numJoints;
00079 };
00080 
00081 
00082 namespace lwr {
00083 #include "ikfast_lwr.cpp"
00084 }
00085 
00086 class IKFastKinematicsPlugin : public kinematics::KinematicsBase
00087 {
00088     std::vector<std::string> joints;
00089     std::vector<double> jointMin;
00090     std::vector<double> jointMax;
00091     std::vector<std::string> links;
00092     std::map<std::string,size_t> link2index;
00093     std::string tip_name, root_name;
00094     KDL::Chain kdl_chain;
00095     boost::shared_ptr<KDL::ChainFkSolverPos_recursive> jnt_to_pose_solver;
00096     ik_solver_base* ik_solver;
00097     size_t numJoints;
00098     std::vector<int> freeParams;
00099 
00100 public:
00101 
00102     IKFastKinematicsPlugin():ik_solver(0) {}
00103 
00104     ~IKFastKinematicsPlugin() {
00105         if(ik_solver) delete ik_solver;
00106     }
00107 
00108     void fillFreeParams(int count, int *array) {
00109         freeParams.clear();
00110         for(int i=0; i<count; ++i) freeParams.push_back(array[i]);
00111     }
00112 
00113     bool initialize(std::string name) {
00114 
00115         ros::NodeHandle private_handle("~/"+name);
00116 
00117         fillFreeParams(lwr::getNumFreeParameters(),lwr::getFreeParameters());
00118         numJoints = lwr::getNumJoints();
00119         ik_solver = new ikfast_solver<lwr::IKSolution>(lwr::ik, numJoints);
00120 
00121         if(freeParams.size()>1) {
00122             ROS_FATAL("Only one free joint paramter supported!");
00123             return false;
00124         }
00125 
00126 
00127         urdf::Model robot_model;
00128         std::string xml_string;
00129 
00130         std::string urdf_xml,full_urdf_xml;
00131         private_handle.param("urdf_xml",urdf_xml,std::string("robot_description"));
00132         private_handle.searchParam(urdf_xml,full_urdf_xml);
00133 
00134 
00135                 TiXmlDocument xml;
00136                 ROS_DEBUG("Reading xml file from parameter server\n");
00137                 std::string result;
00138                 if (private_handle.getParam(full_urdf_xml, result))
00139                 xml.Parse(result.c_str());
00140                 else
00141                 {
00142                 ROS_FATAL("Could not load the xml from parameter server: %s\n", urdf_xml.c_str());
00143                 return false;
00144                 }
00145                 xml_string = result;
00146                 TiXmlElement *root_element = xml.RootElement();
00147                 TiXmlElement *root = xml.FirstChildElement("robot");
00148                 if (!root || !root_element)
00149                 {
00150                 ROS_FATAL("Could not parse the xml from %s\n", urdf_xml.c_str());
00151                 return false;
00152                 }
00153                 robot_model.initXml(root);
00154                 if (!private_handle.getParam("root_name", root_name)){
00155                 ROS_FATAL("PR2IK: No root name found on parameter server");
00156                 return false;
00157                 }
00158                 if (!private_handle.getParam("tip_name", tip_name)){
00159                 ROS_FATAL("PR2IK: No tip name found on parameter server");
00160                 return false;
00161                 }
00162 
00163         KDL::Tree tree;
00164         if (!kdl_parser::treeFromString(xml_string, tree))
00165         {
00166             ROS_ERROR("Could not initialize tree object");
00167             return false;
00168         }
00169 
00170         if (!tree.getChain(root_name, tip_name, kdl_chain))
00171         {
00172             ROS_ERROR("Could not initialize kdl_chain object");
00173             return false;
00174         }
00175 
00176         int i=0; // segment number
00177         while(i < (int)kdl_chain.getNrOfSegments())
00178         {
00179             link2index.insert(std::make_pair(kdl_chain.getSegment(i).getName(),i));
00180             links.push_back(kdl_chain.getSegment(i).getName());
00181 
00182             joints.push_back(kdl_chain.getSegment(i).getJoint().getName());
00183             ROS_INFO("Joint %s",joints.back().c_str());
00184 
00185             i++;
00186         }
00187         if(joints.size() != numJoints) {
00188             ROS_ERROR("Joints count mismatch");
00189             return false;
00190         }
00191 
00192                                 int num_joints = 0;
00193                         boost::shared_ptr<const urdf::Link> link = robot_model.getLink(tip_name);
00194                         while(link && num_joints < 7)
00195                         {
00196                         boost::shared_ptr<const urdf::Joint> joint = robot_model.getJoint(link->parent_joint->name);
00197                         if(!joint)
00198                         {
00199                         ROS_ERROR("Could not find joint: %s",link->parent_joint->name.c_str());
00200                         return false;
00201                         }
00202                         if(joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
00203                         {
00204                         if(joint->type != urdf::Joint::CONTINUOUS)
00205                         {
00206                                 jointMin.push_back(joint->safety->soft_lower_limit);
00207                                 jointMax.push_back(joint->safety->soft_upper_limit);
00208                         }
00209                         else
00210                         {
00211                                 jointMin.push_back(-M_PI);
00212                                 jointMax.push_back(M_PI);
00213                         }
00214                         num_joints++;
00215                         }
00216                         link = robot_model.getLink(link->getParent()->name);
00217                         } 
00218 
00219                                 // We expect order from root to tip, so reverse the order
00220                         std::reverse(jointMin.begin(),jointMin.end());
00221                         std::reverse(jointMax.begin(),jointMax.end());
00222 
00223         jnt_to_pose_solver.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain));
00224 
00225 
00226         return true;
00227     }
00228 bool getCount(int &count, const int &max_count, const int &min_count)
00229 {
00230   if(count > 0)
00231   {
00232     if(-count >= min_count)
00233     {   
00234       count = -count;
00235       return true;
00236     }
00237     else if(count+1 <= max_count)
00238     {
00239       count = count+1;
00240       return true;
00241     }
00242     else
00243     {
00244       return false;
00245     }
00246   }
00247   else
00248   {
00249     if(1-count <= max_count)
00250     {
00251       count = 1-count;
00252       return true;
00253     }
00254     else if(count-1 >= min_count)
00255     {
00256       count = count -1;
00257       return true;
00258     }
00259     else
00260       return false;
00261   }
00262 }
00263 
00264                 bool checkJoints(const std::vector<double> &sol)
00265                 {
00266                         for(size_t i = 0; i<sol.size(); i++)
00267                         {
00268                                 if((sol[i] < jointMin[i]) || (sol[i] > jointMax[i]))
00269                                         return false;
00270                         }
00271                         return true;
00272                 }
00273 
00274     bool getPositionIK(const geometry_msgs::Pose &ik_pose,
00275                        const std::vector<double> &ik_seed_state,
00276                        std::vector<double> &solution,
00277                        int &error_code) {
00278         //ROS_ERROR("getPositionIK");
00279 
00280         std::vector<double> vfree(freeParams.size());
00281         for(std::size_t i = 0; i < freeParams.size(); ++i) {
00282             int p = freeParams[i];
00283 // ROS_ERROR("%u is %f",p,ik_seed_state[p]);
00284             vfree[i] = ik_seed_state[p];
00285         }
00286 
00287         KDL::Frame frame;
00288         tf::PoseMsgToKDL(ik_pose,frame);
00289 
00290         int numsol = ik_solver->solve(frame,vfree);
00291 
00292 
00293         if(numsol) {
00294             ik_solver->getClosestSolution(ik_seed_state,solution);
00295             //ROS_ERROR("Solved!");
00296             error_code = kinematics::SUCCESS;
00297             return true;
00298         }
00299 
00300         error_code = kinematics::NO_IK_SOLUTION;
00301         return false;
00302     }
00303 
00304     bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00305                           const std::vector<double> &ik_seed_state,
00306                           const double &timeout,
00307                           std::vector<double> &solution, int &error_code) {
00308 
00309         //ROS_ERROR("searchPositionIK1");
00310         if(freeParams.size()==0) {
00311             return getPositionIK(ik_pose, ik_seed_state,solution, error_code);
00312         }
00313         bool solved = false;
00314         std::vector<double> sol;
00315         double mindist = 9999999;
00316         KDL::Frame frame;
00317         tf::PoseMsgToKDL(ik_pose,frame);
00318 
00319         std::vector<double> vfree(freeParams.size());
00320 
00321         ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
00322         const double increment = 0.02; //TODO: make it a plugin parameter
00323 
00324         vfree[0] = -2.9; //jointMin[freeParams[0]];
00325 
00326         while (vfree[0] < 2.9 /*jointMax[freeParams[0]]*/) {
00327 
00328             int numsol = ik_solver->solve(frame,vfree);
00329 
00330             if(numsol > 0) {
00331                 double dist = 0;
00332                 ik_solver->getClosestSolution(ik_seed_state,sol);
00333                 for(size_t i = 0; i<sol.size(); i++)
00334                     dist += (sol[i] - ik_seed_state[i]) * (sol[i] - ik_seed_state[i]);
00335                 if(dist < mindist)
00336                 {
00337                     solution = sol;
00338                     mindist = dist;
00339                 }
00340                 solved = true;
00341             }
00342 
00343             vfree[0] += increment;
00344                                                 if(ros::Time::now() > maxTime)
00345                                                         break;
00346         }
00347 
00348         if(solved)
00349         {
00350             //ROS_ERROR("Solved!");
00351             error_code = kinematics::SUCCESS;
00352             return true;
00353         } else {
00354             error_code = kinematics::NO_IK_SOLUTION;
00355             return false;
00356         }
00357     }
00358 
00359     bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00360                           const std::vector<double> &ik_seed_state,
00361                           const double &timeout,
00362                           std::vector<double> &solution,
00363                           const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> &desired_pose_callback,
00364                           const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> &solution_callback,
00365                           int &error_code) {
00366                                 KDL::Frame frame;
00367                                 int count = 0;
00368                                 int numsol;
00369                                 std::vector<double> sol;
00370 
00371         if(!desired_pose_callback.empty())
00372             desired_pose_callback(ik_pose,ik_seed_state,error_code);
00373 
00374         if(error_code < 0)
00375         {
00376             ROS_ERROR("Could not find inverse kinematics for desired end-effector pose since the pose may be in collision");
00377             return false;
00378         }
00379 
00380         
00381                                 
00382         tf::PoseMsgToKDL(ik_pose,frame);
00383 
00384         std::vector<double> vfree(freeParams.size());
00385 
00386         ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
00387         const double search_discretization_angle_ = 0.02; //TODO: make it a plugin parameter
00388 
00389                         double initial_guess = vfree[0] = ik_seed_state[freeParams[0]];
00390 
00391                                 int num_positive_increments = (int)((jointMax[freeParams[0]]-initial_guess)/search_discretization_angle_);
00392                         int num_negative_increments = (int)((jointMin[freeParams[0]] + initial_guess)/search_discretization_angle_);
00393                         ROS_DEBUG("%f %f %f %d %d \n\n",initial_guess,jointMax[freeParams[0]],jointMin[freeParams[0]],num_positive_increments,num_negative_increments);
00394 
00395         while (ros::Time::now() < maxTime) {
00396             if(numsol = ik_solver->solve(frame,vfree) > 0) {
00397                                                         if(solution_callback.empty()) {
00398                 
00399                 ik_solver->getClosestSolution(ik_seed_state,solution);
00400                                                                 error_code = kinematics::SUCCESS;
00401                                                                 return true;
00402                                                         } else {
00403                                                                 for (int s = 0; s < numsol; ++s) {
00404                   ik_solver->getSolution(s,sol);
00405                                                                         if(checkJoints(sol))
00406                                                                         {
00407                         solution_callback(ik_pose,sol,error_code);
00408                                                                         
00409                                                                                 if( error_code == kinematics::SUCCESS) {
00410                                 solution = sol;
00411                                                                                 return true;
00412                                 }
00413                                                                         }
00414                                                                 }
00415               }
00416                         }
00417 
00418                                 if(!getCount(count, num_positive_increments, num_negative_increments)) {
00419                                 error_code = kinematics::NO_IK_SOLUTION;
00420                                 return false;
00421                                 }
00422 
00423                                 vfree[0] = initial_guess + search_discretization_angle_ * count;
00424                                 ROS_DEBUG("Redundancy search, index:%d, free angle value: %f",count,vfree[0]);
00425         }
00426 
00427         ROS_DEBUG("IK Timed out in %f seconds",timeout);
00428                 error_code = kinematics::TIMED_OUT;
00429 
00430                                 return false;
00431     }
00432 
00433     bool getPositionFK(const std::vector<std::string> &link_names,
00434                        const std::vector<double> &joint_angles,
00435                        std::vector<geometry_msgs::Pose> &poses) {
00436         KDL::Frame p_out;
00437         KDL::JntArray jnt_pos_in;
00438         geometry_msgs::PoseStamped pose;
00439         tf::Stamped<tf::Pose> tf_pose;
00440 
00441         jnt_pos_in.resize(joints.size());
00442         for(size_t i=0; i < joints.size(); i++)
00443         {
00444             jnt_pos_in(i) = joint_angles[i];
00445         }
00446 
00447         poses.resize(link_names.size());
00448 
00449         bool valid = true;
00450         for(unsigned int i=0; i < poses.size(); i++)
00451         {
00452             if(jnt_to_pose_solver->JntToCart(jnt_pos_in,p_out,link2index[link_names[i]]) >=0)
00453             {
00454                 tf::PoseKDLToMsg(p_out,poses[i]);
00455             }
00456             else
00457             {
00458                 ROS_ERROR("Could not compute FK for %s",link_names[i].c_str());
00459                 valid = false;
00460             }
00461         }
00462         //ROS_ERROR("getPositionFK %s",valid?"succeded":"failed");
00463 
00464         return valid;
00465     }
00466     std::string getBaseFrame() {
00467         //ROS_ERROR("getBaseFrame");
00468         return root_name;
00469     }
00470     std::string getToolFrame() {
00471         //ROS_ERROR("getToolFrame");
00472         return tip_name;
00473     }
00474     std::vector<std::string> getJointNames() {
00475         //ROS_ERROR("getJointNames");
00476         return joints;
00477     }
00478     std::vector<std::string> getLinkNames() {
00479         //ROS_ERROR("getLinkNames");
00480         return links;
00481     }
00482 };
00483 }
00484 
00485 #include <pluginlib/class_list_macros.h>
00486 PLUGINLIB_DECLARE_CLASS(lwr_arm_kinematics,IKFastKinematicsPlugin, lwr_arm_kinematics::IKFastKinematicsPlugin, kinematics::KinematicsBase)


lwr_kinematics
Author(s): Konrad Banachowicz
autogenerated on Mon Oct 6 2014 02:00:38