pr2_arm_kinematics_utils.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Sachin Chitta */
36 
39 
40 namespace pr2_arm_kinematics
41 {
42 static const double IK_DEFAULT_TIMEOUT = 10.0;
43 bool loadRobotModel(ros::NodeHandle node_handle, urdf::Model &robot_model, std::string &xml_string)
44 {
45  std::string urdf_xml,full_urdf_xml;
46  node_handle.param("urdf_xml",urdf_xml,std::string("robot_description"));
47  node_handle.searchParam(urdf_xml,full_urdf_xml);
48  TiXmlDocument xml;
49  ROS_DEBUG("Reading xml file from parameter server\n");
50  std::string result;
51  if (node_handle.getParam(full_urdf_xml, result))
52  xml.Parse(result.c_str());
53  else
54  {
55  ROS_FATAL("Could not load the xml from parameter server: %s\n", urdf_xml.c_str());
56  return false;
57  }
58  xml_string = result;
59  TiXmlElement *root_element = xml.RootElement();
60  TiXmlElement *root = xml.FirstChildElement("robot");
61  if (!root || !root_element)
62  {
63  ROS_FATAL("Could not parse the xml from %s\n", urdf_xml.c_str());
64  exit(1);
65  }
66  robot_model.initXml(root);
67  return true;
68 }
69 
70 bool getKDLChain(const std::string &xml_string, const std::string &root_name, const std::string &tip_name, KDL::Chain &kdl_chain)
71 {
72  // create robot chain from root to tip
73  KDL::Tree tree;
74  if (!kdl_parser::treeFromString(xml_string, tree))
75  {
76  ROS_ERROR("Could not initialize tree object");
77  return false;
78  }
79  if (!tree.getChain(root_name, tip_name, kdl_chain))
80  {
81  ROS_ERROR_STREAM("Could not initialize chain object for base " << root_name << " tip " << tip_name);
82  return false;
83  }
84  return true;
85 }
86 
87 bool getKDLTree(const std::string &xml_string, const std::string &root_name, const std::string &tip_name, KDL::Tree &kdl_tree)
88 {
89  // create robot chain from root to tip
90  if (!kdl_parser::treeFromString(xml_string, kdl_tree))
91  {
92  ROS_ERROR("Could not initialize tree object");
93  return false;
94  }
95  return true;
96 }
97 
98 
99 Eigen::Matrix4f KDLToEigenMatrix(const KDL::Frame &p)
100 {
101  Eigen::Matrix4f b = Eigen::Matrix4f::Identity();
102  for(int i=0; i < 3; i++)
103  {
104  for(int j=0; j<3; j++)
105  {
106  b(i,j) = p.M(i,j);
107  }
108  b(i,3) = p.p(i);
109  }
110  return b;
111 }
112 
113 double computeEuclideanDistance(const std::vector<double> &array_1, const KDL::JntArray &array_2)
114 {
115  double distance = 0.0;
116  for(int i=0; i< (int) array_1.size(); i++)
117  {
118  distance += (array_1[i] - array_2(i))*(array_1[i] - array_2(i));
119  }
120  return sqrt(distance);
121 }
122 
123 
124 double distance(const urdf::Pose &transform)
125 {
126  return sqrt(transform.position.x*transform.position.x+transform.position.y*transform.position.y+transform.position.z*transform.position.z);
127 }
128 
129 
130 bool solveQuadratic(const double &a, const double &b, const double &c, double *x1, double *x2)
131 {
132  double discriminant = b*b-4*a*c;
133  if(fabs(a) < IK_EPS)
134  {
135  *x1 = -c/b;
136  *x2 = *x1;
137  return true;
138  }
139  //ROS_DEBUG("Discriminant: %f\n",discriminant);
140  if (discriminant >= 0)
141  {
142  *x1 = (-b + sqrt(discriminant))/(2*a);
143  *x2 = (-b - sqrt(discriminant))/(2*a);
144  return true;
145  }
146  else if(fabs(discriminant) < IK_EPS)
147  {
148  *x1 = -b/(2*a);
149  *x2 = -b/(2*a);
150  return true;
151  }
152  else
153  {
154  *x1 = -b/(2*a);
155  *x2 = -b/(2*a);
156  return false;
157  }
158 }
159 
160 Eigen::Matrix4f matrixInverse(const Eigen::Matrix4f &g)
161 {
162  Eigen::Matrix4f result = g;
163  Eigen::Matrix3f Rt = Eigen::Matrix3f::Identity();
164 
165  Eigen::Vector3f p = Eigen::Vector3f::Zero(3);
166  Eigen::Vector3f pinv = Eigen::Vector3f::Zero(3);
167 
168  Rt(0,0) = g(0,0);
169  Rt(1,1) = g(1,1);
170  Rt(2,2) = g(2,2);
171 
172  Rt(0,1) = g(1,0);
173  Rt(1,0) = g(0,1);
174 
175  Rt(0,2) = g(2,0);
176  Rt(2,0) = g(0,2);
177 
178  Rt(1,2) = g(2,1);
179  Rt(2,1) = g(1,2);
180 
181  p(0) = g(0,3);
182  p(1) = g(1,3);
183  p(2) = g(2,3);
184 
185  pinv = -Rt*p;
186 
187  result(0,0) = g(0,0);
188  result(1,1) = g(1,1);
189  result(2,2) = g(2,2);
190 
191  result(0,1) = g(1,0);
192  result(1,0) = g(0,1);
193 
194  result(0,2) = g(2,0);
195  result(2,0) = g(0,2);
196 
197  result(1,2) = g(2,1);
198  result(2,1) = g(1,2);
199 
200  result(0,3) = pinv(0);
201  result(1,3) = pinv(1);
202  result(2,3) = pinv(2);
203 
204  return result;
205 }
206 
207 
208 bool solveCosineEqn(const double &a, const double &b, const double &c, double &soln1, double &soln2)
209 {
210  double theta1 = atan2(b,a);
211  double denom = sqrt(a*a+b*b);
212 
213  if(fabs(denom) < IK_EPS) // should never happen, wouldn't make sense but make sure it is checked nonetheless
214  {
215 #ifdef DEBUG
216  std::cout << "denom: " << denom << std::endl;
217 #endif
218  return false;
219  }
220  double rhs_ratio = c/denom;
221  if(rhs_ratio < -1 || rhs_ratio > 1)
222  {
223 #ifdef DEBUG
224  std::cout << "rhs_ratio: " << rhs_ratio << std::endl;
225 #endif
226  return false;
227  }
228  double acos_term = acos(rhs_ratio);
229  soln1 = theta1 + acos_term;
230  soln2 = theta1 - acos_term;
231 
232  return true;
233 }
234 
235 
236 bool checkJointNames(const std::vector<std::string> &joint_names,
237  const moveit_msgs::KinematicSolverInfo &chain_info)
238 {
239  for(unsigned int i=0; i < chain_info.joint_names.size(); i++)
240  {
241  int index = -1;
242  for(unsigned int j=0; j < joint_names.size(); j++)
243  {
244  if(chain_info.joint_names[i] == joint_names[j])
245  {
246  index = j;
247  break;
248  }
249  }
250  if(index < 0)
251  {
252  ROS_ERROR("Joint state does not contain joint state for %s.",chain_info.joint_names[i].c_str());
253  return false;
254  }
255  }
256  return true;
257 }
258 
259 bool checkLinkNames(const std::vector<std::string> &link_names,
260  const moveit_msgs::KinematicSolverInfo &chain_info)
261 {
262  if(link_names.empty())
263  return false;
264  for(unsigned int i=0; i < link_names.size(); i++)
265  {
266  if(!checkLinkName(link_names[i],chain_info))
267  return false;
268  }
269  return true;
270 }
271 
272 bool checkLinkName(const std::string &link_name,
273  const moveit_msgs::KinematicSolverInfo &chain_info)
274 {
275  for(unsigned int i=0; i < chain_info.link_names.size(); i++)
276  {
277  if(link_name == chain_info.link_names[i])
278  return true;
279  }
280  return false;
281 }
282 
283 bool checkRobotState(moveit_msgs::RobotState &robot_state,
284  const moveit_msgs::KinematicSolverInfo &chain_info)
285 {
286  if((int) robot_state.joint_state.position.size() != (int) robot_state.joint_state.name.size())
287  {
288  ROS_ERROR("Number of joints in robot_state.joint_state does not match number of positions in robot_state.joint_state");
289  return false;
290  }
291  if(!checkJointNames(robot_state.joint_state.name,chain_info))
292  {
293  ROS_ERROR("Robot state must contain joint state for every joint in the kinematic chain");
294  return false;
295  }
296  return true;
297 }
298 
299 bool checkFKService(moveit_msgs::GetPositionFK::Request &request,
300  moveit_msgs::GetPositionFK::Response &response,
301  const moveit_msgs::KinematicSolverInfo &chain_info)
302 {
303  if(!checkLinkNames(request.fk_link_names,chain_info))
304  {
305  ROS_ERROR("Link name in service request does not match links that kinematics can provide solutions for.");
306  response.error_code.val = response.error_code.INVALID_LINK_NAME;
307  return false;
308  }
309  if(!checkRobotState(request.robot_state,chain_info))
310  {
311  response.error_code.val = response.error_code.INVALID_ROBOT_STATE;
312  return false;
313  }
314  return true;
315 }
316 
317 bool checkIKService(moveit_msgs::GetPositionIK::Request &request,
318  moveit_msgs::GetPositionIK::Response &response,
319  const moveit_msgs::KinematicSolverInfo &chain_info)
320 {
321  if(!checkLinkName(request.ik_request.ik_link_name,chain_info))
322  {
323  ROS_ERROR("Link name in service request does not match links that kinematics can provide solutions for.");
324  response.error_code.val = response.error_code.INVALID_LINK_NAME;
325  return false;
326  }
327  if(!checkRobotState(request.ik_request.robot_state,chain_info))
328  {
329  response.error_code.val = response.error_code.INVALID_ROBOT_STATE;
330  return false;
331  }
332  if(request.ik_request.timeout <= ros::Duration(0.0))
333  {
334  response.error_code.val = response.error_code.TIMED_OUT;
335  return false;
336  }
337  return true;
338 }
339 
340 bool convertPoseToRootFrame(const geometry_msgs::PoseStamped &pose_msg,
341  KDL::Frame &pose_kdl,
342  const std::string &root_frame,
344 {
345  geometry_msgs::PoseStamped pose_stamped;
346  if(!convertPoseToRootFrame(pose_msg, pose_stamped, root_frame,tf))
347  return false;
348  tf::poseMsgToKDL(pose_stamped.pose, pose_kdl);
349  return true;
350 }
351 
352 
353 bool convertPoseToRootFrame(const geometry_msgs::PoseStamped &pose_msg,
354  geometry_msgs::PoseStamped &pose_msg_out,
355  const std::string &root_frame,
357 {
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;
369  tf::Stamped<tf::Pose> pose_stamped;
370  poseStampedMsgToTF(pose_msg_in, pose_stamped);
371 
372  if (!tf.canTransform(root_frame, pose_stamped.frame_id_, pose_stamped.stamp_))
373  {
374  std::string err;
375  if (tf.getLatestCommonTime(pose_stamped.frame_id_, root_frame, pose_stamped.stamp_, &err) != tf::NO_ERROR)
376  {
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());
378  return false;
379  }
380  }
381  try
382  {
383  tf.transformPose(root_frame, pose_stamped, pose_stamped);
384  }
385  catch(...)
386  {
387  ROS_ERROR("pr2_arm_ik:: Cannot transform from '%s' to '%s'",pose_stamped.frame_id_.c_str(),root_frame.c_str());
388  return false;
389  }
390  tf::poseStampedTFToMsg(pose_stamped,pose_msg_out);
391  return true;
392 }
393 
394 
395 
396 int getJointIndex(const std::string &name,
397  const moveit_msgs::KinematicSolverInfo &chain_info)
398 {
399  for(unsigned int i=0; i < chain_info.joint_names.size(); i++)
400  {
401  if(chain_info.joint_names[i] == name)
402  {
403  return i;
404  }
405  }
406  return -1;
407 }
408 
409 void getKDLChainInfo(const KDL::Chain &chain,
410  moveit_msgs::KinematicSolverInfo &chain_info)
411 {
412  int i=0; // segment number
413  while(i < (int)chain.getNrOfSegments())
414  {
415  chain_info.link_names.push_back(chain.getSegment(i).getName());
416  i++;
417  }
418 }
419 
421  const std::string &name)
422 {
423  int i=0; // segment number
424  while(i < (int)chain.getNrOfSegments())
425  {
426  if(chain.getSegment(i).getName() == name)
427  {
428  return i+1;
429  }
430  i++;
431  }
432  return -1;
433 }
434 
435 
436 }
static const double IK_DEFAULT_TIMEOUT
#define ROS_FATAL(...)
const Segment & getSegment(unsigned int nr) const
static void poseStampedTFToMsg(const Stamped< Pose > &bt, geometry_msgs::PoseStamped &msg)
Namespace for the PR2ArmKinematics.
bool getKDLChain(const urdf::ModelInterface &model, const std::string &root_name, const std::string &tip_name, KDL::Chain &kdl_chain)
URDF_EXPORT bool initXml(TiXmlElement *xml)
int getLatestCommonTime(const std::string &source_frame, const std::string &target_frame, ros::Time &time, std::string *error_string) const
unsigned int getNrOfSegments() const
bool solveQuadratic(const double &a, const double &b, const double &c, double *x1, double *x2)
bool checkJointNames(const std::vector< std::string > &joint_names, const moveit_msgs::KinematicSolverInfo &chain_info)
void getKDLChainInfo(const KDL::Chain &chain, moveit_msgs::KinematicSolverInfo &chain_info)
bool canTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, std::string *error_msg=NULL) const
Rotation M
const std::string & getName() const
unsigned int index
bool convertPoseToRootFrame(const geometry_msgs::PoseStamped &pose_msg, KDL::Frame &pose_kdl, const std::string &root_frame, tf::TransformListener &tf)
bool checkIKService(moveit_msgs::GetPositionIK::Request &request, moveit_msgs::GetPositionIK::Response &response, const moveit_msgs::KinematicSolverInfo &chain_info)
bool checkLinkName(const std::string &link_name, const moveit_msgs::KinematicSolverInfo &chain_info)
bool searchParam(const std::string &key, std::string &result) const
bool param(const std::string &param_name, T &param_val, const T &default_val) const
bool checkRobotState(moveit_msgs::RobotState &robot_state, const moveit_msgs::KinematicSolverInfo &chain_info)
void poseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k)
int getJointIndex(const std::string &name, const moveit_msgs::KinematicSolverInfo &chain_info)
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
KDL_PARSER_PUBLIC bool treeFromString(const std::string &xml, KDL::Tree &tree)
Eigen::Affine3f KDLToEigenMatrix(const KDL::Frame &p)
ros::Time stamp_
bool getChain(const std::string &chain_root, const std::string &chain_tip, Chain &chain) const
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
std::string frame_id_
void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const
bool getParam(const std::string &key, std::string &s) const
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
bool checkFKService(moveit_msgs::GetPositionFK::Request &request, moveit_msgs::GetPositionFK::Response &response, const moveit_msgs::KinematicSolverInfo &chain_info)
static const double IK_EPS
#define ROS_ERROR_STREAM(args)
bool checkLinkNames(const std::vector< std::string > &link_names, const moveit_msgs::KinematicSolverInfo &chain_info)
#define ROS_ERROR(...)
bool loadRobotModel(ros::NodeHandle node_handle, urdf::Model &robot_model, std::string &xml_string)
double distance(const urdf::Pose &transform)
Eigen::Matrix4f matrixInverse(const Eigen::Matrix4f &g)
bool getKDLTree(const std::string &xml_string, const std::string &root_name, const std::string &tip_name, KDL::Tree &kdl_chain)
bool solveCosineEqn(const double &a, const double &b, const double &c, double &soln1, double &soln2)
int getKDLSegmentIndex(const KDL::Chain &chain, const std::string &name)
double computeEuclideanDistance(const std::vector< double > &array_1, const KDL::JntArray &array_2)
NO_ERROR
#define ROS_DEBUG(...)


pr2_arm_kinematics
Author(s): Sachin Chitta
autogenerated on Wed Jun 5 2019 21:35:32