$search
00001 //Software License Agreement (BSD License) 00002 00003 //Copyright (c) 2008, Willow Garage, Inc. 00004 //All rights reserved. 00005 00006 //Redistribution and use in source and binary forms, with or without 00007 //modification, are permitted provided that the following conditions 00008 //are met: 00009 00010 // * Redistributions of source code must retain the above copyright 00011 // notice, this list of conditions and the following disclaimer. 00012 // * Redistributions in binary form must reproduce the above 00013 // copyright notice, this list of conditions and the following 00014 // disclaimer in the documentation and/or other materials provided 00015 // with the distribution. 00016 // * Neither the name of Willow Garage, Inc. nor the names of its 00017 // contributors may be used to endorse or promote products derived 00018 // from this software without specific prior written permission. 00019 00020 //THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00021 //"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00022 //LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00023 //FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00024 //COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00025 //INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00026 //BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00027 //LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00028 //CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00029 //LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00030 //ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00031 //POSSIBILITY OF SUCH DAMAGE. 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 // create robot chain from root to tip 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 // create robot chain from root to tip 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 //ROS_DEBUG("Discriminant: %f\n",discriminant); 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) // should never happen, wouldn't make sense but make sure it is checked nonetheless 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(arm_navigation_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; // segment number 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; // segment number 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 // arm_kinematics_constraint_aware::reorderJointState(request.ik_request.ik_seed_state,chain_info_); 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 // Get Root and Tip From Parameter Service 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 // get joint maxs and mins 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 arm_navigation_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 arm_navigation_msgs::ArmNavigationErrorCodes kinematicsErrorCodeToMotionPlanningErrorCode(const int &kinematics_error_code) 00574 { 00575 arm_navigation_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 }