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 
425 void RobotState::setJointEfforts(const JointModel* joint, const double* effort)
426 {
427  if (has_acceleration_)
428  {
429  ROS_ERROR_NAMED(LOGNAME, "Unable to set joint efforts because array is being used for accelerations");
430  return;
431  }
432  has_effort_ = true;
433 
434  memcpy(effort_ + joint->getFirstVariableIndex(), effort, joint->getVariableCount() * sizeof(double));
435 }
436 
437 void RobotState::setJointGroupPositions(const JointModelGroup* group, const double* gstate)
438 {
439  const std::vector<int>& il = group->getVariableIndexList();
440  if (group->isContiguousWithinState())
441  memcpy(position_ + il[0], gstate, group->getVariableCount() * sizeof(double));
442  else
443  {
444  for (std::size_t i = 0; i < il.size(); ++i)
445  position_[il[i]] = gstate[i];
446  }
447  updateMimicJoints(group);
448 }
449 
450 void RobotState::setJointGroupPositions(const JointModelGroup* group, const Eigen::VectorXd& values)
451 {
452  const std::vector<int>& il = group->getVariableIndexList();
453  for (std::size_t i = 0; i < il.size(); ++i)
454  position_[il[i]] = values(i);
455  updateMimicJoints(group);
456 }
457 
458 void RobotState::copyJointGroupPositions(const JointModelGroup* group, double* gstate) const
459 {
460  const std::vector<int>& il = group->getVariableIndexList();
461  if (group->isContiguousWithinState())
462  memcpy(gstate, position_ + il[0], group->getVariableCount() * sizeof(double));
463  else
464  for (std::size_t i = 0; i < il.size(); ++i)
465  gstate[i] = position_[il[i]];
466 }
467 
468 void RobotState::copyJointGroupPositions(const JointModelGroup* group, Eigen::VectorXd& values) const
469 {
470  const std::vector<int>& il = group->getVariableIndexList();
471  values.resize(il.size());
472  for (std::size_t i = 0; i < il.size(); ++i)
473  values(i) = position_[il[i]];
474 }
475 
476 void RobotState::setJointGroupVelocities(const JointModelGroup* group, const double* gstate)
477 {
478  markVelocity();
479  const std::vector<int>& il = group->getVariableIndexList();
480  if (group->isContiguousWithinState())
481  memcpy(velocity_ + il[0], gstate, group->getVariableCount() * sizeof(double));
482  else
483  {
484  for (std::size_t i = 0; i < il.size(); ++i)
485  velocity_[il[i]] = gstate[i];
486  }
487 }
488 
489 void RobotState::setJointGroupVelocities(const JointModelGroup* group, const Eigen::VectorXd& values)
490 {
491  markVelocity();
492  const std::vector<int>& il = group->getVariableIndexList();
493  for (std::size_t i = 0; i < il.size(); ++i)
494  velocity_[il[i]] = values(i);
495 }
496 
497 void RobotState::copyJointGroupVelocities(const JointModelGroup* group, double* gstate) const
498 {
499  const std::vector<int>& il = group->getVariableIndexList();
500  if (group->isContiguousWithinState())
501  memcpy(gstate, velocity_ + il[0], group->getVariableCount() * sizeof(double));
502  else
503  for (std::size_t i = 0; i < il.size(); ++i)
504  gstate[i] = velocity_[il[i]];
505 }
506 
507 void RobotState::copyJointGroupVelocities(const JointModelGroup* group, Eigen::VectorXd& values) const
508 {
509  const std::vector<int>& il = group->getVariableIndexList();
510  values.resize(il.size());
511  for (std::size_t i = 0; i < il.size(); ++i)
512  values(i) = velocity_[il[i]];
513 }
514 
515 void RobotState::setJointGroupAccelerations(const JointModelGroup* group, const double* gstate)
516 {
518  const std::vector<int>& il = group->getVariableIndexList();
519  if (group->isContiguousWithinState())
520  memcpy(acceleration_ + il[0], gstate, group->getVariableCount() * sizeof(double));
521  else
522  {
523  for (std::size_t i = 0; i < il.size(); ++i)
524  acceleration_[il[i]] = gstate[i];
525  }
526 }
527 
528 void RobotState::setJointGroupAccelerations(const JointModelGroup* group, const Eigen::VectorXd& values)
529 {
531  const std::vector<int>& il = group->getVariableIndexList();
532  for (std::size_t i = 0; i < il.size(); ++i)
533  acceleration_[il[i]] = values(i);
534 }
535 
536 void RobotState::copyJointGroupAccelerations(const JointModelGroup* group, double* gstate) const
537 {
538  const std::vector<int>& il = group->getVariableIndexList();
539  if (group->isContiguousWithinState())
540  memcpy(gstate, acceleration_ + il[0], group->getVariableCount() * sizeof(double));
541  else
542  for (std::size_t i = 0; i < il.size(); ++i)
543  gstate[i] = acceleration_[il[i]];
544 }
545 
546 void RobotState::copyJointGroupAccelerations(const JointModelGroup* group, Eigen::VectorXd& values) const
547 {
548  const std::vector<int>& il = group->getVariableIndexList();
549  values.resize(il.size());
550  for (std::size_t i = 0; i < il.size(); ++i)
551  values(i) = acceleration_[il[i]];
552 }
553 
554 void RobotState::update(bool force)
555 {
556  // make sure we do everything from scratch if needed
557  if (force)
558  {
559  memset(dirty_joint_transforms_, 1, robot_model_->getJointModelCount() * sizeof(unsigned char));
560  dirty_link_transforms_ = robot_model_->getRootJoint();
561  }
562 
563  // this actually triggers all needed updates
565 }
566 
568 {
569  if (dirty_link_transforms_ != nullptr)
571 
572  if (dirty_collision_body_transforms_ != nullptr)
573  {
574  const std::vector<const LinkModel*>& links = dirty_collision_body_transforms_->getDescendantLinkModels();
576 
577  for (std::size_t i = 0; i < links.size(); ++i)
578  {
579  const EigenSTL::vector_Affine3d& ot = links[i]->getCollisionOriginTransforms();
580  const std::vector<int>& ot_id = links[i]->areCollisionOriginTransformsIdentity();
581  const int index_co = links[i]->getFirstCollisionBodyTransformIndex();
582  const int index_l = links[i]->getLinkIndex();
583  for (std::size_t j = 0; j < ot.size(); ++j)
584  global_collision_body_transforms_[index_co + j].matrix().noalias() =
585  ot_id[j] ? global_link_transforms_[index_l].matrix() :
586  global_link_transforms_[index_l].matrix() * ot[j].matrix();
587  }
588  }
589 }
590 
592 {
593  if (dirty_link_transforms_ != nullptr)
594  {
599  else
601  dirty_link_transforms_ = nullptr;
602  }
603 }
604 
606 {
607  for (const LinkModel* link : start->getDescendantLinkModels())
608  {
609  const LinkModel* parent = link->getParentLinkModel();
610  if (parent) // root JointModel will not have a parent
611  {
612  if (link->parentJointIsFixed())
613  global_link_transforms_[link->getLinkIndex()].matrix().noalias() =
614  global_link_transforms_[parent->getLinkIndex()].matrix() * link->getJointOriginTransform().matrix();
615  else
616  {
617  if (link->jointOriginTransformIsIdentity())
618  global_link_transforms_[link->getLinkIndex()].matrix().noalias() =
619  global_link_transforms_[parent->getLinkIndex()].matrix() *
620  getJointTransform(link->getParentJointModel()).matrix();
621  else
622  global_link_transforms_[link->getLinkIndex()].matrix().noalias() =
623  global_link_transforms_[parent->getLinkIndex()].matrix() * link->getJointOriginTransform().matrix() *
624  getJointTransform(link->getParentJointModel()).matrix();
625  }
626  }
627  else
628  {
629  if (link->jointOriginTransformIsIdentity())
630  global_link_transforms_[link->getLinkIndex()] = getJointTransform(link->getParentJointModel());
631  else
632  global_link_transforms_[link->getLinkIndex()].matrix().noalias() =
633  link->getJointOriginTransform().matrix() * getJointTransform(link->getParentJointModel()).matrix();
634  }
635  }
636 
637  // update attached bodies tf; these are usually very few, so we update them all
638  for (std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.begin();
639  it != attached_body_map_.end(); ++it)
640  it->second->computeTransform(global_link_transforms_[it->second->getAttachedLink()->getLinkIndex()]);
641 }
642 
643 void RobotState::updateStateWithLinkAt(const LinkModel* link, const Eigen::Affine3d& transform, bool backward)
644 {
645  updateLinkTransforms(); // no link transforms must be dirty, otherwise the transform we set will be overwritten
646 
647  // update the fact that collision body transforms are out of date
651  else
653 
654  global_link_transforms_[link->getLinkIndex()] = transform;
655 
656  // update link transforms for descendant links only (leaving the transform for the current link untouched)
657  const std::vector<const JointModel*>& cj = link->getChildJointModels();
658  for (std::size_t i = 0; i < cj.size(); ++i)
660 
661  // if we also need to go backward
662  if (backward)
663  {
664  const LinkModel* parent_link = link;
665  const LinkModel* child_link;
666  while (parent_link->getParentJointModel()->getParentLinkModel())
667  {
668  child_link = parent_link;
669  parent_link = parent_link->getParentJointModel()->getParentLinkModel();
670 
671  // update the transform of the parent
672  global_link_transforms_[parent_link->getLinkIndex()] =
673  global_link_transforms_[child_link->getLinkIndex()] *
674  (child_link->getJointOriginTransform() *
676  .inverse();
677 
678  // update link transforms for descendant links only (leaving the transform for the current link untouched)
679  // with the exception of the child link we are coming backwards from
680  const std::vector<const JointModel*>& cj = parent_link->getChildJointModels();
681  for (std::size_t i = 0; i < cj.size(); ++i)
682  if (cj[i] != child_link->getParentJointModel())
684  }
685  // all collision body transforms are invalid now
687  }
688 
689  // update attached bodies tf; these are usually very few, so we update them all
690  for (std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.begin();
691  it != attached_body_map_.end(); ++it)
692  it->second->computeTransform(global_link_transforms_[it->second->getAttachedLink()->getLinkIndex()]);
693 }
694 
695 bool RobotState::satisfiesBounds(double margin) const
696 {
697  const std::vector<const JointModel*>& jm = robot_model_->getActiveJointModels();
698  for (std::size_t i = 0; i < jm.size(); ++i)
699  if (!satisfiesBounds(jm[i], margin))
700  return false;
701  return true;
702 }
703 
704 bool RobotState::satisfiesBounds(const JointModelGroup* group, double margin) const
705 {
706  const std::vector<const JointModel*>& jm = group->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 
714 {
715  const std::vector<const JointModel*>& jm = robot_model_->getActiveJointModels();
716  for (std::size_t i = 0; i < jm.size(); ++i)
717  enforceBounds(jm[i]);
718 }
719 
721 {
722  const std::vector<const JointModel*>& jm = joint_group->getActiveJointModels();
723  for (std::size_t i = 0; i < jm.size(); ++i)
724  enforceBounds(jm[i]);
725 }
726 
727 std::pair<double, const JointModel*> RobotState::getMinDistanceToPositionBounds() const
728 {
729  return getMinDistanceToPositionBounds(robot_model_->getActiveJointModels());
730 }
731 
732 std::pair<double, const JointModel*> RobotState::getMinDistanceToPositionBounds(const JointModelGroup* group) const
733 {
735 }
736 
737 std::pair<double, const JointModel*>
738 RobotState::getMinDistanceToPositionBounds(const std::vector<const JointModel*>& joints) const
739 {
740  double distance = std::numeric_limits<double>::max();
741  const JointModel* index = nullptr;
742  for (std::size_t i = 0; i < joints.size(); ++i)
743  {
744  if (joints[i]->getType() == JointModel::PLANAR || joints[i]->getType() == JointModel::FLOATING)
745  continue;
746  if (joints[i]->getType() == JointModel::REVOLUTE)
747  if (static_cast<const RevoluteJointModel*>(joints[i])->isContinuous())
748  continue;
749 
750  const double* joint_values = getJointPositions(joints[i]);
751  const JointModel::Bounds& bounds = joints[i]->getVariableBounds();
752  std::vector<double> lower_bounds(bounds.size()), upper_bounds(bounds.size());
753  for (std::size_t j = 0; j < bounds.size(); ++j)
754  {
755  lower_bounds[j] = bounds[j].min_position_;
756  upper_bounds[j] = bounds[j].max_position_;
757  }
758  double new_distance = joints[i]->distance(joint_values, &lower_bounds[0]);
759  if (new_distance < distance)
760  {
761  index = joints[i];
762  distance = new_distance;
763  }
764  new_distance = joints[i]->distance(joint_values, &upper_bounds[0]);
765  if (new_distance < distance)
766  {
767  index = joints[i];
768  distance = new_distance;
769  }
770  }
771  return std::make_pair(distance, index);
772 }
773 
774 bool RobotState::isValidVelocityMove(const RobotState& other, const JointModelGroup* group, double dt) const
775 {
776  const std::vector<const JointModel*>& jm = group->getActiveJointModels();
777  for (std::size_t joint_id = 0; joint_id < jm.size(); ++joint_id)
778  {
779  const int idx = jm[joint_id]->getFirstVariableIndex();
780  const std::vector<VariableBounds>& bounds = jm[joint_id]->getVariableBounds();
781 
782  // Check velocity for each joint variable
783  for (std::size_t var_id = 0; var_id < jm[joint_id]->getVariableCount(); ++var_id)
784  {
785  const double dtheta = std::abs(*(position_ + idx + var_id) - *(other.getVariablePositions() + idx + var_id));
786 
787  if (dtheta > dt * bounds[var_id].max_velocity_)
788  return false;
789  }
790  }
791 
792  return true;
793 }
794 
795 double RobotState::distance(const RobotState& other, const JointModelGroup* joint_group) const
796 {
797  double d = 0.0;
798  const std::vector<const JointModel*>& jm = joint_group->getActiveJointModels();
799  for (std::size_t i = 0; i < jm.size(); ++i)
800  {
801  const int idx = jm[i]->getFirstVariableIndex();
802  d += jm[i]->getDistanceFactor() * jm[i]->distance(position_ + idx, other.position_ + idx);
803  }
804  return d;
805 }
806 
807 void RobotState::interpolate(const RobotState& to, double t, RobotState& state) const
808 {
810 
811  memset(state.dirty_joint_transforms_, 1, state.robot_model_->getJointModelCount() * sizeof(unsigned char));
812  state.dirty_link_transforms_ = state.robot_model_->getRootJoint();
813 }
814 
815 void RobotState::interpolate(const RobotState& to, double t, RobotState& state,
816  const JointModelGroup* joint_group) const
817 {
818  const std::vector<const JointModel*>& jm = joint_group->getActiveJointModels();
819  for (std::size_t i = 0; i < jm.size(); ++i)
820  {
821  const int idx = jm[i]->getFirstVariableIndex();
822  jm[i]->interpolate(position_ + idx, to.position_ + idx, t, state.position_ + idx);
823  }
824  state.updateMimicJoints(joint_group);
825 }
826 
828 {
830 }
831 
832 bool RobotState::hasAttachedBody(const std::string& id) const
833 {
834  return attached_body_map_.find(id) != attached_body_map_.end();
835 }
836 
837 const AttachedBody* RobotState::getAttachedBody(const std::string& id) const
838 {
839  std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.find(id);
840  if (it == attached_body_map_.end())
841  {
842  ROS_ERROR_NAMED(LOGNAME, "Attached body '%s' not found", id.c_str());
843  return nullptr;
844  }
845  else
846  return it->second;
847 }
848 
850 {
851  attached_body_map_[attached_body->getName()] = attached_body;
852  attached_body->computeTransform(getGlobalLinkTransform(attached_body->getAttachedLink()));
854  attached_body_update_callback_(attached_body, true);
855 }
856 
857 void RobotState::attachBody(const std::string& id, const std::vector<shapes::ShapeConstPtr>& shapes,
858  const EigenSTL::vector_Affine3d& attach_trans, const std::set<std::string>& touch_links,
859  const std::string& link, const trajectory_msgs::JointTrajectory& detach_posture)
860 {
861  const LinkModel* l = robot_model_->getLinkModel(link);
862  AttachedBody* ab = new AttachedBody(l, id, shapes, attach_trans, touch_links, detach_posture);
863  attached_body_map_[id] = ab;
867 }
868 
869 void RobotState::getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies) const
870 {
871  attached_bodies.clear();
872  attached_bodies.reserve(attached_body_map_.size());
873  for (std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.begin();
874  it != attached_body_map_.end(); ++it)
875  attached_bodies.push_back(it->second);
876 }
877 
878 void RobotState::getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies,
879  const JointModelGroup* group) const
880 {
881  attached_bodies.clear();
882  for (std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.begin();
883  it != attached_body_map_.end(); ++it)
884  if (group->hasLinkModel(it->second->getAttachedLinkName()))
885  attached_bodies.push_back(it->second);
886 }
887 
888 void RobotState::getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const LinkModel* lm) 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 (it->second->getAttachedLink() == lm)
894  attached_bodies.push_back(it->second);
895 }
896 
898 {
899  for (std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.begin();
900  it != attached_body_map_.end(); ++it)
901  {
903  attached_body_update_callback_(it->second, false);
904  delete it->second;
905  }
906  attached_body_map_.clear();
907 }
908 
910 {
911  std::map<std::string, AttachedBody*>::iterator it = attached_body_map_.begin();
912  while (it != attached_body_map_.end())
913  {
914  if (it->second->getAttachedLink() != link)
915  {
916  ++it;
917  continue;
918  }
920  attached_body_update_callback_(it->second, false);
921  delete it->second;
922  std::map<std::string, AttachedBody*>::iterator del = it++;
923  attached_body_map_.erase(del);
924  }
925 }
926 
928 {
929  std::map<std::string, AttachedBody*>::iterator it = attached_body_map_.begin();
930  while (it != attached_body_map_.end())
931  {
932  if (!group->hasLinkModel(it->second->getAttachedLinkName()))
933  {
934  ++it;
935  continue;
936  }
938  attached_body_update_callback_(it->second, false);
939  delete it->second;
940  std::map<std::string, AttachedBody*>::iterator del = it++;
941  attached_body_map_.erase(del);
942  }
943 }
944 
945 bool RobotState::clearAttachedBody(const std::string& id)
946 {
947  std::map<std::string, AttachedBody*>::iterator it = attached_body_map_.find(id);
948  if (it != attached_body_map_.end())
949  {
951  attached_body_update_callback_(it->second, false);
952  delete it->second;
953  attached_body_map_.erase(it);
954  return true;
955  }
956  else
957  return false;
958 }
959 
960 const Eigen::Affine3d& RobotState::getFrameTransform(const std::string& id)
961 {
963  return static_cast<const RobotState*>(this)->getFrameTransform(id);
964 }
965 
966 const Eigen::Affine3d& RobotState::getFrameTransform(const std::string& id) const
967 {
968  if (!id.empty() && id[0] == '/')
969  return getFrameTransform(id.substr(1));
970  BOOST_VERIFY(checkLinkTransforms());
971 
972  static const Eigen::Affine3d identity_transform = Eigen::Affine3d::Identity();
973  if (id.size() + 1 == robot_model_->getModelFrame().size() && '/' + id == robot_model_->getModelFrame())
974  return identity_transform;
975  if (robot_model_->hasLinkModel(id))
976  {
977  const LinkModel* lm = robot_model_->getLinkModel(id);
979  }
980  std::map<std::string, AttachedBody*>::const_iterator jt = attached_body_map_.find(id);
981  if (jt == attached_body_map_.end())
982  {
983  ROS_ERROR_NAMED(LOGNAME, "Transform from frame '%s' to frame '%s' is not known "
984  "('%s' should be a link name or an attached body id).",
985  id.c_str(), robot_model_->getModelFrame().c_str(), id.c_str());
986  return identity_transform;
987  }
988  const EigenSTL::vector_Affine3d& tf = jt->second->getGlobalCollisionBodyTransforms();
989  if (tf.empty())
990  {
991  ROS_ERROR_NAMED(LOGNAME, "Attached body '%s' has no geometry associated to it. No transform to return.",
992  id.c_str());
993  return identity_transform;
994  }
995  if (tf.size() > 1)
996  ROS_DEBUG_NAMED(LOGNAME, "There are multiple geometries associated to attached body '%s'. "
997  "Returning the transform for the first one.",
998  id.c_str());
999  return tf[0];
1000 }
1001 
1002 bool RobotState::knowsFrameTransform(const std::string& id) const
1003 {
1004  if (!id.empty() && id[0] == '/')
1005  return knowsFrameTransform(id.substr(1));
1006  if (robot_model_->hasLinkModel(id))
1007  return true;
1008  std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.find(id);
1009  return it != attached_body_map_.end() && !it->second->getGlobalCollisionBodyTransforms().empty();
1010 }
1011 
1012 void RobotState::getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1013  const std_msgs::ColorRGBA& color, const std::string& ns, const ros::Duration& dur,
1014  bool include_attached) const
1015 {
1016  std::size_t cur_num = arr.markers.size();
1017  getRobotMarkers(arr, link_names, include_attached);
1018  unsigned int id = cur_num;
1019  for (std::size_t i = cur_num; i < arr.markers.size(); ++i, ++id)
1020  {
1021  arr.markers[i].ns = ns;
1022  arr.markers[i].id = id;
1023  arr.markers[i].lifetime = dur;
1024  arr.markers[i].color = color;
1025  }
1026 }
1027 
1028 void RobotState::getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1029  bool include_attached) const
1030 {
1031  ros::Time tm = ros::Time::now();
1032  for (std::size_t i = 0; i < link_names.size(); ++i)
1033  {
1034  ROS_DEBUG_NAMED(LOGNAME, "Trying to get marker for link '%s'", link_names[i].c_str());
1035  const LinkModel* lm = robot_model_->getLinkModel(link_names[i]);
1036  if (!lm)
1037  continue;
1038  if (include_attached)
1039  for (std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.begin();
1040  it != attached_body_map_.end(); ++it)
1041  if (it->second->getAttachedLink() == lm)
1042  {
1043  for (std::size_t j = 0; j < it->second->getShapes().size(); ++j)
1044  {
1045  visualization_msgs::Marker att_mark;
1046  att_mark.header.frame_id = robot_model_->getModelFrame();
1047  att_mark.header.stamp = tm;
1048  if (shapes::constructMarkerFromShape(it->second->getShapes()[j].get(), att_mark))
1049  {
1050  // if the object is invisible (0 volume) we skip it
1051  if (fabs(att_mark.scale.x * att_mark.scale.y * att_mark.scale.z) < std::numeric_limits<float>::epsilon())
1052  continue;
1053  tf::poseEigenToMsg(it->second->getGlobalCollisionBodyTransforms()[j], att_mark.pose);
1054  arr.markers.push_back(att_mark);
1055  }
1056  }
1057  }
1058 
1059  if (lm->getShapes().empty())
1060  continue;
1061 
1062  for (std::size_t j = 0; j < lm->getShapes().size(); ++j)
1063  {
1064  visualization_msgs::Marker mark;
1065  mark.header.frame_id = robot_model_->getModelFrame();
1066  mark.header.stamp = tm;
1067 
1068  // we prefer using the visual mesh, if a mesh is available and we have one body to render
1069  const std::string& mesh_resource = lm->getVisualMeshFilename();
1070  if (mesh_resource.empty() || lm->getShapes().size() > 1)
1071  {
1072  if (!shapes::constructMarkerFromShape(lm->getShapes()[j].get(), mark))
1073  continue;
1074  // if the object is invisible (0 volume) we skip it
1075  if (fabs(mark.scale.x * mark.scale.y * mark.scale.z) < std::numeric_limits<float>::epsilon())
1076  continue;
1078  }
1079  else
1080  {
1081  mark.type = mark.MESH_RESOURCE;
1082  mark.mesh_use_embedded_materials = false;
1083  mark.mesh_resource = mesh_resource;
1084  const Eigen::Vector3d& mesh_scale = lm->getVisualMeshScale();
1085 
1086  mark.scale.x = mesh_scale[0];
1087  mark.scale.y = mesh_scale[1];
1088  mark.scale.z = mesh_scale[2];
1090  }
1091 
1092  arr.markers.push_back(mark);
1093  }
1094  }
1095 }
1096 
1097 Eigen::MatrixXd RobotState::getJacobian(const JointModelGroup* group,
1098  const Eigen::Vector3d& reference_point_position) const
1099 {
1100  Eigen::MatrixXd result;
1101  if (!getJacobian(group, group->getLinkModels().back(), reference_point_position, result, false))
1102  throw Exception("Unable to compute Jacobian");
1103  return result;
1104 }
1105 
1106 bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link,
1107  const Eigen::Vector3d& reference_point_position, Eigen::MatrixXd& jacobian,
1108  bool use_quaternion_representation) const
1109 {
1110  BOOST_VERIFY(checkLinkTransforms());
1111 
1112  if (!group->isChain())
1113  {
1114  ROS_ERROR_NAMED(LOGNAME, "The group '%s' is not a chain. Cannot compute Jacobian.", group->getName().c_str());
1115  return false;
1116  }
1117 
1118  if (!group->isLinkUpdated(link->getName()))
1119  {
1120  ROS_ERROR_NAMED(LOGNAME, "Link name '%s' does not exist in the chain '%s' or is not a child for this chain",
1121  link->getName().c_str(), group->getName().c_str());
1122  return false;
1123  }
1124 
1125  const robot_model::JointModel* root_joint_model = group->getJointModels()[0]; // group->getJointRoots()[0];
1126  const robot_model::LinkModel* root_link_model = root_joint_model->getParentLinkModel();
1127  Eigen::Affine3d reference_transform =
1128  root_link_model ? getGlobalLinkTransform(root_link_model).inverse() : Eigen::Affine3d::Identity();
1129  int rows = use_quaternion_representation ? 7 : 6;
1130  int columns = group->getVariableCount();
1131  jacobian = Eigen::MatrixXd::Zero(rows, columns);
1132 
1133  Eigen::Affine3d link_transform = reference_transform * getGlobalLinkTransform(link);
1134  Eigen::Vector3d point_transform = link_transform * reference_point_position;
1135 
1136  /*
1137  ROS_DEBUG_NAMED(LOGNAME, "Point from reference origin expressed in world coordinates: %f %f %f",
1138  point_transform.x(),
1139  point_transform.y(),
1140  point_transform.z());
1141  */
1142 
1143  Eigen::Vector3d joint_axis;
1144  Eigen::Affine3d joint_transform;
1145 
1146  while (link)
1147  {
1148  /*
1149  ROS_DEBUG_NAMED(LOGNAME, "Link: %s, %f %f %f",link_state->getName().c_str(),
1150  link_state->getGlobalLinkTransform().translation().x(),
1151  link_state->getGlobalLinkTransform().translation().y(),
1152  link_state->getGlobalLinkTransform().translation().z());
1153  ROS_DEBUG_NAMED(LOGNAME, "Joint: %s",link_state->getParentJointState()->getName().c_str());
1154  */
1155  const JointModel* pjm = link->getParentJointModel();
1156  if (pjm->getVariableCount() > 0)
1157  {
1158  if (not group->hasJointModel(pjm->getName()))
1159  {
1160  link = pjm->getParentLinkModel();
1161  continue;
1162  }
1163  unsigned int joint_index = group->getVariableGroupIndex(pjm->getName());
1165  {
1166  joint_transform = reference_transform * getGlobalLinkTransform(link);
1167  joint_axis = joint_transform.rotation() * static_cast<const robot_model::RevoluteJointModel*>(pjm)->getAxis();
1168  jacobian.block<3, 1>(0, joint_index) =
1169  jacobian.block<3, 1>(0, joint_index) + joint_axis.cross(point_transform - joint_transform.translation());
1170  jacobian.block<3, 1>(3, joint_index) = jacobian.block<3, 1>(3, joint_index) + joint_axis;
1171  }
1172  else if (pjm->getType() == robot_model::JointModel::PRISMATIC)
1173  {
1174  joint_transform = reference_transform * getGlobalLinkTransform(link);
1175  joint_axis = joint_transform * static_cast<const robot_model::PrismaticJointModel*>(pjm)->getAxis();
1176  jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis;
1177  }
1178  else if (pjm->getType() == robot_model::JointModel::PLANAR)
1179  {
1180  joint_transform = reference_transform * getGlobalLinkTransform(link);
1181  joint_axis = joint_transform * Eigen::Vector3d(1.0, 0.0, 0.0);
1182  jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis;
1183  joint_axis = joint_transform * Eigen::Vector3d(0.0, 1.0, 0.0);
1184  jacobian.block<3, 1>(0, joint_index + 1) = jacobian.block<3, 1>(0, joint_index + 1) + joint_axis;
1185  joint_axis = joint_transform * Eigen::Vector3d(0.0, 0.0, 1.0);
1186  jacobian.block<3, 1>(0, joint_index + 2) = jacobian.block<3, 1>(0, joint_index + 2) +
1187  joint_axis.cross(point_transform - joint_transform.translation());
1188  jacobian.block<3, 1>(3, joint_index + 2) = jacobian.block<3, 1>(3, joint_index + 2) + joint_axis;
1189  }
1190  else
1191  ROS_ERROR_NAMED(LOGNAME, "Unknown type of joint in Jacobian computation");
1192  }
1193  if (pjm == root_joint_model)
1194  break;
1195  link = pjm->getParentLinkModel();
1196  }
1197  if (use_quaternion_representation)
1198  { // Quaternion representation
1199  // From "Advanced Dynamics and Motion Simulation" by Paul Mitiguy
1200  // d/dt ( [w] ) = 1/2 * [ -x -y -z ] * [ omega_1 ]
1201  // [x] [ w -z y ] [ omega_2 ]
1202  // [y] [ z w -x ] [ omega_3 ]
1203  // [z] [ -y x w ]
1204  Eigen::Quaterniond q(link_transform.rotation());
1205  double w = q.w(), x = q.x(), y = q.y(), z = q.z();
1206  Eigen::MatrixXd quaternion_update_matrix(4, 3);
1207  quaternion_update_matrix << -x, -y, -z, w, -z, y, z, w, -x, -y, x, w;
1208  jacobian.block(3, 0, 4, columns) = 0.5 * quaternion_update_matrix * jacobian.block(3, 0, 3, columns);
1209  }
1210  return true;
1211 }
1212 
1213 bool RobotState::setFromDiffIK(const JointModelGroup* jmg, const Eigen::VectorXd& twist, const std::string& tip,
1214  double dt, const GroupStateValidityCallbackFn& constraint)
1215 {
1216  Eigen::VectorXd qdot;
1217  computeVariableVelocity(jmg, qdot, twist, getLinkModel(tip));
1218  return integrateVariableVelocity(jmg, qdot, dt, constraint);
1219 }
1220 
1221 bool RobotState::setFromDiffIK(const JointModelGroup* jmg, const geometry_msgs::Twist& twist, const std::string& tip,
1222  double dt, const GroupStateValidityCallbackFn& constraint)
1223 {
1224  Eigen::Matrix<double, 6, 1> t;
1225  tf::twistMsgToEigen(twist, t);
1226  return setFromDiffIK(jmg, t, tip, dt, constraint);
1227 }
1228 
1229 void RobotState::computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot,
1230  const Eigen::VectorXd& twist, const LinkModel* tip) const
1231 {
1232  // Get the Jacobian of the group at the current configuration
1233  Eigen::MatrixXd J(6, jmg->getVariableCount());
1234  Eigen::Vector3d reference_point(0.0, 0.0, 0.0);
1235  getJacobian(jmg, tip, reference_point, J, false);
1236 
1237  // Rotate the jacobian to the end-effector frame
1238  Eigen::Affine3d eMb = getGlobalLinkTransform(tip).inverse();
1239  Eigen::MatrixXd eWb = Eigen::ArrayXXd::Zero(6, 6);
1240  eWb.block(0, 0, 3, 3) = eMb.matrix().block(0, 0, 3, 3);
1241  eWb.block(3, 3, 3, 3) = eMb.matrix().block(0, 0, 3, 3);
1242  J = eWb * J;
1243 
1244  // Do the Jacobian moore-penrose pseudo-inverse
1245  Eigen::JacobiSVD<Eigen::MatrixXd> svdOfJ(J, Eigen::ComputeThinU | Eigen::ComputeThinV);
1246  const Eigen::MatrixXd U = svdOfJ.matrixU();
1247  const Eigen::MatrixXd V = svdOfJ.matrixV();
1248  const Eigen::VectorXd S = svdOfJ.singularValues();
1249 
1250  Eigen::VectorXd Sinv = S;
1251  static const double pinvtoler = std::numeric_limits<float>::epsilon();
1252  double maxsv = 0.0;
1253  for (std::size_t i = 0; i < static_cast<std::size_t>(S.rows()); ++i)
1254  if (fabs(S(i)) > maxsv)
1255  maxsv = fabs(S(i));
1256  for (std::size_t i = 0; i < static_cast<std::size_t>(S.rows()); ++i)
1257  {
1258  // Those singular values smaller than a percentage of the maximum singular value are removed
1259  if (fabs(S(i)) > maxsv * pinvtoler)
1260  Sinv(i) = 1.0 / S(i);
1261  else
1262  Sinv(i) = 0.0;
1263  }
1264  Eigen::MatrixXd Jinv = (V * Sinv.asDiagonal() * U.transpose());
1265 
1266  // Compute joint velocity
1267  qdot = Jinv * twist;
1268 }
1269 
1270 bool RobotState::integrateVariableVelocity(const JointModelGroup* jmg, const Eigen::VectorXd& qdot, double dt,
1271  const GroupStateValidityCallbackFn& constraint)
1272 {
1273  Eigen::VectorXd q(jmg->getVariableCount());
1274  copyJointGroupPositions(jmg, q);
1275  q = q + dt * qdot;
1276  setJointGroupPositions(jmg, q);
1277  enforceBounds(jmg);
1278 
1279  if (constraint)
1280  {
1281  std::vector<double> values;
1282  copyJointGroupPositions(jmg, values);
1283  return constraint(this, jmg, &values[0]);
1284  }
1285  else
1286  return true;
1287 }
1288 
1289 bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::Pose& pose, unsigned int attempts,
1290  double timeout, const GroupStateValidityCallbackFn& constraint,
1291  const kinematics::KinematicsQueryOptions& options)
1292 {
1293  const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance();
1294  if (!solver)
1295  {
1296  ROS_ERROR_NAMED(LOGNAME, "No kinematics solver instantiated for group '%s'", jmg->getName().c_str());
1297  return false;
1298  }
1299  return setFromIK(jmg, pose, solver->getTipFrame(), attempts, timeout, constraint, options);
1300 }
1301 
1302 bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::Pose& pose, const std::string& tip,
1303  unsigned int attempts, double timeout, const GroupStateValidityCallbackFn& constraint,
1304  const kinematics::KinematicsQueryOptions& options)
1305 {
1306  Eigen::Affine3d mat;
1307  tf::poseMsgToEigen(pose, mat);
1308  static std::vector<double> consistency_limits;
1309  return setFromIK(jmg, mat, tip, consistency_limits, attempts, timeout, constraint, options);
1310 }
1311 
1312 bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Affine3d& pose, unsigned int attempts,
1313  double timeout, const GroupStateValidityCallbackFn& constraint,
1314  const kinematics::KinematicsQueryOptions& options)
1315 {
1316  const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance();
1317  if (!solver)
1318  {
1319  ROS_ERROR_NAMED(LOGNAME, "No kinematics solver instantiated for group '%s'", jmg->getName().c_str());
1320  return false;
1321  }
1322  static std::vector<double> consistency_limits;
1323  return setFromIK(jmg, pose, solver->getTipFrame(), consistency_limits, attempts, timeout, constraint, options);
1324 }
1325 
1326 bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Affine3d& pose_in, const std::string& tip_in,
1327  unsigned int attempts, double timeout, const GroupStateValidityCallbackFn& constraint,
1328  const kinematics::KinematicsQueryOptions& options)
1329 {
1330  static std::vector<double> consistency_limits;
1331  return setFromIK(jmg, pose_in, tip_in, consistency_limits, attempts, timeout, constraint, options);
1332 }
1333 
1334 namespace
1335 {
1336 bool ikCallbackFnAdapter(RobotState* state, const JointModelGroup* group,
1337  const GroupStateValidityCallbackFn& constraint, const geometry_msgs::Pose& /*unused*/,
1338  const std::vector<double>& ik_sol, moveit_msgs::MoveItErrorCodes& error_code)
1339 {
1340  const std::vector<unsigned int>& bij = group->getKinematicsSolverJointBijection();
1341  std::vector<double> solution(bij.size());
1342  for (std::size_t i = 0; i < bij.size(); ++i)
1343  solution[bij[i]] = ik_sol[i];
1344  if (constraint(state, group, &solution[0]))
1345  error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
1346  else
1348  return true;
1349 }
1350 }
1351 
1352 bool RobotState::setToIKSolverFrame(Eigen::Affine3d& pose, const kinematics::KinematicsBaseConstPtr& solver)
1353 {
1354  return setToIKSolverFrame(pose, solver->getBaseFrame());
1355 }
1356 
1357 bool RobotState::setToIKSolverFrame(Eigen::Affine3d& pose, const std::string& ik_frame)
1358 {
1359  // Bring the pose to the frame of the IK solver
1360  if (!Transforms::sameFrame(ik_frame, robot_model_->getModelFrame()))
1361  {
1362  const LinkModel* lm = getLinkModel((!ik_frame.empty() && ik_frame[0] == '/') ? ik_frame.substr(1) : ik_frame);
1363  if (!lm)
1364  return false;
1365  pose = getGlobalLinkTransform(lm).inverse() * pose;
1366  }
1367  return true;
1368 }
1369 
1370 bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Affine3d& pose_in, const std::string& tip_in,
1371  const std::vector<double>& consistency_limits_in, unsigned int attempts, double timeout,
1372  const GroupStateValidityCallbackFn& constraint,
1373  const kinematics::KinematicsQueryOptions& options)
1374 {
1375  // Convert from single pose and tip to vectors
1377  poses.push_back(pose_in);
1378 
1379  std::vector<std::string> tips;
1380  tips.push_back(tip_in);
1381 
1382  std::vector<std::vector<double> > consistency_limits;
1383  consistency_limits.push_back(consistency_limits_in);
1384 
1385  return setFromIK(jmg, poses, tips, consistency_limits, attempts, timeout, constraint, options);
1386 }
1387 
1389  const std::vector<std::string>& tips_in, unsigned int attempts, double timeout,
1390  const GroupStateValidityCallbackFn& constraint,
1391  const kinematics::KinematicsQueryOptions& options)
1392 {
1393  static const std::vector<std::vector<double> > consistency_limits;
1394  return setFromIK(jmg, poses_in, tips_in, consistency_limits, attempts, timeout, constraint, options);
1395 }
1396 
1398  const std::vector<std::string>& tips_in,
1399  const std::vector<std::vector<double> >& consistency_limit_sets, unsigned int attempts,
1400  double timeout, const GroupStateValidityCallbackFn& constraint,
1401  const kinematics::KinematicsQueryOptions& options)
1402 {
1403  // Error check
1404  if (poses_in.size() != tips_in.size())
1405  {
1406  ROS_ERROR_NAMED(LOGNAME, "Number of poses must be the same as number of tips");
1407  return false;
1408  }
1409 
1410  // Load solver
1411  const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance();
1412 
1413  // Check if this jmg has a solver
1414  bool valid_solver = true;
1415  if (!solver)
1416  {
1417  valid_solver = false;
1418  }
1419  // Check if this jmg's IK solver can handle multiple tips (non-chain solver)
1420  else if (poses_in.size() > 1)
1421  {
1422  std::string error_msg;
1423  if (!solver->supportsGroup(jmg, &error_msg))
1424  {
1425  ROS_ERROR_NAMED(LOGNAME, "Kinematics solver %s does not support joint group %s. Error: %s",
1426  typeid(*solver).name(), jmg->getName().c_str(), error_msg.c_str());
1427  valid_solver = false;
1428  }
1429  }
1430 
1431  if (!valid_solver)
1432  {
1433  // Check if there are subgroups that can solve this for us (non-chains)
1434  if (poses_in.size() > 1)
1435  {
1436  // Forward to setFromIKSubgroups() to allow different subgroup IK solvers to work together
1437  return setFromIKSubgroups(jmg, poses_in, tips_in, consistency_limit_sets, attempts, timeout, constraint, options);
1438  }
1439  else
1440  {
1441  ROS_ERROR_NAMED(LOGNAME, "No kinematics solver instantiated for group '%s'", jmg->getName().c_str());
1442  return false;
1443  }
1444  }
1445 
1446  // Check that no, or only one set of consistency limits have been passed in, and choose that one
1447  std::vector<double> consistency_limits;
1448  if (consistency_limit_sets.size() > 1)
1449  {
1450  ROS_ERROR_NAMED(LOGNAME, "Invalid number (%zu) of sets of consistency limits for a setFromIK request "
1451  "that is being solved by a single IK solver",
1452  consistency_limit_sets.size());
1453  return false;
1454  }
1455  else if (consistency_limit_sets.size() == 1)
1456  consistency_limits = consistency_limit_sets[0];
1457 
1458  const std::vector<std::string>& solver_tip_frames = solver->getTipFrames();
1459 
1460  // Track which possible tips frames we have filled in so far
1461  std::vector<bool> tip_frames_used(solver_tip_frames.size(), false);
1462 
1463  // Create vector to hold the output frames in the same order as solver_tip_frames
1464  std::vector<geometry_msgs::Pose> ik_queries(solver_tip_frames.size());
1465 
1466  // Bring each pose to the frame of the IK solver
1467  for (std::size_t i = 0; i < poses_in.size(); ++i)
1468  {
1469  // Make non-const
1470  Eigen::Affine3d pose = poses_in[i];
1471  std::string pose_frame = tips_in[i];
1472 
1473  // Remove extra slash
1474  if (!pose_frame.empty() && pose_frame[0] == '/')
1475  pose_frame = pose_frame.substr(1);
1476 
1477  // bring the pose to the frame of the IK solver
1478  if (!setToIKSolverFrame(pose, solver))
1479  return false;
1480 
1481  // try all of the solver's possible tip frames to see if they uniquely align with any of our passed in pose tip
1482  // frames
1483  bool found_valid_frame = false;
1484  std::size_t solver_tip_id; // our current index
1485  for (solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id)
1486  {
1487  // Check if this tip frame is already accounted for
1488  if (tip_frames_used[solver_tip_id])
1489  continue; // already has a pose
1490 
1491  // check if the tip frame can be transformed via fixed transforms to the frame known to the IK solver
1492  std::string solver_tip_frame = solver_tip_frames[solver_tip_id];
1493 
1494  // remove the frame '/' if there is one, so we can avoid calling Transforms::sameFrame() which may copy strings
1495  // more often that we need to
1496  if (!solver_tip_frame.empty() && solver_tip_frame[0] == '/')
1497  solver_tip_frame = solver_tip_frame.substr(1);
1498 
1499  if (pose_frame != solver_tip_frame)
1500  {
1501  if (hasAttachedBody(pose_frame))
1502  {
1503  const AttachedBody* ab = getAttachedBody(pose_frame);
1504  const EigenSTL::vector_Affine3d& ab_trans = ab->getFixedTransforms();
1505  if (ab_trans.size() != 1)
1506  {
1507  ROS_ERROR_NAMED(LOGNAME, "Cannot use an attached body "
1508  "with multiple geometries as a reference frame.");
1509  return false;
1510  }
1511  pose_frame = ab->getAttachedLinkName();
1512  pose = pose * ab_trans[0].inverse();
1513  }
1514  if (pose_frame != solver_tip_frame)
1515  {
1516  const robot_model::LinkModel* lm = getLinkModel(pose_frame);
1517  if (!lm)
1518  return false;
1520  for (robot_model::LinkTransformMap::const_iterator it = fixed_links.begin(); it != fixed_links.end(); ++it)
1521  if (Transforms::sameFrame(it->first->getName(), solver_tip_frame))
1522  {
1523  pose_frame = solver_tip_frame;
1524  pose = pose * it->second;
1525  break;
1526  }
1527  }
1528 
1529  } // end if pose_frame
1530 
1531  // Check if this pose frame works
1532  if (pose_frame == solver_tip_frame)
1533  {
1534  found_valid_frame = true;
1535  break;
1536  }
1537 
1538  } // end for solver_tip_frames
1539 
1540  // Make sure one of the tip frames worked
1541  if (!found_valid_frame)
1542  {
1543  ROS_ERROR_NAMED(LOGNAME, "Cannot compute IK for query %zu pose reference frame '%s'", i, pose_frame.c_str());
1544  // Debug available tip frames
1545  std::stringstream ss;
1546  for (solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id)
1547  ss << solver_tip_frames[solver_tip_id] << ", ";
1548  ROS_ERROR_NAMED(LOGNAME, "Available tip frames: [%s]", ss.str().c_str());
1549  return false;
1550  }
1551 
1552  // Remove that tip from the list of available tip frames because each can only have one pose
1553  tip_frames_used[solver_tip_id] = true;
1554 
1555  // Convert Eigen pose to geometry_msgs pose
1556  geometry_msgs::Pose ik_query;
1557  tf::poseEigenToMsg(pose, ik_query);
1558 
1559  // Save into vectors
1560  ik_queries[solver_tip_id] = ik_query;
1561  } // end for poses_in
1562 
1563  // Create poses for all remaining tips a solver expects, even if not passed into this function
1564  for (std::size_t solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id)
1565  {
1566  // Check if this tip frame is already accounted for
1567  if (tip_frames_used[solver_tip_id])
1568  continue; // already has a pose
1569 
1570  // Process this tip
1571  std::string solver_tip_frame = solver_tip_frames[solver_tip_id];
1572 
1573  // remove the frame '/' if there is one, so we can avoid calling Transforms::sameFrame() which may copy strings more
1574  // often that we need to
1575  if (!solver_tip_frame.empty() && solver_tip_frame[0] == '/')
1576  solver_tip_frame = solver_tip_frame.substr(1);
1577 
1578  // Get the pose of a different EE tip link
1579  Eigen::Affine3d current_pose = getGlobalLinkTransform(solver_tip_frame);
1580 
1581  // bring the pose to the frame of the IK solver
1582  if (!setToIKSolverFrame(current_pose, solver))
1583  return false;
1584 
1585  // Convert Eigen pose to geometry_msgs pose
1586  geometry_msgs::Pose ik_query;
1587  tf::poseEigenToMsg(current_pose, ik_query);
1588 
1589  // Save into vectors - but this needs to be ordered in the same order as the IK solver expects its tip frames
1590  ik_queries[solver_tip_id] = ik_query;
1591 
1592  // Remove that tip from the list of available tip frames because each can only have one pose
1593  tip_frames_used[solver_tip_id] = true;
1594  }
1595 
1596  // if no timeout has been specified, use the default one
1597  if (timeout < std::numeric_limits<double>::epsilon())
1598  timeout = jmg->getDefaultIKTimeout();
1599 
1600  if (attempts == 0)
1601  attempts = jmg->getDefaultIKAttempts();
1602 
1603  // set callback function
1605  if (constraint)
1606  ik_callback_fn = boost::bind(&ikCallbackFnAdapter, this, jmg, constraint, _1, _2, _3);
1607 
1608  // Bijection
1609  const std::vector<unsigned int>& bij = jmg->getKinematicsSolverJointBijection();
1610 
1611  bool first_seed = true;
1612  std::vector<double> initial_values;
1613  for (unsigned int st = 0; st < attempts; ++st)
1614  {
1615  std::vector<double> seed(bij.size());
1616 
1617  // the first seed is the current robot state joint values
1618  if (first_seed)
1619  {
1620  first_seed = false;
1621  copyJointGroupPositions(jmg, initial_values);
1622  for (std::size_t i = 0; i < bij.size(); ++i)
1623  seed[i] = initial_values[bij[i]];
1624  }
1625  else
1626  {
1627  ROS_DEBUG_NAMED(LOGNAME, "Rerunning IK solver with random joint positions");
1628 
1629  // sample a random seed
1631  std::vector<double> random_values;
1632  jmg->getVariableRandomPositions(rng, random_values);
1633  for (std::size_t i = 0; i < bij.size(); ++i)
1634  seed[i] = random_values[bij[i]];
1635 
1636  if (options.lock_redundant_joints)
1637  {
1638  std::vector<unsigned int> red_joints;
1639  solver->getRedundantJoints(red_joints);
1640  copyJointGroupPositions(jmg, initial_values);
1641  for (std::size_t i = 0; i < red_joints.size(); ++i)
1642  seed[red_joints[i]] = initial_values[bij[red_joints[i]]];
1643  }
1644  }
1645 
1646  // compute the IK solution
1647  std::vector<double> ik_sol;
1648  moveit_msgs::MoveItErrorCodes error;
1649 
1650  if (solver->searchPositionIK(ik_queries, seed, timeout, consistency_limits, ik_sol, ik_callback_fn, error, options,
1651  this))
1652  {
1653  std::vector<double> solution(bij.size());
1654  for (std::size_t i = 0; i < bij.size(); ++i)
1655  solution[bij[i]] = ik_sol[i];
1656  setJointGroupPositions(jmg, solution);
1657  return true;
1658  }
1659  }
1660  return false;
1661 }
1662 
1664  const std::vector<std::string>& tips_in,
1665  const std::vector<std::vector<double> >& consistency_limits, unsigned int attempts,
1666  double timeout, const GroupStateValidityCallbackFn& constraint,
1667  const kinematics::KinematicsQueryOptions& options)
1668 {
1669  // Assume we have already ran setFromIK() and those checks
1670 
1671  // Get containing subgroups
1672  std::vector<const JointModelGroup*> sub_groups;
1673  jmg->getSubgroups(sub_groups);
1674 
1675  // Error check
1676  if (poses_in.size() != sub_groups.size())
1677  {
1678  ROS_ERROR_NAMED(LOGNAME, "Number of poses (%zu) must be the same as number of sub-groups (%zu)", poses_in.size(),
1679  sub_groups.size());
1680  return false;
1681  }
1682 
1683  if (tips_in.size() != sub_groups.size())
1684  {
1685  ROS_ERROR_NAMED(LOGNAME, "Number of tip names (%zu) must be same as number of sub-groups (%zu)", tips_in.size(),
1686  sub_groups.size());
1687  return false;
1688  }
1689 
1690  if (!consistency_limits.empty() && consistency_limits.size() != sub_groups.size())
1691  {
1692  ROS_ERROR_NAMED(LOGNAME, "Number of consistency limit vectors must be the same as number of sub-groups");
1693  return false;
1694  }
1695 
1696  for (std::size_t i = 0; i < consistency_limits.size(); ++i)
1697  {
1698  if (consistency_limits[i].size() != sub_groups[i]->getVariableCount())
1699  {
1700  ROS_ERROR_NAMED(LOGNAME, "Number of joints in consistency_limits is %zu but it should be should be %u", i,
1701  sub_groups[i]->getVariableCount());
1702  return false;
1703  }
1704  }
1705 
1706  // Populate list of kin solvers for the various subgroups
1707  std::vector<kinematics::KinematicsBaseConstPtr> solvers;
1708  for (std::size_t i = 0; i < poses_in.size(); ++i)
1709  {
1710  kinematics::KinematicsBaseConstPtr solver = sub_groups[i]->getSolverInstance();
1711  if (!solver)
1712  {
1713  ROS_ERROR_NAMED(LOGNAME, "Could not find solver for group '%s'", sub_groups[i]->getName().c_str());
1714  return false;
1715  }
1716  solvers.push_back(solver);
1717  }
1718 
1719  // Make non-const versions
1720  EigenSTL::vector_Affine3d transformed_poses = poses_in;
1721  std::vector<std::string> pose_frames = tips_in;
1722 
1723  // Each each pose's tip frame naming
1724  for (std::size_t i = 0; i < poses_in.size(); ++i)
1725  {
1726  Eigen::Affine3d& pose = transformed_poses[i];
1727  std::string& pose_frame = pose_frames[i];
1728 
1729  // bring the pose to the frame of the IK solver
1730  if (!setToIKSolverFrame(pose, solvers[i]))
1731  return false;
1732 
1733  // see if the tip frame can be transformed via fixed transforms to the frame known to the IK solver
1734  std::string solver_tip_frame = solvers[i]->getTipFrame();
1735 
1736  // remove the frame '/' if there is one, so we can avoid calling Transforms::sameFrame() which may copy strings more
1737  // often that we need to
1738  if (!solver_tip_frame.empty() && solver_tip_frame[0] == '/')
1739  solver_tip_frame = solver_tip_frame.substr(1);
1740 
1741  if (pose_frame != solver_tip_frame)
1742  {
1743  if (hasAttachedBody(pose_frame))
1744  {
1745  const AttachedBody* ab = getAttachedBody(pose_frame);
1746  const EigenSTL::vector_Affine3d& ab_trans = ab->getFixedTransforms();
1747  if (ab_trans.size() != 1)
1748  {
1749  ROS_ERROR_NAMED(LOGNAME, "Cannot use an attached body with multiple geometries as a reference frame.");
1750  return false;
1751  }
1752  pose_frame = ab->getAttachedLinkName();
1753  pose = pose * ab_trans[0].inverse();
1754  }
1755  if (pose_frame != solver_tip_frame)
1756  {
1757  const robot_model::LinkModel* lm = getLinkModel(pose_frame);
1758  if (!lm)
1759  return false;
1761  for (robot_model::LinkTransformMap::const_iterator it = fixed_links.begin(); it != fixed_links.end(); ++it)
1762  if (it->first->getName() == solver_tip_frame)
1763  {
1764  pose_frame = solver_tip_frame;
1765  pose = pose * it->second;
1766  break;
1767  }
1768  }
1769  }
1770 
1771  if (pose_frame != solver_tip_frame)
1772  {
1773  ROS_ERROR_NAMED(LOGNAME, "Cannot compute IK for query pose reference frame '%s', desired: '%s'",
1774  pose_frame.c_str(), solver_tip_frame.c_str());
1775  return false;
1776  }
1777  }
1778 
1779  // Convert Eigen poses to geometry_msg format
1780  std::vector<geometry_msgs::Pose> ik_queries(poses_in.size());
1782  if (constraint)
1783  ik_callback_fn = boost::bind(&ikCallbackFnAdapter, this, jmg, constraint, _1, _2, _3);
1784 
1785  for (std::size_t i = 0; i < transformed_poses.size(); ++i)
1786  {
1787  Eigen::Quaterniond quat(transformed_poses[i].rotation());
1788  Eigen::Vector3d point(transformed_poses[i].translation());
1789  ik_queries[i].position.x = point.x();
1790  ik_queries[i].position.y = point.y();
1791  ik_queries[i].position.z = point.z();
1792  ik_queries[i].orientation.x = quat.x();
1793  ik_queries[i].orientation.y = quat.y();
1794  ik_queries[i].orientation.z = quat.z();
1795  ik_queries[i].orientation.w = quat.w();
1796  }
1797 
1798  if (attempts == 0)
1799  attempts = jmg->getDefaultIKAttempts();
1800 
1801  // if no timeout has been specified, use the default one
1802  if (timeout < std::numeric_limits<double>::epsilon())
1803  timeout = jmg->getDefaultIKTimeout();
1804 
1805  bool first_seed = true;
1806  for (unsigned int st = 0; st < attempts; ++st)
1807  {
1808  bool found_solution = true;
1809  for (std::size_t sg = 0; sg < sub_groups.size(); ++sg)
1810  {
1811  const std::vector<unsigned int>& bij = sub_groups[sg]->getKinematicsSolverJointBijection();
1812  std::vector<double> seed(bij.size());
1813  // the first seed is the initial state
1814  if (first_seed)
1815  {
1816  std::vector<double> initial_values;
1817  copyJointGroupPositions(sub_groups[sg], initial_values);
1818  for (std::size_t i = 0; i < bij.size(); ++i)
1819  seed[i] = initial_values[bij[i]];
1820  }
1821  else
1822  {
1823  // sample a random seed
1825  std::vector<double> random_values;
1826  sub_groups[sg]->getVariableRandomPositions(rng, random_values);
1827  for (std::size_t i = 0; i < bij.size(); ++i)
1828  seed[i] = random_values[bij[i]];
1829  }
1830 
1831  // compute the IK solution
1832  std::vector<double> ik_sol;
1833  moveit_msgs::MoveItErrorCodes error;
1834  const std::vector<double>& climits = consistency_limits.empty() ? std::vector<double>() : consistency_limits[sg];
1835  if (solvers[sg]->searchPositionIK(ik_queries[sg], seed, timeout, climits, ik_sol, error))
1836  {
1837  std::vector<double> solution(bij.size());
1838  for (std::size_t i = 0; i < bij.size(); ++i)
1839  solution[bij[i]] = ik_sol[i];
1840  setJointGroupPositions(sub_groups[sg], solution);
1841  }
1842  else
1843  {
1844  found_solution = false;
1845  break;
1846  }
1847  ROS_DEBUG_NAMED(LOGNAME, "IK attempt: %d of %d", st, attempts);
1848  }
1849  if (found_solution)
1850  {
1851  std::vector<double> full_solution;
1852  copyJointGroupPositions(jmg, full_solution);
1853  if (constraint ? constraint(this, jmg, &full_solution[0]) : true)
1854  {
1855  ROS_DEBUG_NAMED(LOGNAME, "Found IK solution");
1856  return true;
1857  }
1858  }
1859  }
1860  return false;
1861 }
1862 
1863 double RobotState::computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
1864  const LinkModel* link, const Eigen::Vector3d& direction,
1865  bool global_reference_frame, double distance, const MaxEEFStep& max_step,
1866  const JumpThreshold& jump_threshold,
1867  const GroupStateValidityCallbackFn& validCallback,
1868  const kinematics::KinematicsQueryOptions& options)
1869 {
1870  // this is the Cartesian pose we start from, and have to move in the direction indicated
1871  const Eigen::Affine3d& start_pose = getGlobalLinkTransform(link);
1872 
1873  // the direction can be in the local reference frame (in which case we rotate it)
1874  const Eigen::Vector3d rotated_direction = global_reference_frame ? direction : start_pose.rotation() * direction;
1875 
1876  // The target pose is built by applying a translation to the start pose for the desired direction and distance
1877  Eigen::Affine3d target_pose = start_pose;
1878  target_pose.translation() += rotated_direction * distance;
1879 
1880  // call computeCartesianPath for the computed target pose in the global reference frame
1881  return (distance *
1882  computeCartesianPath(group, traj, link, target_pose, true, max_step, jump_threshold, validCallback, options));
1883 }
1884 
1885 double RobotState::computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
1886  const LinkModel* link, const Eigen::Affine3d& target,
1887  bool global_reference_frame, const MaxEEFStep& max_step,
1888  const JumpThreshold& jump_threshold,
1889  const GroupStateValidityCallbackFn& validCallback,
1890  const kinematics::KinematicsQueryOptions& options)
1891 {
1892  const std::vector<const JointModel*>& cjnt = group->getContinuousJointModels();
1893  // make sure that continuous joints wrap
1894  for (std::size_t i = 0; i < cjnt.size(); ++i)
1895  enforceBounds(cjnt[i]);
1896 
1897  // this is the Cartesian pose we start from, and we move in the direction indicated
1898  Eigen::Affine3d start_pose = getGlobalLinkTransform(link);
1899 
1900  // the target can be in the local reference frame (in which case we rotate it)
1901  Eigen::Affine3d rotated_target = global_reference_frame ? target : start_pose * target;
1902 
1903  Eigen::Quaterniond start_quaternion(start_pose.rotation());
1904  Eigen::Quaterniond target_quaternion(rotated_target.rotation());
1905 
1906  if (max_step.translation <= 0.0 && max_step.rotation <= 0.0)
1907  {
1908  ROS_ERROR_NAMED(LOGNAME,
1909  "Invalid MaxEEFStep passed into computeCartesianPath. Both the MaxEEFStep.rotation and "
1910  "MaxEEFStep.translation components must be non-negative and at least one component must be "
1911  "greater than zero");
1912  return 0.0;
1913  }
1914 
1915  double rotation_distance = start_quaternion.angularDistance(target_quaternion);
1916  double translation_distance = (rotated_target.translation() - start_pose.translation()).norm();
1917 
1918  // decide how many steps we will need for this trajectory
1919  std::size_t translation_steps = 0;
1920  if (max_step.translation > 0.0)
1921  translation_steps = floor(translation_distance / max_step.translation);
1922 
1923  std::size_t rotation_steps = 0;
1924  if (max_step.rotation > 0.0)
1925  rotation_steps = floor(rotation_distance / max_step.rotation);
1926 
1927  // If we are testing for relative jumps, we always want at least MIN_STEPS_FOR_JUMP_THRESH steps
1928  std::size_t steps = std::max(translation_steps, rotation_steps) + 1;
1929  if (jump_threshold.factor > 0 && steps < MIN_STEPS_FOR_JUMP_THRESH)
1930  steps = MIN_STEPS_FOR_JUMP_THRESH;
1931 
1932  traj.clear();
1933  traj.push_back(RobotStatePtr(new RobotState(*this)));
1934 
1935  double last_valid_percentage = 0.0;
1936  for (std::size_t i = 1; i <= steps; ++i)
1937  {
1938  double percentage = (double)i / (double)steps;
1939 
1940  Eigen::Affine3d pose(start_quaternion.slerp(percentage, target_quaternion));
1941  pose.translation() = percentage * rotated_target.translation() + (1 - percentage) * start_pose.translation();
1942 
1943  // Explicitly use a single IK attempt only: We want a smooth trajectory.
1944  // Random seeding (of additional attempts) would probably create IK jumps.
1945  if (setFromIK(group, pose, link->getName(), 1, 0.0, validCallback, options))
1946  traj.push_back(RobotStatePtr(new RobotState(*this)));
1947  else
1948  break;
1949 
1950  last_valid_percentage = percentage;
1951  }
1952 
1953  last_valid_percentage *= testJointSpaceJump(group, traj, jump_threshold);
1954 
1955  return last_valid_percentage;
1956 }
1957 
1958 double RobotState::computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
1959  const LinkModel* link, const EigenSTL::vector_Affine3d& waypoints,
1960  bool global_reference_frame, const MaxEEFStep& max_step,
1961  const JumpThreshold& jump_threshold,
1962  const GroupStateValidityCallbackFn& validCallback,
1963  const kinematics::KinematicsQueryOptions& options)
1964 {
1965  double percentage_solved = 0.0;
1966  for (std::size_t i = 0; i < waypoints.size(); ++i)
1967  {
1968  // Don't test joint space jumps for every waypoint, test them later on the whole trajectory.
1969  static const JumpThreshold no_joint_space_jump_test;
1970  std::vector<RobotStatePtr> waypoint_traj;
1971  double wp_percentage_solved = computeCartesianPath(group, waypoint_traj, link, waypoints[i], global_reference_frame,
1972  max_step, no_joint_space_jump_test, validCallback, options);
1973  if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits<double>::epsilon())
1974  {
1975  percentage_solved = (double)(i + 1) / (double)waypoints.size();
1976  std::vector<RobotStatePtr>::iterator start = waypoint_traj.begin();
1977  if (i > 0 && !waypoint_traj.empty())
1978  std::advance(start, 1);
1979  traj.insert(traj.end(), start, waypoint_traj.end());
1980  }
1981  else
1982  {
1983  percentage_solved += wp_percentage_solved / (double)waypoints.size();
1984  std::vector<RobotStatePtr>::iterator start = waypoint_traj.begin();
1985  if (i > 0 && !waypoint_traj.empty())
1986  std::advance(start, 1);
1987  traj.insert(traj.end(), start, waypoint_traj.end());
1988  break;
1989  }
1990  }
1991 
1992  percentage_solved *= testJointSpaceJump(group, traj, jump_threshold);
1993 
1994  return percentage_solved;
1995 }
1996 
1997 double RobotState::testJointSpaceJump(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
1998  const JumpThreshold& jump_threshold)
1999 {
2000  double percentage_solved = 1.0;
2001  if (traj.size() <= 1)
2002  return percentage_solved;
2003 
2004  if (jump_threshold.factor > 0.0)
2005  percentage_solved *= testRelativeJointSpaceJump(group, traj, jump_threshold.factor);
2006 
2007  if (jump_threshold.revolute > 0.0 || jump_threshold.prismatic > 0.0)
2008  percentage_solved *= testAbsoluteJointSpaceJump(group, traj, jump_threshold.revolute, jump_threshold.prismatic);
2009 
2010  return percentage_solved;
2011 }
2012 
2013 double RobotState::testRelativeJointSpaceJump(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
2014  double jump_threshold_factor)
2015 {
2016  if (traj.size() < MIN_STEPS_FOR_JUMP_THRESH)
2017  {
2018  ROS_WARN_NAMED(LOGNAME, "The computed trajectory is too short to detect jumps in joint-space "
2019  "Need at least %zu steps, only got %zu. Try a lower max_step.",
2020  MIN_STEPS_FOR_JUMP_THRESH, traj.size());
2021  }
2022 
2023  std::vector<double> dist_vector;
2024  dist_vector.reserve(traj.size() - 1);
2025  double total_dist = 0.0;
2026  for (std::size_t i = 1; i < traj.size(); ++i)
2027  {
2028  double dist_prev_point = traj[i]->distance(*traj[i - 1], group);
2029  dist_vector.push_back(dist_prev_point);
2030  total_dist += dist_prev_point;
2031  }
2032 
2033  double percentage = 1.0;
2034  // compute the average distance between the states we looked at
2035  double thres = jump_threshold_factor * (total_dist / (double)dist_vector.size());
2036  for (std::size_t i = 0; i < dist_vector.size(); ++i)
2037  if (dist_vector[i] > thres)
2038  {
2039  ROS_DEBUG_NAMED(LOGNAME, "Truncating Cartesian path due to detected jump in joint-space distance");
2040  percentage = (double)(i + 1) / (double)(traj.size());
2041  traj.resize(i + 1);
2042  break;
2043  }
2044 
2045  return percentage;
2046 }
2047 
2048 double RobotState::testAbsoluteJointSpaceJump(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
2049  double revolute_threshold, double prismatic_threshold)
2050 {
2051  struct LimitData
2052  {
2053  double limit;
2054  bool check;
2055  };
2056  LimitData data[2] = { { revolute_threshold, revolute_threshold > 0.0 },
2057  { prismatic_threshold, prismatic_threshold > 0.0 } };
2058  bool still_valid = true;
2059  const std::vector<const JointModel*>& joints = group->getActiveJointModels();
2060  for (std::size_t traj_ix = 0, ix_end = traj.size() - 1; traj_ix != ix_end; ++traj_ix)
2061  {
2062  for (auto& joint : joints)
2063  {
2064  unsigned int type_index;
2065  switch (joint->getType())
2066  {
2067  case JointModel::REVOLUTE:
2068  type_index = 0;
2069  break;
2070  case JointModel::PRISMATIC:
2071  type_index = 1;
2072  break;
2073  default:
2074  ROS_WARN_NAMED(LOGNAME, "Joint %s has not supported type %s. \n"
2075  "testAbsoluteJointSpaceJump only supports prismatic and revolute joints.",
2076  joint->getName().c_str(), joint->getTypeName().c_str());
2077  continue;
2078  }
2079  if (!data[type_index].check)
2080  continue;
2081 
2082  double distance = traj[traj_ix]->distance(*traj[traj_ix + 1], joint);
2083  if (distance > data[type_index].limit)
2084  {
2085  ROS_DEBUG_NAMED(LOGNAME, "Truncating Cartesian path due to detected jump of %.4f > %.4f in joint %s", distance,
2086  data[type_index].limit, joint->getName().c_str());
2087  still_valid = false;
2088  break;
2089  }
2090  }
2091 
2092  if (!still_valid)
2093  {
2094  double percent_valid = (double)(traj_ix + 1) / (double)(traj.size());
2095  traj.resize(traj_ix + 1);
2096  return percent_valid;
2097  }
2098  }
2099  return 1.0;
2100 }
2101 
2102 void RobotState::computeAABB(std::vector<double>& aabb) const
2103 {
2104  BOOST_VERIFY(checkLinkTransforms());
2105 
2106  core::AABB bounding_box;
2107  std::vector<const LinkModel*> links = robot_model_->getLinkModelsWithCollisionGeometry();
2108  for (std::size_t i = 0; i < links.size(); ++i)
2109  {
2110  Eigen::Affine3d transform = getGlobalLinkTransform(links[i]); // intentional copy, we will translate
2111  const Eigen::Vector3d& extents = links[i]->getShapeExtentsAtOrigin();
2112  transform.translate(links[i]->getCenteredBoundingBoxOffset());
2113  bounding_box.extendWithTransformedBox(transform, extents);
2114  }
2115  for (std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.begin();
2116  it != attached_body_map_.end(); ++it)
2117  {
2118  const EigenSTL::vector_Affine3d& transforms = it->second->getGlobalCollisionBodyTransforms();
2119  const std::vector<shapes::ShapeConstPtr>& shapes = it->second->getShapes();
2120  for (std::size_t i = 0; i < transforms.size(); ++i)
2121  {
2122  Eigen::Vector3d extents = shapes::computeShapeExtents(shapes[i].get());
2123  bounding_box.extendWithTransformedBox(transforms[i], extents);
2124  }
2125  }
2126 
2127  aabb.clear();
2128  aabb.resize(6, 0.0);
2129  if (!bounding_box.isEmpty())
2130  {
2131  // The following is a shorthand for something like:
2132  // aabb[0, 2, 4] = bounding_box.min(); aabb[1, 3, 5] = bounding_box.max();
2133  Eigen::Map<Eigen::VectorXd, Eigen::Unaligned, Eigen::InnerStride<2> >(aabb.data(), 3) = bounding_box.min();
2134  Eigen::Map<Eigen::VectorXd, Eigen::Unaligned, Eigen::InnerStride<2> >(aabb.data() + 1, 3) = bounding_box.max();
2135  }
2136 }
2137 
2138 void RobotState::printStatePositions(std::ostream& out) const
2139 {
2140  const std::vector<std::string>& nm = robot_model_->getVariableNames();
2141  for (std::size_t i = 0; i < nm.size(); ++i)
2142  out << nm[i] << "=" << position_[i] << std::endl;
2143 }
2144 
2145 void RobotState::printDirtyInfo(std::ostream& out) const
2146 {
2147  out << " * Dirty Joint Transforms: " << std::endl;
2148  const std::vector<const JointModel*>& jm = robot_model_->getJointModels();
2149  for (std::size_t i = 0; i < jm.size(); ++i)
2150  if (jm[i]->getVariableCount() > 0 && dirtyJointTransform(jm[i]))
2151  out << " " << jm[i]->getName() << std::endl;
2152  out << " * Dirty Link Transforms: " << (dirty_link_transforms_ ? dirty_link_transforms_->getName() : "NULL")
2153  << std::endl;
2154  out << " * Dirty Collision Body Transforms: "
2156 }
2157 
2158 void RobotState::printStateInfo(std::ostream& out) const
2159 {
2160  out << "Robot State @" << this << std::endl;
2161 
2162  std::size_t n = robot_model_->getVariableCount();
2163  if (position_)
2164  {
2165  out << " * Position: ";
2166  for (std::size_t i = 0; i < n; ++i)
2167  out << position_[i] << " ";
2168  out << std::endl;
2169  }
2170  else
2171  out << " * Position: NULL" << std::endl;
2172 
2173  if (velocity_)
2174  {
2175  out << " * Velocity: ";
2176  for (std::size_t i = 0; i < n; ++i)
2177  out << velocity_[i] << " ";
2178  out << std::endl;
2179  }
2180  else
2181  out << " * Velocity: NULL" << std::endl;
2182 
2183  if (acceleration_)
2184  {
2185  out << " * Acceleration: ";
2186  for (std::size_t i = 0; i < n; ++i)
2187  out << acceleration_[i] << " ";
2188  out << std::endl;
2189  }
2190  else
2191  out << " * Acceleration: NULL" << std::endl;
2192 
2193  out << " * Dirty Link Transforms: " << (dirty_link_transforms_ ? dirty_link_transforms_->getName() : "NULL")
2194  << std::endl;
2195  out << " * Dirty Collision Body Transforms: "
2197 
2198  printTransforms(out);
2199 }
2200 
2201 void RobotState::printTransform(const Eigen::Affine3d& transform, std::ostream& out) const
2202 {
2203  Eigen::Quaterniond q(transform.rotation());
2204  out << "T.xyz = [" << transform.translation().x() << ", " << transform.translation().y() << ", "
2205  << transform.translation().z() << "], Q.xyzw = [" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w()
2206  << "]" << std::endl;
2207 }
2208 
2209 void RobotState::printTransforms(std::ostream& out) const
2210 {
2212  {
2213  out << "No transforms computed" << std::endl;
2214  return;
2215  }
2216 
2217  out << "Joint transforms:" << std::endl;
2218  const std::vector<const JointModel*>& jm = robot_model_->getJointModels();
2219  for (std::size_t i = 0; i < jm.size(); ++i)
2220  {
2221  out << " " << jm[i]->getName();
2222  const int idx = jm[i]->getJointIndex();
2223  if (dirty_joint_transforms_[idx])
2224  out << " [dirty]";
2225  out << ": ";
2227  }
2228 
2229  out << "Link poses:" << std::endl;
2230  const std::vector<const LinkModel*>& lm = robot_model_->getLinkModels();
2231  for (std::size_t i = 0; i < lm.size(); ++i)
2232  {
2233  out << " " << lm[i]->getName() << ": ";
2234  printTransform(global_link_transforms_[lm[i]->getLinkIndex()], out);
2235  }
2236 }
2237 
2238 std::string RobotState::getStateTreeString(const std::string& prefix) const
2239 {
2240  std::stringstream ss;
2241  ss << "ROBOT: " << robot_model_->getName() << std::endl;
2242  getStateTreeJointString(ss, robot_model_->getRootJoint(), " ", true);
2243  return ss.str();
2244 }
2245 
2246 namespace
2247 {
2248 void getPoseString(std::ostream& ss, const Eigen::Affine3d& pose, const std::string& pfx)
2249 {
2250  ss.precision(3);
2251  for (int y = 0; y < 4; ++y)
2252  {
2253  ss << pfx;
2254  for (int x = 0; x < 4; ++x)
2255  {
2256  ss << std::setw(8) << pose(y, x) << " ";
2257  }
2258  ss << std::endl;
2259  }
2260 }
2261 }
2262 
2263 void RobotState::getStateTreeJointString(std::ostream& ss, const JointModel* jm, const std::string& pfx0,
2264  bool last) const
2265 {
2266  std::string pfx = pfx0 + "+--";
2267 
2268  ss << pfx << "Joint: " << jm->getName() << std::endl;
2269 
2270  pfx = pfx0 + (last ? " " : "| ");
2271 
2272  for (std::size_t i = 0; i < jm->getVariableCount(); ++i)
2273  {
2274  ss.precision(3);
2275  ss << pfx << jm->getVariableNames()[i] << std::setw(12) << position_[jm->getFirstVariableIndex() + i] << std::endl;
2276  }
2277 
2278  const LinkModel* lm = jm->getChildLinkModel();
2279 
2280  ss << pfx << "Link: " << lm->getName() << std::endl;
2281  getPoseString(ss, lm->getJointOriginTransform(), pfx + "joint_origin:");
2283  {
2284  getPoseString(ss, variable_joint_transforms_[jm->getJointIndex()], pfx + "joint_variable:");
2285  getPoseString(ss, global_link_transforms_[lm->getLinkIndex()], pfx + "link_global:");
2286  }
2287 
2288  for (std::vector<const JointModel*>::const_iterator it = lm->getChildJointModels().begin();
2289  it != lm->getChildJointModels().end(); ++it)
2290  getStateTreeJointString(ss, *it, pfx, it + 1 == lm->getChildJointModels().end());
2291 }
2292 
2293 std::ostream& operator<<(std::ostream& out, const RobotState& s)
2294 {
2295  s.printStateInfo(out);
2296  return out;
2297 }
2298 
2299 } // end of namespace core
2300 } // 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:1668
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:1766
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:1836
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:827
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:1464
void printStatePositions(std::ostream &out=std::cout) const
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:1794
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:1365
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:875
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:1832
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:1835
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:721
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:1745
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:663
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:1837
bool dirtyLinkTransforms() const
Definition: robot_state.h:1442
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:1357
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:1844
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:769
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:1831
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:615
random_numbers::RandomNumberGenerator * rng_
For certain operations a state needs a random number generator. However, it may be slightly expensive...
Definition: robot_state.h:1851
void setToRandomPositions()
Set all joints to random values. Values will be within default bounds.
RobotModelConstPtr robot_model_
Definition: robot_state.h:1820
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:1387
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:1437
const double * getJointPositions(const std::string &joint_name) const
Definition: robot_state.h:566
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:1834
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:1447
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:1840
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)...
Represents an axis-aligned bounding box.
Definition: aabb.h:47


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Oct 18 2018 02:47:09