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