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