pr2_arm_ik_solver.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 
39 using namespace Eigen;
40 using namespace pr2_arm_kinematics;
41 
42 PR2ArmIKSolver::PR2ArmIKSolver(const urdf::Model &robot_model,
43  const std::string &root_frame_name,
44  const std::string &tip_frame_name,
45  const double &search_discretization_angle,
46  const int &free_angle):ChainIkSolverPos()
47 {
48  pr2_arm_ik_ = new PR2ArmIK();
49  search_discretization_angle_ = search_discretization_angle;
50  free_angle_ = free_angle;
51  root_frame_name_ = root_frame_name;
52  if(!pr2_arm_ik_->init(robot_model, root_frame_name, tip_frame_name))
53  active_ = false;
54  else
55  active_ = true;
56 }
57 
58 void PR2ArmIKSolver::getSolverInfo(moveit_msgs::KinematicSolverInfo &response)
59 {
60  pr2_arm_ik_->getSolverInfo(response);
61 }
62 
64  const KDL::Frame& p_in,
65  KDL::JntArray &q_out)
66 {
67  Eigen::Matrix4f b = KDLToEigenMatrix(p_in);
68  std::vector<std::vector<double> > solution_ik;
69  if(free_angle_ == 0)
70  {
71  ROS_DEBUG("Solving with free angle: %d", free_angle_);
72  pr2_arm_ik_->computeIKShoulderPan(b, q_init(0), solution_ik);
73  }
74  else
75  {
76  pr2_arm_ik_->computeIKShoulderRoll(b, q_init(2), solution_ik);
77  }
78 
79  if(solution_ik.empty())
80  return -1;
81 
82  double min_distance = 1e6;
83  int min_index = -1;
84 
85  for(int i=0; i< (int) solution_ik.size(); ++i)
86  {
87  ROS_DEBUG("Solution : %d", (int)solution_ik.size());
88  for(int j=0; j < (int)solution_ik[i].size(); j++)
89  {
90  ROS_DEBUG("Joint %d: %f", j, solution_ik[i][j]);
91  }
92 
93  double tmp_distance = computeEuclideanDistance(solution_ik[i], q_init);
94  if(tmp_distance < min_distance)
95  {
96  min_distance = tmp_distance;
97  min_index = i;
98  }
99  }
100 
101  if(min_index > -1)
102  {
103  q_out.resize((int)solution_ik[min_index].size());
104  for(int i=0; i < (int)solution_ik[min_index].size(); ++i)
105  {
106  q_out(i) = solution_ik[min_index][i];
107  }
108  return 1;
109  }
110  else
111  return -1;
112 }
113 
114 int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init,
115  const KDL::Frame& p_in,
116  std::vector<KDL::JntArray> &q_out)
117 {
118  Eigen::Matrix4f b = KDLToEigenMatrix(p_in);
119  std::vector<std::vector<double> > solution_ik;
120  KDL::JntArray q;
121 
122  if(free_angle_ == 0)
123  {
124  pr2_arm_ik_->computeIKShoulderPan(b, q_init(0), solution_ik);
125  }
126  else
127  {
128  pr2_arm_ik_->computeIKShoulderRoll(b, q_init(2), solution_ik);
129  }
130 
131  if(solution_ik.empty())
132  return -1;
133 
134  q.resize(7);
135  q_out.clear();
136  for(int i=0; i< (int) solution_ik.size(); ++i)
137  {
138  for(int j=0; j < 7; j++)
139  {
140  q(j) = solution_ik[i][j];
141  }
142  q_out.push_back(q);
143  }
144  return 1;
145 }
146 
147 bool PR2ArmIKSolver::getCount(int &count,
148  const int &max_count,
149  const int &min_count)
150 {
151  if(count > 0)
152  {
153  if(-count >= min_count)
154  {
155  count = -count;
156  return true;
157  }
158  else if(count+1 <= max_count)
159  {
160  count = count+1;
161  return true;
162  }
163  else
164  {
165  return false;
166  }
167  }
168  else
169  {
170  if(1-count <= max_count)
171  {
172  count = 1-count;
173  return true;
174  }
175  else if(count-1 >= min_count)
176  {
177  count = count -1;
178  return true;
179  }
180  else
181  return false;
182  }
183 }
184 
186  const KDL::Frame& p_in,
187  std::vector<KDL::JntArray> &q_out,
188  const double &timeout)
189 {
190  KDL::JntArray q_init = q_in;
191  double initial_guess = q_init(free_angle_);
192 
193  ros::WallTime start_time = ros::WallTime::now();
194  double loop_time = 0;
195  int count = 0;
196 
197  int num_positive_increments = (int)((pr2_arm_ik_->solver_info_.limits[free_angle_].max_position-initial_guess)/search_discretization_angle_);
198  int num_negative_increments = (int)((initial_guess-pr2_arm_ik_->solver_info_.limits[free_angle_].min_position)/search_discretization_angle_);
199  ROS_DEBUG("positive increments, negative increments: %d %d", num_positive_increments, num_negative_increments);
200  while(loop_time < timeout)
201  {
202  if(CartToJnt(q_init, p_in, q_out) > 0)
203  return 1;
204  if(!getCount(count, num_positive_increments, -num_negative_increments))
205  return -1;
206  q_init(free_angle_) = initial_guess + search_discretization_angle_ * count;
207  ROS_DEBUG("%d, %f",count, q_init(free_angle_));
208  loop_time = (ros::WallTime::now()-start_time).toSec();
209  }
210  if(loop_time >= timeout)
211  {
212  ROS_DEBUG("IK Timed out in %f seconds", timeout);
213  return TIMED_OUT;
214  }
215  else
216  {
217  ROS_DEBUG("No IK solution was found");
218  return NO_IK_SOLUTION;
219  }
220  return NO_IK_SOLUTION;
221 }
222 
224  const KDL::Frame& p_in,
225  KDL::JntArray &q_out,
226  const double &timeout,
227  const double& consistency_limit)
228 {
229  moveit_msgs::MoveItErrorCodes error_code;
230  static kinematics::KinematicsBase::IKCallbackFn solution_callback = 0;
231  return CartToJntSearch(q_in, p_in, q_out, timeout, true, consistency_limit, error_code, solution_callback);
232 }
233 
235  const KDL::Frame& p_in,
236  KDL::JntArray &q_out,
237  const double &timeout,
238  moveit_msgs::MoveItErrorCodes &error_code,
239  const kinematics::KinematicsBase::IKCallbackFn &solution_callback)
240 {
241  return CartToJntSearch(q_in, p_in, q_out, timeout, false, 0.0, error_code, solution_callback);
242 }
243 
244 
246  const KDL::Frame& p_in,
247  KDL::JntArray &q_out,
248  const double &timeout,
249  const double& consistency_limit,
250  moveit_msgs::MoveItErrorCodes &error_code,
251  const kinematics::KinematicsBase::IKCallbackFn &solution_callback)
252 {
253  return CartToJntSearch(q_in, p_in, q_out, timeout, true, consistency_limit, error_code, solution_callback);
254 }
255 
257  const KDL::Frame& p_in,
258  KDL::JntArray &q_out,
259  const double &timeout,
260  bool use_consistency_limit,
261  const double &max_consistency,
262  moveit_msgs::MoveItErrorCodes &error_code,
263  const kinematics::KinematicsBase::IKCallbackFn &solution_callback)
264 {
265  KDL::JntArray q_init = q_in;
266  double initial_guess = q_init(free_angle_);
267 
268  ros::WallTime start_time = ros::WallTime::now();
269  double loop_time = 0;
270  int count = 0;
271 
272  double max_limit, min_limit;
273  if(use_consistency_limit)
274  {
275  max_limit = fmin(pr2_arm_ik_->solver_info_.limits[free_angle_].max_position, initial_guess+max_consistency);
276  min_limit = fmax(pr2_arm_ik_->solver_info_.limits[free_angle_].min_position, initial_guess-max_consistency);
277  }
278  else
279  {
280  max_limit = pr2_arm_ik_->solver_info_.limits[free_angle_].max_position;
281  min_limit = pr2_arm_ik_->solver_info_.limits[free_angle_].min_position;
282  }
283 
284  int num_positive_increments = (int)((max_limit-initial_guess)/search_discretization_angle_);
285  int num_negative_increments = (int)((initial_guess-min_limit)/search_discretization_angle_);
286 
287  if(use_consistency_limit)
288  {
289  ROS_DEBUG("Consistency[Joint: %d]: Initial guess %f, consistency %f", free_angle_, initial_guess, max_consistency);
290  ROS_DEBUG("Max limit %f = max(%f, %f)", max_limit, pr2_arm_ik_->solver_info_.limits[free_angle_].max_position, initial_guess+max_consistency);
291  ROS_DEBUG("Min limit %f = min(%f, %f)", min_limit, pr2_arm_ik_->solver_info_.limits[free_angle_].min_position, initial_guess-max_consistency);
292  }
293  else
294  {
295  ROS_DEBUG("Consistency[Joint: %d]: Initial guess %f", free_angle_, initial_guess);
296  ROS_DEBUG("Max limit %f", max_limit);
297  ROS_DEBUG("Min limit %f", min_limit);
298  }
299 
300  ROS_DEBUG("positive increments, negative increments: %d %d", num_positive_increments, num_negative_increments);
301 
302  unsigned int testnum = 0;
303  geometry_msgs::Pose ik_pose_msg;
304  tf::poseKDLToMsg(p_in, ik_pose_msg);
305 
307 
308  while(loop_time < timeout)
309  {
310  testnum++;
311  if(CartToJnt(q_init, p_in, q_out) > 0)
312  {
313  if(solution_callback)
314  {
315  std::vector<double> ik_solution(7,0.0);
316  for(int i=0; i < 7; ++i)
317  ik_solution[i] = q_out(i);
318 
319  solution_callback(ik_pose_msg, ik_solution, error_code);
320  if(error_code.val == error_code.SUCCESS)
321  {
322  ROS_DEBUG("Difference is %f %f", q_in(free_angle_), q_out(free_angle_));
323  ROS_DEBUG("Success with %d in %f", testnum, (ros::WallTime::now()-s).toSec());
324  return 1;
325  }
326  }
327  else
328  {
329  error_code.val = error_code.SUCCESS;
330  return 1;
331  }
332  }
333  if(!getCount(count, num_positive_increments, -num_negative_increments))
334  {
335  ROS_DEBUG("Failure with %d in %f", testnum, (ros::WallTime::now()-s).toSec());
336  error_code.val = error_code.NO_IK_SOLUTION;
337  return -1;
338  }
339  q_init(free_angle_) = initial_guess + search_discretization_angle_ * count;
340  ROS_DEBUG("Redundancy search, index:%d, free angle value: %f", count, q_init(free_angle_));
341  loop_time = (ros::WallTime::now()-start_time).toSec();
342  }
343  if(loop_time >= timeout)
344  {
345  ROS_DEBUG("IK Timed out in %f seconds",timeout);
346  error_code.val = error_code.TIMED_OUT;
347  }
348  else
349  {
350  error_code.val = error_code.NO_IK_SOLUTION;
351  }
352  return -1;
353 }
354 
355 
356 std::string PR2ArmIKSolver::getFrameId()
357 {
358  return root_frame_name_;
359 }
360 
361 void PR2ArmIKSolver::updateInternalDataStructures()
362 {
363  // TODO: move (re)allocation of any internal data structures here
364  // to react to changes in chain
365 }
Namespace for the PR2ArmKinematics.
XmlRpcServer s
bool getCount(int &count, const int &max_count, const int &min_count)
int CartToJntSearch(const KDL::JntArray &q_in, const KDL::Frame &p_in, KDL::JntArray &q_out, const double &timeout)
static const int NO_IK_SOLUTION
void computeIKShoulderRoll(const Eigen::Affine3f &g_in, const double &shoulder_roll_initial_guess, std::vector< std::vector< double > > &solution) const
int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &p_in, KDL::JntArray &q_out)
Eigen::Affine3f KDLToEigenMatrix(const KDL::Frame &p)
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn
void resize(unsigned int newSize)
void getSolverInfo(moveit_msgs::KinematicSolverInfo &info)
Definition: pr2_arm_ik.cpp:190
void computeIKShoulderPan(const Eigen::Affine3f &g_in, const double &shoulder_pan_initial_guess, std::vector< std::vector< double > > &solution) const
static WallTime now()
void poseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m)
static const int TIMED_OUT
moveit_msgs::KinematicSolverInfo solver_info_
get chain information about the arm.
Definition: pr2_arm_ik.h:97
double computeEuclideanDistance(const std::vector< double > &array_1, const KDL::JntArray &array_2)
#define ROS_DEBUG(...)


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