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