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 <arm_kinematics_constraint_aware/arm_kinematics_constraint_aware_utils.h>
00034
00035 namespace arm_kinematics_constraint_aware
00036 {
00037 static const double IK_DEFAULT_TIMEOUT = 10.0;
00038 static const int NUM_JOINTS_ARM7DOF = 7;
00039 static const double IK_EPS = 1e-5;
00040 bool loadRobotModel(ros::NodeHandle node_handle, urdf::Model &robot_model, std::string &root_name, std::string &tip_name, std::string &xml_string)
00041 {
00042 std::string urdf_xml,full_urdf_xml;
00043 node_handle.param("urdf_xml",urdf_xml,std::string("robot_description"));
00044 node_handle.searchParam(urdf_xml,full_urdf_xml);
00045 TiXmlDocument xml;
00046 ROS_DEBUG("Reading xml file from parameter server\n");
00047 std::string result;
00048 if (node_handle.getParam(full_urdf_xml, result))
00049 xml.Parse(result.c_str());
00050 else
00051 {
00052 ROS_FATAL("Could not load the xml from parameter server: %s\n", urdf_xml.c_str());
00053 return false;
00054 }
00055 xml_string = result;
00056 TiXmlElement *root_element = xml.RootElement();
00057 TiXmlElement *root = xml.FirstChildElement("robot");
00058 if (!root || !root_element)
00059 {
00060 ROS_FATAL("Could not parse the xml from %s\n", urdf_xml.c_str());
00061 exit(1);
00062 }
00063 robot_model.initXml(root);
00064 if (!node_handle.getParam("root_name", root_name)){
00065 ROS_FATAL("No root name found on parameter server");
00066 return false;
00067 }
00068 if (!node_handle.getParam("tip_name", tip_name)){
00069 ROS_FATAL("No tip name found on parameter server");
00070 return false;
00071 }
00072 return true;
00073 }
00074
00075 bool getKDLChain(const std::string &xml_string, const std::string &root_name, const std::string &tip_name, KDL::Chain &kdl_chain)
00076 {
00077
00078 KDL::Tree tree;
00079 if (!kdl_parser::treeFromString(xml_string, tree))
00080 {
00081 ROS_ERROR("Could not initialize tree object");
00082 return false;
00083 }
00084 if (!tree.getChain(root_name, tip_name, kdl_chain))
00085 {
00086 ROS_ERROR("Could not initialize chain object");
00087 return false;
00088 }
00089 return true;
00090 }
00091
00092 bool getKDLTree(const std::string &xml_string, const std::string &root_name, const std::string &tip_name, KDL::Tree &kdl_tree)
00093 {
00094
00095 if (!kdl_parser::treeFromString(xml_string, kdl_tree))
00096 {
00097 ROS_ERROR("Could not initialize tree object");
00098 return false;
00099 }
00100 return true;
00101 }
00102
00103
00104 Eigen::Matrix4f KDLToEigenMatrix(const KDL::Frame &p)
00105 {
00106 Eigen::Matrix4f b = Eigen::Matrix4f::Identity();
00107 for(int i=0; i < 3; i++)
00108 {
00109 for(int j=0; j<3; j++)
00110 {
00111 b(i,j) = p.M(i,j);
00112 }
00113 b(i,3) = p.p(i);
00114 }
00115 return b;
00116 }
00117
00118 double computeEuclideanDistance(const std::vector<double> &array_1, const KDL::JntArray &array_2)
00119 {
00120 double distance = 0.0;
00121 for(int i=0; i< (int) array_1.size(); i++)
00122 {
00123 distance += (array_1[i] - array_2(i))*(array_1[i] - array_2(i));
00124 }
00125 return sqrt(distance);
00126 }
00127
00128
00129 double distance(const urdf::Pose &transform)
00130 {
00131 return sqrt(transform.position.x*transform.position.x+transform.position.y*transform.position.y+transform.position.z*transform.position.z);
00132 }
00133
00134
00135 bool solveQuadratic(const double &a, const double &b, const double &c, double *x1, double *x2)
00136 {
00137 double discriminant = b*b-4*a*c;
00138 if(fabs(a) < IK_EPS)
00139 {
00140 *x1 = -c/b;
00141 *x2 = *x1;
00142 return true;
00143 }
00144
00145 if (discriminant >= 0)
00146 {
00147 *x1 = (-b + sqrt(discriminant))/(2*a);
00148 *x2 = (-b - sqrt(discriminant))/(2*a);
00149 return true;
00150 }
00151 else if(fabs(discriminant) < IK_EPS)
00152 {
00153 *x1 = -b/(2*a);
00154 *x2 = -b/(2*a);
00155 return true;
00156 }
00157 else
00158 {
00159 *x1 = -b/(2*a);
00160 *x2 = -b/(2*a);
00161 return false;
00162 }
00163 }
00164
00165 Eigen::Matrix4f matrixInverse(const Eigen::Matrix4f &g)
00166 {
00167 Eigen::Matrix4f result = g;
00168 Eigen::Matrix3f Rt = Eigen::Matrix3f::Identity();
00169
00170 Eigen::Vector3f p = Eigen::Vector3f::Zero(3);
00171 Eigen::Vector3f pinv = Eigen::Vector3f::Zero(3);
00172
00173 Rt(0,0) = g(0,0);
00174 Rt(1,1) = g(1,1);
00175 Rt(2,2) = g(2,2);
00176
00177 Rt(0,1) = g(1,0);
00178 Rt(1,0) = g(0,1);
00179
00180 Rt(0,2) = g(2,0);
00181 Rt(2,0) = g(0,2);
00182
00183 Rt(1,2) = g(2,1);
00184 Rt(2,1) = g(1,2);
00185
00186 p(0) = g(0,3);
00187 p(1) = g(1,3);
00188 p(2) = g(2,3);
00189
00190 pinv = -Rt*p;
00191
00192 result(0,0) = g(0,0);
00193 result(1,1) = g(1,1);
00194 result(2,2) = g(2,2);
00195
00196 result(0,1) = g(1,0);
00197 result(1,0) = g(0,1);
00198
00199 result(0,2) = g(2,0);
00200 result(2,0) = g(0,2);
00201
00202 result(1,2) = g(2,1);
00203 result(2,1) = g(1,2);
00204
00205 result(0,3) = pinv(0);
00206 result(1,3) = pinv(1);
00207 result(2,3) = pinv(2);
00208
00209 return result;
00210 }
00211
00212
00213 bool solveCosineEqn(const double &a, const double &b, const double &c, double &soln1, double &soln2)
00214 {
00215 double theta1 = atan2(b,a);
00216 double denom = sqrt(a*a+b*b);
00217
00218 if(fabs(denom) < IK_EPS)
00219 {
00220 #ifdef DEBUG
00221 std::cout << "denom: " << denom << std::endl;
00222 #endif
00223 return false;
00224 }
00225 double rhs_ratio = c/denom;
00226 if(rhs_ratio < -1 || rhs_ratio > 1)
00227 {
00228 #ifdef DEBUG
00229 std::cout << "rhs_ratio: " << rhs_ratio << std::endl;
00230 #endif
00231 return false;
00232 }
00233 double acos_term = acos(rhs_ratio);
00234 soln1 = theta1 + acos_term;
00235 soln2 = theta1 - acos_term;
00236
00237 return true;
00238 }
00239
00240 bool checkJointNames(const std::vector<std::string> &joint_names,
00241 const kinematics_msgs::KinematicSolverInfo &chain_info)
00242 {
00243 for(unsigned int i=0; i < chain_info.joint_names.size(); i++)
00244 {
00245 int index = -1;
00246 for(unsigned int j=0; j < joint_names.size(); j++)
00247 {
00248 if(chain_info.joint_names[i] == joint_names[j])
00249 {
00250 index = j;
00251 break;
00252 }
00253 }
00254 if(index < 0)
00255 {
00256 ROS_ERROR("Joint state does not contain joint state for %s.",chain_info.joint_names[i].c_str());
00257 return false;
00258 }
00259 }
00260 return true;
00261 }
00262
00263 bool checkLinkNames(const std::vector<std::string> &link_names,
00264 const kinematics_msgs::KinematicSolverInfo &chain_info)
00265 {
00266 if(link_names.empty())
00267 return false;
00268 for(unsigned int i=0; i < link_names.size(); i++)
00269 {
00270 if(!checkLinkName(link_names[i],chain_info))
00271 return false;
00272 }
00273 return true;
00274 }
00275
00276 bool checkLinkName(const std::string &link_name,
00277 const kinematics_msgs::KinematicSolverInfo &chain_info)
00278 {
00279 for(unsigned int i=0; i < chain_info.link_names.size(); i++)
00280 {
00281 if(link_name == chain_info.link_names[i])
00282 return true;
00283 }
00284 return false;
00285 }
00286
00287 bool checkRobotState(motion_planning_msgs::RobotState &robot_state,
00288 const kinematics_msgs::KinematicSolverInfo &chain_info)
00289 {
00290 if((int) robot_state.joint_state.position.size() != (int) robot_state.joint_state.name.size())
00291 {
00292 ROS_ERROR("Number of joints in robot_state.joint_state does not match number of positions in robot_state.joint_state");
00293 return false;
00294 }
00295 if(!checkJointNames(robot_state.joint_state.name,chain_info))
00296 {
00297 ROS_ERROR("Robot state must contain joint state for every joint in the kinematic chain");
00298 return false;
00299 }
00300 return true;
00301 }
00302
00303 bool checkFKService(kinematics_msgs::GetPositionFK::Request &request,
00304 kinematics_msgs::GetPositionFK::Response &response,
00305 const kinematics_msgs::KinematicSolverInfo &chain_info)
00306 {
00307 if(!checkLinkNames(request.fk_link_names,chain_info))
00308 {
00309 ROS_ERROR("Link name in service request does not match links that kinematics can provide solutions for.");
00310 response.error_code.val = response.error_code.INVALID_LINK_NAME;
00311 return false;
00312 }
00313 if(!checkRobotState(request.robot_state,chain_info))
00314 {
00315 response.error_code.val = response.error_code.INVALID_ROBOT_STATE;
00316 return false;
00317 }
00318 return true;
00319 }
00320
00321 bool checkIKService(kinematics_msgs::GetPositionIK::Request &request,
00322 kinematics_msgs::GetPositionIK::Response &response,
00323 const kinematics_msgs::KinematicSolverInfo &chain_info)
00324 {
00325 if(!checkLinkName(request.ik_request.ik_link_name,chain_info))
00326 {
00327 ROS_ERROR("Link name in service request does not match links that kinematics can provide solutions for.");
00328 response.error_code.val = response.error_code.INVALID_LINK_NAME;
00329 return false;
00330 }
00331 if(!checkRobotState(request.ik_request.ik_seed_state,chain_info))
00332 {
00333 response.error_code.val = response.error_code.INVALID_ROBOT_STATE;
00334 return false;
00335 }
00336 if(request.timeout <= ros::Duration(0.0))
00337 {
00338 response.error_code.val = response.error_code.INVALID_TIMEOUT;
00339 return false;
00340 }
00341 return true;
00342 }
00343
00344 bool checkConstraintAwareIKService(kinematics_msgs::GetConstraintAwarePositionIK::Request &request,
00345 kinematics_msgs::GetConstraintAwarePositionIK::Response &response,
00346 const kinematics_msgs::KinematicSolverInfo &chain_info)
00347 {
00348 if(!checkLinkName(request.ik_request.ik_link_name,chain_info))
00349 {
00350 ROS_ERROR("Link name in service request does not match links that kinematics can provide solutions for.");
00351 response.error_code.val = response.error_code.INVALID_LINK_NAME;
00352 return false;
00353 }
00354 if(!checkRobotState(request.ik_request.ik_seed_state,chain_info))
00355 {
00356 response.error_code.val = response.error_code.INVALID_ROBOT_STATE;
00357 return false;
00358 }
00359 if(request.timeout <= ros::Duration(0.0))
00360 {
00361 response.error_code.val = response.error_code.INVALID_TIMEOUT;
00362 return false;
00363 }
00364 return true;
00365 }
00366
00367 bool convertPoseToRootFrame(const geometry_msgs::PoseStamped &pose_msg,
00368 KDL::Frame &pose_kdl,
00369 const std::string &root_frame,
00370 const tf::TransformListener &tf)
00371 {
00372 geometry_msgs::PoseStamped pose_stamped;
00373 if(!convertPoseToRootFrame(pose_msg, pose_stamped, root_frame,tf))
00374 return false;
00375 tf::PoseMsgToKDL(pose_stamped.pose, pose_kdl);
00376 return true;
00377 }
00378
00379
00380 bool convertPoseToRootFrame(const geometry_msgs::PoseStamped &pose_msg,
00381 geometry_msgs::PoseStamped &pose_msg_out,
00382 const std::string &root_frame,
00383 const tf::TransformListener &tf)
00384 {
00385 geometry_msgs::PoseStamped pose_msg_in = pose_msg;
00386 ROS_DEBUG("Request:\nframe_id: %s\nPosition: %f %f %f\n:Orientation: %f %f %f %f\n",
00387 pose_msg_in.header.frame_id.c_str(),
00388 pose_msg_in.pose.position.x,
00389 pose_msg_in.pose.position.y,
00390 pose_msg_in.pose.position.z,
00391 pose_msg_in.pose.orientation.x,
00392 pose_msg_in.pose.orientation.y,
00393 pose_msg_in.pose.orientation.z,
00394 pose_msg_in.pose.orientation.w);
00395 tf::Stamped<tf::Pose> pose_stamped;
00396 poseStampedMsgToTF(pose_msg_in, pose_stamped);
00397
00398 if (!tf.canTransform(root_frame, pose_stamped.frame_id_, pose_stamped.stamp_))
00399 {
00400 std::string err;
00401 if (tf.getLatestCommonTime(pose_stamped.frame_id_, root_frame, pose_stamped.stamp_, &err) != tf::NO_ERROR)
00402 {
00403 ROS_ERROR("Cannot transform from '%s' to '%s'. TF said: %s",pose_stamped.frame_id_.c_str(),root_frame.c_str(), err.c_str());
00404 return false;
00405 }
00406 }
00407 try
00408 {
00409 tf.transformPose(root_frame, pose_stamped, pose_stamped);
00410 }
00411 catch(...)
00412 {
00413 ROS_ERROR("Cannot transform from '%s' to '%s'",pose_stamped.frame_id_.c_str(),root_frame.c_str());
00414 return false;
00415 }
00416 tf::poseStampedTFToMsg(pose_stamped,pose_msg_out);
00417 return true;
00418 }
00419
00420
00421
00422 int getJointIndex(const std::string &name,
00423 const kinematics_msgs::KinematicSolverInfo &chain_info)
00424 {
00425 for(unsigned int i=0; i < chain_info.joint_names.size(); i++)
00426 {
00427 if(chain_info.joint_names[i] == name)
00428 {
00429 return i;
00430 }
00431 }
00432 return -1;
00433 }
00434
00435 void getKDLChainInfo(const KDL::Chain &chain,
00436 kinematics_msgs::KinematicSolverInfo &chain_info)
00437 {
00438 int i=0;
00439 while(i < (int)chain.getNrOfSegments())
00440 {
00441 chain_info.link_names.push_back(chain.getSegment(i).getName());
00442 i++;
00443 }
00444 }
00445
00446 int getKDLSegmentIndex(const KDL::Chain &chain,
00447 const std::string &name)
00448 {
00449 int i=0;
00450 while(i < (int)chain.getNrOfSegments())
00451 {
00452 if(chain.getSegment(i).getName() == name)
00453 {
00454 return i+1;
00455 }
00456 i++;
00457 }
00458 return -1;
00459 }
00460
00461 void reorderJointState(sensor_msgs::JointState &joint_state,
00462 const kinematics_msgs::KinematicSolverInfo &chain_info)
00463 {
00464 sensor_msgs::JointState tmp_state = joint_state;
00465 for(unsigned int i=0; i < joint_state.position.size(); i++)
00466 {
00467 int tmp_index = getJointIndex(joint_state.name[i],chain_info);
00468 if(tmp_index >=0)
00469 {
00470 tmp_state.position[tmp_index] = joint_state.position[i];
00471 tmp_state.name[tmp_index] = joint_state.name[i];
00472 }
00473 }
00474 joint_state = tmp_state;
00475 }
00476
00477
00478 bool getChainInfo(const std::string &name,
00479 kinematics_msgs::KinematicSolverInfo &chain_info)
00480 {
00481 std::string urdf_xml, full_urdf_xml, root_name, tip_name;
00482 ros::NodeHandle node_handle;
00483
00484 ros::NodeHandle private_handle("~"+name);
00485 node_handle.param("urdf_xml",urdf_xml,std::string("robot_description"));
00486 node_handle.searchParam(urdf_xml,full_urdf_xml);
00487 ROS_DEBUG("Reading xml file from parameter server");
00488 std::string result;
00489 if (!node_handle.getParam(full_urdf_xml, result))
00490 {
00491 ROS_FATAL("Could not load the xml from parameter server: %s", urdf_xml.c_str());
00492 return false;
00493 }
00494
00495
00496 if (!private_handle.getParam("root_name", root_name))
00497 {
00498 ROS_FATAL("GenericIK: No root name found on parameter server");
00499 return false;
00500 }
00501 if (!private_handle.getParam("tip_name", tip_name))
00502 {
00503 ROS_FATAL("GenericIK: No tip name found on parameter server");
00504 return false;
00505 }
00506 urdf::Model robot_model;
00507 KDL::Tree tree;
00508 if (!robot_model.initString(result))
00509 {
00510 ROS_FATAL("Could not initialize robot model");
00511 return -1;
00512 }
00513 if (!getChainInfoFromRobotModel(robot_model,root_name,tip_name,chain_info))
00514 {
00515 ROS_FATAL("Could not read information about the joints");
00516 return false;
00517 }
00518 return true;
00519 }
00520
00521 bool getChainInfoFromRobotModel(urdf::Model &robot_model,
00522 const std::string &root_name,
00523 const std::string &tip_name,
00524 kinematics_msgs::KinematicSolverInfo &chain_info)
00525 {
00526
00527 boost::shared_ptr<const urdf::Link> link = robot_model.getLink(tip_name);
00528 boost::shared_ptr<const urdf::Joint> joint;
00529 while (link && link->name != root_name)
00530 {
00531 joint = robot_model.getJoint(link->parent_joint->name);
00532 if (!joint)
00533 {
00534 ROS_ERROR("Could not find joint: %s",link->parent_joint->name.c_str());
00535 return false;
00536 }
00537 if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
00538 {
00539 float lower, upper;
00540 int hasLimits;
00541 if ( joint->type != urdf::Joint::CONTINUOUS )
00542 {
00543 lower = joint->limits->lower;
00544 upper = joint->limits->upper;
00545 hasLimits = 1;
00546 }
00547 else
00548 {
00549 lower = -M_PI;
00550 upper = M_PI;
00551 hasLimits = 0;
00552 }
00553 chain_info.joint_names.push_back(joint->name);
00554 motion_planning_msgs::JointLimits limits;
00555 limits.joint_name = joint->name;
00556 limits.has_position_limits = hasLimits;
00557 limits.min_position = lower;
00558 limits.max_position = upper;
00559 chain_info.limits.push_back(limits);
00560 }
00561 link = robot_model.getLink(link->getParent()->name);
00562 }
00563 link = robot_model.getLink(tip_name);
00564 if(link)
00565 chain_info.link_names.push_back(tip_name);
00566
00567 std::reverse(chain_info.limits.begin(),chain_info.limits.end());
00568 std::reverse(chain_info.joint_names.begin(),chain_info.joint_names.end());
00569
00570 return true;
00571 }
00572
00573 motion_planning_msgs::ArmNavigationErrorCodes kinematicsErrorCodeToMotionPlanningErrorCode(const int &kinematics_error_code)
00574 {
00575 motion_planning_msgs::ArmNavigationErrorCodes error_code;
00576
00577 if(kinematics_error_code == kinematics::SUCCESS)
00578 error_code.val = error_code.SUCCESS;
00579 else if(kinematics_error_code == kinematics::TIMED_OUT)
00580 error_code.val = error_code.TIMED_OUT;
00581 else if(kinematics_error_code == kinematics::NO_IK_SOLUTION)
00582 error_code.val = error_code.NO_IK_SOLUTION;
00583 else if(kinematics_error_code == kinematics::FRAME_TRANSFORM_FAILURE)
00584 error_code.val = error_code.FRAME_TRANSFORM_FAILURE;
00585 else if(kinematics_error_code == kinematics::IK_LINK_INVALID)
00586 error_code.val = error_code.INVALID_LINK_NAME;
00587 else if(kinematics_error_code == kinematics::IK_LINK_IN_COLLISION)
00588 error_code.val = error_code.IK_LINK_IN_COLLISION;
00589 else if(kinematics_error_code == kinematics::STATE_IN_COLLISION)
00590 error_code.val = error_code.COLLISION_CONSTRAINTS_VIOLATED;
00591 else if(kinematics_error_code == kinematics::INVALID_LINK_NAME)
00592 error_code.val = error_code.INVALID_LINK_NAME;
00593 else if(kinematics_error_code == kinematics::GOAL_CONSTRAINTS_VIOLATED)
00594 error_code.val = error_code.GOAL_CONSTRAINTS_VIOLATED;
00595 else if(kinematics_error_code == kinematics::INACTIVE)
00596 error_code.val = 0;
00597 return error_code;
00598 }
00599
00600 }