robot_state.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2013, Ioan A. Sucan
5 * Copyright (c) 2013, Willow Garage, Inc.
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of the Willow Garage nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *********************************************************************/
35 
36 /* Author: Ioan Sucan, Sachin Chitta, Acorn Pooley, Mario Prats, Dave Coleman */
37 
44 #include <boost/bind.hpp>
46 
47 namespace moveit
48 {
49 namespace core
50 {
55 static const std::size_t MIN_STEPS_FOR_JUMP_THRESH = 10;
56 
57 const std::string LOGNAME = "robot_state";
58 
59 RobotState::RobotState(const RobotModelConstPtr& robot_model)
60  : robot_model_(robot_model)
61  , has_velocity_(false)
62  , has_acceleration_(false)
63  , has_effort_(false)
64  , dirty_link_transforms_(robot_model_->getRootJoint())
65  , dirty_collision_body_transforms_(nullptr)
66  , rng_(nullptr)
67 {
68  allocMemory();
69 
70  // all transforms are dirty initially
71  const int nr_doubles_for_dirty_joint_transforms =
72  1 + robot_model_->getJointModelCount() / (sizeof(double) / sizeof(unsigned char));
73  memset(dirty_joint_transforms_, 1, sizeof(double) * nr_doubles_for_dirty_joint_transforms);
74 }
75 
76 RobotState::RobotState(const RobotState& other) : rng_(nullptr)
77 {
78  robot_model_ = other.robot_model_;
79  allocMemory();
80  copyFrom(other);
81 }
82 
84 {
86  free(memory_);
87  if (rng_)
88  delete rng_;
89 }
90 
92 {
93  // memory for the dirty joint transforms
94  const int nr_doubles_for_dirty_joint_transforms =
95  1 + robot_model_->getJointModelCount() / (sizeof(double) / sizeof(unsigned char));
96  const size_t bytes =
97  sizeof(Eigen::Affine3d) * (robot_model_->getJointModelCount() + robot_model_->getLinkModelCount() +
98  robot_model_->getLinkGeometryCount()) +
99  sizeof(double) * (robot_model_->getVariableCount() * 3 + nr_doubles_for_dirty_joint_transforms) + 15;
100  memory_ = malloc(bytes);
101 
102  // make the memory for transforms align at 16 bytes
103  variable_joint_transforms_ = reinterpret_cast<Eigen::Affine3d*>(((uintptr_t)memory_ + 15) & ~(uintptr_t)0x0F);
107  reinterpret_cast<unsigned char*>(global_collision_body_transforms_ + robot_model_->getLinkGeometryCount());
108  position_ = reinterpret_cast<double*>(dirty_joint_transforms_) + nr_doubles_for_dirty_joint_transforms;
109  velocity_ = position_ + robot_model_->getVariableCount();
110  // acceleration and effort share the memory (not both can be specified)
111  effort_ = acceleration_ = velocity_ + robot_model_->getVariableCount();
112 }
113 
115 {
116  if (this != &other)
117  copyFrom(other);
118  return *this;
119 }
120 
122 {
125  has_effort_ = other.has_effort_;
126 
129 
130  if (dirty_link_transforms_ == robot_model_->getRootJoint())
131  {
132  // everything is dirty; no point in copying transforms; copy positions, potentially velocity & acceleration
133  memcpy(position_, other.position_, robot_model_->getVariableCount() * sizeof(double) *
134  (1 + ((has_velocity_ || has_acceleration_ || has_effort_) ? 1 : 0) +
135  ((has_acceleration_ || has_effort_) ? 1 : 0)));
136 
137  // mark all transforms as dirty
138  const int nr_doubles_for_dirty_joint_transforms =
139  1 + robot_model_->getJointModelCount() / (sizeof(double) / sizeof(unsigned char));
140  memset(dirty_joint_transforms_, 1, sizeof(double) * nr_doubles_for_dirty_joint_transforms);
141  }
142  else
143  {
144  // copy all the memory; maybe avoid copying velocity and acceleration if possible
145  const int nr_doubles_for_dirty_joint_transforms =
146  1 + robot_model_->getJointModelCount() / (sizeof(double) / sizeof(unsigned char));
147  const size_t bytes =
148  sizeof(Eigen::Affine3d) * (robot_model_->getJointModelCount() + robot_model_->getLinkModelCount() +
149  robot_model_->getLinkGeometryCount()) +
150  sizeof(double) *
151  (robot_model_->getVariableCount() * (1 + ((has_velocity_ || has_acceleration_ || has_effort_) ? 1 : 0) +
152  ((has_acceleration_ || has_effort_) ? 1 : 0)) +
153  nr_doubles_for_dirty_joint_transforms);
155  }
156 
157  // copy attached bodies
159  for (std::map<std::string, AttachedBody*>::const_iterator it = other.attached_body_map_.begin();
160  it != other.attached_body_map_.end(); ++it)
161  attachBody(it->second->getName(), it->second->getShapes(), it->second->getFixedTransforms(),
162  it->second->getTouchLinks(), it->second->getAttachedLinkName(), it->second->getDetachPosture());
163 }
164 
166 {
167  if (dirtyJointTransform(joint))
168  {
169  ROS_WARN_NAMED(LOGNAME, "Returning dirty joint transforms for joint '%s'", joint->getName().c_str());
170  return false;
171  }
172  return true;
173 }
174 
176 {
177  if (dirtyLinkTransforms())
178  {
179  ROS_WARN_NAMED(LOGNAME, "Returning dirty link transforms");
180  return false;
181  }
182  return true;
183 }
184 
186 {
188  {
189  ROS_WARN_NAMED(LOGNAME, "Returning dirty collision body transforms");
190  return false;
191  }
192  return true;
193 }
194 
196 {
197  if (!has_velocity_)
198  {
199  has_velocity_ = true;
200  memset(velocity_, 0, sizeof(double) * robot_model_->getVariableCount());
201  }
202 }
203 
205 {
206  if (!has_acceleration_)
207  {
208  has_acceleration_ = true;
209  has_effort_ = false;
210  memset(acceleration_, 0, sizeof(double) * robot_model_->getVariableCount());
211  }
212 }
213 
215 {
216  if (!has_effort_)
217  {
218  has_acceleration_ = false;
219  has_effort_ = true;
220  memset(effort_, 0, sizeof(double) * robot_model_->getVariableCount());
221  }
222 }
223 
225 {
227  robot_model_->getVariableRandomPositions(rng, position_);
228  memset(dirty_joint_transforms_, 1, robot_model_->getJointModelCount() * sizeof(unsigned char));
229  dirty_link_transforms_ = robot_model_->getRootJoint();
230  // mimic values are correctly set in RobotModel
231 }
232 
234 {
235  // we do not make calls to RobotModel for random number generation because mimic joints
236  // could trigger updates outside the state of the group itself
238  setToRandomPositions(group, rng);
239 }
241 {
242  const std::vector<const JointModel*>& joints = group->getActiveJointModels();
243  for (std::size_t i = 0; i < joints.size(); ++i)
244  joints[i]->getVariableRandomPositions(rng, position_ + joints[i]->getFirstVariableIndex());
245  updateMimicJoints(group);
246 }
247 
249  const std::vector<double>& distances)
250 {
251  // we do not make calls to RobotModel for random number generation because mimic joints
252  // could trigger updates outside the state of the group itself
254  const std::vector<const JointModel*>& joints = group->getActiveJointModels();
255  assert(distances.size() == joints.size());
256  for (std::size_t i = 0; i < joints.size(); ++i)
257  {
258  const int idx = joints[i]->getFirstVariableIndex();
259  joints[i]->getVariableRandomPositionsNearBy(rng, position_ + joints[i]->getFirstVariableIndex(),
260  near.position_ + idx, distances[i]);
261  }
262  updateMimicJoints(group);
263 }
264 
266 {
267  // we do not make calls to RobotModel for random number generation because mimic joints
268  // could trigger updates outside the state of the group itself
270  const std::vector<const JointModel*>& joints = group->getActiveJointModels();
271  for (std::size_t i = 0; i < joints.size(); ++i)
272  {
273  const int idx = joints[i]->getFirstVariableIndex();
274  joints[i]->getVariableRandomPositionsNearBy(rng, position_ + joints[i]->getFirstVariableIndex(),
275  near.position_ + idx, distance);
276  }
277  updateMimicJoints(group);
278 }
279 
280 bool RobotState::setToDefaultValues(const JointModelGroup* group, const std::string& name)
281 {
282  std::map<std::string, double> m;
283  bool r = group->getVariableDefaultPositions(name, m); // mimic values are updated
285  return r;
286 }
287 
289 {
290  robot_model_->getVariableDefaultPositions(position_); // mimic values are updated
291  // set velocity & acceleration to 0
292  memset(velocity_, 0, sizeof(double) * 2 * robot_model_->getVariableCount());
293  memset(dirty_joint_transforms_, 1, robot_model_->getJointModelCount() * sizeof(unsigned char));
294  dirty_link_transforms_ = robot_model_->getRootJoint();
295 }
296 
297 void RobotState::setVariablePositions(const double* position)
298 {
299  // assume everything is in order in terms of array lengths (for efficiency reasons)
300  memcpy(position_, position, robot_model_->getVariableCount() * sizeof(double));
301 
302  // the full state includes mimic joint values, so no need to update mimic here
303 
304  // Since all joint values have potentially changed, we will need to recompute all transforms
305  memset(dirty_joint_transforms_, 1, robot_model_->getJointModelCount() * sizeof(unsigned char));
306  dirty_link_transforms_ = robot_model_->getRootJoint();
307 }
308 
309 void RobotState::setVariablePositions(const std::map<std::string, double>& variable_map)
310 {
311  for (std::map<std::string, double>::const_iterator it = variable_map.begin(), end = variable_map.end(); it != end;
312  ++it)
313  {
314  const int index = robot_model_->getVariableIndex(it->first);
315  position_[index] = it->second;
316  const JointModel* jm = robot_model_->getJointOfVariable(index);
318  updateMimicJoint(jm);
319  }
320 }
321 
322 void RobotState::getMissingKeys(const std::map<std::string, double>& variable_map,
323  std::vector<std::string>& missing_variables) const
324 {
325  missing_variables.clear();
326  const std::vector<std::string>& nm = robot_model_->getVariableNames();
327  for (std::size_t i = 0; i < nm.size(); ++i)
328  if (variable_map.find(nm[i]) == variable_map.end())
329  if (robot_model_->getJointOfVariable(nm[i])->getMimic() == nullptr)
330  missing_variables.push_back(nm[i]);
331 }
332 
333 void RobotState::setVariablePositions(const std::map<std::string, double>& variable_map,
334  std::vector<std::string>& missing_variables)
335 {
336  setVariablePositions(variable_map);
337  getMissingKeys(variable_map, missing_variables);
338 }
339 
340 void RobotState::setVariablePositions(const std::vector<std::string>& variable_names,
341  const std::vector<double>& variable_position)
342 {
343  for (std::size_t i = 0; i < variable_names.size(); ++i)
344  {
345  const int index = robot_model_->getVariableIndex(variable_names[i]);
346  position_[index] = variable_position[i];
347  const JointModel* jm = robot_model_->getJointOfVariable(index);
349  updateMimicJoint(jm);
350  }
351 }
352 
353 void RobotState::setVariableVelocities(const std::map<std::string, double>& variable_map)
354 {
355  markVelocity();
356  for (std::map<std::string, double>::const_iterator it = variable_map.begin(), end = variable_map.end(); it != end;
357  ++it)
358  velocity_[robot_model_->getVariableIndex(it->first)] = it->second;
359 }
360 
361 void RobotState::setVariableVelocities(const std::map<std::string, double>& variable_map,
362  std::vector<std::string>& missing_variables)
363 {
364  setVariableVelocities(variable_map);
365  getMissingKeys(variable_map, missing_variables);
366 }
367 
368 void RobotState::setVariableVelocities(const std::vector<std::string>& variable_names,
369  const std::vector<double>& variable_velocity)
370 {
371  markVelocity();
372  assert(variable_names.size() == variable_velocity.size());
373  for (std::size_t i = 0; i < variable_names.size(); ++i)
374  velocity_[robot_model_->getVariableIndex(variable_names[i])] = variable_velocity[i];
375 }
376 
377 void RobotState::setVariableAccelerations(const std::map<std::string, double>& variable_map)
378 {
380  for (std::map<std::string, double>::const_iterator it = variable_map.begin(), end = variable_map.end(); it != end;
381  ++it)
382  acceleration_[robot_model_->getVariableIndex(it->first)] = it->second;
383 }
384 
385 void RobotState::setVariableAccelerations(const std::map<std::string, double>& variable_map,
386  std::vector<std::string>& missing_variables)
387 {
388  setVariableAccelerations(variable_map);
389  getMissingKeys(variable_map, missing_variables);
390 }
391 
392 void RobotState::setVariableAccelerations(const std::vector<std::string>& variable_names,
393  const std::vector<double>& variable_acceleration)
394 {
396  assert(variable_names.size() == variable_acceleration.size());
397  for (std::size_t i = 0; i < variable_names.size(); ++i)
398  acceleration_[robot_model_->getVariableIndex(variable_names[i])] = variable_acceleration[i];
399 }
400 
401 void RobotState::setVariableEffort(const std::map<std::string, double>& variable_map)
402 {
403  markEffort();
404  for (std::map<std::string, double>::const_iterator it = variable_map.begin(), end = variable_map.end(); it != end;
405  ++it)
406  acceleration_[robot_model_->getVariableIndex(it->first)] = it->second;
407 }
408 
409 void RobotState::setVariableEffort(const std::map<std::string, double>& variable_map,
410  std::vector<std::string>& missing_variables)
411 {
412  setVariableEffort(variable_map);
413  getMissingKeys(variable_map, missing_variables);
414 }
415 
416 void RobotState::setVariableEffort(const std::vector<std::string>& variable_names,
417  const std::vector<double>& variable_effort)
418 {
419  markEffort();
420  assert(variable_names.size() == variable_effort.size());
421  for (std::size_t i = 0; i < variable_names.size(); ++i)
422  effort_[robot_model_->getVariableIndex(variable_names[i])] = variable_effort[i];
423 }
424 
426 {
427  if (has_velocity_)
428  {
429  for (size_t i = 0; i < robot_model_->getVariableCount(); ++i)
430  velocity_[i] *= -1;
431  }
432 }
433 
434 void RobotState::setJointEfforts(const JointModel* joint, const double* effort)
435 {
436  if (has_acceleration_)
437  {
438  ROS_ERROR_NAMED(LOGNAME, "Unable to set joint efforts because array is being used for accelerations");
439  return;
440  }
441  has_effort_ = true;
442 
443  memcpy(effort_ + joint->getFirstVariableIndex(), effort, joint->getVariableCount() * sizeof(double));
444 }
445 
446 void RobotState::setJointGroupPositions(const JointModelGroup* group, const double* gstate)
447 {
448  const std::vector<int>& il = group->getVariableIndexList();
449  if (group->isContiguousWithinState())
450  memcpy(position_ + il[0], gstate, group->getVariableCount() * sizeof(double));
451  else
452  {
453  for (std::size_t i = 0; i < il.size(); ++i)
454  position_[il[i]] = gstate[i];
455  }
456  updateMimicJoints(group);
457 }
458 
459 void RobotState::setJointGroupPositions(const JointModelGroup* group, const Eigen::VectorXd& values)
460 {
461  const std::vector<int>& il = group->getVariableIndexList();
462  for (std::size_t i = 0; i < il.size(); ++i)
463  position_[il[i]] = values(i);
464  updateMimicJoints(group);
465 }
466 
467 void RobotState::copyJointGroupPositions(const JointModelGroup* group, double* gstate) const
468 {
469  const std::vector<int>& il = group->getVariableIndexList();
470  if (group->isContiguousWithinState())
471  memcpy(gstate, position_ + il[0], group->getVariableCount() * sizeof(double));
472  else
473  for (std::size_t i = 0; i < il.size(); ++i)
474  gstate[i] = position_[il[i]];
475 }
476 
477 void RobotState::copyJointGroupPositions(const JointModelGroup* group, Eigen::VectorXd& values) const
478 {
479  const std::vector<int>& il = group->getVariableIndexList();
480  values.resize(il.size());
481  for (std::size_t i = 0; i < il.size(); ++i)
482  values(i) = position_[il[i]];
483 }
484 
485 void RobotState::setJointGroupVelocities(const JointModelGroup* group, const double* gstate)
486 {
487  markVelocity();
488  const std::vector<int>& il = group->getVariableIndexList();
489  if (group->isContiguousWithinState())
490  memcpy(velocity_ + il[0], gstate, group->getVariableCount() * sizeof(double));
491  else
492  {
493  for (std::size_t i = 0; i < il.size(); ++i)
494  velocity_[il[i]] = gstate[i];
495  }
496 }
497 
498 void RobotState::setJointGroupVelocities(const JointModelGroup* group, const Eigen::VectorXd& values)
499 {
500  markVelocity();
501  const std::vector<int>& il = group->getVariableIndexList();
502  for (std::size_t i = 0; i < il.size(); ++i)
503  velocity_[il[i]] = values(i);
504 }
505 
506 void RobotState::copyJointGroupVelocities(const JointModelGroup* group, double* gstate) const
507 {
508  const std::vector<int>& il = group->getVariableIndexList();
509  if (group->isContiguousWithinState())
510  memcpy(gstate, velocity_ + il[0], group->getVariableCount() * sizeof(double));
511  else
512  for (std::size_t i = 0; i < il.size(); ++i)
513  gstate[i] = velocity_[il[i]];
514 }
515 
516 void RobotState::copyJointGroupVelocities(const JointModelGroup* group, Eigen::VectorXd& values) const
517 {
518  const std::vector<int>& il = group->getVariableIndexList();
519  values.resize(il.size());
520  for (std::size_t i = 0; i < il.size(); ++i)
521  values(i) = velocity_[il[i]];
522 }
523 
524 void RobotState::setJointGroupAccelerations(const JointModelGroup* group, const double* gstate)
525 {
527  const std::vector<int>& il = group->getVariableIndexList();
528  if (group->isContiguousWithinState())
529  memcpy(acceleration_ + il[0], gstate, group->getVariableCount() * sizeof(double));
530  else
531  {
532  for (std::size_t i = 0; i < il.size(); ++i)
533  acceleration_[il[i]] = gstate[i];
534  }
535 }
536 
537 void RobotState::setJointGroupAccelerations(const JointModelGroup* group, const Eigen::VectorXd& values)
538 {
540  const std::vector<int>& il = group->getVariableIndexList();
541  for (std::size_t i = 0; i < il.size(); ++i)
542  acceleration_[il[i]] = values(i);
543 }
544 
545 void RobotState::copyJointGroupAccelerations(const JointModelGroup* group, double* gstate) const
546 {
547  const std::vector<int>& il = group->getVariableIndexList();
548  if (group->isContiguousWithinState())
549  memcpy(gstate, acceleration_ + il[0], group->getVariableCount() * sizeof(double));
550  else
551  for (std::size_t i = 0; i < il.size(); ++i)
552  gstate[i] = acceleration_[il[i]];
553 }
554 
555 void RobotState::copyJointGroupAccelerations(const JointModelGroup* group, Eigen::VectorXd& values) const
556 {
557  const std::vector<int>& il = group->getVariableIndexList();
558  values.resize(il.size());
559  for (std::size_t i = 0; i < il.size(); ++i)
560  values(i) = acceleration_[il[i]];
561 }
562 
563 void RobotState::update(bool force)
564 {
565  // make sure we do everything from scratch if needed
566  if (force)
567  {
568  memset(dirty_joint_transforms_, 1, robot_model_->getJointModelCount() * sizeof(unsigned char));
569  dirty_link_transforms_ = robot_model_->getRootJoint();
570  }
571 
572  // this actually triggers all needed updates
574 }
575 
577 {
578  if (dirty_link_transforms_ != nullptr)
580 
581  if (dirty_collision_body_transforms_ != nullptr)
582  {
583  const std::vector<const LinkModel*>& links = dirty_collision_body_transforms_->getDescendantLinkModels();
585 
586  for (std::size_t i = 0; i < links.size(); ++i)
587  {
588  const EigenSTL::vector_Affine3d& ot = links[i]->getCollisionOriginTransforms();
589  const std::vector<int>& ot_id = links[i]->areCollisionOriginTransformsIdentity();
590  const int index_co = links[i]->getFirstCollisionBodyTransformIndex();
591  const int index_l = links[i]->getLinkIndex();
592  for (std::size_t j = 0; j < ot.size(); ++j)
593  global_collision_body_transforms_[index_co + j].matrix().noalias() =
594  ot_id[j] ? global_link_transforms_[index_l].matrix() :
595  global_link_transforms_[index_l].matrix() * ot[j].matrix();
596  }
597  }
598 }
599 
601 {
602  if (dirty_link_transforms_ != nullptr)
603  {
608  else
610  dirty_link_transforms_ = nullptr;
611  }
612 }
613 
615 {
616  for (const LinkModel* link : start->getDescendantLinkModels())
617  {
618  const LinkModel* parent = link->getParentLinkModel();
619  if (parent) // root JointModel will not have a parent
620  {
621  if (link->parentJointIsFixed())
622  global_link_transforms_[link->getLinkIndex()].matrix().noalias() =
623  global_link_transforms_[parent->getLinkIndex()].matrix() * link->getJointOriginTransform().matrix();
624  else
625  {
626  if (link->jointOriginTransformIsIdentity())
627  global_link_transforms_[link->getLinkIndex()].matrix().noalias() =
628  global_link_transforms_[parent->getLinkIndex()].matrix() *
629  getJointTransform(link->getParentJointModel()).matrix();
630  else
631  global_link_transforms_[link->getLinkIndex()].matrix().noalias() =
632  global_link_transforms_[parent->getLinkIndex()].matrix() * link->getJointOriginTransform().matrix() *
633  getJointTransform(link->getParentJointModel()).matrix();
634  }
635  }
636  else
637  {
638  if (link->jointOriginTransformIsIdentity())
639  global_link_transforms_[link->getLinkIndex()] = getJointTransform(link->getParentJointModel());
640  else
641  global_link_transforms_[link->getLinkIndex()].matrix().noalias() =
642  link->getJointOriginTransform().matrix() * getJointTransform(link->getParentJointModel()).matrix();
643  }
644  }
645 
646  // update attached bodies tf; these are usually very few, so we update them all
647  for (std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.begin();
648  it != attached_body_map_.end(); ++it)
649  it->second->computeTransform(global_link_transforms_[it->second->getAttachedLink()->getLinkIndex()]);
650 }
651 
652 void RobotState::updateStateWithLinkAt(const LinkModel* link, const Eigen::Affine3d& transform, bool backward)
653 {
654  updateLinkTransforms(); // no link transforms must be dirty, otherwise the transform we set will be overwritten
655 
656  // update the fact that collision body transforms are out of date
660  else
662 
663  global_link_transforms_[link->getLinkIndex()] = transform;
664 
665  // update link transforms for descendant links only (leaving the transform for the current link untouched)
666  const std::vector<const JointModel*>& cj = link->getChildJointModels();
667  for (std::size_t i = 0; i < cj.size(); ++i)
669 
670  // if we also need to go backward
671  if (backward)
672  {
673  const LinkModel* parent_link = link;
674  const LinkModel* child_link;
675  while (parent_link->getParentJointModel()->getParentLinkModel())
676  {
677  child_link = parent_link;
678  parent_link = parent_link->getParentJointModel()->getParentLinkModel();
679 
680  // update the transform of the parent
681  global_link_transforms_[parent_link->getLinkIndex()] =
682  global_link_transforms_[child_link->getLinkIndex()] *
683  (child_link->getJointOriginTransform() *
685  .inverse(Eigen::Isometry);
686 
687  // update link transforms for descendant links only (leaving the transform for the current link untouched)
688  // with the exception of the child link we are coming backwards from
689  const std::vector<const JointModel*>& cj = parent_link->getChildJointModels();
690  for (std::size_t i = 0; i < cj.size(); ++i)
691  if (cj[i] != child_link->getParentJointModel())
693  }
694  // all collision body transforms are invalid now
696  }
697 
698  // update attached bodies tf; these are usually very few, so we update them all
699  for (std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.begin();
700  it != attached_body_map_.end(); ++it)
701  it->second->computeTransform(global_link_transforms_[it->second->getAttachedLink()->getLinkIndex()]);
702 }
703 
704 bool RobotState::satisfiesBounds(double margin) const
705 {
706  const std::vector<const JointModel*>& jm = robot_model_->getActiveJointModels();
707  for (std::size_t i = 0; i < jm.size(); ++i)
708  if (!satisfiesBounds(jm[i], margin))
709  return false;
710  return true;
711 }
712 
713 bool RobotState::satisfiesBounds(const JointModelGroup* group, double margin) const
714 {
715  const std::vector<const JointModel*>& jm = group->getActiveJointModels();
716  for (std::size_t i = 0; i < jm.size(); ++i)
717  if (!satisfiesBounds(jm[i], margin))
718  return false;
719  return true;
720 }
721 
723 {
724  const std::vector<const JointModel*>& jm = robot_model_->getActiveJointModels();
725  for (std::size_t i = 0; i < jm.size(); ++i)
726  enforceBounds(jm[i]);
727 }
728 
730 {
731  const std::vector<const JointModel*>& jm = joint_group->getActiveJointModels();
732  for (std::size_t i = 0; i < jm.size(); ++i)
733  enforceBounds(jm[i]);
734 }
735 
736 std::pair<double, const JointModel*> RobotState::getMinDistanceToPositionBounds() const
737 {
738  return getMinDistanceToPositionBounds(robot_model_->getActiveJointModels());
739 }
740 
741 std::pair<double, const JointModel*> RobotState::getMinDistanceToPositionBounds(const JointModelGroup* group) const
742 {
744 }
745 
746 std::pair<double, const JointModel*>
747 RobotState::getMinDistanceToPositionBounds(const std::vector<const JointModel*>& joints) const
748 {
749  double distance = std::numeric_limits<double>::max();
750  const JointModel* index = nullptr;
751  for (std::size_t i = 0; i < joints.size(); ++i)
752  {
753  if (joints[i]->getType() == JointModel::PLANAR || joints[i]->getType() == JointModel::FLOATING)
754  continue;
755  if (joints[i]->getType() == JointModel::REVOLUTE)
756  if (static_cast<const RevoluteJointModel*>(joints[i])->isContinuous())
757  continue;
758 
759  const double* joint_values = getJointPositions(joints[i]);
760  const JointModel::Bounds& bounds = joints[i]->getVariableBounds();
761  std::vector<double> lower_bounds(bounds.size()), upper_bounds(bounds.size());
762  for (std::size_t j = 0; j < bounds.size(); ++j)
763  {
764  lower_bounds[j] = bounds[j].min_position_;
765  upper_bounds[j] = bounds[j].max_position_;
766  }
767  double new_distance = joints[i]->distance(joint_values, &lower_bounds[0]);
768  if (new_distance < distance)
769  {
770  index = joints[i];
771  distance = new_distance;
772  }
773  new_distance = joints[i]->distance(joint_values, &upper_bounds[0]);
774  if (new_distance < distance)
775  {
776  index = joints[i];
777  distance = new_distance;
778  }
779  }
780  return std::make_pair(distance, index);
781 }
782 
783 bool RobotState::isValidVelocityMove(const RobotState& other, const JointModelGroup* group, double dt) const
784 {
785  const std::vector<const JointModel*>& jm = group->getActiveJointModels();
786  for (std::size_t joint_id = 0; joint_id < jm.size(); ++joint_id)
787  {
788  const int idx = jm[joint_id]->getFirstVariableIndex();
789  const std::vector<VariableBounds>& bounds = jm[joint_id]->getVariableBounds();
790 
791  // Check velocity for each joint variable
792  for (std::size_t var_id = 0; var_id < jm[joint_id]->getVariableCount(); ++var_id)
793  {
794  const double dtheta = std::abs(*(position_ + idx + var_id) - *(other.getVariablePositions() + idx + var_id));
795 
796  if (dtheta > dt * bounds[var_id].max_velocity_)
797  return false;
798  }
799  }
800 
801  return true;
802 }
803 
804 double RobotState::distance(const RobotState& other, const JointModelGroup* joint_group) const
805 {
806  double d = 0.0;
807  const std::vector<const JointModel*>& jm = joint_group->getActiveJointModels();
808  for (std::size_t i = 0; i < jm.size(); ++i)
809  {
810  const int idx = jm[i]->getFirstVariableIndex();
811  d += jm[i]->getDistanceFactor() * jm[i]->distance(position_ + idx, other.position_ + idx);
812  }
813  return d;
814 }
815 
816 void RobotState::interpolate(const RobotState& to, double t, RobotState& state) const
817 {
819 
820  memset(state.dirty_joint_transforms_, 1, state.robot_model_->getJointModelCount() * sizeof(unsigned char));
821  state.dirty_link_transforms_ = state.robot_model_->getRootJoint();
822 }
823 
824 void RobotState::interpolate(const RobotState& to, double t, RobotState& state,
825  const JointModelGroup* joint_group) const
826 {
827  const std::vector<const JointModel*>& jm = joint_group->getActiveJointModels();
828  for (std::size_t i = 0; i < jm.size(); ++i)
829  {
830  const int idx = jm[i]->getFirstVariableIndex();
831  jm[i]->interpolate(position_ + idx, to.position_ + idx, t, state.position_ + idx);
832  }
833  state.updateMimicJoints(joint_group);
834 }
835 
837 {
839 }
840 
841 bool RobotState::hasAttachedBody(const std::string& id) const
842 {
843  return attached_body_map_.find(id) != attached_body_map_.end();
844 }
845 
846 const AttachedBody* RobotState::getAttachedBody(const std::string& id) const
847 {
848  std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.find(id);
849  if (it == attached_body_map_.end())
850  {
851  ROS_ERROR_NAMED(LOGNAME, "Attached body '%s' not found", id.c_str());
852  return nullptr;
853  }
854  else
855  return it->second;
856 }
857 
859 {
860  attached_body_map_[attached_body->getName()] = attached_body;
861  attached_body->computeTransform(getGlobalLinkTransform(attached_body->getAttachedLink()));
863  attached_body_update_callback_(attached_body, true);
864 }
865 
866 void RobotState::attachBody(const std::string& id, const std::vector<shapes::ShapeConstPtr>& shapes,
867  const EigenSTL::vector_Affine3d& attach_trans, const std::set<std::string>& touch_links,
868  const std::string& link, const trajectory_msgs::JointTrajectory& detach_posture)
869 {
870  const LinkModel* l = robot_model_->getLinkModel(link);
871  AttachedBody* ab = new AttachedBody(l, id, shapes, attach_trans, touch_links, detach_posture);
872  attached_body_map_[id] = ab;
876 }
877 
878 void RobotState::getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies) const
879 {
880  attached_bodies.clear();
881  attached_bodies.reserve(attached_body_map_.size());
882  for (std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.begin();
883  it != attached_body_map_.end(); ++it)
884  attached_bodies.push_back(it->second);
885 }
886 
887 void RobotState::getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies,
888  const JointModelGroup* group) const
889 {
890  attached_bodies.clear();
891  for (std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.begin();
892  it != attached_body_map_.end(); ++it)
893  if (group->hasLinkModel(it->second->getAttachedLinkName()))
894  attached_bodies.push_back(it->second);
895 }
896 
897 void RobotState::getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const LinkModel* lm) const
898 {
899  attached_bodies.clear();
900  for (std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.begin();
901  it != attached_body_map_.end(); ++it)
902  if (it->second->getAttachedLink() == lm)
903  attached_bodies.push_back(it->second);
904 }
905 
907 {
908  for (std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.begin();
909  it != attached_body_map_.end(); ++it)
910  {
912  attached_body_update_callback_(it->second, false);
913  delete it->second;
914  }
915  attached_body_map_.clear();
916 }
917 
919 {
920  std::map<std::string, AttachedBody*>::iterator it = attached_body_map_.begin();
921  while (it != attached_body_map_.end())
922  {
923  if (it->second->getAttachedLink() != link)
924  {
925  ++it;
926  continue;
927  }
929  attached_body_update_callback_(it->second, false);
930  delete it->second;
931  std::map<std::string, AttachedBody*>::iterator del = it++;
932  attached_body_map_.erase(del);
933  }
934 }
935 
937 {
938  std::map<std::string, AttachedBody*>::iterator it = attached_body_map_.begin();
939  while (it != attached_body_map_.end())
940  {
941  if (!group->hasLinkModel(it->second->getAttachedLinkName()))
942  {
943  ++it;
944  continue;
945  }
947  attached_body_update_callback_(it->second, false);
948  delete it->second;
949  std::map<std::string, AttachedBody*>::iterator del = it++;
950  attached_body_map_.erase(del);
951  }
952 }
953 
954 bool RobotState::clearAttachedBody(const std::string& id)
955 {
956  std::map<std::string, AttachedBody*>::iterator it = attached_body_map_.find(id);
957  if (it != attached_body_map_.end())
958  {
960  attached_body_update_callback_(it->second, false);
961  delete it->second;
962  attached_body_map_.erase(it);
963  return true;
964  }
965  else
966  return false;
967 }
968 
969 const Eigen::Affine3d& RobotState::getFrameTransform(const std::string& id)
970 {
972  return static_cast<const RobotState*>(this)->getFrameTransform(id);
973 }
974 
975 const Eigen::Affine3d& RobotState::getFrameTransform(const std::string& id) const
976 {
977  if (!id.empty() && id[0] == '/')
978  return getFrameTransform(id.substr(1));
979  BOOST_VERIFY(checkLinkTransforms());
980 
981  static const Eigen::Affine3d identity_transform = Eigen::Affine3d::Identity();
982  if (id.size() + 1 == robot_model_->getModelFrame().size() && '/' + id == robot_model_->getModelFrame())
983  return identity_transform;
984  if (robot_model_->hasLinkModel(id))
985  {
986  const LinkModel* lm = robot_model_->getLinkModel(id);
988  }
989  std::map<std::string, AttachedBody*>::const_iterator jt = attached_body_map_.find(id);
990  if (jt == attached_body_map_.end())
991  {
992  ROS_ERROR_NAMED(LOGNAME, "Transform from frame '%s' to frame '%s' is not known "
993  "('%s' should be a link name or an attached body id).",
994  id.c_str(), robot_model_->getModelFrame().c_str(), id.c_str());
995  return identity_transform;
996  }
997  const EigenSTL::vector_Affine3d& tf = jt->second->getGlobalCollisionBodyTransforms();
998  if (tf.empty())
999  {
1000  ROS_ERROR_NAMED(LOGNAME, "Attached body '%s' has no geometry associated to it. No transform to return.",
1001  id.c_str());
1002  return identity_transform;
1003  }
1004  if (tf.size() > 1)
1005  ROS_DEBUG_NAMED(LOGNAME, "There are multiple geometries associated to attached body '%s'. "
1006  "Returning the transform for the first one.",
1007  id.c_str());
1008  return tf[0];
1009 }
1010 
1011 bool RobotState::knowsFrameTransform(const std::string& id) const
1012 {
1013  if (!id.empty() && id[0] == '/')
1014  return knowsFrameTransform(id.substr(1));
1015  if (robot_model_->hasLinkModel(id))
1016  return true;
1017  std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.find(id);
1018  return it != attached_body_map_.end() && !it->second->getGlobalCollisionBodyTransforms().empty();
1019 }
1020 
1021 void RobotState::getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1022  const std_msgs::ColorRGBA& color, const std::string& ns, const ros::Duration& dur,
1023  bool include_attached) const
1024 {
1025  std::size_t cur_num = arr.markers.size();
1026  getRobotMarkers(arr, link_names, include_attached);
1027  unsigned int id = cur_num;
1028  for (std::size_t i = cur_num; i < arr.markers.size(); ++i, ++id)
1029  {
1030  arr.markers[i].ns = ns;
1031  arr.markers[i].id = id;
1032  arr.markers[i].lifetime = dur;
1033  arr.markers[i].color = color;
1034  }
1035 }
1036 
1037 void RobotState::getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1038  bool include_attached) const
1039 {
1040  ros::Time tm = ros::Time::now();
1041  for (std::size_t i = 0; i < link_names.size(); ++i)
1042  {
1043  ROS_DEBUG_NAMED(LOGNAME, "Trying to get marker for link '%s'", link_names[i].c_str());
1044  const LinkModel* lm = robot_model_->getLinkModel(link_names[i]);
1045  if (!lm)
1046  continue;
1047  if (include_attached)
1048  for (std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.begin();
1049  it != attached_body_map_.end(); ++it)
1050  if (it->second->getAttachedLink() == lm)
1051  {
1052  for (std::size_t j = 0; j < it->second->getShapes().size(); ++j)
1053  {
1054  visualization_msgs::Marker att_mark;
1055  att_mark.header.frame_id = robot_model_->getModelFrame();
1056  att_mark.header.stamp = tm;
1057  if (shapes::constructMarkerFromShape(it->second->getShapes()[j].get(), att_mark))
1058  {
1059  // if the object is invisible (0 volume) we skip it
1060  if (fabs(att_mark.scale.x * att_mark.scale.y * att_mark.scale.z) < std::numeric_limits<float>::epsilon())
1061  continue;
1062  tf::poseEigenToMsg(it->second->getGlobalCollisionBodyTransforms()[j], att_mark.pose);
1063  arr.markers.push_back(att_mark);
1064  }
1065  }
1066  }
1067 
1068  if (lm->getShapes().empty())
1069  continue;
1070 
1071  for (std::size_t j = 0; j < lm->getShapes().size(); ++j)
1072  {
1073  visualization_msgs::Marker mark;
1074  mark.header.frame_id = robot_model_->getModelFrame();
1075  mark.header.stamp = tm;
1076 
1077  // we prefer using the visual mesh, if a mesh is available and we have one body to render
1078  const std::string& mesh_resource = lm->getVisualMeshFilename();
1079  if (mesh_resource.empty() || lm->getShapes().size() > 1)
1080  {
1081  if (!shapes::constructMarkerFromShape(lm->getShapes()[j].get(), mark))
1082  continue;
1083  // if the object is invisible (0 volume) we skip it
1084  if (fabs(mark.scale.x * mark.scale.y * mark.scale.z) < std::numeric_limits<float>::epsilon())
1085  continue;
1087  }
1088  else
1089  {
1090  mark.type = mark.MESH_RESOURCE;
1091  mark.mesh_use_embedded_materials = false;
1092  mark.mesh_resource = mesh_resource;
1093  const Eigen::Vector3d& mesh_scale = lm->getVisualMeshScale();
1094 
1095  mark.scale.x = mesh_scale[0];
1096  mark.scale.y = mesh_scale[1];
1097  mark.scale.z = mesh_scale[2];
1099  }
1100 
1101  arr.markers.push_back(mark);
1102  }
1103  }
1104 }
1105 
1106 Eigen::MatrixXd RobotState::getJacobian(const JointModelGroup* group,
1107  const Eigen::Vector3d& reference_point_position) const
1108 {
1109  Eigen::MatrixXd result;
1110  if (!getJacobian(group, group->getLinkModels().back(), reference_point_position, result, false))
1111  throw Exception("Unable to compute Jacobian");
1112  return result;
1113 }
1114 
1115 bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link,
1116  const Eigen::Vector3d& reference_point_position, Eigen::MatrixXd& jacobian,
1117  bool use_quaternion_representation) const
1118 {
1119  BOOST_VERIFY(checkLinkTransforms());
1120 
1121  if (!group->isChain())
1122  {
1123  ROS_ERROR_NAMED(LOGNAME, "The group '%s' is not a chain. Cannot compute Jacobian.", group->getName().c_str());
1124  return false;
1125  }
1126 
1127  if (!group->isLinkUpdated(link->getName()))
1128  {
1129  ROS_ERROR_NAMED(LOGNAME, "Link name '%s' does not exist in the chain '%s' or is not a child for this chain",
1130  link->getName().c_str(), group->getName().c_str());
1131  return false;
1132  }
1133 
1134  const robot_model::JointModel* root_joint_model = group->getJointModels()[0]; // group->getJointRoots()[0];
1135  const robot_model::LinkModel* root_link_model = root_joint_model->getParentLinkModel();
1136  Eigen::Affine3d reference_transform =
1137  root_link_model ? getGlobalLinkTransform(root_link_model).inverse(Eigen::Isometry) : Eigen::Affine3d::Identity();
1138  int rows = use_quaternion_representation ? 7 : 6;
1139  int columns = group->getVariableCount();
1140  jacobian = Eigen::MatrixXd::Zero(rows, columns);
1141 
1142  Eigen::Affine3d link_transform = reference_transform * getGlobalLinkTransform(link);
1143  Eigen::Vector3d point_transform = link_transform * reference_point_position;
1144 
1145  /*
1146  ROS_DEBUG_NAMED(LOGNAME, "Point from reference origin expressed in world coordinates: %f %f %f",
1147  point_transform.x(),
1148  point_transform.y(),
1149  point_transform.z());
1150  */
1151 
1152  Eigen::Vector3d joint_axis;
1153  Eigen::Affine3d joint_transform;
1154 
1155  while (link)
1156  {
1157  /*
1158  ROS_DEBUG_NAMED(LOGNAME, "Link: %s, %f %f %f",link_state->getName().c_str(),
1159  link_state->getGlobalLinkTransform().translation().x(),
1160  link_state->getGlobalLinkTransform().translation().y(),
1161  link_state->getGlobalLinkTransform().translation().z());
1162  ROS_DEBUG_NAMED(LOGNAME, "Joint: %s",link_state->getParentJointState()->getName().c_str());
1163  */
1164  const JointModel* pjm = link->getParentJointModel();
1165  if (pjm->getVariableCount() > 0)
1166  {
1167  if (not group->hasJointModel(pjm->getName()))
1168  {
1169  link = pjm->getParentLinkModel();
1170  continue;
1171  }
1172  unsigned int joint_index = group->getVariableGroupIndex(pjm->getName());
1174  {
1175  joint_transform = reference_transform * getGlobalLinkTransform(link);
1176  joint_axis = joint_transform.linear() * static_cast<const robot_model::RevoluteJointModel*>(pjm)->getAxis();
1177  jacobian.block<3, 1>(0, joint_index) =
1178  jacobian.block<3, 1>(0, joint_index) + joint_axis.cross(point_transform - joint_transform.translation());
1179  jacobian.block<3, 1>(3, joint_index) = jacobian.block<3, 1>(3, joint_index) + joint_axis;
1180  }
1181  else if (pjm->getType() == robot_model::JointModel::PRISMATIC)
1182  {
1183  joint_transform = reference_transform * getGlobalLinkTransform(link);
1184  joint_axis = joint_transform.rotation() * static_cast<const robot_model::PrismaticJointModel*>(pjm)->getAxis();
1185  jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis;
1186  }
1187  else if (pjm->getType() == robot_model::JointModel::PLANAR)
1188  {
1189  joint_transform = reference_transform * getGlobalLinkTransform(link);
1190  joint_axis = joint_transform * Eigen::Vector3d(1.0, 0.0, 0.0);
1191  jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis;
1192  joint_axis = joint_transform * Eigen::Vector3d(0.0, 1.0, 0.0);
1193  jacobian.block<3, 1>(0, joint_index + 1) = jacobian.block<3, 1>(0, joint_index + 1) + joint_axis;
1194  joint_axis = joint_transform * Eigen::Vector3d(0.0, 0.0, 1.0);
1195  jacobian.block<3, 1>(0, joint_index + 2) = jacobian.block<3, 1>(0, joint_index + 2) +
1196  joint_axis.cross(point_transform - joint_transform.translation());
1197  jacobian.block<3, 1>(3, joint_index + 2) = jacobian.block<3, 1>(3, joint_index + 2) + joint_axis;
1198  }
1199  else
1200  ROS_ERROR_NAMED(LOGNAME, "Unknown type of joint in Jacobian computation");
1201  }
1202  if (pjm == root_joint_model)
1203  break;
1204  link = pjm->getParentLinkModel();
1205  }
1206  if (use_quaternion_representation)
1207  { // Quaternion representation
1208  // From "Advanced Dynamics and Motion Simulation" by Paul Mitiguy
1209  // d/dt ( [w] ) = 1/2 * [ -x -y -z ] * [ omega_1 ]
1210  // [x] [ w -z y ] [ omega_2 ]
1211  // [y] [ z w -x ] [ omega_3 ]
1212  // [z] [ -y x w ]
1213  Eigen::Quaterniond q(link_transform.linear());
1214  double w = q.w(), x = q.x(), y = q.y(), z = q.z();
1215  Eigen::MatrixXd quaternion_update_matrix(4, 3);
1216  quaternion_update_matrix << -x, -y, -z, w, -z, y, z, w, -x, -y, x, w;
1217  jacobian.block(3, 0, 4, columns) = 0.5 * quaternion_update_matrix * jacobian.block(3, 0, 3, columns);
1218  }
1219  return true;
1220 }
1221 
1222 bool RobotState::setFromDiffIK(const JointModelGroup* jmg, const Eigen::VectorXd& twist, const std::string& tip,
1223  double dt, const GroupStateValidityCallbackFn& constraint)
1224 {
1225  Eigen::VectorXd qdot;
1226  computeVariableVelocity(jmg, qdot, twist, getLinkModel(tip));
1227  return integrateVariableVelocity(jmg, qdot, dt, constraint);
1228 }
1229 
1230 bool RobotState::setFromDiffIK(const JointModelGroup* jmg, const geometry_msgs::Twist& twist, const std::string& tip,
1231  double dt, const GroupStateValidityCallbackFn& constraint)
1232 {
1233  Eigen::Matrix<double, 6, 1> t;
1234  tf::twistMsgToEigen(twist, t);
1235  return setFromDiffIK(jmg, t, tip, dt, constraint);
1236 }
1237 
1238 void RobotState::computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot,
1239  const Eigen::VectorXd& twist, const LinkModel* tip) const
1240 {
1241  // Get the Jacobian of the group at the current configuration
1242  Eigen::MatrixXd J(6, jmg->getVariableCount());
1243  Eigen::Vector3d reference_point(0.0, 0.0, 0.0);
1244  getJacobian(jmg, tip, reference_point, J, false);
1245 
1246  // Rotate the jacobian to the end-effector frame
1247  Eigen::Affine3d eMb = getGlobalLinkTransform(tip).inverse(Eigen::Isometry);
1248  Eigen::MatrixXd eWb = Eigen::ArrayXXd::Zero(6, 6);
1249  eWb.block(0, 0, 3, 3) = eMb.matrix().block(0, 0, 3, 3);
1250  eWb.block(3, 3, 3, 3) = eMb.matrix().block(0, 0, 3, 3);
1251  J = eWb * J;
1252 
1253  // Do the Jacobian moore-penrose pseudo-inverse
1254  Eigen::JacobiSVD<Eigen::MatrixXd> svdOfJ(J, Eigen::ComputeThinU | Eigen::ComputeThinV);
1255  const Eigen::MatrixXd U = svdOfJ.matrixU();
1256  const Eigen::MatrixXd V = svdOfJ.matrixV();
1257  const Eigen::VectorXd S = svdOfJ.singularValues();
1258 
1259  Eigen::VectorXd Sinv = S;
1260  static const double pinvtoler = std::numeric_limits<float>::epsilon();
1261  double maxsv = 0.0;
1262  for (std::size_t i = 0; i < static_cast<std::size_t>(S.rows()); ++i)
1263  if (fabs(S(i)) > maxsv)
1264  maxsv = fabs(S(i));
1265  for (std::size_t i = 0; i < static_cast<std::size_t>(S.rows()); ++i)
1266  {
1267  // Those singular values smaller than a percentage of the maximum singular value are removed
1268  if (fabs(S(i)) > maxsv * pinvtoler)
1269  Sinv(i) = 1.0 / S(i);
1270  else
1271  Sinv(i) = 0.0;
1272  }
1273  Eigen::MatrixXd Jinv = (V * Sinv.asDiagonal() * U.transpose());
1274 
1275  // Compute joint velocity
1276  qdot = Jinv * twist;
1277 }
1278 
1279 bool RobotState::integrateVariableVelocity(const JointModelGroup* jmg, const Eigen::VectorXd& qdot, double dt,
1280  const GroupStateValidityCallbackFn& constraint)
1281 {
1282  Eigen::VectorXd q(jmg->getVariableCount());
1283  copyJointGroupPositions(jmg, q);
1284  q = q + dt * qdot;
1285  setJointGroupPositions(jmg, q);
1286  enforceBounds(jmg);
1287 
1288  if (constraint)
1289  {
1290  std::vector<double> values;
1291  copyJointGroupPositions(jmg, values);
1292  return constraint(this, jmg, &values[0]);
1293  }
1294  else
1295  return true;
1296 }
1297 
1298 bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::Pose& pose, unsigned int attempts,
1299  double timeout, const GroupStateValidityCallbackFn& constraint,
1300  const kinematics::KinematicsQueryOptions& options)
1301 {
1302  const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance();
1303  if (!solver)
1304  {
1305  ROS_ERROR_NAMED(LOGNAME, "No kinematics solver instantiated for group '%s'", jmg->getName().c_str());
1306  return false;
1307  }
1308  return setFromIK(jmg, pose, solver->getTipFrame(), attempts, timeout, constraint, options);
1309 }
1310 
1311 bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::Pose& pose, const std::string& tip,
1312  unsigned int attempts, double timeout, const GroupStateValidityCallbackFn& constraint,
1313  const kinematics::KinematicsQueryOptions& options)
1314 {
1315  Eigen::Affine3d mat;
1316  tf::poseMsgToEigen(pose, mat);
1317  static std::vector<double> consistency_limits;
1318  return setFromIK(jmg, mat, tip, consistency_limits, attempts, timeout, constraint, options);
1319 }
1320 
1321 bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Affine3d& pose, unsigned int attempts,
1322  double timeout, const GroupStateValidityCallbackFn& constraint,
1323  const kinematics::KinematicsQueryOptions& options)
1324 {
1325  const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance();
1326  if (!solver)
1327  {
1328  ROS_ERROR_NAMED(LOGNAME, "No kinematics solver instantiated for group '%s'", jmg->getName().c_str());
1329  return false;
1330  }
1331  static std::vector<double> consistency_limits;
1332  return setFromIK(jmg, pose, solver->getTipFrame(), consistency_limits, attempts, timeout, constraint, options);
1333 }
1334 
1335 bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Affine3d& pose_in, const std::string& tip_in,
1336  unsigned int attempts, double timeout, const GroupStateValidityCallbackFn& constraint,
1337  const kinematics::KinematicsQueryOptions& options)
1338 {
1339  static std::vector<double> consistency_limits;
1340  return setFromIK(jmg, pose_in, tip_in, consistency_limits, attempts, timeout, constraint, options);
1341 }
1342 
1343 namespace
1344 {
1345 bool ikCallbackFnAdapter(RobotState* state, const JointModelGroup* group,
1346  const GroupStateValidityCallbackFn& constraint, const geometry_msgs::Pose& /*unused*/,
1347  const std::vector<double>& ik_sol, moveit_msgs::MoveItErrorCodes& error_code)
1348 {
1349  const std::vector<unsigned int>& bij = group->getKinematicsSolverJointBijection();
1350  std::vector<double> solution(bij.size());
1351  for (std::size_t i = 0; i < bij.size(); ++i)
1352  solution[bij[i]] = ik_sol[i];
1353  if (constraint(state, group, &solution[0]))
1354  error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
1355  else
1357  return true;
1358 }
1359 }
1360 
1361 bool RobotState::setToIKSolverFrame(Eigen::Affine3d& pose, const kinematics::KinematicsBaseConstPtr& solver)
1362 {
1363  return setToIKSolverFrame(pose, solver->getBaseFrame());
1364 }
1365 
1366 bool RobotState::setToIKSolverFrame(Eigen::Affine3d& pose, const std::string& ik_frame)
1367 {
1368  // Bring the pose to the frame of the IK solver
1369  if (!Transforms::sameFrame(ik_frame, robot_model_->getModelFrame()))
1370  {
1371  const LinkModel* lm = getLinkModel((!ik_frame.empty() && ik_frame[0] == '/') ? ik_frame.substr(1) : ik_frame);
1372  if (!lm)
1373  {
1374  ROS_ERROR_STREAM_NAMED(LOGNAME, "IK frame '" << ik_frame << "' does not exist.");
1375  return false;
1376  }
1377  pose = getGlobalLinkTransform(lm).inverse(Eigen::Isometry) * pose;
1378  }
1379  return true;
1380 }
1381 
1382 bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Affine3d& pose_in, const std::string& tip_in,
1383  const std::vector<double>& consistency_limits_in, unsigned int attempts, double timeout,
1384  const GroupStateValidityCallbackFn& constraint,
1385  const kinematics::KinematicsQueryOptions& options)
1386 {
1387  // Convert from single pose and tip to vectors
1389  poses.push_back(pose_in);
1390 
1391  std::vector<std::string> tips;
1392  tips.push_back(tip_in);
1393 
1394  std::vector<std::vector<double> > consistency_limits;
1395  consistency_limits.push_back(consistency_limits_in);
1396 
1397  return setFromIK(jmg, poses, tips, consistency_limits, attempts, timeout, constraint, options);
1398 }
1399 
1401  const std::vector<std::string>& tips_in, unsigned int attempts, double timeout,
1402  const GroupStateValidityCallbackFn& constraint,
1403  const kinematics::KinematicsQueryOptions& options)
1404 {
1405  static const std::vector<std::vector<double> > consistency_limits;
1406  return setFromIK(jmg, poses_in, tips_in, consistency_limits, attempts, timeout, constraint, options);
1407 }
1408 
1410  const std::vector<std::string>& tips_in,
1411  const std::vector<std::vector<double> >& consistency_limit_sets, unsigned int attempts,
1412  double timeout, const GroupStateValidityCallbackFn& constraint,
1413  const kinematics::KinematicsQueryOptions& options)
1414 {
1415  // Error check
1416  if (poses_in.size() != tips_in.size())
1417  {
1418  ROS_ERROR_NAMED(LOGNAME, "Number of poses must be the same as number of tips");
1419  return false;
1420  }
1421 
1422  // Load solver
1423  const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance();
1424 
1425  // Check if this jmg has a solver
1426  bool valid_solver = true;
1427  if (!solver)
1428  {
1429  valid_solver = false;
1430  }
1431  // Check if this jmg's IK solver can handle multiple tips (non-chain solver)
1432  else if (poses_in.size() > 1)
1433  {
1434  std::string error_msg;
1435  if (!solver->supportsGroup(jmg, &error_msg))
1436  {
1437  ROS_ERROR_NAMED(LOGNAME, "Kinematics solver %s does not support joint group %s. Error: %s",
1438  typeid(*solver).name(), jmg->getName().c_str(), error_msg.c_str());
1439  valid_solver = false;
1440  }
1441  }
1442 
1443  if (!valid_solver)
1444  {
1445  // Check if there are subgroups that can solve this for us (non-chains)
1446  if (poses_in.size() > 1)
1447  {
1448  // Forward to setFromIKSubgroups() to allow different subgroup IK solvers to work together
1449  return setFromIKSubgroups(jmg, poses_in, tips_in, consistency_limit_sets, attempts, timeout, constraint, options);
1450  }
1451  else
1452  {
1453  ROS_ERROR_NAMED(LOGNAME, "No kinematics solver instantiated for group '%s'", jmg->getName().c_str());
1454  return false;
1455  }
1456  }
1457 
1458  // Check that no, or only one set of consistency limits have been passed in, and choose that one
1459  std::vector<double> consistency_limits;
1460  if (consistency_limit_sets.size() > 1)
1461  {
1462  ROS_ERROR_NAMED(LOGNAME, "Invalid number (%zu) of sets of consistency limits for a setFromIK request "
1463  "that is being solved by a single IK solver",
1464  consistency_limit_sets.size());
1465  return false;
1466  }
1467  else if (consistency_limit_sets.size() == 1)
1468  consistency_limits = consistency_limit_sets[0];
1469 
1470  const std::vector<std::string>& solver_tip_frames = solver->getTipFrames();
1471 
1472  // Track which possible tips frames we have filled in so far
1473  std::vector<bool> tip_frames_used(solver_tip_frames.size(), false);
1474 
1475  // Create vector to hold the output frames in the same order as solver_tip_frames
1476  std::vector<geometry_msgs::Pose> ik_queries(solver_tip_frames.size());
1477 
1478  // Bring each pose to the frame of the IK solver
1479  for (std::size_t i = 0; i < poses_in.size(); ++i)
1480  {
1481  // Make non-const
1482  Eigen::Affine3d pose = poses_in[i];
1483  std::string pose_frame = tips_in[i];
1484 
1485  // Remove extra slash
1486  if (!pose_frame.empty() && pose_frame[0] == '/')
1487  pose_frame = pose_frame.substr(1);
1488 
1489  // bring the pose to the frame of the IK solver
1490  if (!setToIKSolverFrame(pose, solver))
1491  return false;
1492 
1493  // try all of the solver's possible tip frames to see if they uniquely align with any of our passed in pose tip
1494  // frames
1495  bool found_valid_frame = false;
1496  std::size_t solver_tip_id; // our current index
1497  for (solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id)
1498  {
1499  // Check if this tip frame is already accounted for
1500  if (tip_frames_used[solver_tip_id])
1501  continue; // already has a pose
1502 
1503  // check if the tip frame can be transformed via fixed transforms to the frame known to the IK solver
1504  std::string solver_tip_frame = solver_tip_frames[solver_tip_id];
1505 
1506  // remove the frame '/' if there is one, so we can avoid calling Transforms::sameFrame() which may copy strings
1507  // more often that we need to
1508  if (!solver_tip_frame.empty() && solver_tip_frame[0] == '/')
1509  solver_tip_frame = solver_tip_frame.substr(1);
1510 
1511  if (pose_frame != solver_tip_frame)
1512  {
1513  if (hasAttachedBody(pose_frame))
1514  {
1515  const AttachedBody* ab = getAttachedBody(pose_frame);
1516  const EigenSTL::vector_Affine3d& ab_trans = ab->getFixedTransforms();
1517  if (ab_trans.size() != 1)
1518  {
1519  ROS_ERROR_NAMED(LOGNAME, "Cannot use an attached body "
1520  "with multiple geometries as a reference frame.");
1521  return false;
1522  }
1523  pose_frame = ab->getAttachedLinkName();
1524  pose = pose * ab_trans[0].inverse(Eigen::Isometry);
1525  }
1526  if (pose_frame != solver_tip_frame)
1527  {
1528  const robot_model::LinkModel* lm = getLinkModel(pose_frame);
1529  if (!lm)
1530  {
1531  ROS_ERROR_STREAM_NAMED(LOGNAME, "Pose frame '" << pose_frame << "' does not exist.");
1532  return false;
1533  }
1535  for (robot_model::LinkTransformMap::const_iterator it = fixed_links.begin(); it != fixed_links.end(); ++it)
1536  if (Transforms::sameFrame(it->first->getName(), solver_tip_frame))
1537  {
1538  pose_frame = solver_tip_frame;
1539  pose = pose * it->second;
1540  break;
1541  }
1542  }
1543 
1544  } // end if pose_frame
1545 
1546  // Check if this pose frame works
1547  if (pose_frame == solver_tip_frame)
1548  {
1549  found_valid_frame = true;
1550  break;
1551  }
1552 
1553  } // end for solver_tip_frames
1554 
1555  // Make sure one of the tip frames worked
1556  if (!found_valid_frame)
1557  {
1558  ROS_ERROR_NAMED(LOGNAME, "Cannot compute IK for query %zu pose reference frame '%s'", i, pose_frame.c_str());
1559  // Debug available tip frames
1560  std::stringstream ss;
1561  for (solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id)
1562  ss << solver_tip_frames[solver_tip_id] << ", ";
1563  ROS_ERROR_NAMED(LOGNAME, "Available tip frames: [%s]", ss.str().c_str());
1564  return false;
1565  }
1566 
1567  // Remove that tip from the list of available tip frames because each can only have one pose
1568  tip_frames_used[solver_tip_id] = true;
1569 
1570  // Convert Eigen pose to geometry_msgs pose
1571  geometry_msgs::Pose ik_query;
1572  tf::poseEigenToMsg(pose, ik_query);
1573 
1574  // Save into vectors
1575  ik_queries[solver_tip_id] = ik_query;
1576  } // end for poses_in
1577 
1578  // Create poses for all remaining tips a solver expects, even if not passed into this function
1579  for (std::size_t solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id)
1580  {
1581  // Check if this tip frame is already accounted for
1582  if (tip_frames_used[solver_tip_id])
1583  continue; // already has a pose
1584 
1585  // Process this tip
1586  std::string solver_tip_frame = solver_tip_frames[solver_tip_id];
1587 
1588  // remove the frame '/' if there is one, so we can avoid calling Transforms::sameFrame() which may copy strings more
1589  // often that we need to
1590  if (!solver_tip_frame.empty() && solver_tip_frame[0] == '/')
1591  solver_tip_frame = solver_tip_frame.substr(1);
1592 
1593  // Get the pose of a different EE tip link
1594  Eigen::Affine3d current_pose = getGlobalLinkTransform(solver_tip_frame);
1595 
1596  // bring the pose to the frame of the IK solver
1597  if (!setToIKSolverFrame(current_pose, solver))
1598  return false;
1599 
1600  // Convert Eigen pose to geometry_msgs pose
1601  geometry_msgs::Pose ik_query;
1602  tf::poseEigenToMsg(current_pose, ik_query);
1603 
1604  // Save into vectors - but this needs to be ordered in the same order as the IK solver expects its tip frames
1605  ik_queries[solver_tip_id] = ik_query;
1606 
1607  // Remove that tip from the list of available tip frames because each can only have one pose
1608  tip_frames_used[solver_tip_id] = true;
1609  }
1610 
1611  // if no timeout has been specified, use the default one
1612  if (timeout < std::numeric_limits<double>::epsilon())
1613  timeout = jmg->getDefaultIKTimeout();
1614 
1615  if (attempts == 0)
1616  attempts = jmg->getDefaultIKAttempts();
1617 
1618  // set callback function
1620  if (constraint)
1621  ik_callback_fn = boost::bind(&ikCallbackFnAdapter, this, jmg, constraint, _1, _2, _3);
1622 
1623  // Bijection
1624  const std::vector<unsigned int>& bij = jmg->getKinematicsSolverJointBijection();
1625 
1626  bool first_seed = true;
1627  std::vector<double> initial_values;
1628  for (unsigned int st = 0; st < attempts; ++st)
1629  {
1630  std::vector<double> seed(bij.size());
1631 
1632  // the first seed is the current robot state joint values
1633  if (first_seed)
1634  {
1635  first_seed = false;
1636  copyJointGroupPositions(jmg, initial_values);
1637  for (std::size_t i = 0; i < bij.size(); ++i)
1638  seed[i] = initial_values[bij[i]];
1639  }
1640  else
1641  {
1642  ROS_DEBUG_NAMED(LOGNAME, "Rerunning IK solver with random joint positions");
1643 
1644  // sample a random seed
1646  std::vector<double> random_values;
1647  jmg->getVariableRandomPositions(rng, random_values);
1648  for (std::size_t i = 0; i < bij.size(); ++i)
1649  seed[i] = random_values[bij[i]];
1650 
1651  if (options.lock_redundant_joints)
1652  {
1653  std::vector<unsigned int> red_joints;
1654  solver->getRedundantJoints(red_joints);
1655  copyJointGroupPositions(jmg, initial_values);
1656  for (std::size_t i = 0; i < red_joints.size(); ++i)
1657  seed[red_joints[i]] = initial_values[bij[red_joints[i]]];
1658  }
1659  }
1660 
1661  // compute the IK solution
1662  std::vector<double> ik_sol;
1663  moveit_msgs::MoveItErrorCodes error;
1664 
1665  if (solver->searchPositionIK(ik_queries, seed, timeout, consistency_limits, ik_sol, ik_callback_fn, error, options,
1666  this))
1667  {
1668  std::vector<double> solution(bij.size());
1669  for (std::size_t i = 0; i < bij.size(); ++i)
1670  solution[bij[i]] = ik_sol[i];
1671  setJointGroupPositions(jmg, solution);
1672  return true;
1673  }
1674  }
1675  return false;
1676 }
1677 
1679  const std::vector<std::string>& tips_in,
1680  const std::vector<std::vector<double> >& consistency_limits, unsigned int attempts,
1681  double timeout, const GroupStateValidityCallbackFn& constraint,
1682  const kinematics::KinematicsQueryOptions& options)
1683 {
1684  // Assume we have already ran setFromIK() and those checks
1685 
1686  // Get containing subgroups
1687  std::vector<const JointModelGroup*> sub_groups;
1688  jmg->getSubgroups(sub_groups);
1689 
1690  // Error check
1691  if (poses_in.size() != sub_groups.size())
1692  {
1693  ROS_ERROR_NAMED(LOGNAME, "Number of poses (%zu) must be the same as number of sub-groups (%zu)", poses_in.size(),
1694  sub_groups.size());
1695  return false;
1696  }
1697 
1698  if (tips_in.size() != sub_groups.size())
1699  {
1700  ROS_ERROR_NAMED(LOGNAME, "Number of tip names (%zu) must be same as number of sub-groups (%zu)", tips_in.size(),
1701  sub_groups.size());
1702  return false;
1703  }
1704 
1705  if (!consistency_limits.empty() && consistency_limits.size() != sub_groups.size())
1706  {
1707  ROS_ERROR_NAMED(LOGNAME, "Number of consistency limit vectors must be the same as number of sub-groups");
1708  return false;
1709  }
1710 
1711  for (std::size_t i = 0; i < consistency_limits.size(); ++i)
1712  {
1713  if (consistency_limits[i].size() != sub_groups[i]->getVariableCount())
1714  {
1715  ROS_ERROR_NAMED(LOGNAME, "Number of joints in consistency_limits is %zu but it should be should be %u", i,
1716  sub_groups[i]->getVariableCount());
1717  return false;
1718  }
1719  }
1720 
1721  // Populate list of kin solvers for the various subgroups
1722  std::vector<kinematics::KinematicsBaseConstPtr> solvers;
1723  for (std::size_t i = 0; i < poses_in.size(); ++i)
1724  {
1725  kinematics::KinematicsBaseConstPtr solver = sub_groups[i]->getSolverInstance();
1726  if (!solver)
1727  {
1728  ROS_ERROR_NAMED(LOGNAME, "Could not find solver for group '%s'", sub_groups[i]->getName().c_str());
1729  return false;
1730  }
1731  solvers.push_back(solver);
1732  }
1733 
1734  // Make non-const versions
1735  EigenSTL::vector_Affine3d transformed_poses = poses_in;
1736  std::vector<std::string> pose_frames = tips_in;
1737 
1738  // Each each pose's tip frame naming
1739  for (std::size_t i = 0; i < poses_in.size(); ++i)
1740  {
1741  Eigen::Affine3d& pose = transformed_poses[i];
1742  std::string& pose_frame = pose_frames[i];
1743 
1744  // bring the pose to the frame of the IK solver
1745  if (!setToIKSolverFrame(pose, solvers[i]))
1746  return false;
1747 
1748  // see if the tip frame can be transformed via fixed transforms to the frame known to the IK solver
1749  std::string solver_tip_frame = solvers[i]->getTipFrame();
1750 
1751  // remove the frame '/' if there is one, so we can avoid calling Transforms::sameFrame() which may copy strings more
1752  // often that we need to
1753  if (!solver_tip_frame.empty() && solver_tip_frame[0] == '/')
1754  solver_tip_frame = solver_tip_frame.substr(1);
1755 
1756  if (pose_frame != solver_tip_frame)
1757  {
1758  if (hasAttachedBody(pose_frame))
1759  {
1760  const AttachedBody* ab = getAttachedBody(pose_frame);
1761  const EigenSTL::vector_Affine3d& ab_trans = ab->getFixedTransforms();
1762  if (ab_trans.size() != 1)
1763  {
1764  ROS_ERROR_NAMED(LOGNAME, "Cannot use an attached body with multiple geometries as a reference frame.");
1765  return false;
1766  }
1767  pose_frame = ab->getAttachedLinkName();
1768  pose = pose * ab_trans[0].inverse(Eigen::Isometry);
1769  }
1770  if (pose_frame != solver_tip_frame)
1771  {
1772  const robot_model::LinkModel* lm = getLinkModel(pose_frame);
1773  if (!lm)
1774  return false;
1776  for (robot_model::LinkTransformMap::const_iterator it = fixed_links.begin(); it != fixed_links.end(); ++it)
1777  if (it->first->getName() == solver_tip_frame)
1778  {
1779  pose_frame = solver_tip_frame;
1780  pose = pose * it->second;
1781  break;
1782  }
1783  }
1784  }
1785 
1786  if (pose_frame != solver_tip_frame)
1787  {
1788  ROS_ERROR_NAMED(LOGNAME, "Cannot compute IK for query pose reference frame '%s', desired: '%s'",
1789  pose_frame.c_str(), solver_tip_frame.c_str());
1790  return false;
1791  }
1792  }
1793 
1794  // Convert Eigen poses to geometry_msg format
1795  std::vector<geometry_msgs::Pose> ik_queries(poses_in.size());
1797  if (constraint)
1798  ik_callback_fn = boost::bind(&ikCallbackFnAdapter, this, jmg, constraint, _1, _2, _3);
1799 
1800  for (std::size_t i = 0; i < transformed_poses.size(); ++i)
1801  {
1802  Eigen::Quaterniond quat(transformed_poses[i].linear());
1803  Eigen::Vector3d point(transformed_poses[i].translation());
1804  ik_queries[i].position.x = point.x();
1805  ik_queries[i].position.y = point.y();
1806  ik_queries[i].position.z = point.z();
1807  ik_queries[i].orientation.x = quat.x();
1808  ik_queries[i].orientation.y = quat.y();
1809  ik_queries[i].orientation.z = quat.z();
1810  ik_queries[i].orientation.w = quat.w();
1811  }
1812 
1813  if (attempts == 0)
1814  attempts = jmg->getDefaultIKAttempts();
1815 
1816  // if no timeout has been specified, use the default one
1817  if (timeout < std::numeric_limits<double>::epsilon())
1818  timeout = jmg->getDefaultIKTimeout();
1819 
1820  bool first_seed = true;
1821  for (unsigned int st = 0; st < attempts; ++st)
1822  {
1823  bool found_solution = true;
1824  for (std::size_t sg = 0; sg < sub_groups.size(); ++sg)
1825  {
1826  const std::vector<unsigned int>& bij = sub_groups[sg]->getKinematicsSolverJointBijection();
1827  std::vector<double> seed(bij.size());
1828  // the first seed is the initial state
1829  if (first_seed)
1830  {
1831  std::vector<double> initial_values;
1832  copyJointGroupPositions(sub_groups[sg], initial_values);
1833  for (std::size_t i = 0; i < bij.size(); ++i)
1834  seed[i] = initial_values[bij[i]];
1835  }
1836  else
1837  {
1838  // sample a random seed
1840  std::vector<double> random_values;
1841  sub_groups[sg]->getVariableRandomPositions(rng, random_values);
1842  for (std::size_t i = 0; i < bij.size(); ++i)
1843  seed[i] = random_values[bij[i]];
1844  }
1845 
1846  // compute the IK solution
1847  std::vector<double> ik_sol;
1848  moveit_msgs::MoveItErrorCodes error;
1849  const std::vector<double>& climits = consistency_limits.empty() ? std::vector<double>() : consistency_limits[sg];
1850  if (solvers[sg]->searchPositionIK(ik_queries[sg], seed, timeout, climits, ik_sol, error))
1851  {
1852  std::vector<double> solution(bij.size());
1853  for (std::size_t i = 0; i < bij.size(); ++i)
1854  solution[bij[i]] = ik_sol[i];
1855  setJointGroupPositions(sub_groups[sg], solution);
1856  }
1857  else
1858  {
1859  found_solution = false;
1860  break;
1861  }
1862  ROS_DEBUG_NAMED(LOGNAME, "IK attempt: %d of %d", st, attempts);
1863  }
1864  if (found_solution)
1865  {
1866  std::vector<double> full_solution;
1867  copyJointGroupPositions(jmg, full_solution);
1868  if (constraint ? constraint(this, jmg, &full_solution[0]) : true)
1869  {
1870  ROS_DEBUG_NAMED(LOGNAME, "Found IK solution");
1871  return true;
1872  }
1873  }
1874  }
1875  return false;
1876 }
1877 
1878 double RobotState::computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
1879  const LinkModel* link, const Eigen::Vector3d& direction,
1880  bool global_reference_frame, double distance, const MaxEEFStep& max_step,
1881  const JumpThreshold& jump_threshold,
1882  const GroupStateValidityCallbackFn& validCallback,
1883  const kinematics::KinematicsQueryOptions& options)
1884 {
1885  // this is the Cartesian pose we start from, and have to move in the direction indicated
1886  const Eigen::Affine3d& start_pose = getGlobalLinkTransform(link);
1887 
1888  // the direction can be in the local reference frame (in which case we rotate it)
1889  const Eigen::Vector3d rotated_direction = global_reference_frame ? direction : start_pose.linear() * direction;
1890 
1891  // The target pose is built by applying a translation to the start pose for the desired direction and distance
1892  Eigen::Affine3d target_pose = start_pose;
1893  target_pose.translation() += rotated_direction * distance;
1894 
1895  // call computeCartesianPath for the computed target pose in the global reference frame
1896  return (distance *
1897  computeCartesianPath(group, traj, link, target_pose, true, max_step, jump_threshold, validCallback, options));
1898 }
1899 
1900 double RobotState::computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
1901  const LinkModel* link, const Eigen::Affine3d& target,
1902  bool global_reference_frame, const MaxEEFStep& max_step,
1903  const JumpThreshold& jump_threshold,
1904  const GroupStateValidityCallbackFn& validCallback,
1905  const kinematics::KinematicsQueryOptions& options)
1906 {
1907  const std::vector<const JointModel*>& cjnt = group->getContinuousJointModels();
1908  // make sure that continuous joints wrap
1909  for (std::size_t i = 0; i < cjnt.size(); ++i)
1910  enforceBounds(cjnt[i]);
1911 
1912  // this is the Cartesian pose we start from, and we move in the direction indicated
1913  Eigen::Affine3d start_pose = getGlobalLinkTransform(link);
1914 
1915  // the target can be in the local reference frame (in which case we rotate it)
1916  Eigen::Affine3d rotated_target = global_reference_frame ? target : start_pose * target;
1917 
1918  Eigen::Quaterniond start_quaternion(start_pose.linear());
1919  Eigen::Quaterniond target_quaternion(rotated_target.linear());
1920 
1921  if (max_step.translation <= 0.0 && max_step.rotation <= 0.0)
1922  {
1923  ROS_ERROR_NAMED(LOGNAME,
1924  "Invalid MaxEEFStep passed into computeCartesianPath. Both the MaxEEFStep.rotation and "
1925  "MaxEEFStep.translation components must be non-negative and at least one component must be "
1926  "greater than zero");
1927  return 0.0;
1928  }
1929 
1930  double rotation_distance = start_quaternion.angularDistance(target_quaternion);
1931  double translation_distance = (rotated_target.translation() - start_pose.translation()).norm();
1932 
1933  // decide how many steps we will need for this trajectory
1934  std::size_t translation_steps = 0;
1935  if (max_step.translation > 0.0)
1936  translation_steps = floor(translation_distance / max_step.translation);
1937 
1938  std::size_t rotation_steps = 0;
1939  if (max_step.rotation > 0.0)
1940  rotation_steps = floor(rotation_distance / max_step.rotation);
1941 
1942  // If we are testing for relative jumps, we always want at least MIN_STEPS_FOR_JUMP_THRESH steps
1943  std::size_t steps = std::max(translation_steps, rotation_steps) + 1;
1944  if (jump_threshold.factor > 0 && steps < MIN_STEPS_FOR_JUMP_THRESH)
1945  steps = MIN_STEPS_FOR_JUMP_THRESH;
1946 
1947  traj.clear();
1948  traj.push_back(RobotStatePtr(new RobotState(*this)));
1949 
1950  double last_valid_percentage = 0.0;
1951  for (std::size_t i = 1; i <= steps; ++i)
1952  {
1953  double percentage = (double)i / (double)steps;
1954 
1955  Eigen::Affine3d pose(start_quaternion.slerp(percentage, target_quaternion));
1956  pose.translation() = percentage * rotated_target.translation() + (1 - percentage) * start_pose.translation();
1957 
1958  // Explicitly use a single IK attempt only: We want a smooth trajectory.
1959  // Random seeding (of additional attempts) would probably create IK jumps.
1960  if (setFromIK(group, pose, link->getName(), 1, 0.0, validCallback, options))
1961  traj.push_back(RobotStatePtr(new RobotState(*this)));
1962  else
1963  break;
1964 
1965  last_valid_percentage = percentage;
1966  }
1967 
1968  last_valid_percentage *= testJointSpaceJump(group, traj, jump_threshold);
1969 
1970  return last_valid_percentage;
1971 }
1972 
1973 double RobotState::computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
1974  const LinkModel* link, const EigenSTL::vector_Affine3d& waypoints,
1975  bool global_reference_frame, const MaxEEFStep& max_step,
1976  const JumpThreshold& jump_threshold,
1977  const GroupStateValidityCallbackFn& validCallback,
1978  const kinematics::KinematicsQueryOptions& options)
1979 {
1980  double percentage_solved = 0.0;
1981  for (std::size_t i = 0; i < waypoints.size(); ++i)
1982  {
1983  // Don't test joint space jumps for every waypoint, test them later on the whole trajectory.
1984  static const JumpThreshold no_joint_space_jump_test;
1985  std::vector<RobotStatePtr> waypoint_traj;
1986  double wp_percentage_solved = computeCartesianPath(group, waypoint_traj, link, waypoints[i], global_reference_frame,
1987  max_step, no_joint_space_jump_test, validCallback, options);
1988  if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits<double>::epsilon())
1989  {
1990  percentage_solved = (double)(i + 1) / (double)waypoints.size();
1991  std::vector<RobotStatePtr>::iterator start = waypoint_traj.begin();
1992  if (i > 0 && !waypoint_traj.empty())
1993  std::advance(start, 1);
1994  traj.insert(traj.end(), start, waypoint_traj.end());
1995  }
1996  else
1997  {
1998  percentage_solved += wp_percentage_solved / (double)waypoints.size();
1999  std::vector<RobotStatePtr>::iterator start = waypoint_traj.begin();
2000  if (i > 0 && !waypoint_traj.empty())
2001  std::advance(start, 1);
2002  traj.insert(traj.end(), start, waypoint_traj.end());
2003  break;
2004  }
2005  }
2006 
2007  percentage_solved *= testJointSpaceJump(group, traj, jump_threshold);
2008 
2009  return percentage_solved;
2010 }
2011 
2012 double RobotState::testJointSpaceJump(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
2013  const JumpThreshold& jump_threshold)
2014 {
2015  double percentage_solved = 1.0;
2016  if (traj.size() <= 1)
2017  return percentage_solved;
2018 
2019  if (jump_threshold.factor > 0.0)
2020  percentage_solved *= testRelativeJointSpaceJump(group, traj, jump_threshold.factor);
2021 
2022  if (jump_threshold.revolute > 0.0 || jump_threshold.prismatic > 0.0)
2023  percentage_solved *= testAbsoluteJointSpaceJump(group, traj, jump_threshold.revolute, jump_threshold.prismatic);
2024 
2025  return percentage_solved;
2026 }
2027 
2028 double RobotState::testRelativeJointSpaceJump(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
2029  double jump_threshold_factor)
2030 {
2031  if (traj.size() < MIN_STEPS_FOR_JUMP_THRESH)
2032  {
2033  ROS_WARN_NAMED(LOGNAME, "The computed trajectory is too short to detect jumps in joint-space "
2034  "Need at least %zu steps, only got %zu. Try a lower max_step.",
2035  MIN_STEPS_FOR_JUMP_THRESH, traj.size());
2036  }
2037 
2038  std::vector<double> dist_vector;
2039  dist_vector.reserve(traj.size() - 1);
2040  double total_dist = 0.0;
2041  for (std::size_t i = 1; i < traj.size(); ++i)
2042  {
2043  double dist_prev_point = traj[i]->distance(*traj[i - 1], group);
2044  dist_vector.push_back(dist_prev_point);
2045  total_dist += dist_prev_point;
2046  }
2047 
2048  double percentage = 1.0;
2049  // compute the average distance between the states we looked at
2050  double thres = jump_threshold_factor * (total_dist / (double)dist_vector.size());
2051  for (std::size_t i = 0; i < dist_vector.size(); ++i)
2052  if (dist_vector[i] > thres)
2053  {
2054  ROS_DEBUG_NAMED(LOGNAME, "Truncating Cartesian path due to detected jump in joint-space distance");
2055  percentage = (double)(i + 1) / (double)(traj.size());
2056  traj.resize(i + 1);
2057  break;
2058  }
2059 
2060  return percentage;
2061 }
2062 
2063 double RobotState::testAbsoluteJointSpaceJump(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
2064  double revolute_threshold, double prismatic_threshold)
2065 {
2066  struct LimitData
2067  {
2068  double limit;
2069  bool check;
2070  };
2071  LimitData data[2] = { { revolute_threshold, revolute_threshold > 0.0 },
2072  { prismatic_threshold, prismatic_threshold > 0.0 } };
2073  bool still_valid = true;
2074  const std::vector<const JointModel*>& joints = group->getActiveJointModels();
2075  for (std::size_t traj_ix = 0, ix_end = traj.size() - 1; traj_ix != ix_end; ++traj_ix)
2076  {
2077  for (auto& joint : joints)
2078  {
2079  unsigned int type_index;
2080  switch (joint->getType())
2081  {
2082  case JointModel::REVOLUTE:
2083  type_index = 0;
2084  break;
2085  case JointModel::PRISMATIC:
2086  type_index = 1;
2087  break;
2088  default:
2089  ROS_WARN_NAMED(LOGNAME, "Joint %s has not supported type %s. \n"
2090  "testAbsoluteJointSpaceJump only supports prismatic and revolute joints.",
2091  joint->getName().c_str(), joint->getTypeName().c_str());
2092  continue;
2093  }
2094  if (!data[type_index].check)
2095  continue;
2096 
2097  double distance = traj[traj_ix]->distance(*traj[traj_ix + 1], joint);
2098  if (distance > data[type_index].limit)
2099  {
2100  ROS_DEBUG_NAMED(LOGNAME, "Truncating Cartesian path due to detected jump of %.4f > %.4f in joint %s", distance,
2101  data[type_index].limit, joint->getName().c_str());
2102  still_valid = false;
2103  break;
2104  }
2105  }
2106 
2107  if (!still_valid)
2108  {
2109  double percent_valid = (double)(traj_ix + 1) / (double)(traj.size());
2110  traj.resize(traj_ix + 1);
2111  return percent_valid;
2112  }
2113  }
2114  return 1.0;
2115 }
2116 
2117 void RobotState::computeAABB(std::vector<double>& aabb) const
2118 {
2119  BOOST_VERIFY(checkLinkTransforms());
2120 
2121  core::AABB bounding_box;
2122  std::vector<const LinkModel*> links = robot_model_->getLinkModelsWithCollisionGeometry();
2123  for (std::size_t i = 0; i < links.size(); ++i)
2124  {
2125  Eigen::Affine3d transform = getGlobalLinkTransform(links[i]); // intentional copy, we will translate
2126  const Eigen::Vector3d& extents = links[i]->getShapeExtentsAtOrigin();
2127  transform.translate(links[i]->getCenteredBoundingBoxOffset());
2128  bounding_box.extendWithTransformedBox(transform, extents);
2129  }
2130  for (std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.begin();
2131  it != attached_body_map_.end(); ++it)
2132  {
2133  const EigenSTL::vector_Affine3d& transforms = it->second->getGlobalCollisionBodyTransforms();
2134  const std::vector<shapes::ShapeConstPtr>& shapes = it->second->getShapes();
2135  for (std::size_t i = 0; i < transforms.size(); ++i)
2136  {
2137  Eigen::Vector3d extents = shapes::computeShapeExtents(shapes[i].get());
2138  bounding_box.extendWithTransformedBox(transforms[i], extents);
2139  }
2140  }
2141 
2142  aabb.clear();
2143  aabb.resize(6, 0.0);
2144  if (!bounding_box.isEmpty())
2145  {
2146  // The following is a shorthand for something like:
2147  // aabb[0, 2, 4] = bounding_box.min(); aabb[1, 3, 5] = bounding_box.max();
2148  Eigen::Map<Eigen::VectorXd, Eigen::Unaligned, Eigen::InnerStride<2> >(aabb.data(), 3) = bounding_box.min();
2149  Eigen::Map<Eigen::VectorXd, Eigen::Unaligned, Eigen::InnerStride<2> >(aabb.data() + 1, 3) = bounding_box.max();
2150  }
2151 }
2152 
2153 void RobotState::printStatePositions(std::ostream& out) const
2154 {
2155  const std::vector<std::string>& nm = robot_model_->getVariableNames();
2156  for (std::size_t i = 0; i < nm.size(); ++i)
2157  out << nm[i] << "=" << position_[i] << std::endl;
2158 }
2159 
2160 void RobotState::printDirtyInfo(std::ostream& out) const
2161 {
2162  out << " * Dirty Joint Transforms: " << std::endl;
2163  const std::vector<const JointModel*>& jm = robot_model_->getJointModels();
2164  for (std::size_t i = 0; i < jm.size(); ++i)
2165  if (jm[i]->getVariableCount() > 0 && dirtyJointTransform(jm[i]))
2166  out << " " << jm[i]->getName() << std::endl;
2167  out << " * Dirty Link Transforms: " << (dirty_link_transforms_ ? dirty_link_transforms_->getName() : "NULL")
2168  << std::endl;
2169  out << " * Dirty Collision Body Transforms: "
2171 }
2172 
2173 void RobotState::printStateInfo(std::ostream& out) const
2174 {
2175  out << "Robot State @" << this << std::endl;
2176 
2177  std::size_t n = robot_model_->getVariableCount();
2178  if (position_)
2179  {
2180  out << " * Position: ";
2181  for (std::size_t i = 0; i < n; ++i)
2182  out << position_[i] << " ";
2183  out << std::endl;
2184  }
2185  else
2186  out << " * Position: NULL" << std::endl;
2187 
2188  if (velocity_)
2189  {
2190  out << " * Velocity: ";
2191  for (std::size_t i = 0; i < n; ++i)
2192  out << velocity_[i] << " ";
2193  out << std::endl;
2194  }
2195  else
2196  out << " * Velocity: NULL" << std::endl;
2197 
2198  if (acceleration_)
2199  {
2200  out << " * Acceleration: ";
2201  for (std::size_t i = 0; i < n; ++i)
2202  out << acceleration_[i] << " ";
2203  out << std::endl;
2204  }
2205  else
2206  out << " * Acceleration: NULL" << std::endl;
2207 
2208  out << " * Dirty Link Transforms: " << (dirty_link_transforms_ ? dirty_link_transforms_->getName() : "NULL")
2209  << std::endl;
2210  out << " * Dirty Collision Body Transforms: "
2212 
2213  printTransforms(out);
2214 }
2215 
2216 void RobotState::printTransform(const Eigen::Affine3d& transform, std::ostream& out) const
2217 {
2218  Eigen::Quaterniond q(transform.linear());
2219  out << "T.xyz = [" << transform.translation().x() << ", " << transform.translation().y() << ", "
2220  << transform.translation().z() << "], Q.xyzw = [" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w()
2221  << "]" << std::endl;
2222 }
2223 
2224 void RobotState::printTransforms(std::ostream& out) const
2225 {
2227  {
2228  out << "No transforms computed" << std::endl;
2229  return;
2230  }
2231 
2232  out << "Joint transforms:" << std::endl;
2233  const std::vector<const JointModel*>& jm = robot_model_->getJointModels();
2234  for (std::size_t i = 0; i < jm.size(); ++i)
2235  {
2236  out << " " << jm[i]->getName();
2237  const int idx = jm[i]->getJointIndex();
2238  if (dirty_joint_transforms_[idx])
2239  out << " [dirty]";
2240  out << ": ";
2242  }
2243 
2244  out << "Link poses:" << std::endl;
2245  const std::vector<const LinkModel*>& lm = robot_model_->getLinkModels();
2246  for (std::size_t i = 0; i < lm.size(); ++i)
2247  {
2248  out << " " << lm[i]->getName() << ": ";
2249  printTransform(global_link_transforms_[lm[i]->getLinkIndex()], out);
2250  }
2251 }
2252 
2253 std::string RobotState::getStateTreeString(const std::string& prefix) const
2254 {
2255  std::stringstream ss;
2256  ss << "ROBOT: " << robot_model_->getName() << std::endl;
2257  getStateTreeJointString(ss, robot_model_->getRootJoint(), " ", true);
2258  return ss.str();
2259 }
2260 
2261 namespace
2262 {
2263 void getPoseString(std::ostream& ss, const Eigen::Affine3d& pose, const std::string& pfx)
2264 {
2265  ss.precision(3);
2266  for (int y = 0; y < 4; ++y)
2267  {
2268  ss << pfx;
2269  for (int x = 0; x < 4; ++x)
2270  {
2271  ss << std::setw(8) << pose(y, x) << " ";
2272  }
2273  ss << std::endl;
2274  }
2275 }
2276 }
2277 
2278 void RobotState::getStateTreeJointString(std::ostream& ss, const JointModel* jm, const std::string& pfx0,
2279  bool last) const
2280 {
2281  std::string pfx = pfx0 + "+--";
2282 
2283  ss << pfx << "Joint: " << jm->getName() << std::endl;
2284 
2285  pfx = pfx0 + (last ? " " : "| ");
2286 
2287  for (std::size_t i = 0; i < jm->getVariableCount(); ++i)
2288  {
2289  ss.precision(3);
2290  ss << pfx << jm->getVariableNames()[i] << std::setw(12) << position_[jm->getFirstVariableIndex() + i] << std::endl;
2291  }
2292 
2293  const LinkModel* lm = jm->getChildLinkModel();
2294 
2295  ss << pfx << "Link: " << lm->getName() << std::endl;
2296  getPoseString(ss, lm->getJointOriginTransform(), pfx + "joint_origin:");
2298  {
2299  getPoseString(ss, variable_joint_transforms_[jm->getJointIndex()], pfx + "joint_variable:");
2300  getPoseString(ss, global_link_transforms_[lm->getLinkIndex()], pfx + "link_global:");
2301  }
2302 
2303  for (std::vector<const JointModel*>::const_iterator it = lm->getChildJointModels().begin();
2304  it != lm->getChildJointModels().end(); ++it)
2305  getStateTreeJointString(ss, *it, pfx, it + 1 == lm->getChildJointModels().end());
2306 }
2307 
2308 std::ostream& operator<<(std::ostream& out, const RobotState& s)
2309 {
2310  s.printStateInfo(out);
2311  return out;
2312 }
2313 
2314 } // end of namespace core
2315 } // end of namespace moveit
const std::string & getAttachedLinkName() const
Get the name of the link this body is attached to.
Definition: attached_body.h:76
d
const std::string & getName() const
The name of this link.
Definition: link_model.h:81
void getAttachedBodies(std::vector< const AttachedBody * > &attached_bodies) const
Get all bodies attached to the model corresponding to this state.
random_numbers::RandomNumberGenerator & getRandomNumberGenerator()
Return the instance of a random number generator.
Definition: robot_state.h:1671
const std::string & getName() const
Get the name of the joint.
Definition: joint_model.h:131
A set of options for the kinematics solver.
std::size_t getVariableCount() const
Get the number of variables that describe this joint.
Definition: joint_model.h:196
bool hasJointModel(const std::string &joint) const
Check if a joint is part of this group.
void updateMimicJoint(const JointModel *joint)
Definition: robot_state.h:1769
const kinematics::KinematicsBaseConstPtr & getSolverInstance() const
Core components of MoveIt!
const std::string & getVisualMeshFilename() const
Get the filename of the mesh resource used for visual display of this link.
Definition: link_model.h:208
ROSCPP_DECL bool check()
Eigen::Affine3d * global_collision_body_transforms_
Definition: robot_state.h:1839
void setJointGroupAccelerations(const std::string &joint_group_name, const double *gstate)
Given accelerations for the variables that make up a group, in the order found in the group (includin...
Definition: robot_state.h:830
bool isChain() const
Check if this group is a linear chain.
void computeVariableVelocity(const JointModelGroup *jmg, Eigen::VectorXd &qdot, const Eigen::VectorXd &twist, const LinkModel *tip) const
Given a twist for a particular link (tip), compute the corresponding velocity for every variable and ...
int getVariableGroupIndex(const std::string &variable) const
Get the index of a variable within the group. Return -1 on error.
void printTransforms(std::ostream &out=std::cout) const
ROSCPP_DECL void start()
double distance(const RobotState &other) const
Definition: robot_state.h:1467
void printStatePositions(std::ostream &out=std::cout) const
#define ROS_ERROR_STREAM_NAMED(name, args)
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
void setJointEfforts(const JointModel *joint, const double *effort)
#define ROS_WARN_NAMED(name,...)
void updateMimicJoints(const JointModelGroup *group)
Update all mimic joints within group.
Definition: robot_state.h:1797
double getDefaultIKTimeout() const
Get the default IK timeout.
bool knowsFrameTransform(const std::string &id) const
Check if a transformation matrix from the model frame to frame id is known.
const std::vector< const JointModel * > & getChildJointModels() const
A link may have 0 or more child joints. From those joints there will certainly be other descendant li...
Definition: link_model.h:128
const Eigen::Affine3d & getGlobalLinkTransform(const std::string &link_name)
Definition: robot_state.h:1368
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
const std::string & getName() const
Get the name of the attached body.
Definition: attached_body.h:70
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
void copyJointGroupAccelerations(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the acceleration values of the variables that make up the group into another ...
Definition: robot_state.h:878
bool getVariableDefaultPositions(const std::string &name, std::map< std::string, double > &values) const
Get the values that correspond to a named state as read from the URDF. Return false on failure...
void setVariableVelocities(const double *velocity)
Given an array with velocity values for all variables, set those values as the velocities in this sta...
Definition: robot_state.h:284
void attachBody(AttachedBody *attached_body)
Add an attached body to this state. Ownership of the memory for the attached body is assumed by the s...
const std::string & getName() const
Get the name of the joint group.
const JointModel * dirty_collision_body_transforms_
Definition: robot_state.h:1835
bool checkCollisionTransforms() const
This function is only called in debug mode.
void clearAttachedBodies()
Clear all attached bodies. This calls delete on the AttachedBody instances, if needed.
static bool sameFrame(const std::string &frame1, const std::string &frame2)
Check if two frames end up being the same once the missing / are added as prefix (if they are missing...
Definition: transforms.cpp:64
const Eigen::Affine3d & getFrameTransform(const std::string &id)
Get the transformation matrix from the model frame to the frame identified by id. ...
std::string getName(void *handle)
void updateLinkTransformsInternal(const JointModel *start)
bool checkLinkTransforms() const
This function is only called in debug mode.
Eigen::Affine3d * global_link_transforms_
Definition: robot_state.h:1838
void setJointGroupVelocities(const std::string &joint_group_name, const double *gstate)
Given velocities for the variables that make up a group, in the order found in the group (including v...
Definition: robot_state.h:724
static const int NO_IK_SOLUTION
double * getVariablePositions()
Get a raw pointer to the positions of the variables stored in this state. Use carefully. If you change these values externally you need to make sure you trigger a forced update for the state by calling update(true).
Definition: robot_state.h:181
void interpolate(const RobotState &to, double t, RobotState &state) const
Interpolate from this state towards state to, at time t in [0,1]. The result is stored in state...
void setVariablePositions(const double *position)
It is assumed positions is an array containing the new positions for all variables in this state...
std::map< const LinkModel *, Eigen::Affine3d, std::less< const LinkModel * >, Eigen::aligned_allocator< std::pair< const LinkModel *const, Eigen::Affine3d > > > LinkTransformMap
Map from link model instances to Eigen transforms.
Definition: link_model.h:69
void getMissingKeys(const std::map< std::string, double > &variable_map, std::vector< std::string > &missing_variables) const
bool setToIKSolverFrame(Eigen::Affine3d &pose, const kinematics::KinematicsBaseConstPtr &solver)
Convert the frame of reference of the pose to that same frame as the IK solver expects.
void markDirtyJointTransforms(const JointModel *joint)
Definition: robot_state.h:1748
static double testAbsoluteJointSpaceJump(const JointModelGroup *group, std::vector< RobotStatePtr > &traj, double revolute_jump_threshold, double prismatic_jump_threshold)
Tests for absolute joint space jumps of the trajectory traj.
void copyJointGroupPositions(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the position values of the variables that make up the group into another loca...
Definition: robot_state.h:666
int getFirstCollisionBodyTransformIndex() const
Definition: link_model.h:97
void setAttachedBodyUpdateCallback(const AttachedBodyCallback &callback)
double y
const AttachedBody * getAttachedBody(const std::string &name) const
Get the attached body named name. Return NULL if not found.
const JointModel * getParentJointModel() const
Get the joint model whose child this link is. There will always be a parent joint.
Definition: link_model.h:108
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
bool isLinkUpdated(const std::string &name) const
True if this name is in the set of links that are to be updated when the state of this group changes...
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
const std::vector< const JointModel * > & getContinuousJointModels() const
Get the array of continuous joints used in this group (may include mimic joints). ...
const std::vector< unsigned int > & getKinematicsSolverJointBijection() const
Return the mapping between the order of the joints in this group and the order of the joints in the k...
void copyFrom(const RobotState &other)
int getLinkIndex() const
The index of this joint when traversing the kinematic tree in depth first fashion.
Definition: link_model.h:87
bool hasAttachedBody(const std::string &id) const
Check if an attached body named id exists in this state.
#define ROS_DEBUG_NAMED(name,...)
const Eigen::Affine3d & getVisualMeshOrigin() const
Get the transform for the visual mesh origin.
Definition: link_model.h:220
void computeAABB(std::vector< double > &aabb) const
Compute an axis-aligned bounding box that contains the current state. The format for aabb is (minx...
unsigned int index
int getJointIndex() const
Get the index of this joint within the robot model.
Definition: joint_model.h:214
Struct for containing jump_threshold.
Definition: robot_state.h:71
bool getJacobian(const JointModelGroup *group, const LinkModel *link, const Eigen::Vector3d &reference_point_position, Eigen::MatrixXd &jacobian, bool use_quaternion_representation=false) const
Compute the Jacobian with reference to a particular point on a given link, for a specified group...
const EigenSTL::vector_Affine3d & getFixedTransforms() const
Get the fixed transform (the transforms to the shapes associated with this body)
virtual double distance(const double *value1, const double *value2) const =0
Compute the distance between two joint states of the same model (represented by the variable values) ...
void update(bool force=false)
Update all transforms.
boost::function< void(AttachedBody *body, bool attached)> AttachedBodyCallback
Definition: attached_body.h:50
double computeCartesianPath(const JointModelGroup *group, std::vector< RobotStatePtr > &traj, const LinkModel *link, const Eigen::Vector3d &direction, bool global_reference_frame, double distance, const MaxEEFStep &max_step, const JumpThreshold &jump_threshold, const GroupStateValidityCallbackFn &validCallback=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Compute the sequence of joint values that correspond to a straight Cartesian path for a particular gr...
void getStateTreeJointString(std::ostream &ss, const JointModel *jm, const std::string &pfx0, bool last) const
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values) const
Compute random values for the state of the joint group.
static double testJointSpaceJump(const JointModelGroup *group, std::vector< RobotStatePtr > &traj, const JumpThreshold &jump_threshold)
Tests joint space jumps of a trajectory.
void setToRandomPositionsNearBy(const JointModelGroup *group, const RobotState &near, double distance)
Set all joints in group to random values near the value in . distance is the maximum amount each join...
unsigned char * dirty_joint_transforms_
Definition: robot_state.h:1840
bool dirtyLinkTransforms() const
Definition: robot_state.h:1445
This may be thrown if unrecoverable errors occur.
Definition: exceptions.h:53
void updateStateWithLinkAt(const std::string &link_name, const Eigen::Affine3d &transform, bool backward=false)
Update the state after setting a particular link to the input global transform pose.
Definition: robot_state.h:1360
double z
std::pair< double, const JointModel * > getMinDistanceToPositionBounds() const
Get the minimm distance from this state to the bounds. The minimum distance and the joint for which t...
AttachedBodyCallback attached_body_update_callback_
This event is called when there is a change in the attached bodies for this state; The event specifie...
Definition: robot_state.h:1847
const LinkModel * getAttachedLink() const
Get the model of the link this body is attached to.
Definition: attached_body.h:82
const std::vector< shapes::ShapeConstPtr > & getShapes() const
Get shape associated to the collision geometry for this link.
Definition: link_model.h:174
std::string getStateTreeString(const std::string &prefix="") const
Struct for containing max_step for computeCartesianPath.
Definition: robot_state.h:96
void computeTransform(const Eigen::Affine3d &parent_link_global_transform)
Recompute global_collision_body_transform given the transform of the parent link. ...
bool integrateVariableVelocity(const JointModelGroup *jmg, const Eigen::VectorXd &qdot, double dt, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn())
Given the velocities for the variables in this group (qdot) and an amount of time (dt)...
RobotState & operator=(const RobotState &other)
Copy operator.
bool satisfiesBounds(double margin=0.0) const
static double testRelativeJointSpaceJump(const JointModelGroup *group, std::vector< RobotStatePtr > &traj, double jump_threshold_factor)
Tests for relative joint space jumps of the trajectory traj.
void extendWithTransformedBox(const Eigen::Affine3d &transform, const Eigen::Vector3d &box)
Extend with a box transformed by the given transform.
Definition: aabb.cpp:39
bool constructMarkerFromShape(const Shape *shape, visualization_msgs::Marker &mk, bool use_mesh_triangle_list=false)
static const std::size_t MIN_STEPS_FOR_JUMP_THRESH
It is recommended that there are at least 10 steps per trajectory for testing jump thresholds with co...
Definition: robot_state.cpp:55
void printStateInfo(std::ostream &out=std::cout) const
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn
The signature for a callback that can compute IK.
bool setFromIKSubgroups(const JointModelGroup *group, const EigenSTL::vector_Affine3d &poses, const std::vector< std::string > &tips, const std::vector< std::vector< double > > &consistency_limits, unsigned int attempts=0, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
setFromIK for multiple poses and tips (end effectors) when no solver exists for the jmg that can solv...
A joint from the robot. Models the transform that this joint applies in the kinematic chain...
Definition: joint_model.h:108
const std::vector< const JointModel * > & getJointModels() const
Get all the joints in this group (including fixed and mimic joints).
bool hasLinkModel(const std::string &link) const
Check if a link is part of this group.
const std::vector< std::string > & getVariableNames() const
Get the names of the variables that make up this joint, in the order they appear in corresponding sta...
Definition: joint_model.h:176
RobotState(const RobotModelConstPtr &robot_model)
A state can be constructed from a specified robot model. No values are initialized. Call setToDefaultValues() if a state needs to provide valid information.
Definition: robot_state.cpp:59
const std::string LOGNAME
Definition: robot_model.cpp:53
void getRobotMarkers(visualization_msgs::MarkerArray &arr, const std::vector< std::string > &link_names, const std_msgs::ColorRGBA &color, const std::string &ns, const ros::Duration &dur, bool include_attached=false) const
Get a MarkerArray that fully describes the robot markers for a given robot.
ROSCPP_DECL bool del(const std::string &key)
unsigned int getDefaultIKAttempts() const
Get the default IK attempts.
const std::vector< int > & getVariableIndexList() const
Get the index locations in the complete robot state for all the variables in this group...
void copyJointGroupVelocities(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the velocity values of the variables that make up the group into another loca...
Definition: robot_state.h:772
bool setFromDiffIK(const JointModelGroup *group, const Eigen::VectorXd &twist, const std::string &tip, double dt, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn())
Set the joint values from a Cartesian velocity applied during a time dt.
virtual void computeTransform(const double *joint_values, Eigen::Affine3d &transf) const =0
Given the joint values for a joint, compute the corresponding transform.
const JointModel * dirty_link_transforms_
Definition: robot_state.h:1834
int getFirstVariableIndex() const
Get the index of this joint&#39;s first variable within the full robot state.
Definition: joint_model.h:202
const Eigen::Vector3d & getVisualMeshScale() const
Get the scale of the mesh resource for this link.
Definition: link_model.h:214
const LinkModel * getChildLinkModel() const
Get the link that this joint connects to. There will always be such a link.
Definition: joint_model.h:154
const LinkModel * getLinkModel(const std::string &link) const
Get the model of a particular link.
Definition: robot_state.h:156
void setJointGroupPositions(const std::string &joint_group_name, const double *gstate)
Given positions for the variables that make up a group, in the order found in the group (including va...
Definition: robot_state.h:618
random_numbers::RandomNumberGenerator * rng_
For certain operations a state needs a random number generator. However, it may be slightly expensive...
Definition: robot_state.h:1854
void setToRandomPositions()
Set all joints to random values. Values will be within default bounds.
RobotModelConstPtr robot_model_
Definition: robot_state.h:1823
void setVariableAccelerations(const double *acceleration)
Given an array with acceleration values for all variables, set those values as the accelerations in t...
Definition: robot_state.h:373
#define ROS_ERROR_NAMED(name,...)
const LinkTransformMap & getAssociatedFixedTransforms() const
Get the set of links that are attached to this one via fixed transforms.
Definition: link_model.h:196
static Time now()
void updateCollisionBodyTransforms()
Update the transforms for the collision bodies. This call is needed before calling collision checking...
const LinkModel * getParentLinkModel() const
Get the link that this joint connects to. The robot is assumed to start with a joint, so the root joint will return a NULL pointer here.
Definition: joint_model.h:148
const Eigen::Affine3d & getJointTransform(const std::string &joint_name)
Definition: robot_state.h:1390
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:123
const std::vector< const JointModel * > & getActiveJointModels() const
Get the active joints in this group (that have controllable DOF). This does not include mimic joints...
void updateLinkTransforms()
Update the reference frame transforms for links. This call is needed before using the transforms of l...
const std::vector< const LinkModel * > & getLinkModels() const
Get the links that are part of this joint group.
A link from the robot. Contains the constant transform applied to the link and its geometry...
Definition: link_model.h:72
const Eigen::Affine3d & getJointOriginTransform() const
When transforms are computed for this link, they are usually applied to the link&#39;s origin...
Definition: link_model.h:142
void setVariableEffort(const double *effort)
Given an array with effort values for all variables, set those values as the effort in this state...
Definition: robot_state.h:464
const LinkModel * getParentLinkModel() const
Get the link model whose child this link is (through some joint). There may not always be a parent li...
Definition: link_model.h:117
const std::vector< const LinkModel * > & getDescendantLinkModels() const
Get all the link models that descend from this joint, in the kinematic tree.
Definition: joint_model.h:394
bool dirtyJointTransform(const JointModel *joint) const
Definition: robot_state.h:1440
const double * getJointPositions(const std::string &joint_name) const
Definition: robot_state.h:569
void twistMsgToEigen(const geometry_msgs::Twist &m, Eigen::Matrix< double, 6, 1 > &e)
bool isValidVelocityMove(const RobotState &other, const JointModelGroup *group, double dt) const
Check that the time to move between two waypoints is sufficient given velocity limits and time step...
Main namespace for MoveIt!
bool setFromIK(const JointModelGroup *group, const geometry_msgs::Pose &pose, unsigned int attempts=0, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
If the group this state corresponds to is a chain and a solver is available, then the joint values ca...
Eigen::Vector3d computeShapeExtents(const ShapeMsg &shape_msg)
std::size_t getVariableCount() const
Get the number of variables that make up this state.
Definition: robot_state.h:144
Object defining bodies that can be attached to robot links. This is useful when handling objects pick...
Definition: attached_body.h:56
double x
Eigen::Affine3d * variable_joint_transforms_
Definition: robot_state.h:1837
r
std::ostream & operator<<(std::ostream &out, const VariableBounds &b)
Operator overload for printing variable bounds to a stream.
std::vector< VariableBounds > Bounds
The datatype for the joint bounds.
Definition: joint_model.h:123
JointType getType() const
Get the type of joint.
Definition: joint_model.h:137
bool dirtyCollisionBodyTransforms() const
Definition: robot_state.h:1450
boost::function< bool(RobotState *robot_state, const JointModelGroup *joint_group, const double *joint_group_variable_values)> GroupStateValidityCallbackFn
Signature for functions that can verify that if the group joint_group in robot_state is set to joint_...
Definition: robot_state.h:64
void printDirtyInfo(std::ostream &out=std::cout) const
std::map< std::string, AttachedBody * > attached_body_map_
All attached bodies that are part of this state, indexed by their name.
Definition: robot_state.h:1843
bool checkJointTransforms(const JointModel *joint) const
This function is only called in debug mode.
void printTransform(const Eigen::Affine3d &transform, std::ostream &out=std::cout) const
void getSubgroups(std::vector< const JointModelGroup * > &sub_groups) const
Get the groups that are subsets of this one (in terms of joints set)
bool clearAttachedBody(const std::string &id)
Remove the attached body named id. Return false if the object was not found (and thus not removed)...
void invertVelocity()
Invert velocity if present.
Represents an axis-aligned bounding box.
Definition: aabb.h:47


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sat Feb 9 2019 04:09:03