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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Tue Jun 18 2024 02:23:34