kdl_kinematics_plugin.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, 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, David Lu!!, Ugo Cupcic */
36 
39 
40 //#include <tf/transform_datatypes.h>
41 #include <tf_conversions/tf_kdl.h>
43 
44 // URDF, SRDF
45 #include <urdf_model/model.h>
46 #include <srdfdom/model.h>
47 
49 
50 // register KDLKinematics as a KinematicsBase implementation
52 
54 {
55 KDLKinematicsPlugin::KDLKinematicsPlugin() : active_(false)
56 {
57 }
58 
59 void KDLKinematicsPlugin::getRandomConfiguration(KDL::JntArray& jnt_array, bool lock_redundancy) const
60 {
61  std::vector<double> jnt_array_vector(dimension_, 0.0);
62  state_->setToRandomPositions(joint_model_group_);
63  state_->copyJointGroupPositions(joint_model_group_, &jnt_array_vector[0]);
64  for (std::size_t i = 0; i < dimension_; ++i)
65  {
66  if (lock_redundancy)
67  if (isRedundantJoint(i))
68  continue;
69  jnt_array(i) = jnt_array_vector[i];
70  }
71 }
72 
73 bool KDLKinematicsPlugin::isRedundantJoint(unsigned int index) const
74 {
75  for (std::size_t j = 0; j < redundant_joint_indices_.size(); ++j)
76  if (redundant_joint_indices_[j] == index)
77  return true;
78  return false;
79 }
80 
82  const std::vector<double>& consistency_limits,
83  KDL::JntArray& jnt_array, bool lock_redundancy) const
84 {
85  std::vector<double> values(dimension_, 0.0);
86  std::vector<double> near(dimension_, 0.0);
87  for (std::size_t i = 0; i < dimension_; ++i)
88  near[i] = seed_state(i);
89 
90  // Need to resize the consistency limits to remove mimic joints
91  std::vector<double> consistency_limits_mimic;
92  for (std::size_t i = 0; i < dimension_; ++i)
93  {
94  if (!mimic_joints_[i].active)
95  continue;
96  consistency_limits_mimic.push_back(consistency_limits[i]);
97  }
98 
99  joint_model_group_->getVariableRandomPositionsNearBy(state_->getRandomNumberGenerator(), values, near,
100  consistency_limits_mimic);
101 
102  for (std::size_t i = 0; i < dimension_; ++i)
103  {
104  bool skip = false;
105  if (lock_redundancy)
106  for (std::size_t j = 0; j < redundant_joint_indices_.size(); ++j)
107  if (redundant_joint_indices_[j] == i)
108  {
109  skip = true;
110  break;
111  }
112  if (skip)
113  continue;
114  jnt_array(i) = values[i];
115  }
116 }
117 
119  const std::vector<double>& consistency_limits,
120  const KDL::JntArray& solution) const
121 {
122  for (std::size_t i = 0; i < dimension_; ++i)
123  if (fabs(seed_state(i) - solution(i)) > consistency_limits[i])
124  return false;
125  return true;
126 }
127 
128 bool KDLKinematicsPlugin::initialize(const std::string& robot_description, const std::string& group_name,
129  const std::string& base_frame, const std::string& tip_frame,
130  double search_discretization)
131 {
132  setValues(robot_description, group_name, base_frame, tip_frame, search_discretization);
133 
135  const srdf::ModelSharedPtr& srdf = rdf_loader.getSRDF();
136  const urdf::ModelInterfaceSharedPtr& urdf_model = rdf_loader.getURDF();
137 
138  if (!urdf_model || !srdf)
139  {
140  ROS_ERROR_NAMED("kdl", "URDF and SRDF must be loaded for KDL kinematics solver to work.");
141  return false;
142  }
143 
144  robot_model_.reset(new robot_model::RobotModel(urdf_model, srdf));
145 
146  robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name);
147  if (!joint_model_group)
148  return false;
149 
150  if (!joint_model_group->isChain())
151  {
152  ROS_ERROR_NAMED("kdl", "Group '%s' is not a chain", group_name.c_str());
153  return false;
154  }
155  if (!joint_model_group->isSingleDOFJoints())
156  {
157  ROS_ERROR_NAMED("kdl", "Group '%s' includes joints that have more than 1 DOF", group_name.c_str());
158  return false;
159  }
160 
161  KDL::Tree kdl_tree;
162 
163  if (!kdl_parser::treeFromUrdfModel(*urdf_model, kdl_tree))
164  {
165  ROS_ERROR_NAMED("kdl", "Could not initialize tree object");
166  return false;
167  }
168  if (!kdl_tree.getChain(base_frame_, getTipFrame(), kdl_chain_))
169  {
170  ROS_ERROR_NAMED("kdl", "Could not initialize chain object");
171  return false;
172  }
173 
174  dimension_ = joint_model_group->getActiveJointModels().size() + joint_model_group->getMimicJointModels().size();
175  for (std::size_t i = 0; i < joint_model_group->getJointModels().size(); ++i)
176  {
177  if (joint_model_group->getJointModels()[i]->getType() == moveit::core::JointModel::REVOLUTE ||
178  joint_model_group->getJointModels()[i]->getType() == moveit::core::JointModel::PRISMATIC)
179  {
180  ik_chain_info_.joint_names.push_back(joint_model_group->getJointModelNames()[i]);
181  const std::vector<moveit_msgs::JointLimits>& jvec =
182  joint_model_group->getJointModels()[i]->getVariableBoundsMsg();
183  ik_chain_info_.limits.insert(ik_chain_info_.limits.end(), jvec.begin(), jvec.end());
184  }
185  }
186 
187  fk_chain_info_.joint_names = ik_chain_info_.joint_names;
188  fk_chain_info_.limits = ik_chain_info_.limits;
189 
190  if (!joint_model_group->hasLinkModel(getTipFrame()))
191  {
192  ROS_ERROR_NAMED("kdl", "Could not find tip name in joint group '%s'", group_name.c_str());
193  return false;
194  }
195  ik_chain_info_.link_names.push_back(getTipFrame());
196  fk_chain_info_.link_names = joint_model_group->getLinkModelNames();
197 
198  joint_min_.resize(ik_chain_info_.limits.size());
199  joint_max_.resize(ik_chain_info_.limits.size());
200 
201  for (unsigned int i = 0; i < ik_chain_info_.limits.size(); i++)
202  {
203  joint_min_(i) = ik_chain_info_.limits[i].min_position;
204  joint_max_(i) = ik_chain_info_.limits[i].max_position;
205  }
206 
207  // Get Solver Parameters
208  int max_solver_iterations;
209  double epsilon;
210  bool position_ik;
211 
212  lookupParam("max_solver_iterations", max_solver_iterations, 500);
213  lookupParam("epsilon", epsilon, 1e-5);
214  lookupParam("position_only_ik", position_ik, false);
215  ROS_DEBUG_NAMED("kdl", "Looking for param name: position_only_ik");
216 
217  if (position_ik)
218  ROS_INFO_NAMED("kdl", "Using position only ik");
219 
221  kdl_chain_.getNrOfJoints() - joint_model_group->getMimicJointModels().size() - (position_ik ? 3 : 6);
222 
223  // Check for mimic joints
224  std::vector<unsigned int> redundant_joints_map_index;
225 
226  std::vector<JointMimic> mimic_joints;
227  unsigned int joint_counter = 0;
228  for (std::size_t i = 0; i < kdl_chain_.getNrOfSegments(); ++i)
229  {
230  const robot_model::JointModel* jm = robot_model_->getJointModel(kdl_chain_.segments[i].getJoint().getName());
231 
232  // first check whether it belongs to the set of active joints in the group
233  if (jm->getMimic() == NULL && jm->getVariableCount() > 0)
234  {
235  JointMimic mimic_joint;
236  mimic_joint.reset(joint_counter);
237  mimic_joint.joint_name = kdl_chain_.segments[i].getJoint().getName();
238  mimic_joint.active = true;
239  mimic_joints.push_back(mimic_joint);
240  ++joint_counter;
241  continue;
242  }
243  if (joint_model_group->hasJointModel(jm->getName()))
244  {
245  if (jm->getMimic() && joint_model_group->hasJointModel(jm->getMimic()->getName()))
246  {
247  JointMimic mimic_joint;
248  mimic_joint.reset(joint_counter);
249  mimic_joint.joint_name = kdl_chain_.segments[i].getJoint().getName();
250  mimic_joint.offset = jm->getMimicOffset();
251  mimic_joint.multiplier = jm->getMimicFactor();
252  mimic_joints.push_back(mimic_joint);
253  continue;
254  }
255  }
256  }
257  for (std::size_t i = 0; i < mimic_joints.size(); ++i)
258  {
259  if (!mimic_joints[i].active)
260  {
261  const robot_model::JointModel* joint_model =
262  joint_model_group->getJointModel(mimic_joints[i].joint_name)->getMimic();
263  for (std::size_t j = 0; j < mimic_joints.size(); ++j)
264  {
265  if (mimic_joints[j].joint_name == joint_model->getName())
266  {
267  mimic_joints[i].map_index = mimic_joints[j].map_index;
268  }
269  }
270  }
271  }
272  mimic_joints_ = mimic_joints;
273 
274  // Setup the joint state groups that we need
275  state_.reset(new robot_state::RobotState(robot_model_));
276  state_2_.reset(new robot_state::RobotState(robot_model_));
277 
278  // Store things for when the set of redundant joints may change
279  position_ik_ = position_ik;
280  joint_model_group_ = joint_model_group;
281  max_solver_iterations_ = max_solver_iterations;
282  epsilon_ = epsilon;
283 
284  active_ = true;
285  ROS_DEBUG_NAMED("kdl", "KDL solver initialized");
286  return true;
287 }
288 
289 bool KDLKinematicsPlugin::setRedundantJoints(const std::vector<unsigned int>& redundant_joints)
290 {
292  {
293  ROS_ERROR_NAMED("kdl", "This group cannot have redundant joints");
294  return false;
295  }
296  if (static_cast<int>(redundant_joints.size()) > num_possible_redundant_joints_)
297  {
298  ROS_ERROR_NAMED("kdl", "This group can only have %d redundant joints", num_possible_redundant_joints_);
299  return false;
300  }
301  /*
302  XmlRpc::XmlRpcValue joint_list;
303  if(private_handle.getParam(group_name+"/redundant_joints", joint_list))
304  {
305  ROS_ASSERT(joint_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
306  std::vector<std::string> redundant_joints;
307  for (std::size_t i = 0; i < joint_list.size(); ++i)
308  {
309  ROS_ASSERT(joint_list[i].getType() == XmlRpc::XmlRpcValue::TypeString);
310  redundant_joints.push_back(static_cast<std::string>(joint_list[i]));
311  ROS_INFO_NAMED("kdl","Designated joint: %s as redundant joint", redundant_joints.back().c_str());
312  }
313  }
314  */
315  std::vector<unsigned int> redundant_joints_map_index;
316  unsigned int counter = 0;
317  for (std::size_t i = 0; i < dimension_; ++i)
318  {
319  bool is_redundant_joint = false;
320  for (std::size_t j = 0; j < redundant_joints.size(); ++j)
321  {
322  if (i == redundant_joints[j])
323  {
324  is_redundant_joint = true;
325  counter++;
326  break;
327  }
328  }
329  if (!is_redundant_joint)
330  {
331  // check for mimic
332  if (mimic_joints_[i].active)
333  {
334  redundant_joints_map_index.push_back(counter);
335  counter++;
336  }
337  }
338  }
339  for (std::size_t i = 0; i < redundant_joints_map_index.size(); ++i)
340  ROS_DEBUG_NAMED("kdl", "Redundant joint map index: %d %d", (int)i, (int)redundant_joints_map_index[i]);
341 
342  redundant_joints_map_index_ = redundant_joints_map_index;
343  redundant_joint_indices_ = redundant_joints;
344  return true;
345 }
346 
347 int KDLKinematicsPlugin::getJointIndex(const std::string& name) const
348 {
349  for (unsigned int i = 0; i < ik_chain_info_.joint_names.size(); i++)
350  {
351  if (ik_chain_info_.joint_names[i] == name)
352  return i;
353  }
354  return -1;
355 }
356 
357 int KDLKinematicsPlugin::getKDLSegmentIndex(const std::string& name) const
358 {
359  int i = 0;
360  while (i < (int)kdl_chain_.getNrOfSegments())
361  {
362  if (kdl_chain_.getSegment(i).getName() == name)
363  {
364  return i + 1;
365  }
366  i++;
367  }
368  return -1;
369 }
370 
371 bool KDLKinematicsPlugin::timedOut(const ros::WallTime& start_time, double duration) const
372 {
373  return ((ros::WallTime::now() - start_time).toSec() >= duration);
374 }
375 
376 bool KDLKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
377  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
378  const kinematics::KinematicsQueryOptions& options) const
379 {
380  const IKCallbackFn solution_callback = 0;
381  std::vector<double> consistency_limits;
382 
383  return searchPositionIK(ik_pose, ik_seed_state, default_timeout_, solution, solution_callback, error_code,
384  consistency_limits, options);
385 }
386 
387 bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
388  double timeout, std::vector<double>& solution,
389  moveit_msgs::MoveItErrorCodes& error_code,
390  const kinematics::KinematicsQueryOptions& options) const
391 {
392  const IKCallbackFn solution_callback = 0;
393  std::vector<double> consistency_limits;
394 
395  return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
396  options);
397 }
398 
399 bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
400  double timeout, const std::vector<double>& consistency_limits,
401  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
402  const kinematics::KinematicsQueryOptions& options) const
403 {
404  const IKCallbackFn solution_callback = 0;
405  return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
406  options);
407 }
408 
409 bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
410  double timeout, std::vector<double>& solution,
411  const IKCallbackFn& solution_callback,
412  moveit_msgs::MoveItErrorCodes& error_code,
413  const kinematics::KinematicsQueryOptions& options) const
414 {
415  std::vector<double> consistency_limits;
416  return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
417  options);
418 }
419 
420 bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
421  double timeout, const std::vector<double>& consistency_limits,
422  std::vector<double>& solution, const IKCallbackFn& solution_callback,
423  moveit_msgs::MoveItErrorCodes& error_code,
424  const kinematics::KinematicsQueryOptions& options) const
425 {
426  return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
427  options);
428 }
429 
430 bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
431  double timeout, std::vector<double>& solution,
432  const IKCallbackFn& solution_callback,
433  moveit_msgs::MoveItErrorCodes& error_code,
434  const std::vector<double>& consistency_limits,
435  const kinematics::KinematicsQueryOptions& options) const
436 {
438  if (!active_)
439  {
440  ROS_ERROR_NAMED("kdl", "kinematics not active");
441  error_code.val = error_code.NO_IK_SOLUTION;
442  return false;
443  }
444 
445  if (ik_seed_state.size() != dimension_)
446  {
447  ROS_ERROR_STREAM_NAMED("kdl", "Seed state must have size " << dimension_ << " instead of size "
448  << ik_seed_state.size());
449  error_code.val = error_code.NO_IK_SOLUTION;
450  return false;
451  }
452 
453  if (!consistency_limits.empty() && consistency_limits.size() != dimension_)
454  {
455  ROS_ERROR_STREAM_NAMED("kdl", "Consistency limits be empty or must have size " << dimension_ << " instead of size "
456  << consistency_limits.size());
457  error_code.val = error_code.NO_IK_SOLUTION;
458  return false;
459  }
460 
461  KDL::JntArray jnt_seed_state(dimension_);
462  KDL::JntArray jnt_pos_in(dimension_);
463  KDL::JntArray jnt_pos_out(dimension_);
464 
466  KDL::ChainIkSolverVel_pinv_mimic ik_solver_vel(kdl_chain_, joint_model_group_->getMimicJointModels().size(),
468  KDL::ChainIkSolverPos_NR_JL_Mimic ik_solver_pos(kdl_chain_, joint_min_, joint_max_, fk_solver, ik_solver_vel,
470  ik_solver_vel.setMimicJoints(mimic_joints_);
471  ik_solver_pos.setMimicJoints(mimic_joints_);
472 
473  if ((redundant_joint_indices_.size() > 0) && !ik_solver_vel.setRedundantJointsMapIndex(redundant_joints_map_index_))
474  {
475  ROS_ERROR_NAMED("kdl", "Could not set redundant joints");
476  return false;
477  }
478 
479  if (options.lock_redundant_joints)
480  {
481  ik_solver_vel.lockRedundantJoints();
482  }
483 
484  solution.resize(dimension_);
485 
486  KDL::Frame pose_desired;
487  tf::poseMsgToKDL(ik_pose, pose_desired);
488 
489  ROS_DEBUG_STREAM_NAMED("kdl", "searchPositionIK2: Position request pose is "
490  << ik_pose.position.x << " " << ik_pose.position.y << " " << ik_pose.position.z
491  << " " << ik_pose.orientation.x << " " << ik_pose.orientation.y << " "
492  << ik_pose.orientation.z << " " << ik_pose.orientation.w);
493  // Do the IK
494  for (unsigned int i = 0; i < dimension_; i++)
495  jnt_seed_state(i) = ik_seed_state[i];
496  jnt_pos_in = jnt_seed_state;
497 
498  unsigned int counter(0);
499  while (1)
500  {
501  // ROS_DEBUG_NAMED("kdl","Iteration: %d, time: %f, Timeout:
502  // %f",counter,(ros::WallTime::now()-n1).toSec(),timeout);
503  counter++;
504  if (timedOut(n1, timeout))
505  {
506  ROS_DEBUG_NAMED("kdl", "IK timed out");
507  error_code.val = error_code.TIMED_OUT;
508  ik_solver_vel.unlockRedundantJoints();
509  return false;
510  }
511  int ik_valid = ik_solver_pos.CartToJnt(jnt_pos_in, pose_desired, jnt_pos_out);
512  ROS_DEBUG_NAMED("kdl", "IK valid: %d", ik_valid);
513  if (!consistency_limits.empty())
514  {
515  getRandomConfiguration(jnt_seed_state, consistency_limits, jnt_pos_in, options.lock_redundant_joints);
516  if ((ik_valid < 0 && !options.return_approximate_solution) ||
517  !checkConsistency(jnt_seed_state, consistency_limits, jnt_pos_out))
518  {
519  ROS_DEBUG_NAMED("kdl", "Could not find IK solution: does not match consistency limits");
520  continue;
521  }
522  }
523  else
524  {
525  getRandomConfiguration(jnt_pos_in, options.lock_redundant_joints);
526  ROS_DEBUG_NAMED("kdl", "New random configuration");
527  for (unsigned int j = 0; j < dimension_; j++)
528  ROS_DEBUG_NAMED("kdl", "%d %f", j, jnt_pos_in(j));
529 
530  if (ik_valid < 0 && !options.return_approximate_solution)
531  {
532  ROS_DEBUG_NAMED("kdl", "Could not find IK solution");
533  continue;
534  }
535  }
536  ROS_DEBUG_NAMED("kdl", "Found IK solution");
537  for (unsigned int j = 0; j < dimension_; j++)
538  solution[j] = jnt_pos_out(j);
539  if (!solution_callback.empty())
540  solution_callback(ik_pose, solution, error_code);
541  else
542  error_code.val = error_code.SUCCESS;
543 
544  if (error_code.val == error_code.SUCCESS)
545  {
546  ROS_DEBUG_STREAM_NAMED("kdl", "Solved after " << counter << " iterations");
547  ik_solver_vel.unlockRedundantJoints();
548  return true;
549  }
550  }
551  ROS_DEBUG_NAMED("kdl", "An IK that satisifes the constraints and is collision free could not be found");
552  error_code.val = error_code.NO_IK_SOLUTION;
553  ik_solver_vel.unlockRedundantJoints();
554  return false;
555 }
556 
557 bool KDLKinematicsPlugin::getPositionFK(const std::vector<std::string>& link_names,
558  const std::vector<double>& joint_angles,
559  std::vector<geometry_msgs::Pose>& poses) const
560 {
562  if (!active_)
563  {
564  ROS_ERROR_NAMED("kdl", "kinematics not active");
565  return false;
566  }
567  poses.resize(link_names.size());
568  if (joint_angles.size() != dimension_)
569  {
570  ROS_ERROR_NAMED("kdl", "Joint angles vector must have size: %d", dimension_);
571  return false;
572  }
573 
574  KDL::Frame p_out;
575  geometry_msgs::PoseStamped pose;
576  tf::Stamped<tf::Pose> tf_pose;
577 
578  KDL::JntArray jnt_pos_in(dimension_);
579  for (unsigned int i = 0; i < dimension_; i++)
580  {
581  jnt_pos_in(i) = joint_angles[i];
582  }
583 
585 
586  bool valid = true;
587  for (unsigned int i = 0; i < poses.size(); i++)
588  {
589  ROS_DEBUG_NAMED("kdl", "End effector index: %d", getKDLSegmentIndex(link_names[i]));
590  if (fk_solver.JntToCart(jnt_pos_in, p_out, getKDLSegmentIndex(link_names[i])) >= 0)
591  {
592  tf::poseKDLToMsg(p_out, poses[i]);
593  }
594  else
595  {
596  ROS_ERROR_NAMED("kdl", "Could not compute FK for %s", link_names[i].c_str());
597  valid = false;
598  }
599  }
600  return valid;
601 }
602 
603 const std::vector<std::string>& KDLKinematicsPlugin::getJointNames() const
604 {
605  return ik_chain_info_.joint_names;
606 }
607 
608 const std::vector<std::string>& KDLKinematicsPlugin::getLinkNames() const
609 {
610  return ik_chain_info_.link_names;
611 }
612 
613 } // namespace
std::vector< unsigned int > redundant_joint_indices_
#define ROS_INFO_NAMED(name,...)
const Segment & getSegment(unsigned int nr) const
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)
#define ROS_DEBUG_STREAM_NAMED(name, args)
#define ROS_ERROR_STREAM_NAMED(name, args)
KDL_PARSER_PUBLIC bool treeFromUrdfModel(const urdf::ModelInterface &robot_model, KDL::Tree &tree)
std::vector< double > values
const srdf::ModelSharedPtr & getSRDF() const
int getKDLSegmentIndex(const std::string &name) const
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)
robot_model::JointModelGroup * joint_model_group_
unsigned int getNrOfSegments() const
double multiplier
Multiplier for this joint value from the joint that it mimics.
Definition: joint_mimic.hpp:57
virtual bool setRedundantJoints(const std::vector< unsigned int > &redundant_joint_indices)
void getRandomConfiguration(KDL::JntArray &jnt_array, bool lock_redundancy) const
bool timedOut(const ros::WallTime &start_time, double duration) const
int getJointIndex(const std::string &name) const
#define ROS_DEBUG_NAMED(name,...)
const std::string & getName() const
virtual bool getPositionFK(const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::Pose > &poses) const
bool checkConsistency(const KDL::JntArray &seed_state, const std::vector< double > &consistency_limit, const KDL::JntArray &solution) const
Check whether the solution lies within the consistency limit of the seed state.
Specific implementation of kinematics using KDL. This version can be used with any robot...
virtual const std::string & getTipFrame() const
bool active
If true, this joint is an active DOF and not a mimic joint.
Definition: joint_mimic.hpp:63
moveit_msgs::KinematicSolverInfo fk_chain_info_
void poseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k)
double epsilon
const std::vector< std::string > & getJointNames() const
Return all the joint names in the order they are used internally.
unsigned int getNrOfJoints() const
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)
bool lookupParam(const std::string &param, T &val, const T &default_val) const
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn
std::string joint_name
Name of this joint.
Definition: joint_mimic.hpp:61
A model of a mimic joint. Mimic joints are typically unactuated joints that are constrained to follow...
Definition: joint_mimic.hpp:46
void resize(unsigned int newSize)
bool getChain(const std::string &chain_root, const std::string &chain_tip, Chain &chain) const
static WallTime now()
void poseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m)
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
std::vector< Segment > segments
#define ROS_ERROR_NAMED(name,...)
double offset
Offset for this joint value from the joint that it mimics.
Definition: joint_mimic.hpp:52
void reset(unsigned int index)
Definition: joint_mimic.hpp:65
moveit_msgs::KinematicSolverInfo ik_chain_info_
bool isRedundantJoint(unsigned int index) const
std::vector< unsigned int > redundant_joints_map_index_
const std::vector< std::string > & getLinkNames() const
Return all the link names in the order they are represented internally.
CLASS_LOADER_REGISTER_CLASS(Dog, Base)
const urdf::ModelInterfaceSharedPtr & getURDF() const
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_kinematics
Author(s): Dave Coleman , Ioan Sucan , Sachin Chitta
autogenerated on Sat Apr 21 2018 02:55:30