45 std::string urdf_xml,full_urdf_xml;
46 node_handle.
param(
"urdf_xml",urdf_xml,std::string(
"robot_description"));
49 ROS_DEBUG(
"Reading xml file from parameter server\n");
51 if (node_handle.
getParam(full_urdf_xml, result))
52 xml.Parse(result.c_str());
55 ROS_FATAL(
"Could not load the xml from parameter server: %s\n", urdf_xml.c_str());
59 TiXmlElement *root_element = xml.RootElement();
60 TiXmlElement *root = xml.FirstChildElement(
"robot");
61 if (!root || !root_element)
63 ROS_FATAL(
"Could not parse the xml from %s\n", urdf_xml.c_str());
70 bool getKDLChain(
const std::string &xml_string,
const std::string &root_name,
const std::string &tip_name, KDL::Chain &kdl_chain)
76 ROS_ERROR(
"Could not initialize tree object");
79 if (!tree.getChain(root_name, tip_name, kdl_chain))
81 ROS_ERROR_STREAM(
"Could not initialize chain object for base " << root_name <<
" tip " << tip_name);
87 bool getKDLTree(
const std::string &xml_string,
const std::string &root_name,
const std::string &tip_name, KDL::Tree &kdl_tree)
92 ROS_ERROR(
"Could not initialize tree object");
101 Eigen::Matrix4f b = Eigen::Matrix4f::Identity();
102 for(
int i=0; i < 3; i++)
104 for(
int j=0; j<3; j++)
116 for(
int i=0; i< (int) array_1.size(); i++)
118 distance += (array_1[i] - array_2(i))*(array_1[i] - array_2(i));
124 double distance(
const urdf::Pose &transform)
130 bool solveQuadratic(
const double &a,
const double &b,
const double &c,
double *x1,
double *x2)
132 double discriminant = b*b-4*a*c;
140 if (discriminant >= 0)
142 *x1 = (-b + sqrt(discriminant))/(2*a);
143 *x2 = (-b - sqrt(discriminant))/(2*a);
146 else if(fabs(discriminant) <
IK_EPS)
162 Eigen::Matrix4f result = g;
163 Eigen::Matrix3f Rt = Eigen::Matrix3f::Identity();
165 Eigen::Vector3f p = Eigen::Vector3f::Zero(3);
166 Eigen::Vector3f pinv = Eigen::Vector3f::Zero(3);
187 result(0,0) = g(0,0);
188 result(1,1) = g(1,1);
189 result(2,2) = g(2,2);
191 result(0,1) = g(1,0);
192 result(1,0) = g(0,1);
194 result(0,2) = g(2,0);
195 result(2,0) = g(0,2);
197 result(1,2) = g(2,1);
198 result(2,1) = g(1,2);
200 result(0,3) = pinv(0);
201 result(1,3) = pinv(1);
202 result(2,3) = pinv(2);
208 bool solveCosineEqn(
const double &a,
const double &b,
const double &c,
double &soln1,
double &soln2)
210 double theta1 = atan2(b,a);
211 double denom = sqrt(a*a+b*b);
216 std::cout <<
"denom: " << denom << std::endl;
220 double rhs_ratio = c/denom;
221 if(rhs_ratio < -1 || rhs_ratio > 1)
224 std::cout <<
"rhs_ratio: " << rhs_ratio << std::endl;
228 double acos_term = acos(rhs_ratio);
229 soln1 = theta1 + acos_term;
230 soln2 = theta1 - acos_term;
237 const moveit_msgs::KinematicSolverInfo &chain_info)
239 for(
unsigned int i=0; i < chain_info.joint_names.size(); i++)
242 for(
unsigned int j=0; j < joint_names.size(); j++)
244 if(chain_info.joint_names[i] == joint_names[j])
252 ROS_ERROR(
"Joint state does not contain joint state for %s.",chain_info.joint_names[i].c_str());
260 const moveit_msgs::KinematicSolverInfo &chain_info)
262 if(link_names.empty())
264 for(
unsigned int i=0; i < link_names.size(); i++)
273 const moveit_msgs::KinematicSolverInfo &chain_info)
275 for(
unsigned int i=0; i < chain_info.link_names.size(); i++)
277 if(link_name == chain_info.link_names[i])
284 const moveit_msgs::KinematicSolverInfo &chain_info)
286 if((
int) robot_state.joint_state.position.size() != (
int) robot_state.joint_state.name.size())
288 ROS_ERROR(
"Number of joints in robot_state.joint_state does not match number of positions in robot_state.joint_state");
293 ROS_ERROR(
"Robot state must contain joint state for every joint in the kinematic chain");
300 moveit_msgs::GetPositionFK::Response &response,
301 const moveit_msgs::KinematicSolverInfo &chain_info)
305 ROS_ERROR(
"Link name in service request does not match links that kinematics can provide solutions for.");
318 moveit_msgs::GetPositionIK::Response &response,
319 const moveit_msgs::KinematicSolverInfo &chain_info)
321 if(!
checkLinkName(request.ik_request.ik_link_name,chain_info))
323 ROS_ERROR(
"Link name in service request does not match links that kinematics can provide solutions for.");
341 KDL::Frame &pose_kdl,
342 const std::string &root_frame,
345 geometry_msgs::PoseStamped pose_stamped;
354 geometry_msgs::PoseStamped &pose_msg_out,
355 const std::string &root_frame,
358 geometry_msgs::PoseStamped pose_msg_in = pose_msg;
359 ROS_DEBUG(
"Request:\nframe_id: %s\nPosition: %f %f %f\n:Orientation: %f %f %f %f\n",
360 pose_msg_in.header.frame_id.c_str(),
361 pose_msg_in.pose.position.x,
362 pose_msg_in.pose.position.y,
363 pose_msg_in.pose.position.z,
364 pose_msg_in.pose.orientation.x,
365 pose_msg_in.pose.orientation.y,
366 pose_msg_in.pose.orientation.z,
367 pose_msg_in.pose.orientation.w);
368 pose_msg_out = pose_msg;
370 poseStampedMsgToTF(pose_msg_in, pose_stamped);
377 ROS_ERROR(
"pr2_arm_ik:: Cannot transform from '%s' to '%s'. TF said: %s",pose_stamped.
frame_id_.c_str(),root_frame.c_str(), err.c_str());
383 tf.transformPose(root_frame, pose_stamped, pose_stamped);
387 ROS_ERROR(
"pr2_arm_ik:: Cannot transform from '%s' to '%s'",pose_stamped.
frame_id_.c_str(),root_frame.c_str());
397 const moveit_msgs::KinematicSolverInfo &chain_info)
399 for(
unsigned int i=0; i < chain_info.joint_names.size(); i++)
401 if(chain_info.joint_names[i] == name)
410 moveit_msgs::KinematicSolverInfo &chain_info)
413 while(i < (
int)chain.getNrOfSegments())
415 chain_info.link_names.push_back(chain.getSegment(i).getName());
421 const std::string &name)
424 while(i < (
int)chain.getNrOfSegments())
426 if(chain.getSegment(i).getName() == name)