moveit2
The MoveIt Motion Planning Framework for ROS 2.
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 <tf2_kdl/tf2_kdl.hpp>
41 #include <tf2/transform_datatypes.h>
42 
43 #include <kdl_parser/kdl_parser.hpp>
44 #include <kdl/chainfksolverpos_recursive.hpp>
45 #include <kdl/frames_io.hpp>
46 #include <kdl/kinfam_io.hpp>
47 
48 namespace kdl_kinematics_plugin
49 {
50 static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_kdl_kinematics_plugin.kdl_kinematics_plugin");
51 static rclcpp::Clock steady_clock = rclcpp::Clock(RCL_ROS_TIME);
52 
54 {
55 }
56 
57 void KDLKinematicsPlugin::getRandomConfiguration(Eigen::VectorXd& jnt_array) const
58 {
59  state_->setToRandomPositions(joint_model_group_);
60  state_->copyJointGroupPositions(joint_model_group_, &jnt_array[0]);
61 }
62 
63 void KDLKinematicsPlugin::getRandomConfiguration(const Eigen::VectorXd& seed_state,
64  const std::vector<double>& consistency_limits,
65  Eigen::VectorXd& jnt_array) const
66 {
67  joint_model_group_->getVariableRandomPositionsNearBy(state_->getRandomNumberGenerator(), &jnt_array[0],
68  &seed_state[0], consistency_limits);
69 }
70 
71 bool KDLKinematicsPlugin::checkConsistency(const Eigen::VectorXd& seed_state,
72  const std::vector<double>& consistency_limits,
73  const Eigen::VectorXd& solution) const
74 {
75  for (std::size_t i = 0; i < dimension_; ++i)
76  {
77  if (fabs(seed_state(i) - solution(i)) > consistency_limits[i])
78  return false;
79  }
80  return true;
81 }
82 
83 void KDLKinematicsPlugin::getJointWeights()
84 {
85  const std::vector<std::string> joint_names = joint_model_group_->getActiveJointModelNames();
86  // Default all joint weights to 1.0
87  joint_weights_ = std::vector<double>(joint_names.size(), 1.0);
88 
89  // Check if joint weight is assigned in kinematics YAML
90  // Loop through map (key: joint name and value: Struct with a weight member variable)
91  for (const auto& joint_weight : params_.joints_map)
92  {
93  // Check if joint is an active joint in the group
94  const auto joint_name = joint_weight.first;
95  auto it = std::find(joint_names.begin(), joint_names.end(), joint_name);
96  if (it == joint_names.cend())
97  {
98  RCLCPP_WARN(LOGGER, "Joint '%s' is not an active joint in group '%s'", joint_name.c_str(),
99  joint_model_group_->getName().c_str());
100  continue;
101  }
102 
103  // Find index of the joint name and assign weight to the coressponding index
104  joint_weights_.at(it - joint_names.begin()) = joint_weight.second.weight;
105  }
106 
107  RCLCPP_INFO_STREAM(
108  LOGGER, "Joint weights for group '"
109  << getGroupName() << "': "
110  << Eigen::Map<const Eigen::VectorXd>(joint_weights_.data(), joint_weights_.size()).transpose());
111 }
112 
113 bool KDLKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModel& robot_model,
114  const std::string& group_name, const std::string& base_frame,
115  const std::vector<std::string>& tip_frames, double search_discretization)
116 {
117  node_ = node;
118 
119  // Get Solver Parameters
120  std::string kinematics_param_prefix = "robot_description_kinematics." + group_name;
121  param_listener_ = std::make_shared<kdl_kinematics::ParamListener>(node, kinematics_param_prefix);
122  params_ = param_listener_->get_params();
123 
124  storeValues(robot_model, group_name, base_frame, tip_frames, search_discretization);
125  joint_model_group_ = robot_model_->getJointModelGroup(group_name);
126  if (!joint_model_group_)
127  return false;
128 
129  if (!joint_model_group_->isChain())
130  {
131  RCLCPP_ERROR(LOGGER, "Group '%s' is not a chain", group_name.c_str());
132  return false;
133  }
134  if (!joint_model_group_->isSingleDOFJoints())
135  {
136  RCLCPP_ERROR(LOGGER, "Group '%s' includes joints that have more than 1 DOF", group_name.c_str());
137  return false;
138  }
139 
140  KDL::Tree kdl_tree;
141 
142  if (!kdl_parser::treeFromUrdfModel(*robot_model.getURDF(), kdl_tree))
143  {
144  RCLCPP_ERROR(LOGGER, "Could not initialize tree object");
145  return false;
146  }
147  if (!kdl_tree.getChain(base_frame_, getTipFrame(), kdl_chain_))
148  {
149  RCLCPP_ERROR(LOGGER, "Could not initialize chain object");
150  return false;
151  }
152 
153  dimension_ = joint_model_group_->getActiveJointModels().size() + joint_model_group_->getMimicJointModels().size();
154  for (std::size_t i = 0; i < joint_model_group_->getJointModels().size(); ++i)
155  {
156  if (joint_model_group_->getJointModels()[i]->getType() == moveit::core::JointModel::REVOLUTE ||
157  joint_model_group_->getJointModels()[i]->getType() == moveit::core::JointModel::PRISMATIC)
158  {
159  solver_info_.joint_names.push_back(joint_model_group_->getJointModelNames()[i]);
160  const std::vector<moveit_msgs::msg::JointLimits>& jvec =
161  joint_model_group_->getJointModels()[i]->getVariableBoundsMsg();
162  solver_info_.limits.insert(solver_info_.limits.end(), jvec.begin(), jvec.end());
163  }
164  }
165 
166  if (!joint_model_group_->hasLinkModel(getTipFrame()))
167  {
168  RCLCPP_ERROR(LOGGER, "Could not find tip name in joint group '%s'", group_name.c_str());
169  return false;
170  }
171  solver_info_.link_names.push_back(getTipFrame());
172 
173  joint_min_.resize(solver_info_.limits.size());
174  joint_max_.resize(solver_info_.limits.size());
175 
176  for (unsigned int i = 0; i < solver_info_.limits.size(); ++i)
177  {
178  joint_min_(i) = solver_info_.limits[i].min_position;
179  joint_max_(i) = solver_info_.limits[i].max_position;
180  }
181 
182  getJointWeights();
183 
184  // Check for mimic joints
185  unsigned int joint_counter = 0;
186  for (std::size_t i = 0; i < kdl_chain_.getNrOfSegments(); ++i)
187  {
188  const moveit::core::JointModel* jm = robot_model_->getJointModel(kdl_chain_.segments[i].getJoint().getName());
189 
190  // first check whether it belongs to the set of active joints in the group
191  if (jm->getMimic() == nullptr && jm->getVariableCount() > 0)
192  {
193  JointMimic mimic_joint;
194  mimic_joint.reset(joint_counter);
195  mimic_joint.joint_name = kdl_chain_.segments[i].getJoint().getName();
196  mimic_joint.active = true;
197  mimic_joints_.push_back(mimic_joint);
198  ++joint_counter;
199  continue;
200  }
201  if (joint_model_group_->hasJointModel(jm->getName()))
202  {
203  if (jm->getMimic() && joint_model_group_->hasJointModel(jm->getMimic()->getName()))
204  {
205  JointMimic mimic_joint;
206  mimic_joint.joint_name = kdl_chain_.segments[i].getJoint().getName();
207  mimic_joint.offset = jm->getMimicOffset();
208  mimic_joint.multiplier = jm->getMimicFactor();
209  mimic_joints_.push_back(mimic_joint);
210  continue;
211  }
212  }
213  }
214  for (JointMimic& mimic_joint : mimic_joints_)
215  {
216  if (!mimic_joint.active)
217  {
218  const moveit::core::JointModel* joint_model =
219  joint_model_group_->getJointModel(mimic_joint.joint_name)->getMimic();
220  for (JointMimic& mimic_joint_recal : mimic_joints_)
221  {
222  if (mimic_joint_recal.joint_name == joint_model->getName())
223  {
224  mimic_joint.map_index = mimic_joint_recal.map_index;
225  }
226  }
227  }
228  }
229 
230  // Setup the joint state groups that we need
231  state_ = std::make_shared<moveit::core::RobotState>(robot_model_);
232 
233  fk_solver_ = std::make_unique<KDL::ChainFkSolverPos_recursive>(kdl_chain_);
234 
235  initialized_ = true;
236  RCLCPP_DEBUG(LOGGER, "KDL solver initialized");
237  return true;
238 }
239 
240 bool KDLKinematicsPlugin::timedOut(const rclcpp::Time& start_time, double duration) const
241 {
242  return ((steady_clock.now() - start_time).seconds() >= duration);
243 }
244 
245 bool KDLKinematicsPlugin::getPositionIK(const geometry_msgs::msg::Pose& ik_pose,
246  const std::vector<double>& ik_seed_state, std::vector<double>& solution,
247  moveit_msgs::msg::MoveItErrorCodes& error_code,
249 {
250  std::vector<double> consistency_limits;
251 
252  // limit search to a single attempt by setting a timeout of zero
253  return searchPositionIK(ik_pose, ik_seed_state, 0.0, consistency_limits, solution, IKCallbackFn(), error_code,
254  options);
255 }
256 
257 bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose,
258  const std::vector<double>& ik_seed_state, double timeout,
259  std::vector<double>& solution,
260  moveit_msgs::msg::MoveItErrorCodes& error_code,
262 {
263  std::vector<double> consistency_limits;
264 
265  return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, IKCallbackFn(), error_code,
266  options);
267 }
268 
269 bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose,
270  const std::vector<double>& ik_seed_state, double timeout,
271  const std::vector<double>& consistency_limits, std::vector<double>& solution,
272  moveit_msgs::msg::MoveItErrorCodes& error_code,
274 {
275  return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, IKCallbackFn(), error_code,
276  options);
277 }
278 
279 bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose,
280  const std::vector<double>& ik_seed_state, double timeout,
281  std::vector<double>& solution, const IKCallbackFn& solution_callback,
282  moveit_msgs::msg::MoveItErrorCodes& error_code,
284 {
285  std::vector<double> consistency_limits;
286  return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
287  options);
288 }
289 
290 bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose,
291  const std::vector<double>& ik_seed_state, double timeout,
292  const std::vector<double>& consistency_limits, std::vector<double>& solution,
293  const IKCallbackFn& solution_callback,
294  moveit_msgs::msg::MoveItErrorCodes& error_code,
296 {
297  const rclcpp::Time start_time = steady_clock.now();
298  if (!initialized_)
299  {
300  RCLCPP_ERROR(LOGGER, "kinematics solver not initialized");
301  error_code.val = error_code.NO_IK_SOLUTION;
302  return false;
303  }
304 
305  if (ik_seed_state.size() != dimension_)
306  {
307  RCLCPP_ERROR(LOGGER, "Seed state must have size %d instead of size %zu\n", dimension_, ik_seed_state.size());
308  error_code.val = error_code.NO_IK_SOLUTION;
309  return false;
310  }
311 
312  // Resize consistency limits to remove mimic joints
313  std::vector<double> consistency_limits_mimic;
314  if (!consistency_limits.empty())
315  {
316  if (consistency_limits.size() != dimension_)
317  {
318  RCLCPP_ERROR(LOGGER, "Consistency limits must be empty or have size %d instead of size %zu\n", dimension_,
319  consistency_limits.size());
320  error_code.val = error_code.NO_IK_SOLUTION;
321  return false;
322  }
323 
324  for (std::size_t i = 0; i < dimension_; ++i)
325  {
326  if (mimic_joints_[i].active)
327  consistency_limits_mimic.push_back(consistency_limits[i]);
328  }
329  }
330 
331  auto orientation_vs_position_weight = params_.position_only_ik ? 0.0 : params_.orientation_vs_position;
332  if (orientation_vs_position_weight == 0.0)
333  RCLCPP_INFO(LOGGER, "Using position only ik");
334 
335  Eigen::Matrix<double, 6, 1> cartesian_weights;
336  cartesian_weights.topRows<3>().setConstant(1.0);
337  cartesian_weights.bottomRows<3>().setConstant(orientation_vs_position_weight);
338 
339  KDL::JntArray jnt_seed_state(dimension_);
340  KDL::JntArray jnt_pos_in(dimension_);
341  KDL::JntArray jnt_pos_out(dimension_);
342  jnt_seed_state.data = Eigen::Map<const Eigen::VectorXd>(ik_seed_state.data(), ik_seed_state.size());
343  jnt_pos_in = jnt_seed_state;
344 
345  KDL::ChainIkSolverVelMimicSVD ik_solver_vel(kdl_chain_, mimic_joints_, orientation_vs_position_weight == 0.0);
346  solution.resize(dimension_);
347 
348  KDL::Frame pose_desired;
349  tf2::fromMsg(ik_pose, pose_desired);
350 
351  RCLCPP_DEBUG_STREAM(LOGGER, "searchPositionIK: Position request pose is "
352  << ik_pose.position.x << ' ' << ik_pose.position.y << ' ' << ik_pose.position.z << ' '
353  << ik_pose.orientation.x << ' ' << ik_pose.orientation.y << ' '
354  << ik_pose.orientation.z << ' ' << ik_pose.orientation.w);
355 
356  unsigned int attempt = 0;
357  do
358  {
359  ++attempt;
360  if (attempt > 1) // randomly re-seed after first attempt
361  {
362  if (!consistency_limits_mimic.empty())
363  {
364  getRandomConfiguration(jnt_seed_state.data, consistency_limits_mimic, jnt_pos_in.data);
365  }
366  else
367  {
368  getRandomConfiguration(jnt_pos_in.data);
369  }
370  RCLCPP_DEBUG_STREAM(LOGGER, "New random configuration (" << attempt << "): " << jnt_pos_in);
371  }
372 
373  int ik_valid =
374  CartToJnt(ik_solver_vel, jnt_pos_in, pose_desired, jnt_pos_out, params_.max_solver_iterations,
375  Eigen::Map<const Eigen::VectorXd>(joint_weights_.data(), joint_weights_.size()), cartesian_weights);
376  if (ik_valid == 0 || options.return_approximate_solution) // found acceptable solution
377  {
378  if (!consistency_limits_mimic.empty() &&
379  !checkConsistency(jnt_seed_state.data, consistency_limits_mimic, jnt_pos_out.data))
380  continue;
381 
382  Eigen::Map<Eigen::VectorXd>(solution.data(), solution.size()) = jnt_pos_out.data;
383  if (solution_callback)
384  {
385  solution_callback(ik_pose, solution, error_code);
386  if (error_code.val != error_code.SUCCESS)
387  continue;
388  }
389 
390  // solution passed consistency check and solution callback
391  error_code.val = error_code.SUCCESS;
392  RCLCPP_DEBUG_STREAM(LOGGER, "Solved after " << (steady_clock.now() - start_time).seconds() << " < " << timeout
393  << "s and " << attempt << " attempts");
394  return true;
395  }
396  } while (!timedOut(start_time, timeout));
397 
398  RCLCPP_DEBUG_STREAM(LOGGER, "IK timed out after " << (steady_clock.now() - start_time).seconds() << " > " << timeout
399  << "s and " << attempt << " attempts");
400  error_code.val = error_code.TIMED_OUT;
401  return false;
402 }
403 
404 // NOLINTNEXTLINE(readability-identifier-naming)
405 int KDLKinematicsPlugin::CartToJnt(KDL::ChainIkSolverVelMimicSVD& ik_solver, const KDL::JntArray& q_init,
406  const KDL::Frame& p_in, KDL::JntArray& q_out, const unsigned int max_iter,
407  const Eigen::VectorXd& joint_weights, const Twist& cartesian_weights) const
408 {
409  double last_delta_twist_norm = DBL_MAX;
410  double step_size = 1.0;
411  KDL::Frame f;
412  KDL::Twist delta_twist;
413  KDL::JntArray delta_q(q_out.rows()), q_backup(q_out.rows());
414  Eigen::ArrayXd extra_joint_weights(joint_weights.rows());
415  extra_joint_weights.setOnes();
416 
417  q_out = q_init;
418  RCLCPP_DEBUG_STREAM(LOGGER, "Input: " << q_init);
419 
420  unsigned int i;
421  bool success = false;
422  for (i = 0; i < max_iter; ++i)
423  {
424  fk_solver_->JntToCart(q_out, f);
425  delta_twist = diff(f, p_in);
426  RCLCPP_DEBUG_STREAM(LOGGER, "[" << std::setw(3) << i << "] delta_twist: " << delta_twist);
427 
428  // check norms of position and orientation errors
429  const double position_error = delta_twist.vel.Norm();
430  const double orientation_error = ik_solver.isPositionOnly() ? 0 : delta_twist.rot.Norm();
431  const double delta_twist_norm = std::max(position_error, orientation_error);
432  if (delta_twist_norm <= params_.epsilon)
433  {
434  success = true;
435  break;
436  }
437 
438  if (delta_twist_norm >= last_delta_twist_norm)
439  {
440  // if the error increased, we are close to a singularity -> reduce step size
441  double old_step_size = step_size;
442  step_size *= std::min(0.2, last_delta_twist_norm / delta_twist_norm); // reduce scale;
443  KDL::Multiply(delta_q, step_size / old_step_size, delta_q);
444  RCLCPP_DEBUG(LOGGER, " error increased: %f -> %f, scale: %f", last_delta_twist_norm, delta_twist_norm,
445  step_size);
446  q_out = q_backup; // restore previous unclipped joint values
447  }
448  else
449  {
450  q_backup = q_out; // remember joint values of last successful step
451  step_size = 1.0; // reset step size
452  last_delta_twist_norm = delta_twist_norm;
453 
454  ik_solver.CartToJnt(q_out, delta_twist, delta_q, extra_joint_weights * joint_weights.array(), cartesian_weights);
455  }
456 
457  clipToJointLimits(q_out, delta_q, extra_joint_weights);
458 
459  const double delta_q_norm = delta_q.data.lpNorm<1>();
460  RCLCPP_DEBUG(LOGGER, "[%3d] pos err: %f rot err: %f delta_q: %f", i, position_error, orientation_error,
461  delta_q_norm);
462  if (delta_q_norm < params_.epsilon) // stuck in singularity
463  {
464  if (step_size < params_.epsilon) // cannot reach target
465  break;
466  // wiggle joints
467  last_delta_twist_norm = DBL_MAX;
468  delta_q.data.setRandom();
469  delta_q.data *= std::min(0.1, delta_twist_norm);
470  clipToJointLimits(q_out, delta_q, extra_joint_weights);
471  extra_joint_weights.setOnes();
472  }
473 
474  KDL::Add(q_out, delta_q, q_out);
475 
476  RCLCPP_DEBUG_STREAM(LOGGER, " delta_q: " << delta_q);
477  RCLCPP_DEBUG_STREAM(LOGGER, " q: " << q_out);
478  }
479 
480  int result = (i == max_iter) ? -3 : (success ? 0 : -2);
481  RCLCPP_DEBUG_STREAM(LOGGER, "Result " << result << " after " << i << " iterations: " << q_out);
482 
483  return result;
484 }
485 
486 void KDLKinematicsPlugin::clipToJointLimits(const KDL::JntArray& q, KDL::JntArray& q_delta,
487  Eigen::ArrayXd& weighting) const
488 {
489  weighting.setOnes();
490  for (std::size_t i = 0; i < q.rows(); ++i)
491  {
492  const double delta_max = joint_max_(i) - q(i);
493  const double delta_min = joint_min_(i) - q(i);
494  if (q_delta(i) > delta_max)
495  {
496  q_delta(i) = delta_max;
497  }
498  else if (q_delta(i) < delta_min)
499  {
500  q_delta(i) = delta_min;
501  }
502  else
503  {
504  continue;
505  }
506 
507  weighting[mimic_joints_[i].map_index] = 0.01;
508  }
509 }
510 
511 bool KDLKinematicsPlugin::getPositionFK(const std::vector<std::string>& link_names,
512  const std::vector<double>& joint_angles,
513  std::vector<geometry_msgs::msg::Pose>& poses) const
514 {
515  if (!initialized_)
516  {
517  RCLCPP_ERROR(LOGGER, "kinematics solver not initialized");
518  return false;
519  }
520  poses.resize(link_names.size());
521  if (joint_angles.size() != dimension_)
522  {
523  RCLCPP_ERROR(LOGGER, "Joint angles vector must have size: %d", dimension_);
524  return false;
525  }
526 
527  KDL::Frame p_out;
528  KDL::JntArray jnt_pos_in(dimension_);
529  jnt_pos_in.data = Eigen::Map<const Eigen::VectorXd>(joint_angles.data(), joint_angles.size());
530 
531  bool valid = true;
532  for (unsigned int i = 0; i < poses.size(); ++i)
533  {
534  if (fk_solver_->JntToCart(jnt_pos_in, p_out) >= 0)
535  {
536  poses[i] = tf2::toMsg(p_out);
537  }
538  else
539  {
540  RCLCPP_ERROR(LOGGER, "Could not compute FK for %s", link_names[i].c_str());
541  valid = false;
542  }
543  }
544  return valid;
545 }
546 
547 const std::vector<std::string>& KDLKinematicsPlugin::getJointNames() const
548 {
549  return solver_info_.joint_names;
550 }
551 
552 const std::vector<std::string>& KDLKinematicsPlugin::getLinkNames() const
553 {
554  return solver_info_.link_names;
555 }
556 
557 } // namespace kdl_kinematics_plugin
558 
559 // register KDLKinematics as a KinematicsBase implementation
560 #include <class_loader/class_loader.hpp>
int CartToJnt(const JntArray &q_in, const Twist &v_in, JntArray &qdot_out) override
bool isPositionOnly() const
Return true iff we ignore orientation but only consider position for inverse kinematics.
A model of a mimic joint. Mimic joints are typically unactuated joints that are constrained to follow...
Definition: joint_mimic.hpp:46
double multiplier
Multiplier for this joint value from the joint that it mimics.
Definition: joint_mimic.hpp:56
bool active
If true, this joint is an active DOF and not a mimic joint.
Definition: joint_mimic.hpp:62
std::string joint_name
Name of this joint.
Definition: joint_mimic.hpp:60
void reset(unsigned int index)
Definition: joint_mimic.hpp:64
double offset
Offset for this joint value from the joint that it mimics.
Definition: joint_mimic.hpp:54
Specific implementation of kinematics using KDL. This version supports any kinematic chain,...
bool getPositionIK(const geometry_msgs::msg::Pose &ik_pose, const std::vector< double > &ik_seed_state, std::vector< double > &solution, moveit_msgs::msg::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.
bool initialize(const rclcpp::Node::SharedPtr &node, 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, for use with kinematic chain IK solvers.
bool searchPositionIK(const geometry_msgs::msg::Pose &ik_pose, const std::vector< double > &ik_seed_state, double timeout, std::vector< double > &solution, moveit_msgs::msg::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....
const std::vector< std::string > & getJointNames() const override
Return all the joint names in the order they are used internally.
const std::vector< std::string > & getLinkNames() const override
Return all the link names in the order they are represented internally.
bool getPositionFK(const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::msg::Pose > &poses) const override
Given a set of joint angles and a set of links, compute their pose.
int CartToJnt(KDL::ChainIkSolverVelMimicSVD &ik_solver, const KDL::JntArray &q_init, const KDL::Frame &p_in, KDL::JntArray &q_out, const unsigned int max_iter, const Eigen::VectorXd &joint_weights, const Twist &cartesian_weights) const
Solve position IK given initial joint values.
Provides an interface for kinematics solvers.
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)
virtual const std::string & getTipFrame() const
Return the name of the tip frame of the chain on which the solver is operating. This is usually a lin...
static const rclcpp::Logger LOGGER
rclcpp::Node::SharedPtr node_
std::function< void(const geometry_msgs::msg::Pose &, const std::vector< double > &, moveit_msgs::msg::MoveItErrorCodes &)> IKCallbackFn
Signature for a callback to validate an IK solution. Typically used for collision checking.
moveit::core::RobotModelConstPtr robot_model_
virtual const std::string & getGroupName() const
Return the name of the group that the solver is operating on.
bool isChain() const
Check if this group is a linear chain.
const std::vector< const JointModel * > & getActiveJointModels() const
Get the active joints in this group (that have controllable DOF). This does not include mimic joints.
const std::string & getName() const
Get the name of the joint group.
void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator &rng, double *values, const double *near, const double distance) const
Compute random values for the state of the joint group.
const std::vector< std::string > & getJointModelNames() const
Get the names of the joints in this group. These are the names of the joints returned by getJointMode...
const std::vector< const JointModel * > & getJointModels() const
Get all the joints in this group (including fixed and mimic joints).
const JointModel * getJointModel(const std::string &joint) const
Get a joint by its name. Throw an exception if the joint is not part of this group.
bool hasJointModel(const std::string &joint) const
Check if a joint is part of this group.
bool hasLinkModel(const std::string &link) const
Check if a link is part of this group.
bool isSingleDOFJoints() const
Return true if the group consists only of joints that are single DOF.
const std::vector< const JointModel * > & getMimicJointModels() const
Get the mimic joints that are part of this group.
const std::vector< std::string > & getActiveJointModelNames() const
Get the names of the active joints in this group. These are the names of the joints returned by getJo...
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
Definition: joint_model.h:117
double getMimicFactor() const
If mimicking a joint, this is the multiplicative factor for that joint's value.
Definition: joint_model.h:402
std::size_t getVariableCount() const
Get the number of variables that describe this joint.
Definition: joint_model.h:210
const std::string & getName() const
Get the name of the joint.
Definition: joint_model.h:145
double getMimicOffset() const
If mimicking a joint, this is the offset added to that joint's value.
Definition: joint_model.h:396
const JointModel * getMimic() const
Get the joint this one is mimicking.
Definition: joint_model.h:390
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:76
const urdf::ModelInterfaceSharedPtr & getURDF() const
Get the parsed URDF model.
Definition: robot_model.h:106
CLASS_LOADER_REGISTER_CLASS(stomp_moveit::StompSmoothingAdapter, planning_request_adapter::PlanningRequestAdapter)
A set of options for the kinematics solver.