pr2_arm_kinematics_plugin.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 
38 #include <geometry_msgs/PoseStamped.h>
40 #include <tf_conversions/tf_kdl.h>
41 #include <ros/ros.h>
42 #include <algorithm>
43 #include <numeric>
44 
46 
47 using namespace KDL;
48 using namespace tf;
49 using namespace std;
50 using namespace ros;
51 
52 //register PR2ArmKinematics as a KinematicsBase implementation
54 
55 namespace pr2_arm_kinematics {
56 
57 PR2ArmKinematicsPlugin::PR2ArmKinematicsPlugin():active_(false){}
58 
60 {
61  if(active_)
62  return true;
63  return false;
64 }
65 
66 bool PR2ArmKinematicsPlugin::initialize(const std::string& robot_description,
67  const std::string& group_name,
68  const std::string& base_frame,
69  const std::string& tip_frame,
70  double search_discretization)
71 {
72  setValues(robot_description, group_name, base_frame, tip_frame, search_discretization);
73  urdf::Model robot_model;
74  std::string xml_string;
75  ros::NodeHandle private_handle("~/"+group_name);
76  dimension_ = 7;
77  while(!loadRobotModel(private_handle,robot_model,xml_string) && private_handle.ok())
78  {
79  ROS_ERROR("Could not load robot model. Are you sure the robot model is on the parameter server?");
80  ros::Duration(0.5).sleep();
81  }
82 
83  ROS_DEBUG("Loading KDL Tree");
85  {
86  active_ = false;
87  ROS_ERROR("Could not load kdl tree");
88  }
89  ROS_DEBUG("Advertising services");
91  private_handle.param<int>("free_angle",free_angle_,2);
92 
94  if(!pr2_arm_ik_solver_->active_)
95  {
96  ROS_ERROR("Could not load ik");
97  active_ = false;
98  }
99  else
100  {
101 
102  pr2_arm_ik_solver_->getSolverInfo(ik_solver_info_);
104  fk_solver_info_.joint_names = ik_solver_info_.joint_names;
105 
106  for(unsigned int i=0; i < ik_solver_info_.joint_names.size(); i++)
107  {
108  ROS_DEBUG("PR2Kinematics:: joint name: %s",ik_solver_info_.joint_names[i].c_str());
109  }
110  for(unsigned int i=0; i < ik_solver_info_.link_names.size(); i++)
111  {
112  ROS_DEBUG("PR2Kinematics can solve IK for %s",ik_solver_info_.link_names[i].c_str());
113  }
114  for(unsigned int i=0; i < fk_solver_info_.link_names.size(); i++)
115  {
116  ROS_DEBUG("PR2Kinematics can solve FK for %s",fk_solver_info_.link_names[i].c_str());
117  }
118  ROS_DEBUG("PR2KinematicsPlugin::active for %s",group_name.c_str());
119  active_ = true;
120  }
121  pr2_arm_ik_solver_->setFreeAngle(2);
122  return active_;
123 }
124 
125 bool PR2ArmKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose,
126  const std::vector<double> &ik_seed_state,
127  std::vector<double> &solution,
128  moveit_msgs::MoveItErrorCodes &error_code,
129  const kinematics::KinematicsQueryOptions &options) const
130 {
131  if(!active_)
132  {
133  ROS_ERROR("kinematics not active");
134  error_code.val = error_code.NO_IK_SOLUTION;
135  return false;
136  }
137 
138  KDL::Frame pose_desired;
139  tf::poseMsgToKDL(ik_pose, pose_desired);
140 
141  //Do the IK
142  KDL::JntArray jnt_pos_in;
143  KDL::JntArray jnt_pos_out;
144  jnt_pos_in.resize(dimension_);
145  for(int i=0; i < dimension_; i++)
146  {
147  jnt_pos_in(i) = ik_seed_state[i];
148  }
149 
150  int ik_valid = pr2_arm_ik_solver_->CartToJnt(jnt_pos_in,
151  pose_desired,
152  jnt_pos_out);
153  if(ik_valid == pr2_arm_kinematics::NO_IK_SOLUTION)
154  {
155  error_code.val = error_code.NO_IK_SOLUTION;
156  return false;
157  }
158 
159  if(ik_valid >= 0)
160  {
161  solution.resize(dimension_);
162  for(int i=0; i < dimension_; i++)
163  {
164  solution[i] = jnt_pos_out(i);
165  }
166  error_code.val = error_code.SUCCESS;
167  return true;
168  }
169  else
170  {
171  ROS_DEBUG("An IK solution could not be found");
172  error_code.val = error_code.NO_IK_SOLUTION;
173  return false;
174  }
175 }
176 
177 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
178  const std::vector<double> &ik_seed_state,
179  double timeout,
180  std::vector<double> &solution,
181  moveit_msgs::MoveItErrorCodes &error_code,
182  const kinematics::KinematicsQueryOptions &options) const
183 {
184  static IKCallbackFn solution_callback = 0;
185  static std::vector<double> consistency_limits;
186  return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code);
187 }
188 
189 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
190  const std::vector<double> &ik_seed_state,
191  double timeout,
192  const std::vector<double> &consistency_limits,
193  std::vector<double> &solution,
194  moveit_msgs::MoveItErrorCodes &error_code,
195  const kinematics::KinematicsQueryOptions &options) const
196 {
197  static IKCallbackFn solution_callback = 0;
198  return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code);
199 }
200 
201 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
202  const std::vector<double> &ik_seed_state,
203  double timeout,
204  std::vector<double> &solution,
205  const IKCallbackFn &solution_callback,
206  moveit_msgs::MoveItErrorCodes &error_code,
207  const kinematics::KinematicsQueryOptions &options) const
208 {
209  static std::vector<double> consistency_limits;
210  return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code);
211 }
212 
213 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
214  const std::vector<double> &ik_seed_state,
215  double timeout,
216  const std::vector<double> &consistency_limits,
217  std::vector<double> &solution,
218  const IKCallbackFn &solution_callback,
219  moveit_msgs::MoveItErrorCodes &error_code,
220  const kinematics::KinematicsQueryOptions &options) const
221 {
222  if(!active_)
223  {
224  ROS_ERROR("kinematics not active");
225  error_code.val = error_code.FAILURE;
226  return false;
227  }
228  if(!consistency_limits.empty() && consistency_limits.size() != dimension_)
229  {
230  ROS_ERROR("Consistency limits should be of size: %d",dimension_);
231  error_code.val = error_code.FAILURE;
232  return false;
233  }
234 
235  KDL::Frame pose_desired;
236  tf::poseMsgToKDL(ik_pose, pose_desired);
237 
238  //Do the IK
239  KDL::JntArray jnt_pos_in;
240  KDL::JntArray jnt_pos_out;
241  jnt_pos_in.resize(dimension_);
242  for(int i=0; i < dimension_; i++)
243  {
244  jnt_pos_in(i) = ik_seed_state[i];
245  }
246 
247  int ik_valid;
248  if(consistency_limits.empty())
249  {
250  ik_valid = pr2_arm_ik_solver_->CartToJntSearch(jnt_pos_in,
251  pose_desired,
252  jnt_pos_out,
253  timeout,
254  error_code,
255  solution_callback ?
256  boost::bind(solution_callback, _1, _2, _3):
257  IKCallbackFn());
258  }
259  else
260  {
261  ik_valid = pr2_arm_ik_solver_->CartToJntSearch(jnt_pos_in,
262  pose_desired,
263  jnt_pos_out,
264  timeout,
265  consistency_limits[free_angle_],
266  error_code,
267  solution_callback ?
268  boost::bind(solution_callback, _1, _2, _3):
269  IKCallbackFn());
270  }
271 
272  if(ik_valid == pr2_arm_kinematics::NO_IK_SOLUTION)
273  return false;
274 
275  if(ik_valid >= 0)
276  {
277  solution.resize(dimension_);
278  for(int i=0; i < dimension_; i++)
279  {
280  solution[i] = jnt_pos_out(i);
281  }
282  return true;
283  }
284  else
285  {
286  ROS_DEBUG("An IK solution could not be found");
287  return false;
288  }
289 }
290 
291 bool PR2ArmKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
292  const std::vector<double> &joint_angles,
293  std::vector<geometry_msgs::Pose> &poses) const
294 {
295  if(!active_)
296  {
297  ROS_ERROR("kinematics not active");
298  return false;
299  }
300 
301  KDL::Frame p_out;
302  KDL::JntArray jnt_pos_in;
303  geometry_msgs::PoseStamped pose;
304  tf::Stamped<tf::Pose> tf_pose;
305 
306  jnt_pos_in.resize(dimension_);
307  for(int i=0; i < dimension_; i++)
308  {
309  jnt_pos_in(i) = joint_angles[i];
310  // ROS_DEBUG("Joint angle: %d %f",i,joint_angles[i]);
311  }
312 
313  poses.resize(link_names.size());
314 
315  bool valid = true;
316  for(unsigned int i=0; i < poses.size(); i++)
317  {
318  // ROS_DEBUG("End effector index: %d",pr2_arm_kinematics::getKDLSegmentIndex(kdl_chain_,link_names[i]));
319  if(jnt_to_pose_solver_->JntToCart(jnt_pos_in,p_out,pr2_arm_kinematics::getKDLSegmentIndex(kdl_chain_,link_names[i])) >= 0)
320  {
321  tf::poseKDLToMsg(p_out,poses[i]);
322  }
323  else
324  {
325  ROS_ERROR("Could not compute FK for %s",link_names[i].c_str());
326  valid = false;
327  }
328  }
329  return valid;
330 }
331 
332 const std::vector<std::string>& PR2ArmKinematicsPlugin::getJointNames() const
333 {
334  if(!active_)
335  {
336  ROS_ERROR("kinematics not active");
337  }
338  return ik_solver_info_.joint_names;
339 }
340 
341 const std::vector<std::string>& PR2ArmKinematicsPlugin::getLinkNames() const
342 {
343  if(!active_)
344  {
345  ROS_ERROR("kinematics not active");
346  }
347  return fk_solver_info_.link_names;
348 }
349 
350 } // namespace
const std::vector< std::string > & getJointNames() const
Namespace for the PR2ArmKinematics.
const std::vector< std::string > & getLinkNames() const
bool getKDLChain(const urdf::ModelInterface &model, const std::string &root_name, const std::string &tip_name, KDL::Chain &kdl_chain)
bool sleep() const
pr2_arm_kinematics::PR2ArmIKSolverPtr pr2_arm_ik_solver_
virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_seed_state, double timeout, std::vector< double > &solution, moveit_msgs::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const
moveit_msgs::KinematicSolverInfo fk_solver_info_
static const int NO_IK_SOLUTION
std::shared_ptr< KDL::ChainFkSolverPos_recursive > jnt_to_pose_solver_
PLUGINLIB_EXPORT_CLASS(pr2_arm_kinematics::PR2ArmKinematicsPlugin, kinematics::KinematicsBase)
void getKDLChainInfo(const KDL::Chain &chain, moveit_msgs::KinematicSolverInfo &chain_info)
virtual bool getPositionFK(const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::Pose > &poses) const
virtual bool getPositionIK(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_seed_state, std::vector< double > &solution, moveit_msgs::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const
void poseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k)
virtual void setValues(const std::string &robot_description, const std::string &group_name, const std::string &base_frame, const std::string &tip_frame, double search_discretization)
def xml_string(rootXml, addHeader=True)
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn
moveit_msgs::KinematicSolverInfo ik_solver_info_
void resize(unsigned int newSize)
void poseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m)
virtual bool initialize(const std::string &robot_description, const std::string &group_name, const std::string &base_name, const std::string &tip_name, double search_discretization)
#define ROS_ERROR(...)
bool loadRobotModel(ros::NodeHandle node_handle, urdf::Model &robot_model, std::string &xml_string)
int getKDLSegmentIndex(const KDL::Chain &chain, const std::string &name)
#define ROS_DEBUG(...)


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