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 #include <geometry_msgs/PoseStamped.h>
00038 #include <kdl_parser/kdl_parser.hpp>
00039 #include <eigen_conversions/eigen_msg.h>
00040 #include <eigen_conversions/eigen_kdl.h>
00041 #include <algorithm>
00042 #include <numeric>
00043
00044 #include "pr2_arm_kinematics_plugin.h"
00045
00046 using namespace KDL;
00047 using namespace std;
00048
00049 namespace pr2_arm_kinematics {
00050
00051 bool PR2ArmIKSolver::getCount(int &count,
00052 const int &max_count,
00053 const int &min_count)
00054 {
00055 if(count > 0)
00056 {
00057 if(-count >= min_count)
00058 {
00059 count = -count;
00060 return true;
00061 }
00062 else if(count+1 <= max_count)
00063 {
00064 count = count+1;
00065 return true;
00066 }
00067 else
00068 {
00069 return false;
00070 }
00071 }
00072 else
00073 {
00074 if(1-count <= max_count)
00075 {
00076 count = 1-count;
00077 return true;
00078 }
00079 else if(count-1 >= min_count)
00080 {
00081 count = count -1;
00082 return true;
00083 }
00084 else
00085 return false;
00086 }
00087 }
00088
00089 PR2ArmIKSolver::PR2ArmIKSolver(const urdf::ModelInterface &robot_model,
00090 const std::string &root_frame_name,
00091 const std::string &tip_frame_name,
00092 const double &search_discretization_angle,
00093 const int &free_angle):ChainIkSolverPos()
00094 {
00095 search_discretization_angle_ = search_discretization_angle;
00096 free_angle_ = free_angle;
00097 root_frame_name_ = root_frame_name;
00098 if(!pr2_arm_ik_.init(robot_model,root_frame_name,tip_frame_name))
00099 active_ = false;
00100 else
00101 active_ = true;
00102 }
00103
00104 int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init,
00105 const KDL::Frame& p_in,
00106 KDL::JntArray &q_out)
00107 {
00108 Eigen::Matrix4f b = KDLToEigenMatrix(p_in);
00109 std::vector<std::vector<double> > solution_ik;
00110 if(free_angle_ == 0)
00111 {
00112 ROS_DEBUG("Solving with %f",q_init(0));
00113 pr2_arm_ik_.computeIKShoulderPan(b,q_init(0),solution_ik);
00114 }
00115 else
00116 {
00117 pr2_arm_ik_.computeIKShoulderRoll(b,q_init(2),solution_ik);
00118 }
00119
00120 if(solution_ik.empty())
00121 return -1;
00122
00123 double min_distance = 1e6;
00124 int min_index = -1;
00125
00126 for(int i=0; i< (int) solution_ik.size(); i++)
00127 {
00128 ROS_DEBUG("Solution : %d",(int)solution_ik.size());
00129
00130 for(int j=0; j < (int)solution_ik[i].size(); j++)
00131 {
00132 ROS_DEBUG("%d: %f",j,solution_ik[i][j]);
00133 }
00134 ROS_DEBUG(" ");
00135 ROS_DEBUG(" ");
00136
00137 double tmp_distance = computeEuclideanDistance(solution_ik[i],q_init);
00138 if(tmp_distance < min_distance)
00139 {
00140 min_distance = tmp_distance;
00141 min_index = i;
00142 }
00143 }
00144
00145 if(min_index > -1)
00146 {
00147 q_out.resize((int)solution_ik[min_index].size());
00148 for(int i=0; i < (int)solution_ik[min_index].size(); i++)
00149 {
00150 q_out(i) = solution_ik[min_index][i];
00151 }
00152 return 1;
00153 }
00154 else
00155 return -1;
00156 }
00157
00158 int PR2ArmIKSolver::CartToJntSearch(const KDL::JntArray& q_in,
00159 const KDL::Frame& p_in,
00160 KDL::JntArray &q_out,
00161 const double &timeout)
00162 {
00163 KDL::JntArray q_init = q_in;
00164 double initial_guess = q_init(free_angle_);
00165
00166 ros::Time start_time = ros::Time::now();
00167 double loop_time = 0;
00168 int count = 0;
00169
00170 int num_positive_increments = (int)((pr2_arm_ik_.solver_info_.limits[free_angle_].max_position-initial_guess)/search_discretization_angle_);
00171 int num_negative_increments = (int)((initial_guess-pr2_arm_ik_.solver_info_.limits[free_angle_].min_position)/search_discretization_angle_);
00172 ROS_DEBUG("%f %f %f %d %d \n\n",initial_guess,pr2_arm_ik_.solver_info_.limits[free_angle_].max_position,pr2_arm_ik_.solver_info_.limits[free_angle_].min_position,num_positive_increments,num_negative_increments);
00173 while(loop_time < timeout)
00174 {
00175 if(CartToJnt(q_init,p_in,q_out) > 0)
00176 return 1;
00177 if(!getCount(count,num_positive_increments,-num_negative_increments))
00178 return -1;
00179 q_init(free_angle_) = initial_guess + search_discretization_angle_ * count;
00180 ROS_DEBUG("%d, %f",count,q_init(free_angle_));
00181 loop_time = (ros::Time::now()-start_time).toSec();
00182 }
00183 if(loop_time >= timeout)
00184 {
00185 ROS_DEBUG("IK Timed out in %f seconds",timeout);
00186 return TIMED_OUT;
00187 }
00188 else
00189 {
00190 ROS_DEBUG("No IK solution was found");
00191 return NO_IK_SOLUTION;
00192 }
00193 return NO_IK_SOLUTION;
00194 }
00195
00196 bool getKDLChain(const urdf::ModelInterface& model, const std::string &root_name, const std::string &tip_name, KDL::Chain &kdl_chain)
00197 {
00198
00199 KDL::Tree tree;
00200 if (!kdl_parser::treeFromUrdfModel(model, tree))
00201 {
00202 ROS_ERROR("Could not initialize tree object");
00203 return false;
00204 }
00205 if (!tree.getChain(root_name, tip_name, kdl_chain))
00206 {
00207 ROS_ERROR_STREAM("Could not initialize chain object for base " << root_name << " tip " << tip_name);
00208 return false;
00209 }
00210 return true;
00211 }
00212
00213 Eigen::Matrix4f KDLToEigenMatrix(const KDL::Frame &p)
00214 {
00215 Eigen::Matrix4f b = Eigen::Matrix4f::Identity();
00216 for(int i=0; i < 3; i++)
00217 {
00218 for(int j=0; j<3; j++)
00219 {
00220 b(i,j) = p.M(i,j);
00221 }
00222 b(i,3) = p.p(i);
00223 }
00224 return b;
00225 }
00226
00227 double computeEuclideanDistance(const std::vector<double> &array_1, const KDL::JntArray &array_2)
00228 {
00229 double distance = 0.0;
00230 for(int i=0; i< (int) array_1.size(); i++)
00231 {
00232 distance += (array_1[i] - array_2(i))*(array_1[i] - array_2(i));
00233 }
00234 return sqrt(distance);
00235 }
00236
00237 void getKDLChainInfo(const KDL::Chain &chain,
00238 moveit_msgs::KinematicSolverInfo &chain_info)
00239 {
00240 int i=0;
00241 while(i < (int)chain.getNrOfSegments())
00242 {
00243 chain_info.link_names.push_back(chain.getSegment(i).getName());
00244 i++;
00245 }
00246 }
00247
00248 PR2ArmKinematicsPlugin::PR2ArmKinematicsPlugin():active_(false){}
00249
00250 bool PR2ArmKinematicsPlugin::isActive()
00251 {
00252 if(active_)
00253 return true;
00254 return false;
00255 }
00256
00257 void PR2ArmKinematicsPlugin::setRobotModel(boost::shared_ptr<urdf::ModelInterface>& robot_model)
00258 {
00259 robot_model_ = robot_model;
00260 }
00261
00262 bool PR2ArmKinematicsPlugin::initialize(const std::string& robot_description,
00263 const std::string& group_name,
00264 const std::string& base_name,
00265 const std::string& tip_name,
00266 double search_discretization)
00267 {
00268 setValues(robot_description, group_name, base_name, tip_name,search_discretization);
00269
00270 std::string xml_string;
00271 dimension_ = 7;
00272
00273 ROS_DEBUG("Loading KDL Tree");
00274 if(!getKDLChain(*robot_model_.get(),base_frame_,tip_frame_,kdl_chain_))
00275 {
00276 active_ = false;
00277 ROS_ERROR("Could not load kdl tree");
00278 }
00279 jnt_to_pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
00280 free_angle_ = 2;
00281
00282 pr2_arm_ik_solver_.reset(new pr2_arm_kinematics::PR2ArmIKSolver(*robot_model_.get(), base_frame_,tip_frame_, search_discretization_,free_angle_));
00283 if(!pr2_arm_ik_solver_->active_)
00284 {
00285 ROS_ERROR("Could not load ik");
00286 active_ = false;
00287 }
00288 else
00289 {
00290 pr2_arm_ik_solver_->getSolverInfo(ik_solver_info_);
00291 pr2_arm_kinematics::getKDLChainInfo(kdl_chain_,fk_solver_info_);
00292 fk_solver_info_.joint_names = ik_solver_info_.joint_names;
00293
00294 for(unsigned int i=0; i < ik_solver_info_.joint_names.size(); i++)
00295 {
00296 ROS_DEBUG("PR2Kinematics:: joint name: %s",ik_solver_info_.joint_names[i].c_str());
00297 }
00298 for(unsigned int i=0; i < ik_solver_info_.link_names.size(); i++)
00299 {
00300 ROS_DEBUG("PR2Kinematics can solve IK for %s",ik_solver_info_.link_names[i].c_str());
00301 }
00302 for(unsigned int i=0; i < fk_solver_info_.link_names.size(); i++)
00303 {
00304 ROS_DEBUG("PR2Kinematics can solve FK for %s",fk_solver_info_.link_names[i].c_str());
00305 }
00306 ROS_DEBUG("PR2KinematicsPlugin::active for %s",group_name.c_str());
00307 active_ = true;
00308 }
00309 return active_;
00310 }
00311
00312 bool PR2ArmKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose,
00313 const std::vector<double> &ik_seed_state,
00314 std::vector<double> &solution,
00315 moveit_msgs::MoveItErrorCodes &error_code,
00316 const kinematics::KinematicsQueryOptions &options) const
00317 {
00318 return false;
00319 }
00320
00321 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00322 const std::vector<double> &ik_seed_state,
00323 double timeout,
00324 std::vector<double> &solution,
00325 moveit_msgs::MoveItErrorCodes &error_code,
00326 const kinematics::KinematicsQueryOptions &options) const
00327 {
00328 if(!active_)
00329 {
00330 ROS_ERROR("kinematics not active");
00331 error_code.val = error_code.PLANNING_FAILED;
00332 return false;
00333 }
00334 KDL::Frame pose_desired;
00335 Eigen::Affine3d tp;
00336 tf::poseMsgToEigen(ik_pose, tp);
00337 tf::transformEigenToKDL(tp, pose_desired);
00338
00339
00340 KDL::JntArray jnt_pos_in;
00341 KDL::JntArray jnt_pos_out;
00342 jnt_pos_in.resize(dimension_);
00343 for(int i=0; i < dimension_; i++)
00344 {
00345 jnt_pos_in(i) = ik_seed_state[i];
00346 }
00347
00348 int ik_valid = pr2_arm_ik_solver_->CartToJntSearch(jnt_pos_in,
00349 pose_desired,
00350 jnt_pos_out,
00351 timeout);
00352 if(ik_valid == pr2_arm_kinematics::NO_IK_SOLUTION)
00353 {
00354 error_code.val = error_code.NO_IK_SOLUTION;
00355 return false;
00356 }
00357
00358 if(ik_valid >= 0)
00359 {
00360 solution.resize(dimension_);
00361 for(int i=0; i < dimension_; i++)
00362 {
00363 solution[i] = jnt_pos_out(i);
00364 }
00365 error_code.val = error_code.SUCCESS;
00366 return true;
00367 }
00368 else
00369 {
00370 ROS_DEBUG("An IK solution could not be found");
00371 error_code.val = error_code.NO_IK_SOLUTION;
00372 return false;
00373 }
00374 }
00375
00376 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00377 const std::vector<double> &ik_seed_state,
00378 double timeout,
00379 const std::vector<double> &consistency_limit,
00380 std::vector<double> &solution,
00381 moveit_msgs::MoveItErrorCodes &error_code,
00382 const kinematics::KinematicsQueryOptions &options) const
00383 {
00384 return false;
00385 }
00386
00387 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00388 const std::vector<double> &ik_seed_state,
00389 double timeout,
00390 std::vector<double> &solution,
00391 const IKCallbackFn &solution_callback,
00392 moveit_msgs::MoveItErrorCodes &error_code,
00393 const kinematics::KinematicsQueryOptions &options) const
00394 {
00395 return false;
00396 }
00397
00398 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00399 const std::vector<double> &ik_seed_state,
00400 double timeout,
00401 const std::vector<double> &consistency_limit,
00402 std::vector<double> &solution,
00403 const IKCallbackFn &solution_callback,
00404 moveit_msgs::MoveItErrorCodes &error_code,
00405 const kinematics::KinematicsQueryOptions &options) const
00406 {
00407 return false;
00408 }
00409
00410 bool PR2ArmKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
00411 const std::vector<double> &joint_angles,
00412 std::vector<geometry_msgs::Pose> &poses) const
00413 {
00414 return false;
00415 }
00416
00417 const std::vector<std::string>& PR2ArmKinematicsPlugin::getJointNames() const
00418 {
00419 if(!active_)
00420 {
00421 ROS_ERROR("kinematics not active");
00422 }
00423 return ik_solver_info_.joint_names;
00424 }
00425
00426 const std::vector<std::string>& PR2ArmKinematicsPlugin::getLinkNames() const
00427 {
00428 if(!active_)
00429 {
00430 ROS_ERROR("kinematics not active");
00431 }
00432 return fk_solver_info_.link_names;
00433 }
00434
00435 }