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 
37 #include <geometry_msgs/PoseStamped.h>
39 #include <tf2_kdl/tf2_kdl.h>
40 #include <algorithm>
41 
44 
45 using namespace KDL;
46 using namespace std;
47 
48 namespace pr2_arm_kinematics
49 {
50 bool PR2ArmIKSolver::getCount(int& count, const int& max_count, const int& min_count)
51 {
52  if (count > 0)
53  {
54  if (-count >= min_count)
55  {
56  count = -count;
57  return true;
58  }
59  else if (count + 1 <= max_count)
60  {
61  count = count + 1;
62  return true;
63  }
64  else
65  {
66  return false;
67  }
68  }
69  else
70  {
71  if (1 - count <= max_count)
72  {
73  count = 1 - count;
74  return true;
75  }
76  else if (count - 1 >= min_count)
77  {
78  count = count - 1;
79  return true;
80  }
81  else
82  return false;
83  }
84 }
85 
86 PR2ArmIKSolver::PR2ArmIKSolver(const urdf::ModelInterface& robot_model, const std::string& root_frame_name,
87  const std::string& tip_frame_name, const double& search_discretization_angle,
88  const int& free_angle)
89  : ChainIkSolverPos()
90 {
91  search_discretization_angle_ = search_discretization_angle;
92  free_angle_ = free_angle;
93  root_frame_name_ = root_frame_name;
94  active_ = pr2_arm_ik_.init(robot_model, root_frame_name, tip_frame_name);
95 }
96 
98 {
99  // TODO: move (re)allocation of any internal data structures here
100  // to react to changes in chain
101 }
102 
103 int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& p_in, KDL::JntArray& q_out)
104 {
105  const bool verbose = false;
106  Eigen::Isometry3f b = KDLToEigenMatrix(p_in);
107  std::vector<std::vector<double> > solution_ik;
108  if (free_angle_ == 0)
109  {
110  if (verbose)
111  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "Solving with %f", q_init(0));
112  pr2_arm_ik_.computeIKShoulderPan(b, q_init(0), solution_ik);
113  }
114  else
115  {
116  pr2_arm_ik_.computeIKShoulderRoll(b, q_init(2), solution_ik);
117  }
118 
119  if (solution_ik.empty())
120  return -1;
121 
122  double min_distance = 1e6;
123  int min_index = -1;
124 
125  for (int i = 0; i < (int)solution_ik.size(); i++)
126  {
127  if (verbose)
128  {
129  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "Solution : %d", (int)solution_ik.size());
130 
131  for (int j = 0; j < (int)solution_ik[i].size(); j++)
132  {
133  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "%d: %f", j, solution_ik[i][j]);
134  }
135  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", " ");
136  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", " ");
137  }
138  double tmp_distance = computeEuclideanDistance(solution_ik[i], q_init);
139  if (tmp_distance < min_distance)
140  {
141  min_distance = tmp_distance;
142  min_index = i;
143  }
144  }
145 
146  if (min_index > -1)
147  {
148  q_out.resize((int)solution_ik[min_index].size());
149  for (int i = 0; i < (int)solution_ik[min_index].size(); i++)
150  {
151  q_out(i) = solution_ik[min_index][i];
152  }
153  return 1;
154  }
155  else
156  return -1;
157 }
158 
159 int PR2ArmIKSolver::cartToJntSearch(const KDL::JntArray& q_in, const KDL::Frame& p_in, KDL::JntArray& q_out,
160  const double& timeout)
161 {
162  const bool verbose = false;
163  KDL::JntArray q_init = q_in;
164  double initial_guess = q_init(free_angle_);
165 
166  ros::Time start_time = ros::Time::now();
167  double loop_time = 0;
168  int count = 0;
169 
170  int num_positive_increments =
171  (int)((pr2_arm_ik_.solver_info_.limits[free_angle_].max_position - initial_guess) / search_discretization_angle_);
172  int num_negative_increments =
173  (int)((initial_guess - pr2_arm_ik_.solver_info_.limits[free_angle_].min_position) / search_discretization_angle_);
174  if (verbose)
175  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "%f %f %f %d %d \n\n", initial_guess,
176  pr2_arm_ik_.solver_info_.limits[free_angle_].max_position,
177  pr2_arm_ik_.solver_info_.limits[free_angle_].min_position, num_positive_increments,
178  num_negative_increments);
179  while (loop_time < timeout)
180  {
181  if (CartToJnt(q_init, p_in, q_out) > 0)
182  return 1;
183  if (!getCount(count, num_positive_increments, -num_negative_increments))
184  return -1;
185  q_init(free_angle_) = initial_guess + search_discretization_angle_ * count;
186  if (verbose)
187  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "%d, %f", count, q_init(free_angle_));
188  loop_time = (ros::Time::now() - start_time).toSec();
189  }
190  if (loop_time >= timeout)
191  {
192  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "IK Timed out in %f seconds", timeout);
193  return TIMED_OUT;
194  }
195  else
196  {
197  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "No IK solution was found");
198  return NO_IK_SOLUTION;
199  }
200  return NO_IK_SOLUTION;
201 }
202 
203 bool getKDLChain(const urdf::ModelInterface& model, const std::string& root_name, const std::string& tip_name,
204  KDL::Chain& kdl_chain)
205 {
206  // create robot chain from root to tip
207  KDL::Tree tree;
208  if (!kdl_parser::treeFromUrdfModel(model, tree))
209  {
210  ROS_ERROR("Could not initialize tree object");
211  return false;
212  }
213  if (!tree.getChain(root_name, tip_name, kdl_chain))
214  {
215  ROS_ERROR_STREAM("Could not initialize chain object for base " << root_name << " tip " << tip_name);
216  return false;
217  }
218  return true;
219 }
220 
221 Eigen::Isometry3f KDLToEigenMatrix(const KDL::Frame& p)
222 {
223  Eigen::Isometry3f b = Eigen::Isometry3f::Identity();
224  for (int i = 0; i < 3; i++)
225  {
226  for (int j = 0; j < 3; j++)
227  {
228  b(i, j) = p.M(i, j);
229  }
230  b(i, 3) = p.p(i);
231  }
232  return b;
233 }
234 
235 double computeEuclideanDistance(const std::vector<double>& array_1, const KDL::JntArray& array_2)
236 {
237  double distance = 0.0;
238  for (int i = 0; i < (int)array_1.size(); i++)
239  {
240  distance += (array_1[i] - array_2(i)) * (array_1[i] - array_2(i));
241  }
242  return sqrt(distance);
243 }
244 
245 void getKDLChainInfo(const KDL::Chain& chain, moveit_msgs::KinematicSolverInfo& chain_info)
246 {
247  int i = 0; // segment number
248  while (i < (int)chain.getNrOfSegments())
249  {
250  chain_info.link_names.push_back(chain.getSegment(i).getName());
251  i++;
252  }
253 }
254 
256 {
257 }
258 
260 {
261  return active_;
262 }
263 
265  const std::string& base_frame, const std::vector<std::string>& tip_frames,
266  double search_discretization)
267 {
268  storeValues(robot_model, group_name, base_frame, tip_frames, search_discretization);
269  const bool verbose = false;
270  std::string xml_string;
271  dimension_ = 7;
272 
273  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "Loading KDL Tree");
275  {
276  active_ = false;
277  ROS_ERROR("Could not load kdl tree");
278  }
279  jnt_to_pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
280  free_angle_ = 2;
281 
283  search_discretization, free_angle_));
284  if (!pr2_arm_ik_solver_->active_)
285  {
286  ROS_ERROR("Could not load ik");
287  active_ = false;
288  }
289  else
290  {
291  pr2_arm_ik_solver_->getSolverInfo(ik_solver_info_);
293  fk_solver_info_.joint_names = ik_solver_info_.joint_names;
294 
295  if (verbose)
296  {
297  for (const std::string& joint_name : ik_solver_info_.joint_names)
298  {
299  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "PR2Kinematics:: joint name: %s", joint_name.c_str());
300  }
301  for (const std::string& link_name : ik_solver_info_.link_names)
302  {
303  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "PR2Kinematics can solve IK for %s", link_name.c_str());
304  }
305  for (const std::string& link_name : fk_solver_info_.link_names)
306  {
307  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "PR2Kinematics can solve FK for %s", link_name.c_str());
308  }
309  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "PR2KinematicsPlugin::active for %s", group_name.c_str());
310  }
311  active_ = true;
312  }
313  return active_;
314 }
315 
316 bool PR2ArmKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
317  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
318  const kinematics::KinematicsQueryOptions& options) const
319 {
320  return false;
321 }
322 
323 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
324  const std::vector<double>& ik_seed_state, double timeout,
325  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
326  const kinematics::KinematicsQueryOptions& options) const
327 {
328  if (!active_)
329  {
330  ROS_ERROR("kinematics not active");
331  error_code.val = error_code.PLANNING_FAILED;
332  return false;
333  }
334  KDL::Frame pose_desired;
335  tf2::fromMsg(ik_pose, pose_desired);
336 
337  // Do the IK
338  KDL::JntArray jnt_pos_in;
339  KDL::JntArray jnt_pos_out;
340  jnt_pos_in.resize(dimension_);
341  for (int i = 0; i < dimension_; i++)
342  {
343  jnt_pos_in(i) = ik_seed_state[i];
344  }
345 
346  int ik_valid = pr2_arm_ik_solver_->cartToJntSearch(jnt_pos_in, pose_desired, jnt_pos_out, timeout);
347  if (ik_valid == pr2_arm_kinematics::NO_IK_SOLUTION)
348  {
349  error_code.val = error_code.NO_IK_SOLUTION;
350  return false;
351  }
352 
353  if (ik_valid >= 0)
354  {
355  solution.resize(dimension_);
356  for (int i = 0; i < dimension_; i++)
357  {
358  solution[i] = jnt_pos_out(i);
359  }
360  error_code.val = error_code.SUCCESS;
361  return true;
362  }
363  else
364  {
365  ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "An IK solution could not be found");
366  error_code.val = error_code.NO_IK_SOLUTION;
367  return false;
368  }
369 }
370 
371 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
372  const std::vector<double>& ik_seed_state, double timeout,
373  const std::vector<double>& consistency_limit,
374  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
375  const kinematics::KinematicsQueryOptions& options) const
376 {
377  return false;
378 }
379 
380 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
381  const std::vector<double>& ik_seed_state, double timeout,
382  std::vector<double>& solution, const IKCallbackFn& solution_callback,
383  moveit_msgs::MoveItErrorCodes& error_code,
384  const kinematics::KinematicsQueryOptions& options) const
385 {
386  return false;
387 }
388 
389 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
390  const std::vector<double>& ik_seed_state, double timeout,
391  const std::vector<double>& consistency_limit,
392  std::vector<double>& solution, const IKCallbackFn& solution_callback,
393  moveit_msgs::MoveItErrorCodes& error_code,
394  const kinematics::KinematicsQueryOptions& options) const
395 {
396  return false;
397 }
398 
399 bool PR2ArmKinematicsPlugin::getPositionFK(const std::vector<std::string>& link_names,
400  const std::vector<double>& joint_angles,
401  std::vector<geometry_msgs::Pose>& poses) const
402 {
403  return false;
404 }
405 
406 const std::vector<std::string>& PR2ArmKinematicsPlugin::getJointNames() const
407 {
408  if (!active_)
409  {
410  ROS_ERROR("kinematics not active");
411  }
412  return ik_solver_info_.joint_names;
413 }
414 
415 const std::vector<std::string>& PR2ArmKinematicsPlugin::getLinkNames() const
416 {
417  if (!active_)
418  {
419  ROS_ERROR("kinematics not active");
420  }
421  return fk_solver_info_.link_names;
422 }
423 
424 } // namespace pr2_arm_kinematics
pr2_arm_kinematics::PR2ArmKinematicsPlugin::getPositionFK
bool getPositionFK(const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::Pose > &poses) const override
Given a set of joint angles and a set of links, compute their pose.
Definition: pr2_arm_kinematics_plugin.cpp:399
moveit::core
Core components of MoveIt.
Definition: kinematics_base.h:81
pr2_arm_kinematics::PR2ArmKinematicsPlugin::fk_solver_info_
moveit_msgs::KinematicSolverInfo fk_solver_info_
Definition: pr2_arm_kinematics_plugin.h:281
pr2_arm_kinematics
Definition: pr2_arm_ik.h:48
pr2_arm_kinematics::TIMED_OUT
static const int TIMED_OUT
Definition: pr2_arm_kinematics_plugin.h:96
xml_string
def xml_string(rootXml, addHeader=True)
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
kinematics::KinematicsBase::storeValues
void storeValues(const moveit::core::RobotModel &robot_model, const std::string &group_name, const std::string &base_frame, const std::vector< std::string > &tip_frames, double search_discretization)
Definition: kinematics_base.cpp:51
pr2_arm_kinematics::PR2ArmIKSolver::getCount
bool getCount(int &count, const int &max_count, const int &min_count)
Definition: pr2_arm_kinematics_plugin.cpp:50
pr2_arm_kinematics::PR2ArmKinematicsPlugin::getPositionIK
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 override
Given a desired pose of the end-effector, compute the joint angles to reach it.
Definition: pr2_arm_kinematics_plugin.cpp:316
tf2::fromMsg
void fromMsg(const A &, B &b)
moveit::core::RobotModel
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:132
pr2_arm_kinematics::getKDLChainInfo
void getKDLChainInfo(const KDL::Chain &chain, moveit_msgs::KinematicSolverInfo &chain_info)
Definition: pr2_arm_kinematics_plugin.cpp:245
pr2_arm_kinematics::PR2ArmKinematicsPlugin::active_
bool active_
Definition: pr2_arm_kinematics_plugin.h:274
pr2_arm_kinematics::PR2ArmKinematicsPlugin::kdl_chain_
KDL::Chain kdl_chain_
Definition: pr2_arm_kinematics_plugin.h:280
pr2_arm_kinematics_plugin.h
pr2_arm_kinematics::PR2ArmKinematicsPlugin::isActive
bool isActive()
Specifies if the node is active or not.
Definition: pr2_arm_kinematics_plugin.cpp:259
kinematics::KinematicsBase::IKCallbackFn
boost::function< void(const geometry_msgs::Pose &, const std::vector< double > &, moveit_msgs::MoveItErrorCodes &)> IKCallbackFn
Signature for a callback to validate an IK solution. Typically used for collision checking.
Definition: kinematics_base.h:152
kdl_parser::treeFromUrdfModel
KDL_PARSER_PUBLIC bool treeFromUrdfModel(const urdf::ModelInterface &robot_model, KDL::Tree &tree)
pr2_arm_kinematics::PR2ArmIK::computeIKShoulderRoll
void computeIKShoulderRoll(const Eigen::Isometry3f &g_in, const double &shoulder_roll_initial_guess, std::vector< std::vector< double > > &solution) const
compute IK based on an initial guess for the shoulder roll angle. h
Definition: pr2_arm_ik.cpp:462
pr2_arm_kinematics::getKDLChain
bool getKDLChain(const urdf::ModelInterface &model, const std::string &root_name, const std::string &tip_name, KDL::Chain &kdl_chain)
Definition: pr2_arm_kinematics_plugin.cpp:203
robot_model.h
pr2_arm_kinematics::PR2ArmIKSolver::CartToJnt
int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &p_in, KDL::JntArray &q_out) override
Definition: pr2_arm_kinematics_plugin.cpp:103
pr2_arm_kinematics::PR2ArmKinematicsPlugin::dimension_
int dimension_
Definition: pr2_arm_kinematics_plugin.h:278
pr2_arm_kinematics::PR2ArmKinematicsPlugin::getLinkNames
const std::vector< std::string > & getLinkNames() const override
Return all the link names in the order they are represented internally.
Definition: pr2_arm_kinematics_plugin.cpp:415
pr2_arm_kinematics::PR2ArmKinematicsPlugin::getJointNames
const std::vector< std::string > & getJointNames() const override
Return all the joint names in the order they are used internally.
Definition: pr2_arm_kinematics_plugin.cpp:406
pr2_arm_kinematics::PR2ArmIK::init
bool init(const urdf::ModelInterface &robot_model, const std::string &root_name, const std::string &tip_name)
Initialize the solver by providing a urdf::Model and a root and tip name.
Definition: pr2_arm_ik.cpp:56
pr2_arm_kinematics::PR2ArmKinematicsPlugin::initialize
bool initialize(const moveit::core::RobotModel &robot_model, const std::string &group_name, const std::string &base_frame, const std::vector< std::string > &tip_frames, double search_discretization) override
Initialization function for the kinematics.
Definition: pr2_arm_kinematics_plugin.cpp:264
pr2_arm_kinematics::PR2ArmKinematicsPlugin::free_angle_
int free_angle_
Definition: pr2_arm_kinematics_plugin.h:275
ROS_DEBUG_NAMED
#define ROS_DEBUG_NAMED(name,...)
pr2_arm_kinematics::PR2ArmIKSolver
Definition: pr2_arm_kinematics_plugin.h:101
pr2_arm_kinematics::PR2ArmIK::computeIKShoulderPan
void computeIKShoulderPan(const Eigen::Isometry3f &g_in, const double &shoulder_pan_initial_guess, std::vector< std::vector< double > > &solution) const
compute IK based on an initial guess for the shoulder pan angle.
Definition: pr2_arm_ik.cpp:197
pr2_arm_kinematics::PR2ArmKinematicsPlugin::pr2_arm_ik_solver_
pr2_arm_kinematics::PR2ArmIKSolverPtr pr2_arm_ik_solver_
Definition: pr2_arm_kinematics_plugin.h:276
pr2_arm_kinematics::computeEuclideanDistance
double computeEuclideanDistance(const std::vector< double > &array_1, const KDL::JntArray &array_2)
Definition: pr2_arm_kinematics_plugin.cpp:235
ROS_ERROR
#define ROS_ERROR(...)
pr2_arm_kinematics::PR2ArmKinematicsPlugin::jnt_to_pose_solver_
std::shared_ptr< KDL::ChainFkSolverPos_recursive > jnt_to_pose_solver_
Definition: pr2_arm_kinematics_plugin.h:279
pr2_arm_kinematics::NO_IK_SOLUTION
static const int NO_IK_SOLUTION
Definition: pr2_arm_kinematics_plugin.h:95
pr2_arm_kinematics::KDLToEigenMatrix
Eigen::Isometry3f KDLToEigenMatrix(const KDL::Frame &p)
Definition: pr2_arm_kinematics_plugin.cpp:221
kdl_parser.hpp
pr2_arm_kinematics::PR2ArmIKSolver::root_frame_name_
std::string root_frame_name_
Definition: pr2_arm_kinematics_plugin.h:154
pr2_arm_kinematics::PR2ArmKinematicsPlugin::PR2ArmKinematicsPlugin
PR2ArmKinematicsPlugin()
Definition: pr2_arm_kinematics_plugin.cpp:255
kinematics::KinematicsBase::tip_frames_
std::vector< std::string > tip_frames_
Definition: kinematics_base.h:590
ros::Time
pr2_arm_kinematics::PR2ArmIKSolver::cartToJntSearch
int cartToJntSearch(const KDL::JntArray &q_in, const KDL::Frame &p_in, KDL::JntArray &q_out, const double &timeout)
Definition: pr2_arm_kinematics_plugin.cpp:159
kinematics::KinematicsQueryOptions
A set of options for the kinematics solver.
Definition: kinematics_base.h:107
std
pr2_arm_kinematics::PR2ArmIKSolver::updateInternalDataStructures
void updateInternalDataStructures()
Definition: pr2_arm_kinematics_plugin.cpp:97
pr2_arm_kinematics::PR2ArmIKSolver::free_angle_
int free_angle_
Definition: pr2_arm_kinematics_plugin.h:152
pr2_arm_kinematics::PR2ArmIKSolver::active_
bool active_
Indicates whether the solver has been successfully initialized.
Definition: pr2_arm_kinematics_plugin.h:136
pr2_arm_kinematics::PR2ArmIKSolver::pr2_arm_ik_
PR2ArmIK pr2_arm_ik_
The PR2 inverse kinematics solver.
Definition: pr2_arm_kinematics_plugin.h:131
pr2_arm_kinematics::PR2ArmKinematicsPlugin::ik_solver_info_
moveit_msgs::KinematicSolverInfo ik_solver_info_
Definition: pr2_arm_kinematics_plugin.h:281
pr2_arm_kinematics::distance
double distance(const urdf::Pose &transform)
Definition: pr2_arm_ik.h:86
kinematics::KinematicsBase::base_frame_
std::string base_frame_
Definition: kinematics_base.h:589
pr2_arm_kinematics::PR2ArmKinematicsPlugin::searchPositionIK
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 override
Given a desired pose of the end-effector, search for the joint angles required to reach it....
Definition: pr2_arm_kinematics_plugin.cpp:323
pr2_arm_kinematics::PR2ArmIKSolver::search_discretization_angle_
double search_discretization_angle_
Definition: pr2_arm_kinematics_plugin.h:150
pr2_arm_kinematics::PR2ArmIK::solver_info_
moveit_msgs::KinematicSolverInfo solver_info_
get chain information about the arm.
Definition: pr2_arm_ik.h:197
ros::Time::now
static Time now()
verbose
bool verbose


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Tue Nov 24 2020 03:26:40