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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Jan 21 2018 03:54:29