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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Apr 18 2018 02:49:03