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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Sep 18 2022 02:26:15