lma_kinematics_plugin.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2016, CRI group, NTU, Singapore
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 CRI group 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: Francisco Suarez-Ruiz */
36 
39 
40 #include <tf_conversions/tf_kdl.h>
42 
43 // URDF, SRDF
44 #include <urdf_model/model.h>
45 #include <srdfdom/model.h>
46 
48 
49 // register KDLKinematics as a KinematicsBase implementation
51 
53 {
54 LMAKinematicsPlugin::LMAKinematicsPlugin() : active_(false)
55 {
56 }
57 
58 void LMAKinematicsPlugin::getRandomConfiguration(KDL::JntArray& jnt_array, bool lock_redundancy) const
59 {
60  std::vector<double> jnt_array_vector(dimension_, 0.0);
61  state_->setToRandomPositions(joint_model_group_);
62  state_->copyJointGroupPositions(joint_model_group_, &jnt_array_vector[0]);
63  for (std::size_t i = 0; i < dimension_; ++i)
64  {
65  if (lock_redundancy)
66  if (isRedundantJoint(i))
67  continue;
68  jnt_array(i) = jnt_array_vector[i];
69  }
70 }
71 
72 bool LMAKinematicsPlugin::isRedundantJoint(unsigned int index) const
73 {
74  for (std::size_t j = 0; j < redundant_joint_indices_.size(); ++j)
75  if (redundant_joint_indices_[j] == index)
76  return true;
77  return false;
78 }
79 
81  const std::vector<double>& consistency_limits,
82  KDL::JntArray& jnt_array, bool lock_redundancy) const
83 {
84  std::vector<double> values(dimension_, 0.0);
85  std::vector<double> near(dimension_, 0.0);
86  for (std::size_t i = 0; i < dimension_; ++i)
87  near[i] = seed_state(i);
88 
89  // Need to resize the consistency limits to remove mimic joints
90  std::vector<double> consistency_limits_mimic;
91  for (std::size_t i = 0; i < dimension_; ++i)
92  {
93  if (!mimic_joints_[i].active)
94  continue;
95  consistency_limits_mimic.push_back(consistency_limits[i]);
96  }
97 
98  joint_model_group_->getVariableRandomPositionsNearBy(state_->getRandomNumberGenerator(), values, near,
99  consistency_limits_mimic);
100 
101  for (std::size_t i = 0; i < dimension_; ++i)
102  {
103  bool skip = false;
104  if (lock_redundancy)
105  for (std::size_t j = 0; j < redundant_joint_indices_.size(); ++j)
106  if (redundant_joint_indices_[j] == i)
107  {
108  skip = true;
109  break;
110  }
111  if (skip)
112  continue;
113  jnt_array(i) = values[i];
114  }
115 }
116 
118  const std::vector<double>& consistency_limits,
119  const KDL::JntArray& solution) const
120 {
121  for (std::size_t i = 0; i < dimension_; ++i)
122  if (fabs(seed_state(i) - solution(i)) > consistency_limits[i])
123  return false;
124  return true;
125 }
126 
127 bool LMAKinematicsPlugin::initialize(const std::string& robot_description, const std::string& group_name,
128  const std::string& base_frame, const std::string& tip_frame,
129  double search_discretization)
130 {
131  setValues(robot_description, group_name, base_frame, tip_frame, search_discretization);
132 
134  const srdf::ModelSharedPtr& srdf = rdf_loader.getSRDF();
135  const urdf::ModelInterfaceSharedPtr& urdf_model = rdf_loader.getURDF();
136 
137  if (!urdf_model || !srdf)
138  {
139  ROS_ERROR_NAMED("lma", "URDF and SRDF must be loaded for KDL kinematics solver to work.");
140  return false;
141  }
142 
143  robot_model_.reset(new robot_model::RobotModel(urdf_model, srdf));
144 
145  robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name);
146  if (!joint_model_group)
147  return false;
148 
149  if (!joint_model_group->isChain())
150  {
151  ROS_ERROR_NAMED("lma", "Group '%s' is not a chain", group_name.c_str());
152  return false;
153  }
154  if (!joint_model_group->isSingleDOFJoints())
155  {
156  ROS_ERROR_NAMED("lma", "Group '%s' includes joints that have more than 1 DOF", group_name.c_str());
157  return false;
158  }
159 
160  KDL::Tree kdl_tree;
161 
162  if (!kdl_parser::treeFromUrdfModel(*urdf_model, kdl_tree))
163  {
164  ROS_ERROR_NAMED("lma", "Could not initialize tree object");
165  return false;
166  }
167  if (!kdl_tree.getChain(base_frame_, getTipFrame(), kdl_chain_))
168  {
169  ROS_ERROR_NAMED("lma", "Could not initialize chain object");
170  return false;
171  }
172 
173  dimension_ = joint_model_group->getActiveJointModels().size() + joint_model_group->getMimicJointModels().size();
174  for (std::size_t i = 0; i < joint_model_group->getJointModels().size(); ++i)
175  {
176  if (joint_model_group->getJointModels()[i]->getType() == moveit::core::JointModel::REVOLUTE ||
177  joint_model_group->getJointModels()[i]->getType() == moveit::core::JointModel::PRISMATIC)
178  {
179  ik_chain_info_.joint_names.push_back(joint_model_group->getJointModelNames()[i]);
180  const std::vector<moveit_msgs::JointLimits>& jvec =
181  joint_model_group->getJointModels()[i]->getVariableBoundsMsg();
182  ik_chain_info_.limits.insert(ik_chain_info_.limits.end(), jvec.begin(), jvec.end());
183  }
184  }
185 
186  fk_chain_info_.joint_names = ik_chain_info_.joint_names;
187  fk_chain_info_.limits = ik_chain_info_.limits;
188 
189  if (!joint_model_group->hasLinkModel(getTipFrame()))
190  {
191  ROS_ERROR_NAMED("lma", "Could not find tip name in joint group '%s'", group_name.c_str());
192  return false;
193  }
194  ik_chain_info_.link_names.push_back(getTipFrame());
195  fk_chain_info_.link_names = joint_model_group->getLinkModelNames();
196 
197  joint_min_.resize(ik_chain_info_.limits.size());
198  joint_max_.resize(ik_chain_info_.limits.size());
199 
200  for (unsigned int i = 0; i < ik_chain_info_.limits.size(); i++)
201  {
202  joint_min_(i) = ik_chain_info_.limits[i].min_position;
203  joint_max_(i) = ik_chain_info_.limits[i].max_position;
204  }
205 
206  // Get Solver Parameters
207  int max_solver_iterations;
208  double epsilon;
209  bool position_ik;
210 
211  lookupParam("max_solver_iterations", max_solver_iterations, 500);
212  lookupParam("epsilon", epsilon, 1e-5);
213  lookupParam("position_only_ik", position_ik, false);
214  ROS_DEBUG_NAMED("lma", "Looking for param name: position_only_ik");
215 
216  if (position_ik)
217  ROS_INFO_NAMED("lma", "Using position only ik");
218 
220  kdl_chain_.getNrOfJoints() - joint_model_group->getMimicJointModels().size() - (position_ik ? 3 : 6);
221 
222  // Check for mimic joints
223  std::vector<unsigned int> redundant_joints_map_index;
224 
225  std::vector<JointMimic> mimic_joints;
226  unsigned int joint_counter = 0;
227  for (std::size_t i = 0; i < kdl_chain_.getNrOfSegments(); ++i)
228  {
229  const robot_model::JointModel* jm = robot_model_->getJointModel(kdl_chain_.segments[i].getJoint().getName());
230 
231  // first check whether it belongs to the set of active joints in the group
232  if (jm->getMimic() == NULL && jm->getVariableCount() > 0)
233  {
234  JointMimic mimic_joint;
235  mimic_joint.reset(joint_counter);
236  mimic_joint.joint_name = kdl_chain_.segments[i].getJoint().getName();
237  mimic_joint.active = true;
238  mimic_joints.push_back(mimic_joint);
239  ++joint_counter;
240  continue;
241  }
242  if (joint_model_group->hasJointModel(jm->getName()))
243  {
244  if (jm->getMimic() && joint_model_group->hasJointModel(jm->getMimic()->getName()))
245  {
246  JointMimic mimic_joint;
247  mimic_joint.reset(joint_counter);
248  mimic_joint.joint_name = kdl_chain_.segments[i].getJoint().getName();
249  mimic_joint.offset = jm->getMimicOffset();
250  mimic_joint.multiplier = jm->getMimicFactor();
251  mimic_joints.push_back(mimic_joint);
252  continue;
253  }
254  }
255  }
256  for (std::size_t i = 0; i < mimic_joints.size(); ++i)
257  {
258  if (!mimic_joints[i].active)
259  {
260  const robot_model::JointModel* joint_model =
261  joint_model_group->getJointModel(mimic_joints[i].joint_name)->getMimic();
262  for (std::size_t j = 0; j < mimic_joints.size(); ++j)
263  {
264  if (mimic_joints[j].joint_name == joint_model->getName())
265  {
266  mimic_joints[i].map_index = mimic_joints[j].map_index;
267  }
268  }
269  }
270  }
271  mimic_joints_ = mimic_joints;
272 
273  // Setup the joint state groups that we need
274  state_.reset(new robot_state::RobotState(robot_model_));
275  state_2_.reset(new robot_state::RobotState(robot_model_));
276 
277  // Store things for when the set of redundant joints may change
278  position_ik_ = position_ik;
279  joint_model_group_ = joint_model_group;
280  max_solver_iterations_ = max_solver_iterations;
281  epsilon_ = epsilon;
282 
283  active_ = true;
284  ROS_DEBUG_NAMED("lma", "KDL solver initialized");
285  return true;
286 }
287 
288 bool LMAKinematicsPlugin::setRedundantJoints(const std::vector<unsigned int>& redundant_joints)
289 {
291  {
292  ROS_ERROR_NAMED("lma", "This group cannot have redundant joints");
293  return false;
294  }
295  if (int(redundant_joints.size()) > num_possible_redundant_joints_)
296  {
297  ROS_ERROR_NAMED("lma", "This group can only have %d redundant joints", num_possible_redundant_joints_);
298  return false;
299  }
300  /*
301  XmlRpc::XmlRpcValue joint_list;
302  if(private_handle.getParam(group_name+"/redundant_joints", joint_list))
303  {
304  ROS_ASSERT(joint_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
305  std::vector<std::string> redundant_joints;
306  for (std::size_t i = 0; i < joint_list.size(); ++i)
307  {
308  ROS_ASSERT(joint_list[i].getType() == XmlRpc::XmlRpcValue::TypeString);
309  redundant_joints.push_back(static_cast<std::string>(joint_list[i]));
310  ROS_INFO_NAMED("lma","Designated joint: %s as redundant joint",
311  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("lma", "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 LMAKinematicsPlugin::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 LMAKinematicsPlugin::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 LMAKinematicsPlugin::timedOut(const ros::WallTime& start_time, double duration) const
372 {
373  return ((ros::WallTime::now() - start_time).toSec() >= duration);
374 }
375 
376 bool LMAKinematicsPlugin::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 LMAKinematicsPlugin::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 LMAKinematicsPlugin::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 LMAKinematicsPlugin::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 LMAKinematicsPlugin::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 LMAKinematicsPlugin::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("lma", "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("lma", "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("lma", "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 
465  // Build Solvers
466  Eigen::Matrix<double, 6, 1> L;
467  L(0) = 1;
468  L(1) = 1;
469  L(2) = 1;
470  L(3) = 0.01;
471  L(4) = 0.01;
472  L(5) = 0.01;
473 
476  KDL::ChainIkSolverVel_pinv_mimic ik_solver_vel(kdl_chain_, joint_model_group_->getMimicJointModels().size(),
478  KDL::ChainIkSolverPos_LMA_JL_Mimic ik_solver_pos(kdl_chain_, joint_min_, joint_max_, fk_solver, ik_solver,
480  ik_solver_vel.setMimicJoints(mimic_joints_);
481  ik_solver_pos.setMimicJoints(mimic_joints_);
482 
483  if ((redundant_joint_indices_.size() > 0) && !ik_solver_vel.setRedundantJointsMapIndex(redundant_joints_map_index_))
484  {
485  ROS_ERROR_NAMED("lma", "Could not set redundant joints");
486  return false;
487  }
488 
489  if (options.lock_redundant_joints)
490  {
491  ik_solver_vel.lockRedundantJoints();
492  }
493 
494  solution.resize(dimension_);
495 
496  KDL::Frame pose_desired;
497  tf::poseMsgToKDL(ik_pose, pose_desired);
498 
499  ROS_DEBUG_STREAM_NAMED("lma", "searchPositionIK2: Position request pose is "
500  << ik_pose.position.x << " " << ik_pose.position.y << " " << ik_pose.position.z
501  << " " << ik_pose.orientation.x << " " << ik_pose.orientation.y << " "
502  << ik_pose.orientation.z << " " << ik_pose.orientation.w);
503  // Do the IK
504  for (unsigned int i = 0; i < dimension_; i++)
505  jnt_seed_state(i) = ik_seed_state[i];
506  jnt_pos_in = jnt_seed_state;
507 
508  unsigned int counter(0);
509  while (1)
510  {
511  // ROS_DEBUG_NAMED("lma","Iteration: %d, time: %f, Timeout:
512  // %f",counter,(ros::WallTime::now()-n1).toSec(),timeout);
513  counter++;
514  if (timedOut(n1, timeout))
515  {
516  ROS_DEBUG_NAMED("lma", "IK timed out");
517  error_code.val = error_code.TIMED_OUT;
518  ik_solver_vel.unlockRedundantJoints();
519  return false;
520  }
521  int ik_valid = ik_solver_pos.CartToJnt(jnt_pos_in, pose_desired, jnt_pos_out);
522  ROS_DEBUG_NAMED("lma", "IK valid: %d", ik_valid);
523  if (!consistency_limits.empty())
524  {
525  getRandomConfiguration(jnt_seed_state, consistency_limits, jnt_pos_in, options.lock_redundant_joints);
526  if ((ik_valid < 0 && !options.return_approximate_solution) ||
527  !checkConsistency(jnt_seed_state, consistency_limits, jnt_pos_out))
528  {
529  ROS_DEBUG_NAMED("lma", "Could not find IK solution: does not match consistency limits");
530  continue;
531  }
532  }
533  else
534  {
535  getRandomConfiguration(jnt_pos_in, options.lock_redundant_joints);
536  ROS_DEBUG_NAMED("lma", "New random configuration");
537  for (unsigned int j = 0; j < dimension_; j++)
538  ROS_DEBUG_NAMED("lma", "%d %f", j, jnt_pos_in(j));
539 
540  if (ik_valid < 0 && !options.return_approximate_solution)
541  {
542  ROS_DEBUG_NAMED("lma", "Could not find IK solution");
543  continue;
544  }
545  }
546  ROS_DEBUG_NAMED("lma", "Found IK solution");
547  for (unsigned int j = 0; j < dimension_; j++)
548  solution[j] = jnt_pos_out(j);
549  if (!solution_callback.empty())
550  solution_callback(ik_pose, solution, error_code);
551  else
552  error_code.val = error_code.SUCCESS;
553 
554  if (error_code.val == error_code.SUCCESS)
555  {
556  ROS_DEBUG_STREAM_NAMED("lma", "Solved after " << counter << " iterations");
557  ik_solver_vel.unlockRedundantJoints();
558  return true;
559  }
560  }
561  ROS_DEBUG_NAMED("lma", "An IK that satisifes the constraints and is collision free could not be found");
562  error_code.val = error_code.NO_IK_SOLUTION;
563  ik_solver_vel.unlockRedundantJoints();
564  return false;
565 }
566 
567 bool LMAKinematicsPlugin::getPositionFK(const std::vector<std::string>& link_names,
568  const std::vector<double>& joint_angles,
569  std::vector<geometry_msgs::Pose>& poses) const
570 {
571  if (!active_)
572  {
573  ROS_ERROR_NAMED("lma", "kinematics not active");
574  return false;
575  }
576  poses.resize(link_names.size());
577  if (joint_angles.size() != dimension_)
578  {
579  ROS_ERROR_NAMED("lma", "Joint angles vector must have size: %d", dimension_);
580  return false;
581  }
582 
583  KDL::Frame p_out;
584  geometry_msgs::PoseStamped pose;
585  tf::Stamped<tf::Pose> tf_pose;
586 
587  KDL::JntArray jnt_pos_in(dimension_);
588  for (unsigned int i = 0; i < dimension_; i++)
589  {
590  jnt_pos_in(i) = joint_angles[i];
591  }
592 
594 
595  bool valid = true;
596  for (unsigned int i = 0; i < poses.size(); i++)
597  {
598  ROS_DEBUG_NAMED("lma", "End effector index: %d", getKDLSegmentIndex(link_names[i]));
599  if (fk_solver.JntToCart(jnt_pos_in, p_out, getKDLSegmentIndex(link_names[i])) >= 0)
600  {
601  tf::poseKDLToMsg(p_out, poses[i]);
602  }
603  else
604  {
605  ROS_ERROR_NAMED("lma", "Could not compute FK for %s", link_names[i].c_str());
606  valid = false;
607  }
608  }
609  return valid;
610 }
611 
612 const std::vector<std::string>& LMAKinematicsPlugin::getJointNames() const
613 {
614  return ik_chain_info_.joint_names;
615 }
616 
617 const std::vector<std::string>& LMAKinematicsPlugin::getLinkNames() const
618 {
619  return ik_chain_info_.link_names;
620 }
621 
622 } // namespace
std::vector< unsigned int > redundant_joint_indices_
A model of a mimic joint. Mimic joints are typically unactuated joints that are constrained to follow...
Definition: joint_mimic.h:46
bool timedOut(const ros::WallTime &start_time, double duration) const
int getJointIndex(const std::string &name) const
const std::vector< std::string > & getJointNames() const
Return all the joint names in the order they are used internally.
#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
unsigned int getNrOfSegments() const
moveit_msgs::KinematicSolverInfo fk_chain_info_
int getKDLSegmentIndex(const std::string &name) const
virtual bool getPositionFK(const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::Pose > &poses) const
#define ROS_DEBUG_NAMED(name,...)
const std::string & getName() const
virtual const std::string & getTipFrame() const
void poseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k)
double epsilon
unsigned int getNrOfJoints() const
Specific implementation of kinematics using Levenberg-Marquardt method available at KDL...
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)
virtual bool setRedundantJoints(const std::vector< unsigned int > &redundant_joint_indices)
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
const std::vector< std::string > & getLinkNames() const
Return all the link names in the order they are represented internally.
void resize(unsigned int newSize)
bool getChain(const std::string &chain_root, const std::string &chain_tip, Chain &chain) const
void getRandomConfiguration(KDL::JntArray &jnt_array, bool lock_redundancy) const
static WallTime now()
void poseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m)
std::vector< Segment > segments
std::string joint_name
Name of this joint.
Definition: joint_mimic.h:61
double offset
Offset for this joint value from the joint that it mimics.
Definition: joint_mimic.h:52
#define ROS_ERROR_NAMED(name,...)
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
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.
moveit_msgs::KinematicSolverInfo ik_chain_info_
robot_model::JointModelGroup * joint_model_group_
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
void reset(unsigned int index)
Definition: joint_mimic.h:65
bool active
If true, this joint is an active DOF and not a mimic joint.
Definition: joint_mimic.h:63
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)
bool isRedundantJoint(unsigned int index) const
double multiplier
Multiplier for this joint value from the joint that it mimics.
Definition: joint_mimic.h:57
CLASS_LOADER_REGISTER_CLASS(Dog, Base)
const urdf::ModelInterfaceSharedPtr & getURDF() const
std::vector< unsigned int > redundant_joints_map_index_


moveit_kinematics
Author(s): Dave Coleman , Ioan Sucan , Sachin Chitta
autogenerated on Wed Jul 10 2019 04:03:41