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