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