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