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  bool has_mimic_joints = joint_model_group->getMimicJointModels().size() > 0;
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("lma", "KDL solver initialized");
286  return true;
287 }
288 
289 bool LMAKinematicsPlugin::setRedundantJoints(const std::vector<unsigned int>& redundant_joints)
290 {
292  {
293  ROS_ERROR_NAMED("lma", "This group cannot have redundant joints");
294  return false;
295  }
296  if (redundant_joints.size() > num_possible_redundant_joints_)
297  {
298  ROS_ERROR_NAMED("lma", "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("lma","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("lma", "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 LMAKinematicsPlugin::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 LMAKinematicsPlugin::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 LMAKinematicsPlugin::timedOut(const ros::WallTime& start_time, double duration) const
373 {
374  return ((ros::WallTime::now() - start_time).toSec() >= duration);
375 }
376 
377 bool LMAKinematicsPlugin::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 LMAKinematicsPlugin::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 LMAKinematicsPlugin::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 LMAKinematicsPlugin::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 LMAKinematicsPlugin::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 LMAKinematicsPlugin::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("lma", "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("lma", "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("lma", "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 
466  // Build Solvers
467  Eigen::Matrix<double, 6, 1> L;
468  L(0) = 1;
469  L(1) = 1;
470  L(2) = 1;
471  L(3) = 0.01;
472  L(4) = 0.01;
473  L(5) = 0.01;
474 
477  KDL::ChainIkSolverVel_pinv_mimic ik_solver_vel(kdl_chain_, joint_model_group_->getMimicJointModels().size(),
479  KDL::ChainIkSolverPos_LMA_JL_Mimic ik_solver_pos(kdl_chain_, joint_min_, joint_max_, fk_solver, ik_solver,
481  ik_solver_vel.setMimicJoints(mimic_joints_);
482  ik_solver_pos.setMimicJoints(mimic_joints_);
483 
484  if ((redundant_joint_indices_.size() > 0) && !ik_solver_vel.setRedundantJointsMapIndex(redundant_joints_map_index_))
485  {
486  ROS_ERROR_NAMED("lma", "Could not set redundant joints");
487  return false;
488  }
489 
490  if (options.lock_redundant_joints)
491  {
492  ik_solver_vel.lockRedundantJoints();
493  }
494 
495  solution.resize(dimension_);
496 
497  KDL::Frame pose_desired;
498  tf::poseMsgToKDL(ik_pose, pose_desired);
499 
500  ROS_DEBUG_STREAM_NAMED("lma", "searchPositionIK2: Position request pose is "
501  << ik_pose.position.x << " " << ik_pose.position.y << " " << ik_pose.position.z
502  << " " << ik_pose.orientation.x << " " << ik_pose.orientation.y << " "
503  << ik_pose.orientation.z << " " << ik_pose.orientation.w);
504  // Do the IK
505  for (unsigned int i = 0; i < dimension_; i++)
506  jnt_seed_state(i) = ik_seed_state[i];
507  jnt_pos_in = jnt_seed_state;
508 
509  unsigned int counter(0);
510  while (1)
511  {
512  // ROS_DEBUG_NAMED("lma","Iteration: %d, time: %f, Timeout:
513  // %f",counter,(ros::WallTime::now()-n1).toSec(),timeout);
514  counter++;
515  if (timedOut(n1, timeout))
516  {
517  ROS_DEBUG_NAMED("lma", "IK timed out");
518  error_code.val = error_code.TIMED_OUT;
519  ik_solver_vel.unlockRedundantJoints();
520  return false;
521  }
522  int ik_valid = ik_solver_pos.CartToJnt(jnt_pos_in, pose_desired, jnt_pos_out);
523  ROS_DEBUG_NAMED("lma", "IK valid: %d", ik_valid);
524  if (!consistency_limits.empty())
525  {
526  getRandomConfiguration(jnt_seed_state, consistency_limits, jnt_pos_in, options.lock_redundant_joints);
527  if ((ik_valid < 0 && !options.return_approximate_solution) ||
528  !checkConsistency(jnt_seed_state, consistency_limits, jnt_pos_out))
529  {
530  ROS_DEBUG_NAMED("lma", "Could not find IK solution: does not match consistency limits");
531  continue;
532  }
533  }
534  else
535  {
536  getRandomConfiguration(jnt_pos_in, options.lock_redundant_joints);
537  ROS_DEBUG_NAMED("lma", "New random configuration");
538  for (unsigned int j = 0; j < dimension_; j++)
539  ROS_DEBUG_NAMED("lma", "%d %f", j, jnt_pos_in(j));
540 
541  if (ik_valid < 0 && !options.return_approximate_solution)
542  {
543  ROS_DEBUG_NAMED("lma", "Could not find IK solution");
544  continue;
545  }
546  }
547  ROS_DEBUG_NAMED("lma", "Found IK solution");
548  for (unsigned int j = 0; j < dimension_; j++)
549  solution[j] = jnt_pos_out(j);
550  if (!solution_callback.empty())
551  solution_callback(ik_pose, solution, error_code);
552  else
553  error_code.val = error_code.SUCCESS;
554 
555  if (error_code.val == error_code.SUCCESS)
556  {
557  ROS_DEBUG_STREAM_NAMED("lma", "Solved after " << counter << " iterations");
558  ik_solver_vel.unlockRedundantJoints();
559  return true;
560  }
561  }
562  ROS_DEBUG_NAMED("lma", "An IK that satisifes the constraints and is collision free could not be found");
563  error_code.val = error_code.NO_IK_SOLUTION;
564  ik_solver_vel.unlockRedundantJoints();
565  return false;
566 }
567 
568 bool LMAKinematicsPlugin::getPositionFK(const std::vector<std::string>& link_names,
569  const std::vector<double>& joint_angles,
570  std::vector<geometry_msgs::Pose>& poses) const
571 {
573  if (!active_)
574  {
575  ROS_ERROR_NAMED("lma", "kinematics not active");
576  return false;
577  }
578  poses.resize(link_names.size());
579  if (joint_angles.size() != dimension_)
580  {
581  ROS_ERROR_NAMED("lma", "Joint angles vector must have size: %d", dimension_);
582  return false;
583  }
584 
585  KDL::Frame p_out;
586  geometry_msgs::PoseStamped pose;
587  tf::Stamped<tf::Pose> tf_pose;
588 
589  KDL::JntArray jnt_pos_in(dimension_);
590  for (unsigned int i = 0; i < dimension_; i++)
591  {
592  jnt_pos_in(i) = joint_angles[i];
593  }
594 
596 
597  bool valid = true;
598  for (unsigned int i = 0; i < poses.size(); i++)
599  {
600  ROS_DEBUG_NAMED("lma", "End effector index: %d", getKDLSegmentIndex(link_names[i]));
601  if (fk_solver.JntToCart(jnt_pos_in, p_out, getKDLSegmentIndex(link_names[i])) >= 0)
602  {
603  tf::poseKDLToMsg(p_out, poses[i]);
604  }
605  else
606  {
607  ROS_ERROR_NAMED("lma", "Could not compute FK for %s", link_names[i].c_str());
608  valid = false;
609  }
610  }
611  return valid;
612 }
613 
614 const std::vector<std::string>& LMAKinematicsPlugin::getJointNames() const
615 {
616  return ik_chain_info_.joint_names;
617 }
618 
619 const std::vector<std::string>& LMAKinematicsPlugin::getLinkNames() const
620 {
621  return ik_chain_info_.link_names;
622 }
623 
624 } // 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 Tue Jun 12 2018 02:48:29