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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Tue Sep 21 2021 02:23:25