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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sat Jun 22 2019 19:53:16