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 #include <moveit/pr2_arm_kinematics/pr2_arm_ik_solver.h>
00034
00035 using namespace Eigen;
00036 using namespace pr2_arm_kinematics;
00037
00038 PR2ArmIKSolver::PR2ArmIKSolver(const urdf::Model &robot_model,
00039 const std::string &root_frame_name,
00040 const std::string &tip_frame_name,
00041 const double &search_discretization_angle,
00042 const int &free_angle):ChainIkSolverPos()
00043 {
00044 search_discretization_angle_ = search_discretization_angle;
00045 free_angle_ = free_angle;
00046 root_frame_name_ = root_frame_name;
00047 if(!pr2_arm_ik_.init(robot_model, root_frame_name, tip_frame_name))
00048 active_ = false;
00049 else
00050 active_ = true;
00051 }
00052
00053 void PR2ArmIKSolver::getSolverInfo(moveit_msgs::KinematicSolverInfo &response)
00054 {
00055 pr2_arm_ik_.getSolverInfo(response);
00056 }
00057
00058 int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init,
00059 const KDL::Frame& p_in,
00060 KDL::JntArray &q_out)
00061 {
00062 Eigen::Matrix4f b = KDLToEigenMatrix(p_in);
00063 std::vector<std::vector<double> > solution_ik;
00064 if(free_angle_ == 0)
00065 {
00066 ROS_DEBUG("Solving with free angle: %d", free_angle_);
00067 pr2_arm_ik_.computeIKShoulderPan(b, q_init(0), solution_ik);
00068 }
00069 else
00070 {
00071 pr2_arm_ik_.computeIKShoulderRoll(b, q_init(2), solution_ik);
00072 }
00073
00074 if(solution_ik.empty())
00075 return -1;
00076
00077 double min_distance = 1e6;
00078 int min_index = -1;
00079
00080 for(int i=0; i< (int) solution_ik.size(); ++i)
00081 {
00082 ROS_DEBUG("Solution : %d", (int)solution_ik.size());
00083 for(int j=0; j < (int)solution_ik[i].size(); j++)
00084 {
00085 ROS_DEBUG("Joint %d: %f", j, solution_ik[i][j]);
00086 }
00087
00088 double tmp_distance = computeEuclideanDistance(solution_ik[i], q_init);
00089 if(tmp_distance < min_distance)
00090 {
00091 min_distance = tmp_distance;
00092 min_index = i;
00093 }
00094 }
00095
00096 if(min_index > -1)
00097 {
00098 q_out.resize((int)solution_ik[min_index].size());
00099 for(int i=0; i < (int)solution_ik[min_index].size(); ++i)
00100 {
00101 q_out(i) = solution_ik[min_index][i];
00102 }
00103 return 1;
00104 }
00105 else
00106 return -1;
00107 }
00108
00109 int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init,
00110 const KDL::Frame& p_in,
00111 std::vector<KDL::JntArray> &q_out)
00112 {
00113 Eigen::Matrix4f b = KDLToEigenMatrix(p_in);
00114 std::vector<std::vector<double> > solution_ik;
00115 KDL::JntArray q;
00116
00117 if(free_angle_ == 0)
00118 {
00119 pr2_arm_ik_.computeIKShoulderPan(b, q_init(0), solution_ik);
00120 }
00121 else
00122 {
00123 pr2_arm_ik_.computeIKShoulderRoll(b, q_init(2), solution_ik);
00124 }
00125
00126 if(solution_ik.empty())
00127 return -1;
00128
00129 q.resize(7);
00130 q_out.clear();
00131 for(int i=0; i< (int) solution_ik.size(); ++i)
00132 {
00133 for(int j=0; j < 7; j++)
00134 {
00135 q(j) = solution_ik[i][j];
00136 }
00137 q_out.push_back(q);
00138 }
00139 return 1;
00140 }
00141
00142 bool PR2ArmIKSolver::getCount(int &count,
00143 const int &max_count,
00144 const int &min_count)
00145 {
00146 if(count > 0)
00147 {
00148 if(-count >= min_count)
00149 {
00150 count = -count;
00151 return true;
00152 }
00153 else if(count+1 <= max_count)
00154 {
00155 count = count+1;
00156 return true;
00157 }
00158 else
00159 {
00160 return false;
00161 }
00162 }
00163 else
00164 {
00165 if(1-count <= max_count)
00166 {
00167 count = 1-count;
00168 return true;
00169 }
00170 else if(count-1 >= min_count)
00171 {
00172 count = count -1;
00173 return true;
00174 }
00175 else
00176 return false;
00177 }
00178 }
00179
00180 int PR2ArmIKSolver::CartToJntSearch(const KDL::JntArray& q_in,
00181 const KDL::Frame& p_in,
00182 std::vector<KDL::JntArray> &q_out,
00183 const double &timeout)
00184 {
00185 KDL::JntArray q_init = q_in;
00186 double initial_guess = q_init(free_angle_);
00187
00188 ros::WallTime start_time = ros::WallTime::now();
00189 double loop_time = 0;
00190 int count = 0;
00191
00192 int num_positive_increments = (int)((pr2_arm_ik_.solver_info_.limits[free_angle_].max_position-initial_guess)/search_discretization_angle_);
00193 int num_negative_increments = (int)((initial_guess-pr2_arm_ik_.solver_info_.limits[free_angle_].min_position)/search_discretization_angle_);
00194 ROS_DEBUG("positive increments, negative increments: %d %d", num_positive_increments, num_negative_increments);
00195 while(loop_time < timeout)
00196 {
00197 if(CartToJnt(q_init, p_in, q_out) > 0)
00198 return 1;
00199 if(!getCount(count, num_positive_increments, -num_negative_increments))
00200 return -1;
00201 q_init(free_angle_) = initial_guess + search_discretization_angle_ * count;
00202 ROS_DEBUG("%d, %f",count, q_init(free_angle_));
00203 loop_time = (ros::WallTime::now()-start_time).toSec();
00204 }
00205 if(loop_time >= timeout)
00206 {
00207 ROS_DEBUG("IK Timed out in %f seconds", timeout);
00208 return TIMED_OUT;
00209 }
00210 else
00211 {
00212 ROS_DEBUG("No IK solution was found");
00213 return NO_IK_SOLUTION;
00214 }
00215 return NO_IK_SOLUTION;
00216 }
00217
00218 int PR2ArmIKSolver::CartToJntSearch(const KDL::JntArray& q_in,
00219 const KDL::Frame& p_in,
00220 KDL::JntArray &q_out,
00221 const double &timeout,
00222 const double& consistency_limit)
00223 {
00224 moveit_msgs::MoveItErrorCodes error_code;
00225 static kinematics::KinematicsBase::IKCallbackFn solution_callback = 0;
00226 return CartToJntSearch(q_in, p_in, q_out, timeout, true, consistency_limit, error_code, solution_callback);
00227 }
00228
00229 int PR2ArmIKSolver:: CartToJntSearch(const KDL::JntArray& q_in,
00230 const KDL::Frame& p_in,
00231 KDL::JntArray &q_out,
00232 const double &timeout,
00233 moveit_msgs::MoveItErrorCodes &error_code,
00234 const kinematics::KinematicsBase::IKCallbackFn &solution_callback)
00235 {
00236 return CartToJntSearch(q_in, p_in, q_out, timeout, false, 0.0, error_code, solution_callback);
00237 }
00238
00239
00240 int PR2ArmIKSolver::CartToJntSearch(const KDL::JntArray& q_in,
00241 const KDL::Frame& p_in,
00242 KDL::JntArray &q_out,
00243 const double &timeout,
00244 const double& consistency_limit,
00245 moveit_msgs::MoveItErrorCodes &error_code,
00246 const kinematics::KinematicsBase::IKCallbackFn &solution_callback)
00247 {
00248 return CartToJntSearch(q_in, p_in, q_out, timeout, true, consistency_limit, error_code, solution_callback);
00249 }
00250
00251 int PR2ArmIKSolver::CartToJntSearch(const KDL::JntArray& q_in,
00252 const KDL::Frame& p_in,
00253 KDL::JntArray &q_out,
00254 const double &timeout,
00255 bool use_consistency_limit,
00256 const double &max_consistency,
00257 moveit_msgs::MoveItErrorCodes &error_code,
00258 const kinematics::KinematicsBase::IKCallbackFn &solution_callback)
00259 {
00260 KDL::JntArray q_init = q_in;
00261 double initial_guess = q_init(free_angle_);
00262
00263 ros::WallTime start_time = ros::WallTime::now();
00264 double loop_time = 0;
00265 int count = 0;
00266
00267 double max_limit, min_limit;
00268 if(use_consistency_limit)
00269 {
00270 max_limit = fmin(pr2_arm_ik_.solver_info_.limits[free_angle_].max_position, initial_guess+max_consistency);
00271 min_limit = fmax(pr2_arm_ik_.solver_info_.limits[free_angle_].min_position, initial_guess-max_consistency);
00272 }
00273 else
00274 {
00275 max_limit = pr2_arm_ik_.solver_info_.limits[free_angle_].max_position;
00276 min_limit = pr2_arm_ik_.solver_info_.limits[free_angle_].min_position;
00277 }
00278
00279 int num_positive_increments = (int)((max_limit-initial_guess)/search_discretization_angle_);
00280 int num_negative_increments = (int)((initial_guess-min_limit)/search_discretization_angle_);
00281
00282 if(use_consistency_limit)
00283 {
00284 ROS_DEBUG("Consistency[Joint: %d]: Initial guess %f, consistency %f", free_angle_, initial_guess, max_consistency);
00285 ROS_DEBUG("Max limit %f = max(%f, %f)", max_limit, pr2_arm_ik_.solver_info_.limits[free_angle_].max_position, initial_guess+max_consistency);
00286 ROS_DEBUG("Min limit %f = min(%f, %f)", min_limit, pr2_arm_ik_.solver_info_.limits[free_angle_].min_position, initial_guess-max_consistency);
00287 }
00288 else
00289 {
00290 ROS_DEBUG("Consistency[Joint: %d]: Initial guess %f", free_angle_, initial_guess);
00291 ROS_DEBUG("Max limit %f", max_limit);
00292 ROS_DEBUG("Min limit %f", min_limit);
00293 }
00294
00295 ROS_DEBUG("positive increments, negative increments: %d %d", num_positive_increments, num_negative_increments);
00296
00297 unsigned int testnum = 0;
00298 geometry_msgs::Pose ik_pose_msg;
00299 tf::poseKDLToMsg(p_in, ik_pose_msg);
00300
00301 ros::WallTime s = ros::WallTime::now();
00302
00303 while(loop_time < timeout)
00304 {
00305 testnum++;
00306 if(CartToJnt(q_init, p_in, q_out) > 0)
00307 {
00308 if(solution_callback)
00309 {
00310 std::vector<double> ik_solution(7,0.0);
00311 for(int i=0; i < 7; ++i)
00312 ik_solution[i] = q_out(i);
00313
00314 solution_callback(ik_pose_msg, ik_solution, error_code);
00315 if(error_code.val == error_code.SUCCESS)
00316 {
00317 ROS_DEBUG("Difference is %f %f", q_in(free_angle_), q_out(free_angle_));
00318 ROS_DEBUG("Success with %d in %f", testnum, (ros::WallTime::now()-s).toSec());
00319 return 1;
00320 }
00321 }
00322 else
00323 {
00324 error_code.val = error_code.SUCCESS;
00325 return 1;
00326 }
00327 }
00328 if(!getCount(count, num_positive_increments, -num_negative_increments))
00329 {
00330 ROS_DEBUG("Failure with %d in %f", testnum, (ros::WallTime::now()-s).toSec());
00331 error_code.val = error_code.NO_IK_SOLUTION;
00332 return -1;
00333 }
00334 q_init(free_angle_) = initial_guess + search_discretization_angle_ * count;
00335 ROS_DEBUG("Redundancy search, index:%d, free angle value: %f", count, q_init(free_angle_));
00336 loop_time = (ros::WallTime::now()-start_time).toSec();
00337 }
00338 if(loop_time >= timeout)
00339 {
00340 ROS_DEBUG("IK Timed out in %f seconds",timeout);
00341 error_code.val = error_code.TIMED_OUT;
00342 }
00343 else
00344 {
00345 error_code.val = error_code.NO_IK_SOLUTION;
00346 }
00347 return -1;
00348 }
00349
00350
00351 std::string PR2ArmIKSolver::getFrameId()
00352 {
00353 return root_frame_name_;
00354 }