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