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",
312  redundant_joints.back().c_str());
313  }
314  }
315  */
316  std::vector<unsigned int> redundant_joints_map_index;
317  unsigned int counter = 0;
318  for (std::size_t i = 0; i < dimension_; ++i)
319  {
320  bool is_redundant_joint = false;
321  for (std::size_t j = 0; j < redundant_joints.size(); ++j)
322  {
323  if (i == redundant_joints[j])
324  {
325  is_redundant_joint = true;
326  counter++;
327  break;
328  }
329  }
330  if (!is_redundant_joint)
331  {
332  // check for mimic
333  if (mimic_joints_[i].active)
334  {
335  redundant_joints_map_index.push_back(counter);
336  counter++;
337  }
338  }
339  }
340  for (std::size_t i = 0; i < redundant_joints_map_index.size(); ++i)
341  ROS_DEBUG_NAMED("kdl", "Redundant joint map index: %d %d", (int)i, (int)redundant_joints_map_index[i]);
342 
343  redundant_joints_map_index_ = redundant_joints_map_index;
344  redundant_joint_indices_ = redundant_joints;
345  return true;
346 }
347 
348 int KDLKinematicsPlugin::getJointIndex(const std::string& name) const
349 {
350  for (unsigned int i = 0; i < ik_chain_info_.joint_names.size(); i++)
351  {
352  if (ik_chain_info_.joint_names[i] == name)
353  return i;
354  }
355  return -1;
356 }
357 
358 int KDLKinematicsPlugin::getKDLSegmentIndex(const std::string& name) const
359 {
360  int i = 0;
361  while (i < (int)kdl_chain_.getNrOfSegments())
362  {
363  if (kdl_chain_.getSegment(i).getName() == name)
364  {
365  return i + 1;
366  }
367  i++;
368  }
369  return -1;
370 }
371 
372 bool KDLKinematicsPlugin::timedOut(const ros::WallTime& start_time, double duration) const
373 {
374  return ((ros::WallTime::now() - start_time).toSec() >= duration);
375 }
376 
377 bool KDLKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
378  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
379  const kinematics::KinematicsQueryOptions& options) const
380 {
381  const IKCallbackFn solution_callback = 0;
382  std::vector<double> consistency_limits;
383 
384  return searchPositionIK(ik_pose, ik_seed_state, default_timeout_, solution, solution_callback, error_code,
385  consistency_limits, options);
386 }
387 
388 bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
389  double timeout, std::vector<double>& solution,
390  moveit_msgs::MoveItErrorCodes& error_code,
391  const kinematics::KinematicsQueryOptions& options) const
392 {
393  const IKCallbackFn solution_callback = 0;
394  std::vector<double> consistency_limits;
395 
396  return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
397  options);
398 }
399 
400 bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
401  double timeout, const std::vector<double>& consistency_limits,
402  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
403  const kinematics::KinematicsQueryOptions& options) const
404 {
405  const IKCallbackFn solution_callback = 0;
406  return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
407  options);
408 }
409 
410 bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
411  double timeout, std::vector<double>& solution,
412  const IKCallbackFn& solution_callback,
413  moveit_msgs::MoveItErrorCodes& error_code,
414  const kinematics::KinematicsQueryOptions& options) const
415 {
416  std::vector<double> consistency_limits;
417  return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
418  options);
419 }
420 
421 bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
422  double timeout, const std::vector<double>& consistency_limits,
423  std::vector<double>& solution, const IKCallbackFn& solution_callback,
424  moveit_msgs::MoveItErrorCodes& error_code,
425  const kinematics::KinematicsQueryOptions& options) const
426 {
427  return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
428  options);
429 }
430 
431 bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
432  double timeout, std::vector<double>& solution,
433  const IKCallbackFn& solution_callback,
434  moveit_msgs::MoveItErrorCodes& error_code,
435  const std::vector<double>& consistency_limits,
436  const kinematics::KinematicsQueryOptions& options) const
437 {
439  if (!active_)
440  {
441  ROS_ERROR_NAMED("kdl", "kinematics not active");
442  error_code.val = error_code.NO_IK_SOLUTION;
443  return false;
444  }
445 
446  if (ik_seed_state.size() != dimension_)
447  {
448  ROS_ERROR_STREAM_NAMED("kdl", "Seed state must have size " << dimension_ << " instead of size "
449  << ik_seed_state.size());
450  error_code.val = error_code.NO_IK_SOLUTION;
451  return false;
452  }
453 
454  if (!consistency_limits.empty() && consistency_limits.size() != dimension_)
455  {
456  ROS_ERROR_STREAM_NAMED("kdl", "Consistency limits be empty or must have size " << dimension_ << " instead of size "
457  << consistency_limits.size());
458  error_code.val = error_code.NO_IK_SOLUTION;
459  return false;
460  }
461 
462  KDL::JntArray jnt_seed_state(dimension_);
463  KDL::JntArray jnt_pos_in(dimension_);
464  KDL::JntArray jnt_pos_out(dimension_);
465 
467  KDL::ChainIkSolverVel_pinv_mimic ik_solver_vel(kdl_chain_, joint_model_group_->getMimicJointModels().size(),
469  KDL::ChainIkSolverPos_NR_JL_Mimic ik_solver_pos(kdl_chain_, joint_min_, joint_max_, fk_solver, ik_solver_vel,
471  ik_solver_vel.setMimicJoints(mimic_joints_);
472  ik_solver_pos.setMimicJoints(mimic_joints_);
473 
474  if ((redundant_joint_indices_.size() > 0) && !ik_solver_vel.setRedundantJointsMapIndex(redundant_joints_map_index_))
475  {
476  ROS_ERROR_NAMED("kdl", "Could not set redundant joints");
477  return false;
478  }
479 
480  if (options.lock_redundant_joints)
481  {
482  ik_solver_vel.lockRedundantJoints();
483  }
484 
485  solution.resize(dimension_);
486 
487  KDL::Frame pose_desired;
488  tf::poseMsgToKDL(ik_pose, pose_desired);
489 
490  ROS_DEBUG_STREAM_NAMED("kdl", "searchPositionIK2: Position request pose is "
491  << ik_pose.position.x << " " << ik_pose.position.y << " " << ik_pose.position.z
492  << " " << ik_pose.orientation.x << " " << ik_pose.orientation.y << " "
493  << ik_pose.orientation.z << " " << ik_pose.orientation.w);
494  // Do the IK
495  for (unsigned int i = 0; i < dimension_; i++)
496  jnt_seed_state(i) = ik_seed_state[i];
497  jnt_pos_in = jnt_seed_state;
498 
499  unsigned int counter(0);
500  while (1)
501  {
502  // ROS_DEBUG_NAMED("kdl","Iteration: %d, time: %f, Timeout:
503  // %f",counter,(ros::WallTime::now()-n1).toSec(),timeout);
504  counter++;
505  if (timedOut(n1, timeout))
506  {
507  ROS_DEBUG_NAMED("kdl", "IK timed out");
508  error_code.val = error_code.TIMED_OUT;
509  ik_solver_vel.unlockRedundantJoints();
510  return false;
511  }
512  int ik_valid = ik_solver_pos.CartToJnt(jnt_pos_in, pose_desired, jnt_pos_out);
513  ROS_DEBUG_NAMED("kdl", "IK valid: %d", ik_valid);
514  if (!consistency_limits.empty())
515  {
516  getRandomConfiguration(jnt_seed_state, consistency_limits, jnt_pos_in, options.lock_redundant_joints);
517  if ((ik_valid < 0 && !options.return_approximate_solution) ||
518  !checkConsistency(jnt_seed_state, consistency_limits, jnt_pos_out))
519  {
520  ROS_DEBUG_NAMED("kdl", "Could not find IK solution: does not match consistency limits");
521  continue;
522  }
523  }
524  else
525  {
526  getRandomConfiguration(jnt_pos_in, options.lock_redundant_joints);
527  ROS_DEBUG_NAMED("kdl", "New random configuration");
528  for (unsigned int j = 0; j < dimension_; j++)
529  ROS_DEBUG_NAMED("kdl", "%d %f", j, jnt_pos_in(j));
530 
531  if (ik_valid < 0 && !options.return_approximate_solution)
532  {
533  ROS_DEBUG_NAMED("kdl", "Could not find IK solution");
534  continue;
535  }
536  }
537  ROS_DEBUG_NAMED("kdl", "Found IK solution");
538  for (unsigned int j = 0; j < dimension_; j++)
539  solution[j] = jnt_pos_out(j);
540  if (!solution_callback.empty())
541  solution_callback(ik_pose, solution, error_code);
542  else
543  error_code.val = error_code.SUCCESS;
544 
545  if (error_code.val == error_code.SUCCESS)
546  {
547  ROS_DEBUG_STREAM_NAMED("kdl", "Solved after " << counter << " iterations");
548  ik_solver_vel.unlockRedundantJoints();
549  return true;
550  }
551  }
552  ROS_DEBUG_NAMED("kdl", "An IK that satisifes the constraints and is collision free could not be found");
553  error_code.val = error_code.NO_IK_SOLUTION;
554  ik_solver_vel.unlockRedundantJoints();
555  return false;
556 }
557 
558 bool KDLKinematicsPlugin::getPositionFK(const std::vector<std::string>& link_names,
559  const std::vector<double>& joint_angles,
560  std::vector<geometry_msgs::Pose>& poses) const
561 {
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 Wed Jul 10 2019 04:03:41