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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Fri Aug 14 2020 03:57:26