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::getLinkModelIncludingAttachedBodies(const std::string& frame) const
803 {
804  // If the frame is a link, return that link.
805  if (getRobotModel()->hasLinkModel(frame))
806  {
807  return getLinkModel(frame);
808  }
809 
810  // If the frame is an attached body, return the link the body is attached to.
811  if (const auto it = attached_body_map_.find(frame); it != attached_body_map_.end())
812  {
813  const auto& body{ it->second };
814  return body->getAttachedLink();
815  }
816 
817  // If the frame is a subframe of an attached body, return the link the body is attached to.
818  for (const auto& it : attached_body_map_)
819  {
820  const auto& body{ it.second };
821  if (body->hasSubframeTransform(frame))
822  {
823  return body->getAttachedLink();
824  }
825  }
826 
827  // If the frame is none of the above, return nullptr.
828  return nullptr;
829 }
830 
832  const moveit::core::JointModelGroup* jmg) const
833 {
834  const LinkModel* link = getLinkModelIncludingAttachedBodies(frame);
835 
836  return getRobotModel()->getRigidlyConnectedParentLinkModel(link, jmg);
837 }
838 
839 bool RobotState::satisfiesBounds(double margin) const
840 {
841  const std::vector<const JointModel*>& jm = robot_model_->getActiveJointModels();
842  for (const JointModel* joint : jm)
843  if (!satisfiesBounds(joint, margin))
844  return false;
845  return true;
846 }
847 
848 bool RobotState::satisfiesBounds(const JointModelGroup* group, double margin) const
849 {
850  const std::vector<const JointModel*>& jm = group->getActiveJointModels();
851  for (const JointModel* joint : jm)
852  if (!satisfiesBounds(joint, margin))
853  return false;
854  return true;
855 }
856 
858 {
859  const std::vector<const JointModel*>& jm = robot_model_->getActiveJointModels();
860  for (const JointModel* joint : jm)
861  enforceBounds(joint);
862 }
863 
865 {
866  const std::vector<const JointModel*>& jm = joint_group->getActiveJointModels();
867  for (const JointModel* joint : jm)
868  enforceBounds(joint);
869 }
870 
872 {
873  for (const JointModel* jm : robot_model_->getActiveJointModels())
874  harmonizePosition(jm);
875 }
876 
878 {
879  for (const JointModel* jm : joint_group->getActiveJointModels())
880  harmonizePosition(jm);
881 }
882 
883 std::pair<double, const JointModel*> RobotState::getMinDistanceToPositionBounds() const
884 {
885  return getMinDistanceToPositionBounds(robot_model_->getActiveJointModels());
886 }
887 
888 std::pair<double, const JointModel*> RobotState::getMinDistanceToPositionBounds(const JointModelGroup* group) const
889 {
890  return getMinDistanceToPositionBounds(group->getActiveJointModels());
891 }
892 
893 std::pair<double, const JointModel*>
894 RobotState::getMinDistanceToPositionBounds(const std::vector<const JointModel*>& joints) const
895 {
896  double distance = std::numeric_limits<double>::max();
897  const JointModel* index = nullptr;
898  for (const JointModel* joint : joints)
899  {
900  if (joint->getType() == JointModel::PLANAR || joint->getType() == JointModel::FLOATING)
901  continue;
902  if (joint->getType() == JointModel::REVOLUTE)
903  if (static_cast<const RevoluteJointModel*>(joint)->isContinuous())
904  continue;
905 
906  const double* joint_values = getJointPositions(joint);
907  const JointModel::Bounds& bounds = joint->getVariableBounds();
908  std::vector<double> lower_bounds(bounds.size()), upper_bounds(bounds.size());
909  for (std::size_t j = 0; j < bounds.size(); ++j)
910  {
911  lower_bounds[j] = bounds[j].min_position_;
912  upper_bounds[j] = bounds[j].max_position_;
913  }
914  double new_distance = joint->distance(joint_values, &lower_bounds[0]);
915  if (new_distance < distance)
916  {
917  index = joint;
918  distance = new_distance;
919  }
920  new_distance = joint->distance(joint_values, &upper_bounds[0]);
921  if (new_distance < distance)
922  {
923  index = joint;
924  distance = new_distance;
925  }
926  }
927  return std::make_pair(distance, index);
928 }
929 
930 bool RobotState::isValidVelocityMove(const RobotState& other, const JointModelGroup* group, double dt) const
931 {
932  const std::vector<const JointModel*>& jm = group->getActiveJointModels();
933  for (const JointModel* joint_id : jm)
934  {
935  const int idx = joint_id->getFirstVariableIndex();
936  const std::vector<VariableBounds>& bounds = joint_id->getVariableBounds();
937 
938  // Check velocity for each joint variable
939  for (std::size_t var_id = 0; var_id < joint_id->getVariableCount(); ++var_id)
940  {
941  const double dtheta = std::abs(*(position_ + idx + var_id) - *(other.getVariablePositions() + idx + var_id));
942 
943  if (dtheta > dt * bounds[var_id].max_velocity_)
944  return false;
945  }
946  }
947  return true;
948 }
949 
950 double RobotState::distance(const RobotState& other, const JointModelGroup* joint_group) const
951 {
952  double d = 0.0;
953  const std::vector<const JointModel*>& jm = joint_group->getActiveJointModels();
954  for (const JointModel* joint : jm)
955  {
956  const int idx = joint->getFirstVariableIndex();
957  d += joint->getDistanceFactor() * joint->distance(position_ + idx, other.position_ + idx);
958  }
959  return d;
960 }
961 
962 void RobotState::interpolate(const RobotState& to, double t, RobotState& state) const
963 {
964  moveit::core::checkInterpolationParamBounds(LOGGER, t);
965  robot_model_->interpolate(getVariablePositions(), to.getVariablePositions(), t, state.getVariablePositions());
966 
967  memset(state.dirty_joint_transforms_, 1, state.robot_model_->getJointModelCount() * sizeof(unsigned char));
968  state.dirty_link_transforms_ = state.robot_model_->getRootJoint();
969 }
970 
971 void RobotState::interpolate(const RobotState& to, double t, RobotState& state, const JointModelGroup* joint_group) const
972 {
973  moveit::core::checkInterpolationParamBounds(LOGGER, t);
974  const std::vector<const JointModel*>& jm = joint_group->getActiveJointModels();
975  for (const JointModel* joint : jm)
976  {
977  const int idx = joint->getFirstVariableIndex();
978  joint->interpolate(position_ + idx, to.position_ + idx, t, state.position_ + idx);
979  }
980  state.updateMimicJoints(joint_group);
981 }
982 
984 {
985  attached_body_update_callback_ = callback;
986 }
987 
988 bool RobotState::hasAttachedBody(const std::string& id) const
989 {
990  return attached_body_map_.find(id) != attached_body_map_.end();
991 }
992 
993 const AttachedBody* RobotState::getAttachedBody(const std::string& id) const
994 {
995  const auto it = attached_body_map_.find(id);
996  if (it == attached_body_map_.end())
997  {
998  RCLCPP_ERROR(LOGGER, "Attached body '%s' not found", id.c_str());
999  return nullptr;
1000  }
1001  else
1002  return it->second.get();
1003 }
1004 
1005 void RobotState::attachBody(std::unique_ptr<AttachedBody> attached_body)
1006 {
1007  // If an attached body with the same id exists, remove it
1008  clearAttachedBody(attached_body->getName());
1009 
1010  attached_body->computeTransform(getGlobalLinkTransform(attached_body->getAttachedLink()));
1011  if (attached_body_update_callback_)
1012  attached_body_update_callback_(attached_body.get(), true);
1013  attached_body_map_[attached_body->getName()] = std::move(attached_body);
1014 }
1015 
1017 {
1018  attachBody(std::unique_ptr<AttachedBody>(attached_body));
1019 }
1020 
1021 void RobotState::attachBody(const std::string& id, const Eigen::Isometry3d& pose,
1022  const std::vector<shapes::ShapeConstPtr>& shapes,
1023  const EigenSTL::vector_Isometry3d& shape_poses, const std::set<std::string>& touch_links,
1024  const std::string& link, const trajectory_msgs::msg::JointTrajectory& detach_posture,
1025  const moveit::core::FixedTransformsMap& subframe_poses)
1026 {
1027  attachBody(std::make_unique<AttachedBody>(robot_model_->getLinkModel(link), id, pose, shapes, shape_poses,
1028  touch_links, detach_posture, subframe_poses));
1029 }
1030 
1031 void RobotState::getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies) const
1032 {
1033  attached_bodies.clear();
1034  attached_bodies.reserve(attached_body_map_.size());
1035  for (const auto& it : attached_body_map_)
1036  attached_bodies.push_back(it.second.get());
1037 }
1038 
1039 void RobotState::getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const JointModelGroup* group) const
1040 {
1041  attached_bodies.clear();
1042  for (const auto& it : attached_body_map_)
1043  if (group->hasLinkModel(it.second->getAttachedLinkName()))
1044  attached_bodies.push_back(it.second.get());
1045 }
1046 
1047 void RobotState::getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const LinkModel* link_model) const
1048 {
1049  attached_bodies.clear();
1050  for (const auto& it : attached_body_map_)
1051  if (it.second->getAttachedLink() == link_model)
1052  attached_bodies.push_back(it.second.get());
1053 }
1054 
1056 {
1057  for (const auto& it : attached_body_map_)
1058  {
1059  if (attached_body_update_callback_)
1060  attached_body_update_callback_(it.second.get(), false);
1061  }
1062  attached_body_map_.clear();
1063 }
1064 
1066 {
1067  for (auto it = attached_body_map_.cbegin(); it != attached_body_map_.cend(); ++it)
1068  {
1069  if (it->second->getAttachedLink() != link)
1070  {
1071  continue;
1072  }
1073  if (attached_body_update_callback_)
1074  attached_body_update_callback_(it->second.get(), false);
1075  const auto del = it++;
1076  attached_body_map_.erase(del);
1077  }
1078 }
1079 
1081 {
1082  for (auto it = attached_body_map_.cbegin(); it != attached_body_map_.cend(); ++it)
1083  {
1084  if (!group->hasLinkModel(it->second->getAttachedLinkName()))
1085  {
1086  continue;
1087  }
1088  if (attached_body_update_callback_)
1089  attached_body_update_callback_(it->second.get(), false);
1090  const auto del = it++;
1091  attached_body_map_.erase(del);
1092  }
1093 }
1094 
1095 bool RobotState::clearAttachedBody(const std::string& id)
1096 {
1097  const auto it = attached_body_map_.find(id);
1098  if (it != attached_body_map_.end())
1099  {
1100  if (attached_body_update_callback_)
1101  attached_body_update_callback_(it->second.get(), false);
1102  attached_body_map_.erase(it);
1103  return true;
1104  }
1105  else
1106  return false;
1107 }
1108 
1109 const Eigen::Isometry3d& RobotState::getFrameTransform(const std::string& frame_id, bool* frame_found)
1110 {
1112  return static_cast<const RobotState*>(this)->getFrameTransform(frame_id, frame_found);
1113 }
1114 
1115 const Eigen::Isometry3d& RobotState::getFrameTransform(const std::string& frame_id, bool* frame_found) const
1116 {
1117  const LinkModel* ignored_link;
1118  bool found;
1119  const auto& result = getFrameInfo(frame_id, ignored_link, found);
1120 
1121  if (frame_found)
1122  *frame_found = found;
1123  else if (!found)
1124  RCLCPP_WARN(LOGGER, "getFrameTransform() did not find a frame with name %s.", frame_id.c_str());
1125 
1126  return result;
1127 }
1128 
1129 const Eigen::Isometry3d& RobotState::getFrameInfo(const std::string& frame_id, const LinkModel*& robot_link,
1130  bool& frame_found) const
1131 {
1132  if (!frame_id.empty() && frame_id[0] == '/')
1133  return getFrameInfo(frame_id.substr(1), robot_link, frame_found);
1134 
1135  static const Eigen::Isometry3d IDENTITY_TRANSFORM = Eigen::Isometry3d::Identity();
1136  if (frame_id == robot_model_->getModelFrame())
1137  {
1138  robot_link = robot_model_->getRootLink();
1139  frame_found = true;
1140  return IDENTITY_TRANSFORM;
1141  }
1142  if ((robot_link = robot_model_->getLinkModel(frame_id, &frame_found)))
1143  {
1144  assert(checkLinkTransforms());
1145  return global_link_transforms_[robot_link->getLinkIndex()];
1146  }
1147  robot_link = nullptr;
1148 
1149  // Check names of the attached bodies
1150  const auto jt = attached_body_map_.find(frame_id);
1151  if (jt != attached_body_map_.end())
1152  {
1153  const Eigen::Isometry3d& transform = jt->second->getGlobalPose();
1154  robot_link = jt->second->getAttachedLink();
1155  frame_found = true;
1156  assert(checkLinkTransforms());
1157  return transform;
1158  }
1159 
1160  // Check if an AttachedBody has a subframe with name frame_id
1161  for (const auto& body : attached_body_map_)
1162  {
1163  const Eigen::Isometry3d& transform = body.second->getGlobalSubframeTransform(frame_id, &frame_found);
1164  if (frame_found)
1165  {
1166  robot_link = body.second->getAttachedLink();
1167  assert(checkLinkTransforms());
1168  return transform;
1169  }
1170  }
1171 
1172  robot_link = nullptr;
1173  frame_found = false;
1174  return IDENTITY_TRANSFORM;
1175 }
1176 
1177 bool RobotState::knowsFrameTransform(const std::string& frame_id) const
1178 {
1179  if (!frame_id.empty() && frame_id[0] == '/')
1180  return knowsFrameTransform(frame_id.substr(1));
1181  if (robot_model_->hasLinkModel(frame_id))
1182  return true;
1183 
1184  // Check if an AttachedBody with name frame_id exists
1185  const auto it = attached_body_map_.find(frame_id);
1186  if (it != attached_body_map_.end())
1187  return !it->second->getGlobalCollisionBodyTransforms().empty();
1188 
1189  // Check if an AttachedBody has a subframe with name frame_id
1190  for (const auto& body : attached_body_map_)
1191  {
1192  if (body.second->hasSubframeTransform(frame_id))
1193  return true;
1194  }
1195  return false;
1196 }
1197 
1198 void RobotState::getRobotMarkers(visualization_msgs::msg::MarkerArray& arr, const std::vector<std::string>& link_names,
1199  const std_msgs::msg::ColorRGBA& color, const std::string& ns,
1200  const rclcpp::Duration& dur, bool include_attached) const
1201 {
1202  std::size_t cur_num = arr.markers.size();
1203  getRobotMarkers(arr, link_names, include_attached);
1204  unsigned int id = cur_num;
1205  for (std::size_t i = cur_num; i < arr.markers.size(); ++i, ++id)
1206  {
1207  arr.markers[i].ns = ns;
1208  arr.markers[i].id = id;
1209  arr.markers[i].lifetime = dur;
1210  arr.markers[i].color = color;
1211  }
1212 }
1213 
1214 void RobotState::getRobotMarkers(visualization_msgs::msg::MarkerArray& arr, const std::vector<std::string>& link_names,
1215  bool include_attached) const
1216 {
1217  rclcpp::Clock clock;
1218  for (const std::string& link_name : link_names)
1219  {
1220  RCLCPP_DEBUG(LOGGER, "Trying to get marker for link '%s'", link_name.c_str());
1221  const LinkModel* link_model = robot_model_->getLinkModel(link_name);
1222  if (!link_model)
1223  continue;
1224  if (include_attached)
1225  for (const auto& it : attached_body_map_)
1226  if (it.second->getAttachedLink() == link_model)
1227  {
1228  for (std::size_t j = 0; j < it.second->getShapes().size(); ++j)
1229  {
1230  visualization_msgs::msg::Marker att_mark;
1231  att_mark.header.frame_id = robot_model_->getModelFrame();
1232  att_mark.header.stamp = clock.now();
1233  if (shapes::constructMarkerFromShape(it.second->getShapes()[j].get(), att_mark))
1234  {
1235  // if the object is invisible (0 volume) we skip it
1236  if (fabs(att_mark.scale.x * att_mark.scale.y * att_mark.scale.z) < std::numeric_limits<float>::epsilon())
1237  continue;
1238  att_mark.pose = tf2::toMsg(it.second->getGlobalCollisionBodyTransforms()[j]);
1239  arr.markers.push_back(att_mark);
1240  }
1241  }
1242  }
1243 
1244  if (link_model->getShapes().empty())
1245  continue;
1246 
1247  for (std::size_t j = 0; j < link_model->getShapes().size(); ++j)
1248  {
1249  visualization_msgs::msg::Marker mark;
1250  mark.header.frame_id = robot_model_->getModelFrame();
1251  mark.header.stamp = clock.now();
1252 
1253  // we prefer using the visual mesh, if a mesh is available and we have one body to render
1254  const std::string& mesh_resource = link_model->getVisualMeshFilename();
1255  if (mesh_resource.empty() || link_model->getShapes().size() > 1)
1256  {
1257  if (!shapes::constructMarkerFromShape(link_model->getShapes()[j].get(), mark))
1258  continue;
1259  // if the object is invisible (0 volume) we skip it
1260  if (fabs(mark.scale.x * mark.scale.y * mark.scale.z) < std::numeric_limits<float>::epsilon())
1261  continue;
1262  mark.pose =
1263  tf2::toMsg(global_collision_body_transforms_[link_model->getFirstCollisionBodyTransformIndex() + j]);
1264  }
1265  else
1266  {
1267  mark.type = mark.MESH_RESOURCE;
1268  mark.mesh_use_embedded_materials = false;
1269  mark.mesh_resource = mesh_resource;
1270  const Eigen::Vector3d& mesh_scale = link_model->getVisualMeshScale();
1271 
1272  mark.scale.x = mesh_scale[0];
1273  mark.scale.y = mesh_scale[1];
1274  mark.scale.z = mesh_scale[2];
1275  mark.pose = tf2::toMsg(global_link_transforms_[link_model->getLinkIndex()] * link_model->getVisualMeshOrigin());
1276  }
1277 
1278  arr.markers.push_back(mark);
1279  }
1280  }
1281 }
1282 
1284  const Eigen::Vector3d& reference_point_position) const
1285 {
1286  Eigen::MatrixXd result;
1287  if (!getJacobian(group, group->getLinkModels().back(), reference_point_position, result, false))
1288  throw Exception("Unable to compute Jacobian");
1289  return result;
1290 }
1291 
1293  const Eigen::Vector3d& reference_point_position, Eigen::MatrixXd& jacobian,
1294  bool use_quaternion_representation) const
1295 {
1296  assert(checkLinkTransforms());
1297 
1298  if (!group->isChain())
1299  {
1300  RCLCPP_ERROR(LOGGER, "The group '%s' is not a chain. Cannot compute Jacobian.", group->getName().c_str());
1301  return false;
1302  }
1303 
1304  if (!group->isLinkUpdated(link->getName()))
1305  {
1306  RCLCPP_ERROR(LOGGER, "Link name '%s' does not exist in the chain '%s' or is not a child for this chain",
1307  link->getName().c_str(), group->getName().c_str());
1308  return false;
1309  }
1310 
1311  const moveit::core::JointModel* root_joint_model = group->getJointModels()[0]; // group->getJointRoots()[0];
1312  const moveit::core::LinkModel* root_link_model = root_joint_model->getParentLinkModel();
1313  // getGlobalLinkTransform() returns a valid isometry by contract
1314  Eigen::Isometry3d reference_transform =
1315  root_link_model ? getGlobalLinkTransform(root_link_model).inverse() : Eigen::Isometry3d::Identity();
1316  int rows = use_quaternion_representation ? 7 : 6;
1317  int columns = group->getVariableCount();
1318  jacobian = Eigen::MatrixXd::Zero(rows, columns);
1319 
1320  // getGlobalLinkTransform() returns a valid isometry by contract
1321  Eigen::Isometry3d link_transform = reference_transform * getGlobalLinkTransform(link); // valid isometry
1322  Eigen::Vector3d point_transform = link_transform * reference_point_position;
1323 
1324  // RCLCPP_DEBUG(LOGGER, "Point from reference origin expressed in world coordinates: %f %f %f",
1325  // point_transform.x(),
1326  // point_transform.y(),
1327  // point_transform.z());
1328 
1329  Eigen::Vector3d joint_axis;
1330  Eigen::Isometry3d joint_transform;
1331 
1332  while (link)
1333  {
1334  // RCLCPP_DEBUG(LOGGER, "Link: %s, %f %f %f",link_state->getName().c_str(),
1335  // link_state->getGlobalLinkTransform().translation().x(),
1336  // link_state->getGlobalLinkTransform().translation().y(),
1337  // link_state->getGlobalLinkTransform().translation().z());
1338  // RCLCPP_DEBUG(LOGGER, "Joint: %s",link_state->getParentJointState()->getName().c_str());
1339 
1340  const JointModel* pjm = link->getParentJointModel();
1341  if (pjm->getVariableCount() > 0)
1342  {
1343  if (!group->hasJointModel(pjm->getName()))
1344  {
1345  link = pjm->getParentLinkModel();
1346  continue;
1347  }
1348  unsigned int joint_index = group->getVariableGroupIndex(pjm->getName());
1349  // getGlobalLinkTransform() returns a valid isometry by contract
1350  joint_transform = reference_transform * getGlobalLinkTransform(link); // valid isometry
1352  {
1353  joint_axis = joint_transform.linear() * static_cast<const moveit::core::RevoluteJointModel*>(pjm)->getAxis();
1354  jacobian.block<3, 1>(0, joint_index) =
1355  jacobian.block<3, 1>(0, joint_index) + joint_axis.cross(point_transform - joint_transform.translation());
1356  jacobian.block<3, 1>(3, joint_index) = jacobian.block<3, 1>(3, joint_index) + joint_axis;
1357  }
1358  else if (pjm->getType() == moveit::core::JointModel::PRISMATIC)
1359  {
1360  joint_axis = joint_transform.linear() * static_cast<const moveit::core::PrismaticJointModel*>(pjm)->getAxis();
1361  jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis;
1362  }
1363  else if (pjm->getType() == moveit::core::JointModel::PLANAR)
1364  {
1365  joint_axis = joint_transform * Eigen::Vector3d(1.0, 0.0, 0.0);
1366  jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis;
1367  joint_axis = joint_transform * Eigen::Vector3d(0.0, 1.0, 0.0);
1368  jacobian.block<3, 1>(0, joint_index + 1) = jacobian.block<3, 1>(0, joint_index + 1) + joint_axis;
1369  joint_axis = joint_transform * Eigen::Vector3d(0.0, 0.0, 1.0);
1370  jacobian.block<3, 1>(0, joint_index + 2) = jacobian.block<3, 1>(0, joint_index + 2) +
1371  joint_axis.cross(point_transform - joint_transform.translation());
1372  jacobian.block<3, 1>(3, joint_index + 2) = jacobian.block<3, 1>(3, joint_index + 2) + joint_axis;
1373  }
1374  else
1375  RCLCPP_ERROR(LOGGER, "Unknown type of joint in Jacobian computation");
1376  }
1377  if (pjm == root_joint_model)
1378  break;
1379  link = pjm->getParentLinkModel();
1380  }
1381  if (use_quaternion_representation)
1382  { // Quaternion representation
1383  // From "Advanced Dynamics and Motion Simulation" by Paul Mitiguy
1384  // d/dt ( [w] ) = 1/2 * [ -x -y -z ] * [ omega_1 ]
1385  // [x] [ w -z y ] [ omega_2 ]
1386  // [y] [ z w -x ] [ omega_3 ]
1387  // [z] [ -y x w ]
1388  Eigen::Quaterniond q(link_transform.linear());
1389  double w = q.w(), x = q.x(), y = q.y(), z = q.z();
1390  Eigen::MatrixXd quaternion_update_matrix(4, 3);
1391  quaternion_update_matrix << -x, -y, -z, w, -z, y, z, w, -x, -y, x, w;
1392  jacobian.block(3, 0, 4, columns) = 0.5 * quaternion_update_matrix * jacobian.block(3, 0, 3, columns);
1393  }
1394  return true;
1395 }
1396 
1397 bool RobotState::setFromDiffIK(const JointModelGroup* jmg, const Eigen::VectorXd& twist, const std::string& tip,
1398  double dt, const GroupStateValidityCallbackFn& constraint)
1399 {
1400  Eigen::VectorXd qdot;
1401  computeVariableVelocity(jmg, qdot, twist, getLinkModel(tip));
1402  return integrateVariableVelocity(jmg, qdot, dt, constraint);
1403 }
1404 
1405 bool RobotState::setFromDiffIK(const JointModelGroup* jmg, const geometry_msgs::msg::Twist& twist,
1406  const std::string& tip, double dt, const GroupStateValidityCallbackFn& constraint)
1407 {
1408  Eigen::Matrix<double, 6, 1> t;
1409  tf2::fromMsg(twist, t);
1410  return setFromDiffIK(jmg, t, tip, dt, constraint);
1411 }
1412 
1413 void RobotState::computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot,
1414  const Eigen::VectorXd& twist, const LinkModel* tip) const
1415 {
1416  // Get the Jacobian of the group at the current configuration
1417  Eigen::MatrixXd j(6, jmg->getVariableCount());
1418  Eigen::Vector3d reference_point(0.0, 0.0, 0.0);
1419  getJacobian(jmg, tip, reference_point, j, false);
1420 
1421  // Rotate the jacobian to the end-effector frame
1422  Eigen::Isometry3d e_mb = getGlobalLinkTransform(tip).inverse();
1423  Eigen::MatrixXd e_wb = Eigen::ArrayXXd::Zero(6, 6);
1424  e_wb.block(0, 0, 3, 3) = e_mb.matrix().block(0, 0, 3, 3);
1425  e_wb.block(3, 3, 3, 3) = e_mb.matrix().block(0, 0, 3, 3);
1426  j = e_wb * j;
1427 
1428  // Do the Jacobian moore-penrose pseudo-inverse
1429  Eigen::JacobiSVD<Eigen::MatrixXd> svd_of_j(j, Eigen::ComputeThinU | Eigen::ComputeThinV);
1430  const Eigen::MatrixXd& u = svd_of_j.matrixU();
1431  const Eigen::MatrixXd& v = svd_of_j.matrixV();
1432  const Eigen::VectorXd& s = svd_of_j.singularValues();
1433 
1434  Eigen::VectorXd sinv = s;
1435  static const double PINVTOLER = std::numeric_limits<float>::epsilon();
1436  double maxsv = 0.0;
1437  for (std::size_t i = 0; i < static_cast<std::size_t>(s.rows()); ++i)
1438  if (fabs(s(i)) > maxsv)
1439  maxsv = fabs(s(i));
1440  for (std::size_t i = 0; i < static_cast<std::size_t>(s.rows()); ++i)
1441  {
1442  // Those singular values smaller than a percentage of the maximum singular value are removed
1443  if (fabs(s(i)) > maxsv * PINVTOLER)
1444  sinv(i) = 1.0 / s(i);
1445  else
1446  sinv(i) = 0.0;
1447  }
1448  Eigen::MatrixXd jinv = (v * sinv.asDiagonal() * u.transpose());
1449 
1450  // Compute joint velocity
1451  qdot = jinv * twist;
1452 }
1453 
1454 bool RobotState::integrateVariableVelocity(const JointModelGroup* jmg, const Eigen::VectorXd& qdot, double dt,
1455  const GroupStateValidityCallbackFn& constraint)
1456 {
1457  Eigen::VectorXd q(jmg->getVariableCount());
1458  copyJointGroupPositions(jmg, q);
1459  q = q + dt * qdot;
1460  setJointGroupPositions(jmg, q);
1461  enforceBounds(jmg);
1462 
1463  if (constraint)
1464  {
1465  std::vector<double> values;
1466  copyJointGroupPositions(jmg, values);
1467  return constraint(this, jmg, &values[0]);
1468  }
1469  else
1470  return true;
1471 }
1472 
1473 bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::msg::Pose& pose, double timeout,
1474  const GroupStateValidityCallbackFn& constraint,
1476  const kinematics::KinematicsBase::IKCostFn& cost_function)
1477 {
1478  const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance();
1479  if (!solver)
1480  {
1481  RCLCPP_ERROR(LOGGER, "No kinematics solver instantiated for group '%s'", jmg->getName().c_str());
1482  return false;
1483  }
1484  return setFromIK(jmg, pose, solver->getTipFrame(), timeout, constraint, options, cost_function);
1485 }
1486 
1487 bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::msg::Pose& pose, const std::string& tip,
1488  double timeout, const GroupStateValidityCallbackFn& constraint,
1490  const kinematics::KinematicsBase::IKCostFn& cost_function)
1491 {
1492  Eigen::Isometry3d mat;
1493  tf2::fromMsg(pose, mat);
1494  static std::vector<double> consistency_limits;
1495  return setFromIK(jmg, mat, tip, consistency_limits, timeout, constraint, options, cost_function);
1496 }
1497 
1498 bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Isometry3d& pose, double timeout,
1499  const GroupStateValidityCallbackFn& constraint,
1501  const kinematics::KinematicsBase::IKCostFn& cost_function)
1502 {
1503  const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance();
1504  if (!solver)
1505  {
1506  RCLCPP_ERROR(LOGGER, "No kinematics solver instantiated for group '%s'", jmg->getName().c_str());
1507  return false;
1508  }
1509  static std::vector<double> consistency_limits;
1510  return setFromIK(jmg, pose, solver->getTipFrame(), consistency_limits, timeout, constraint, options, cost_function);
1511 }
1512 
1513 bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Isometry3d& pose_in, const std::string& tip_in,
1514  double timeout, const GroupStateValidityCallbackFn& constraint,
1516  const kinematics::KinematicsBase::IKCostFn& cost_function)
1517 {
1518  static std::vector<double> consistency_limits;
1519  return setFromIK(jmg, pose_in, tip_in, consistency_limits, timeout, constraint, options, cost_function);
1520 }
1521 
1522 namespace
1523 {
1524 bool ikCallbackFnAdapter(RobotState* state, const JointModelGroup* group,
1525  const GroupStateValidityCallbackFn& constraint, const geometry_msgs::msg::Pose& /*unused*/,
1526  const std::vector<double>& ik_sol, moveit_msgs::msg::MoveItErrorCodes& error_code)
1527 {
1528  const std::vector<size_t>& bij = group->getKinematicsSolverJointBijection();
1529  std::vector<double> solution(bij.size());
1530  for (std::size_t i = 0; i < bij.size(); ++i)
1531  solution[bij[i]] = ik_sol[i];
1532  if (constraint(state, group, &solution[0]))
1533  error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
1534  else
1535  error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION;
1536  return true;
1537 }
1538 } // namespace
1539 
1540 bool RobotState::setToIKSolverFrame(Eigen::Isometry3d& pose, const kinematics::KinematicsBaseConstPtr& solver)
1541 {
1542  return setToIKSolverFrame(pose, solver->getBaseFrame());
1543 }
1544 
1545 bool RobotState::setToIKSolverFrame(Eigen::Isometry3d& pose, const std::string& ik_frame)
1546 {
1547  // Bring the pose to the frame of the IK solver
1548  if (!Transforms::sameFrame(ik_frame, robot_model_->getModelFrame()))
1549  {
1550  const LinkModel* link_model =
1551  getLinkModel((!ik_frame.empty() && ik_frame[0] == '/') ? ik_frame.substr(1) : ik_frame);
1552  if (!link_model)
1553  {
1554  RCLCPP_ERROR(LOGGER, "The following IK frame does not exist: %s", ik_frame.c_str());
1555  return false;
1556  }
1557  pose = getGlobalLinkTransform(link_model).inverse() * pose;
1558  }
1559  return true;
1560 }
1561 
1562 bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Isometry3d& pose_in, const std::string& tip_in,
1563  const std::vector<double>& consistency_limits_in, double timeout,
1564  const GroupStateValidityCallbackFn& constraint,
1566  const kinematics::KinematicsBase::IKCostFn& cost_function)
1567 {
1568  // Convert from single pose and tip to vectors
1569  EigenSTL::vector_Isometry3d poses;
1570  poses.push_back(pose_in);
1571 
1572  std::vector<std::string> tips;
1573  tips.push_back(tip_in);
1574 
1575  std::vector<std::vector<double> > consistency_limits;
1576  consistency_limits.push_back(consistency_limits_in);
1577 
1578  return setFromIK(jmg, poses, tips, consistency_limits, timeout, constraint, options, cost_function);
1579 }
1580 
1581 bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Isometry3d& poses_in,
1582  const std::vector<std::string>& tips_in, double timeout,
1583  const GroupStateValidityCallbackFn& constraint,
1585  const kinematics::KinematicsBase::IKCostFn& cost_function)
1586 {
1587  const std::vector<std::vector<double> > consistency_limits;
1588  return setFromIK(jmg, poses_in, tips_in, consistency_limits, timeout, constraint, options, cost_function);
1589 }
1590 
1591 bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Isometry3d& poses_in,
1592  const std::vector<std::string>& tips_in,
1593  const std::vector<std::vector<double> >& consistency_limit_sets, double timeout,
1594  const GroupStateValidityCallbackFn& constraint,
1596  const kinematics::KinematicsBase::IKCostFn& cost_function)
1597 {
1598  // Error check
1599  if (poses_in.size() != tips_in.size())
1600  {
1601  RCLCPP_ERROR(LOGGER, "Number of poses must be the same as number of tips");
1602  return false;
1603  }
1604 
1605  // Load solver
1606  const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance();
1607 
1608  // Check if this jmg has a solver
1609  bool valid_solver = true;
1610  if (!solver)
1611  {
1612  valid_solver = false;
1613  }
1614  // Check if this jmg's IK solver can handle multiple tips (non-chain solver)
1615  else if (poses_in.size() > 1)
1616  {
1617  std::string error_msg;
1618  if (!solver->supportsGroup(jmg, &error_msg))
1619  {
1620  const kinematics::KinematicsBase& solver_ref = *solver;
1621  RCLCPP_ERROR(LOGGER, "Kinematics solver %s does not support joint group %s. Error: %s",
1622  typeid(solver_ref).name(), jmg->getName().c_str(), error_msg.c_str());
1623  valid_solver = false;
1624  }
1625  }
1626 
1627  if (!valid_solver)
1628  {
1629  // Check if there are subgroups that can solve this for us (non-chains)
1630  if (poses_in.size() > 1)
1631  {
1632  // Forward to setFromIKSubgroups() to allow different subgroup IK solvers to work together
1633  return setFromIKSubgroups(jmg, poses_in, tips_in, consistency_limit_sets, timeout, constraint, options);
1634  }
1635  else
1636  {
1637  RCLCPP_ERROR(LOGGER, "No kinematics solver instantiated for group '%s'", jmg->getName().c_str());
1638  return false;
1639  }
1640  }
1641 
1642  // Check that no, or only one set of consistency limits has been passed in, and choose that one
1643  std::vector<double> consistency_limits;
1644  if (consistency_limit_sets.size() > 1)
1645  {
1646  RCLCPP_ERROR(LOGGER,
1647  "Invalid number (%zu) of sets of consistency limits for a setFromIK request "
1648  "that is being solved by a single IK solver",
1649  consistency_limit_sets.size());
1650  return false;
1651  }
1652  else if (consistency_limit_sets.size() == 1)
1653  consistency_limits = consistency_limit_sets[0];
1654 
1655  const std::vector<std::string>& solver_tip_frames = solver->getTipFrames();
1656 
1657  // Track which possible tips frames we have filled in so far
1658  std::vector<bool> tip_frames_used(solver_tip_frames.size(), false);
1659 
1660  // Create vector to hold the output frames in the same order as solver_tip_frames
1661  std::vector<geometry_msgs::msg::Pose> ik_queries(solver_tip_frames.size());
1662 
1663  // Bring each pose to the frame of the IK solver
1664  for (std::size_t i = 0; i < poses_in.size(); ++i)
1665  {
1666  // Make non-const
1667  Eigen::Isometry3d pose = poses_in[i];
1668  std::string pose_frame = tips_in[i];
1669 
1670  // Remove extra slash
1671  if (!pose_frame.empty() && pose_frame[0] == '/')
1672  pose_frame = pose_frame.substr(1);
1673 
1674  // bring the pose to the frame of the IK solver
1675  if (!setToIKSolverFrame(pose, solver))
1676  return false;
1677 
1678  // try all of the solver's possible tip frames to see if they uniquely align with any of our passed in pose tip
1679  // frames
1680  bool found_valid_frame = false;
1681  std::size_t solver_tip_id; // our current index
1682  for (solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id)
1683  {
1684  // Check if this tip frame is already accounted for
1685  if (tip_frames_used[solver_tip_id])
1686  continue; // already has a pose
1687 
1688  // check if the tip frame can be transformed via fixed transforms to the frame known to the IK solver
1689  std::string solver_tip_frame = solver_tip_frames[solver_tip_id];
1690 
1691  // remove the frame '/' if there is one, so we can avoid calling Transforms::sameFrame() which may copy strings
1692  // more often that we need to
1693  if (!solver_tip_frame.empty() && solver_tip_frame[0] == '/')
1694  solver_tip_frame = solver_tip_frame.substr(1);
1695 
1696  if (pose_frame != solver_tip_frame)
1697  {
1698  auto* pose_parent = getRigidlyConnectedParentLinkModel(pose_frame);
1699  if (!pose_parent)
1700  {
1701  RCLCPP_ERROR_STREAM(LOGGER, "The following Pose Frame does not exist: " << pose_frame);
1702  return false;
1703  }
1704  Eigen::Isometry3d pose_parent_to_frame = getFrameTransform(pose_frame);
1705  auto* tip_parent = getRigidlyConnectedParentLinkModel(solver_tip_frame);
1706  if (!tip_parent)
1707  {
1708  RCLCPP_ERROR_STREAM(LOGGER, "The following Solver Tip Frame does not exist: " << solver_tip_frame);
1709  return false;
1710  }
1711  Eigen::Isometry3d tip_parent_to_tip = getFrameTransform(solver_tip_frame);
1712  if (pose_parent == tip_parent)
1713  {
1714  // transform goal pose as target for solver_tip_frame (instead of pose_frame)
1715  pose = pose * pose_parent_to_frame.inverse() * tip_parent_to_tip;
1716  found_valid_frame = true;
1717  break;
1718  }
1719  }
1720  else
1721  {
1722  found_valid_frame = true;
1723  break;
1724  } // end if pose_frame
1725  } // end for solver_tip_frames
1726 
1727  // Make sure one of the tip frames worked
1728  if (!found_valid_frame)
1729  {
1730  RCLCPP_ERROR(LOGGER, "Cannot compute IK for query %zu pose reference frame '%s'", i, pose_frame.c_str());
1731  // Debug available tip frames
1732  std::stringstream ss;
1733  for (solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id)
1734  ss << solver_tip_frames[solver_tip_id] << ", ";
1735  RCLCPP_ERROR(LOGGER, "Available tip frames: [%s]", ss.str().c_str());
1736  return false;
1737  }
1738 
1739  // Remove that tip from the list of available tip frames because each can only have one pose
1740  tip_frames_used[solver_tip_id] = true;
1741 
1742  // Convert Eigen pose to geometry_msgs pose
1743  geometry_msgs::msg::Pose ik_query;
1744  ik_query = tf2::toMsg(pose);
1745 
1746  // Save into vectors
1747  ik_queries[solver_tip_id] = ik_query;
1748  } // end for poses_in
1749 
1750  // Create poses for all remaining tips a solver expects, even if not passed into this function
1751  for (std::size_t solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id)
1752  {
1753  // Check if this tip frame is already accounted for
1754  if (tip_frames_used[solver_tip_id])
1755  continue; // already has a pose
1756 
1757  // Process this tip
1758  std::string solver_tip_frame = solver_tip_frames[solver_tip_id];
1759 
1760  // remove the frame '/' if there is one, so we can avoid calling Transforms::sameFrame() which may copy strings more
1761  // often that we need to
1762  if (!solver_tip_frame.empty() && solver_tip_frame[0] == '/')
1763  solver_tip_frame = solver_tip_frame.substr(1);
1764 
1765  // Get the pose of a different EE tip link
1766  Eigen::Isometry3d current_pose = getGlobalLinkTransform(solver_tip_frame);
1767 
1768  // bring the pose to the frame of the IK solver
1769  if (!setToIKSolverFrame(current_pose, solver))
1770  return false;
1771 
1772  // Convert Eigen pose to geometry_msgs pose
1773  geometry_msgs::msg::Pose ik_query;
1774  ik_query = tf2::toMsg(current_pose);
1775 
1776  // Save into vectors - but this needs to be ordered in the same order as the IK solver expects its tip frames
1777  ik_queries[solver_tip_id] = ik_query;
1778 
1779  // Remove that tip from the list of available tip frames because each can only have one pose
1780  tip_frames_used[solver_tip_id] = true;
1781  }
1782 
1783  // if no timeout has been specified, use the default one
1784  if (timeout < std::numeric_limits<double>::epsilon())
1785  timeout = jmg->getDefaultIKTimeout();
1786 
1787  // set callback function
1789  if (constraint)
1790  ik_callback_fn = ik_callback_fn = [this, jmg, constraint](const geometry_msgs::msg::Pose& pose,
1791  const std::vector<double>& joints,
1792  moveit_msgs::msg::MoveItErrorCodes& error_code) {
1793  ikCallbackFnAdapter(this, jmg, constraint, pose, joints, error_code);
1794  };
1795 
1796  // Bijection
1797  const std::vector<size_t>& bij = jmg->getKinematicsSolverJointBijection();
1798 
1799  std::vector<double> initial_values;
1800  copyJointGroupPositions(jmg, initial_values);
1801  std::vector<double> seed(bij.size());
1802  for (std::size_t i = 0; i < bij.size(); ++i)
1803  seed[i] = initial_values[bij[i]];
1804 
1805  // compute the IK solution
1806  std::vector<double> ik_sol;
1807  moveit_msgs::msg::MoveItErrorCodes error;
1808 
1809  if (solver->searchPositionIK(ik_queries, seed, timeout, consistency_limits, ik_sol, ik_callback_fn, cost_function,
1810  error, options, this))
1811  {
1812  std::vector<double> solution(bij.size());
1813  for (std::size_t i = 0; i < bij.size(); ++i)
1814  solution[bij[i]] = ik_sol[i];
1815  setJointGroupPositions(jmg, solution);
1816  return true;
1817  }
1818  return false;
1819 }
1820 
1821 bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL::vector_Isometry3d& poses_in,
1822  const std::vector<std::string>& tips_in,
1823  const std::vector<std::vector<double> >& consistency_limits, double timeout,
1824  const GroupStateValidityCallbackFn& constraint,
1825  const kinematics::KinematicsQueryOptions& /*options*/)
1826 {
1827  // Assume we have already ran setFromIK() and those checks
1828 
1829  // Get containing subgroups
1830  std::vector<const JointModelGroup*> sub_groups;
1831  jmg->getSubgroups(sub_groups);
1832 
1833  // Error check
1834  if (poses_in.size() != sub_groups.size())
1835  {
1836  RCLCPP_ERROR(LOGGER, "Number of poses (%zu) must be the same as number of sub-groups (%zu)", poses_in.size(),
1837  sub_groups.size());
1838  return false;
1839  }
1840 
1841  if (tips_in.size() != sub_groups.size())
1842  {
1843  RCLCPP_ERROR(LOGGER, "Number of tip names (%zu) must be same as number of sub-groups (%zu)", tips_in.size(),
1844  sub_groups.size());
1845  return false;
1846  }
1847 
1848  if (!consistency_limits.empty() && consistency_limits.size() != sub_groups.size())
1849  {
1850  RCLCPP_ERROR(LOGGER, "Number of consistency limit vectors must be the same as number of sub-groups");
1851  return false;
1852  }
1853 
1854  for (std::size_t i = 0; i < consistency_limits.size(); ++i)
1855  {
1856  if (consistency_limits[i].size() != sub_groups[i]->getVariableCount())
1857  {
1858  RCLCPP_ERROR(LOGGER, "Number of joints in consistency_limits[%zu] is %lu but it should be should be %u", i,
1859  consistency_limits[i].size(), sub_groups[i]->getVariableCount());
1860  return false;
1861  }
1862  }
1863 
1864  // Populate list of kin solvers for the various subgroups
1865  std::vector<kinematics::KinematicsBaseConstPtr> solvers;
1866  for (std::size_t i = 0; i < poses_in.size(); ++i)
1867  {
1868  kinematics::KinematicsBaseConstPtr solver = sub_groups[i]->getSolverInstance();
1869  if (!solver)
1870  {
1871  RCLCPP_ERROR(LOGGER, "Could not find solver for group '%s'", sub_groups[i]->getName().c_str());
1872  return false;
1873  }
1874  solvers.push_back(solver);
1875  }
1876 
1877  // Make non-const versions
1878  EigenSTL::vector_Isometry3d transformed_poses = poses_in;
1879  std::vector<std::string> pose_frames = tips_in;
1880 
1881  // Each each pose's tip frame naming
1882  for (std::size_t i = 0; i < poses_in.size(); ++i)
1883  {
1884  ASSERT_ISOMETRY(transformed_poses[i]) // unsanitized input, could contain a non-isometry
1885  Eigen::Isometry3d& pose = transformed_poses[i];
1886  std::string& pose_frame = pose_frames[i];
1887 
1888  // bring the pose to the frame of the IK solver
1889  if (!setToIKSolverFrame(pose, solvers[i]))
1890  return false;
1891 
1892  // see if the tip frame can be transformed via fixed transforms to the frame known to the IK solver
1893  std::string solver_tip_frame = solvers[i]->getTipFrame();
1894 
1895  // remove the frame '/' if there is one, so we can avoid calling Transforms::sameFrame() which may copy strings more
1896  // often that we need to
1897  if (!solver_tip_frame.empty() && solver_tip_frame[0] == '/')
1898  solver_tip_frame = solver_tip_frame.substr(1);
1899 
1900  if (pose_frame != solver_tip_frame)
1901  {
1902  if (hasAttachedBody(pose_frame))
1903  {
1904  const AttachedBody* body = getAttachedBody(pose_frame);
1905  pose_frame = body->getAttachedLinkName();
1906  pose = pose * body->getPose().inverse(); // valid isometry
1907  }
1908  if (pose_frame != solver_tip_frame)
1909  {
1910  const moveit::core::LinkModel* link_model = getLinkModel(pose_frame);
1911  if (!link_model)
1912  return false;
1913  // getAssociatedFixedTransforms() returns valid isometries by contract
1914  const moveit::core::LinkTransformMap& fixed_links = link_model->getAssociatedFixedTransforms();
1915  for (const std::pair<const LinkModel* const, Eigen::Isometry3d>& fixed_link : fixed_links)
1916  if (fixed_link.first->getName() == solver_tip_frame)
1917  {
1918  pose_frame = solver_tip_frame;
1919  pose = pose * fixed_link.second; // valid isometry
1920  break;
1921  }
1922  }
1923  }
1924 
1925  if (pose_frame != solver_tip_frame)
1926  {
1927  RCLCPP_ERROR(LOGGER, "Cannot compute IK for query pose reference frame '%s', desired: '%s'", pose_frame.c_str(),
1928  solver_tip_frame.c_str());
1929  return false;
1930  }
1931  }
1932 
1933  // Convert Eigen poses to geometry_msg format
1934  std::vector<geometry_msgs::msg::Pose> ik_queries(poses_in.size());
1936  if (constraint)
1937  ik_callback_fn = [this, jmg, constraint](const geometry_msgs::msg::Pose pose, const std::vector<double>& joints,
1938  moveit_msgs::msg::MoveItErrorCodes& error_code) {
1939  ikCallbackFnAdapter(this, jmg, constraint, pose, joints, error_code);
1940  };
1941 
1942  for (std::size_t i = 0; i < transformed_poses.size(); ++i)
1943  {
1944  Eigen::Quaterniond quat(transformed_poses[i].linear());
1945  Eigen::Vector3d point(transformed_poses[i].translation());
1946  ik_queries[i].position.x = point.x();
1947  ik_queries[i].position.y = point.y();
1948  ik_queries[i].position.z = point.z();
1949  ik_queries[i].orientation.x = quat.x();
1950  ik_queries[i].orientation.y = quat.y();
1951  ik_queries[i].orientation.z = quat.z();
1952  ik_queries[i].orientation.w = quat.w();
1953  }
1954 
1955  // if no timeout has been specified, use the default one
1956  if (timeout < std::numeric_limits<double>::epsilon())
1957  timeout = jmg->getDefaultIKTimeout();
1958 
1959  auto start = std::chrono::system_clock::now();
1960  double elapsed = 0;
1961 
1962  bool first_seed = true;
1963  unsigned int attempts = 0;
1964  do
1965  {
1966  ++attempts;
1967  RCLCPP_DEBUG(LOGGER, "IK attempt: %d", attempts);
1968  bool found_solution = true;
1969  for (std::size_t sg = 0; sg < sub_groups.size(); ++sg)
1970  {
1971  const std::vector<size_t>& bij = sub_groups[sg]->getKinematicsSolverJointBijection();
1972  std::vector<double> seed(bij.size());
1973  // the first seed is the initial state
1974  if (first_seed)
1975  {
1976  std::vector<double> initial_values;
1977  copyJointGroupPositions(sub_groups[sg], initial_values);
1978  for (std::size_t i = 0; i < bij.size(); ++i)
1979  seed[i] = initial_values[bij[i]];
1980  }
1981  else
1982  {
1983  // sample a random seed
1984  random_numbers::RandomNumberGenerator& rng = getRandomNumberGenerator();
1985  std::vector<double> random_values;
1986  sub_groups[sg]->getVariableRandomPositions(rng, random_values);
1987  for (std::size_t i = 0; i < bij.size(); ++i)
1988  seed[i] = random_values[bij[i]];
1989  }
1990 
1991  // compute the IK solution
1992  std::vector<double> ik_sol;
1993  moveit_msgs::msg::MoveItErrorCodes error;
1994  const std::vector<double>& climits = consistency_limits.empty() ? std::vector<double>() : consistency_limits[sg];
1995  if (solvers[sg]->searchPositionIK(ik_queries[sg], seed, (timeout - elapsed) / sub_groups.size(), climits, ik_sol,
1996  error))
1997  {
1998  std::vector<double> solution(bij.size());
1999  for (std::size_t i = 0; i < bij.size(); ++i)
2000  solution[bij[i]] = ik_sol[i];
2001  setJointGroupPositions(sub_groups[sg], solution);
2002  }
2003  else
2004  {
2005  found_solution = false;
2006  break;
2007  }
2008  }
2009  if (found_solution)
2010  {
2011  std::vector<double> full_solution;
2012  copyJointGroupPositions(jmg, full_solution);
2013  if (constraint ? constraint(this, jmg, &full_solution[0]) : true)
2014  {
2015  RCLCPP_DEBUG(LOGGER, "Found IK solution");
2016  return true;
2017  }
2018  }
2019  elapsed = std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now() - start).count();
2020  first_seed = false;
2021  } while (elapsed < timeout);
2022  return false;
2023 }
2024 
2025 double RobotState::computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
2026  const LinkModel* link, const Eigen::Vector3d& direction,
2027  bool global_reference_frame, double distance, double max_step,
2028  double jump_threshold_factor, const GroupStateValidityCallbackFn& validCallback,
2030 {
2031  return CartesianInterpolator::computeCartesianPath(this, group, traj, link, direction, global_reference_frame,
2032  distance, MaxEEFStep(max_step),
2033  JumpThreshold(jump_threshold_factor), validCallback, options);
2034 }
2035 
2036 double RobotState::computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
2037  const LinkModel* link, const Eigen::Isometry3d& target,
2038  bool global_reference_frame, double max_step, double jump_threshold_factor,
2039  const GroupStateValidityCallbackFn& validCallback,
2041 {
2042  return CartesianInterpolator::computeCartesianPath(this, group, traj, link, target, global_reference_frame,
2043  MaxEEFStep(max_step), JumpThreshold(jump_threshold_factor),
2044  validCallback, options);
2045 }
2046 
2047 double RobotState::computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
2048  const LinkModel* link, const EigenSTL::vector_Isometry3d& waypoints,
2049  bool global_reference_frame, double max_step, double jump_threshold_factor,
2050  const GroupStateValidityCallbackFn& validCallback,
2052 {
2053  return CartesianInterpolator::computeCartesianPath(this, group, traj, link, waypoints, global_reference_frame,
2054  MaxEEFStep(max_step), JumpThreshold(jump_threshold_factor),
2055  validCallback, options);
2056 }
2057 
2058 void RobotState::computeAABB(std::vector<double>& aabb) const
2059 {
2060  assert(checkLinkTransforms());
2061 
2062  core::AABB bounding_box;
2063  std::vector<const LinkModel*> links = robot_model_->getLinkModelsWithCollisionGeometry();
2064  for (const LinkModel* link : links)
2065  {
2066  Eigen::Isometry3d transform = getGlobalLinkTransform(link); // intentional copy, we will translate
2067  const Eigen::Vector3d& extents = link->getShapeExtentsAtOrigin();
2068  transform.translate(link->getCenteredBoundingBoxOffset());
2069  bounding_box.extendWithTransformedBox(transform, extents);
2070  }
2071  for (const auto& it : attached_body_map_)
2072  {
2073  const EigenSTL::vector_Isometry3d& transforms = it.second->getGlobalCollisionBodyTransforms();
2074  const std::vector<shapes::ShapeConstPtr>& shapes = it.second->getShapes();
2075  for (std::size_t i = 0; i < transforms.size(); ++i)
2076  {
2077  Eigen::Vector3d extents = shapes::computeShapeExtents(shapes[i].get());
2078  bounding_box.extendWithTransformedBox(transforms[i], extents);
2079  }
2080  }
2081 
2082  aabb.clear();
2083  aabb.resize(6, 0.0);
2084  if (!bounding_box.isEmpty())
2085  {
2086  // The following is a shorthand for something like:
2087  // aabb[0, 2, 4] = bounding_box.min(); aabb[1, 3, 5] = bounding_box.max();
2088  Eigen::Map<Eigen::VectorXd, Eigen::Unaligned, Eigen::InnerStride<2> >(aabb.data(), 3) = bounding_box.min();
2089  Eigen::Map<Eigen::VectorXd, Eigen::Unaligned, Eigen::InnerStride<2> >(aabb.data() + 1, 3) = bounding_box.max();
2090  }
2091 }
2092 
2093 void RobotState::printStatePositions(std::ostream& out) const
2094 {
2095  const std::vector<std::string>& nm = robot_model_->getVariableNames();
2096  for (std::size_t i = 0; i < nm.size(); ++i)
2097  out << nm[i] << "=" << position_[i] << '\n';
2098 }
2099 
2101 {
2102  // TODO(davetcoleman): support joints with multiple variables / multiple DOFs such as floating joints
2103  // TODO(davetcoleman): support unbounded joints
2104 
2105  const std::vector<const moveit::core::JointModel*>& joints = jmg->getActiveJointModels();
2106 
2107  // Loop through joints
2108  for (const JointModel* joint : joints)
2109  {
2110  // Ignore joints with more than one variable
2111  if (joint->getVariableCount() > 1)
2112  continue;
2113 
2114  double current_value = getVariablePosition(joint->getName());
2115 
2116  // check if joint is beyond limits
2117  bool out_of_bounds = !satisfiesBounds(joint);
2118 
2119  const moveit::core::VariableBounds& bound = joint->getVariableBounds()[0];
2120 
2121  if (out_of_bounds)
2122  out << MOVEIT_CONSOLE_COLOR_RED;
2123 
2124  out << " " << std::fixed << std::setprecision(5) << bound.min_position_ << "\t";
2125  double delta = bound.max_position_ - bound.min_position_;
2126  double step = delta / 20.0;
2127 
2128  bool marker_shown = false;
2129  for (double value = bound.min_position_; value < bound.max_position_; value += step)
2130  {
2131  // show marker of current value
2132  if (!marker_shown && current_value < value)
2133  {
2134  out << "|";
2135  marker_shown = true;
2136  }
2137  else
2138  out << "-";
2139  }
2140  if (!marker_shown)
2141  out << "|";
2142 
2143  // show max position
2144  out << " \t" << std::fixed << std::setprecision(5) << bound.max_position_ << " \t" << joint->getName()
2145  << " current: " << std::fixed << std::setprecision(5) << current_value << '\n';
2146 
2147  if (out_of_bounds)
2149  }
2150 }
2151 
2152 void RobotState::printDirtyInfo(std::ostream& out) const
2153 {
2154  out << " * Dirty Joint Transforms: \n";
2155  const std::vector<const JointModel*>& jm = robot_model_->getJointModels();
2156  for (const JointModel* joint : jm)
2157  if (joint->getVariableCount() > 0 && dirtyJointTransform(joint))
2158  out << " " << joint->getName() << '\n';
2159  out << " * Dirty Link Transforms: " << (dirty_link_transforms_ ? dirty_link_transforms_->getName() : "NULL") << '\n';
2160  out << " * Dirty Collision Body Transforms: "
2161  << (dirty_collision_body_transforms_ ? dirty_collision_body_transforms_->getName() : "NULL\n");
2162 }
2163 
2164 void RobotState::printStateInfo(std::ostream& out) const
2165 {
2166  out << "Robot State @" << this << '\n';
2167 
2168  std::size_t n = robot_model_->getVariableCount();
2169  if (position_)
2170  {
2171  out << " * Position: ";
2172  for (std::size_t i = 0; i < n; ++i)
2173  out << position_[i] << " ";
2174  out << '\n';
2175  }
2176  else
2177  out << " * Position: NULL\n";
2178 
2179  if (velocity_)
2180  {
2181  out << " * Velocity: ";
2182  for (std::size_t i = 0; i < n; ++i)
2183  out << velocity_[i] << " ";
2184  out << '\n';
2185  }
2186  else
2187  out << " * Velocity: NULL\n";
2188 
2189  if (acceleration_)
2190  {
2191  out << " * Acceleration: ";
2192  for (std::size_t i = 0; i < n; ++i)
2193  out << acceleration_[i] << " ";
2194  out << '\n';
2195  }
2196  else
2197  out << " * Acceleration: NULL\n";
2198 
2199  out << " * Dirty Link Transforms: " << (dirty_link_transforms_ ? dirty_link_transforms_->getName() : "NULL\n");
2200  out << " * Dirty Collision Body Transforms: "
2201  << (dirty_collision_body_transforms_ ? dirty_collision_body_transforms_->getName() : "NULL\n");
2202 
2203  printTransforms(out);
2204 }
2205 
2206 void RobotState::printTransform(const Eigen::Isometry3d& transform, std::ostream& out) const
2207 {
2208  if (checkIsometry(transform, CHECK_ISOMETRY_PRECISION, false))
2209  {
2210  Eigen::Quaterniond q(transform.linear());
2211  out << "T.xyz = [" << transform.translation().x() << ", " << transform.translation().y() << ", "
2212  << transform.translation().z() << "], Q.xyzw = [" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w()
2213  << "]";
2214  }
2215  else
2216  {
2217  out << "[NON-ISOMETRY] "
2218  << transform.matrix().format(
2219  Eigen::IOFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", "; ", "", "", "[", "]"));
2220  }
2221  out << "\n";
2222 }
2223 
2224 void RobotState::printTransforms(std::ostream& out) const
2225 {
2226  if (!variable_joint_transforms_)
2227  {
2228  out << "No transforms computed\n";
2229  return;
2230  }
2231 
2232  out << "Joint transforms:\n";
2233  const std::vector<const JointModel*>& jm = robot_model_->getJointModels();
2234  for (const JointModel* joint : jm)
2235  {
2236  out << " " << joint->getName();
2237  const int idx = joint->getJointIndex();
2238  if (dirty_joint_transforms_[idx])
2239  out << " [dirty]";
2240  out << ": ";
2241  printTransform(variable_joint_transforms_[idx], out);
2242  }
2243 
2244  out << "Link poses:\n";
2245  const std::vector<const LinkModel*>& link_model = robot_model_->getLinkModels();
2246  for (const LinkModel* link : link_model)
2247  {
2248  out << " " << link->getName() << ": ";
2249  printTransform(global_link_transforms_[link->getLinkIndex()], out);
2250  }
2251 }
2252 
2254 {
2255  std::stringstream ss;
2256  ss << "ROBOT: " << robot_model_->getName() << '\n';
2257  getStateTreeJointString(ss, robot_model_->getRootJoint(), " ", true);
2258  return ss.str();
2259 }
2260 
2261 namespace
2262 {
2263 void getPoseString(std::ostream& ss, const Eigen::Isometry3d& pose, const std::string& pfx)
2264 {
2265  ss.precision(3);
2266  for (int y = 0; y < 4; ++y)
2267  {
2268  ss << pfx;
2269  for (int x = 0; x < 4; ++x)
2270  {
2271  ss << std::setw(8) << pose(y, x) << " ";
2272  }
2273  ss << '\n';
2274  }
2275 }
2276 } // namespace
2277 
2278 void RobotState::getStateTreeJointString(std::ostream& ss, const JointModel* jm, const std::string& pfx0,
2279  bool last) const
2280 {
2281  std::string pfx = pfx0 + "+--";
2282 
2283  ss << pfx << "Joint: " << jm->getName() << '\n';
2284 
2285  pfx = pfx0 + (last ? " " : "| ");
2286 
2287  for (std::size_t i = 0; i < jm->getVariableCount(); ++i)
2288  {
2289  ss.precision(3);
2290  ss << pfx << jm->getVariableNames()[i] << std::setw(12) << position_[jm->getFirstVariableIndex() + i] << '\n';
2291  }
2292 
2293  const LinkModel* link_model = jm->getChildLinkModel();
2294 
2295  ss << pfx << "Link: " << link_model->getName() << '\n';
2296  getPoseString(ss, link_model->getJointOriginTransform(), pfx + "joint_origin:");
2297  if (variable_joint_transforms_)
2298  {
2299  getPoseString(ss, variable_joint_transforms_[jm->getJointIndex()], pfx + "joint_variable:");
2300  getPoseString(ss, global_link_transforms_[link_model->getLinkIndex()], pfx + "link_global:");
2301  }
2302 
2303  for (std::vector<const JointModel*>::const_iterator it = link_model->getChildJointModels().begin();
2304  it != link_model->getChildJointModels().end(); ++it)
2305  getStateTreeJointString(ss, *it, pfx, it + 1 == link_model->getChildJointModels().end());
2306 }
2307 
2308 std::ostream& operator<<(std::ostream& out, const RobotState& s)
2309 {
2310  s.printStateInfo(out);
2311  return out;
2312 }
2313 
2314 } // end of namespace core
2315 } // 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 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:1340
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:1402
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:1440
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:1430
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:1458
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:1435
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:1712
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:1531
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.
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...
const moveit::core::LinkModel * getRigidlyConnectedParentLinkModel(const std::string &frame, const moveit::core::JointModelGroup *jmg=nullptr) const
Get the latest link upwards the kinematic tree which is only connected via fixed joints.
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.