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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sat Dec 12 2020 03:25:45