collision_robot_distance_field.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: E. Gil Jones */
36 
41 #include <ros/console.h>
42 #include <ros/assert.h>
43 #include <tf2_eigen/tf2_eigen.h>
44 
45 namespace collision_detection
46 {
47 const double EPSILON = 0.001f;
48 
50  : CollisionRobot(robot_model)
51 {
52  // planning_scene_.reset(new planning_scene::PlanningScene(robot_model_));
53 
54  std::map<std::string, std::vector<CollisionSphere>> link_body_decompositions;
56  initialize(link_body_decompositions, size, Eigen::Vector3d(0, 0, 0), DEFAULT_USE_SIGNED_DISTANCE_FIELD,
58  setPadding(0.0);
59 }
60 
62  const robot_model::RobotModelConstPtr& robot_model,
63  const std::map<std::string, std::vector<CollisionSphere>>& link_body_decompositions, double size_x, double size_y,
64  double size_z, bool use_signed_distance_field, double resolution, double collision_tolerance,
65  double max_propogation_distance, double padding, double scale)
66  : CollisionRobot(robot_model, padding, scale)
67 {
68  initialize(link_body_decompositions, Eigen::Vector3d(size_x, size_y, size_z), Eigen::Vector3d(0, 0, 0),
69  use_signed_distance_field, resolution, collision_tolerance, max_propogation_distance);
70 }
71 
73  const Eigen::Vector3d& origin, bool use_signed_distance_field,
74  double resolution, double collision_tolerance,
75  double max_propogation_distance, double padding)
76  : CollisionRobot(col_robot)
77 {
78  std::map<std::string, std::vector<CollisionSphere>> link_body_decompositions;
79  initialize(link_body_decompositions, size, origin, use_signed_distance_field, resolution, collision_tolerance,
80  max_propogation_distance);
81  setPadding(padding);
82 }
83 
85  : CollisionRobot(other)
86 {
87  size_ = other.size_;
88  origin_ = other.origin_;
89 
91  resolution_ = other.resolution_;
99 }
100 
102  const std::map<std::string, std::vector<CollisionSphere>>& link_body_decompositions, const Eigen::Vector3d& size,
103  const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution, double collision_tolerance,
104  double max_propogation_distance)
105 {
106  size_ = size;
107  origin_ = origin;
108  use_signed_distance_field_ = use_signed_distance_field;
109  resolution_ = resolution;
110  collision_tolerance_ = collision_tolerance;
111  max_propogation_distance_ = max_propogation_distance;
112  addLinkBodyDecompositions(resolution_, link_body_decompositions);
115 
116  const std::vector<const moveit::core::JointModelGroup*>& jmg = robot_model_->getJointModelGroups();
117  for (std::vector<const moveit::core::JointModelGroup*>::const_iterator it = jmg.begin(); it != jmg.end(); it++)
118  {
119  const moveit::core::JointModelGroup* jm = *it;
120 
121  std::map<std::string, bool> updated_group_entry;
122  std::vector<std::string> links = jm->getUpdatedLinkModelsWithGeometryNames();
123  for (unsigned int i = 0; i < links.size(); i++)
124  {
125  updated_group_entry[links[i]] = true;
126  }
127  in_group_update_map_[jm->getName()] = updated_group_entry;
128  state.updateLinkTransforms();
129  DistanceFieldCacheEntryPtr dfce =
130  generateDistanceFieldCacheEntry(jm->getName(), state, &planning_scene_->getAllowedCollisionMatrix(), false);
132  }
133 }
134 
136  const std::string& group_name, const moveit::core::RobotState& state,
137  const collision_detection::AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr,
138  bool generate_distance_field) const
139 {
140  DistanceFieldCacheEntryConstPtr dfce = getDistanceFieldCacheEntry(group_name, state, acm);
141  if (!dfce || (generate_distance_field && !dfce->distance_field_))
142  {
143  // ROS_DEBUG_STREAM_NAMED("collision_distance_field", "Generating new
144  // DistanceFieldCacheEntry for CollisionRobot");
145  DistanceFieldCacheEntryPtr new_dfce =
146  generateDistanceFieldCacheEntry(group_name, state, acm, generate_distance_field);
147  boost::mutex::scoped_lock slock(update_cache_lock_);
148  (const_cast<CollisionRobotDistanceField*>(this))->distance_field_cache_entry_ = new_dfce;
149  dfce = new_dfce;
150  }
151  getGroupStateRepresentation(dfce, state, gsr);
152 }
153 
156  const moveit::core::RobotState& state,
158  GroupStateRepresentationPtr& gsr) const
159 {
160  if (!gsr)
161  {
162  generateCollisionCheckingStructures(req.group_name, state, acm, gsr, true);
163  }
164  else
165  {
167  }
168  // ros::WallTime n = ros::WallTime::now();
169  bool done = getSelfCollisions(req, res, gsr);
170 
171  if (!done)
172  {
173  getIntraGroupCollisions(req, res, gsr);
174  ROS_DEBUG_COND(res.collision, "Intra Group Collision found");
175  }
176 }
177 
178 DistanceFieldCacheEntryConstPtr
180  const moveit::core::RobotState& state,
182 {
183  DistanceFieldCacheEntryConstPtr ret;
185  {
186  ROS_DEBUG_STREAM("No current Distance field cache entry");
187  return ret;
188  }
189  DistanceFieldCacheEntryConstPtr cur = distance_field_cache_entry_;
190  if (group_name != cur->group_name_)
191  {
192  ROS_DEBUG("No cache entry as group name changed from %s to %s", cur->group_name_.c_str(), group_name.c_str());
193  return ret;
194  }
195  else if (!compareCacheEntryToState(cur, state))
196  {
197  // Regenerating distance field as state has changed from last time
198  // ROS_DEBUG_STREAM_NAMED("collision_distance_field", "Regenerating distance field as
199  // state has changed from last time");
200  return ret;
201  }
202  else if (acm && !compareCacheEntryToAllowedCollisionMatrix(cur, *acm))
203  {
204  ROS_DEBUG("Regenerating distance field as some relevant part of the acm changed");
205  return ret;
206  }
207  return cur;
208 }
209 
212  const moveit::core::RobotState& state) const
213 {
214  GroupStateRepresentationPtr gsr;
215  checkSelfCollisionHelper(req, res, state, nullptr, gsr);
216 }
217 
220  const moveit::core::RobotState& state,
221  GroupStateRepresentationPtr& gsr) const
222 {
223  checkSelfCollisionHelper(req, res, state, nullptr, gsr);
224 }
225 
228  const moveit::core::RobotState& state,
230 {
231  GroupStateRepresentationPtr gsr;
232  checkSelfCollisionHelper(req, res, state, &acm, gsr);
233 }
234 
237  const moveit::core::RobotState& state,
239  GroupStateRepresentationPtr& gsr) const
240 {
241  if (gsr)
242  {
243  ROS_WARN("Shouldn't be calling this function with initialized gsr - ACM "
244  "will be ignored");
245  }
246  checkSelfCollisionHelper(req, res, state, &acm, gsr);
247 }
248 
251  GroupStateRepresentationPtr& gsr) const
252 {
253  for (unsigned int i = 0; i < gsr->dfce_->link_names_.size() + gsr->dfce_->attached_body_names_.size(); i++)
254  {
255  bool is_link = i < gsr->dfce_->link_names_.size();
256  if ((is_link && !gsr->dfce_->link_has_geometry_[i]) || !gsr->dfce_->self_collision_enabled_[i])
257  continue;
258  const std::vector<CollisionSphere>* collision_spheres_1;
259  const EigenSTL::vector_Vector3d* sphere_centers_1;
260 
261  if (is_link)
262  {
263  collision_spheres_1 = &(gsr->link_body_decompositions_[i]->getCollisionSpheres());
264  sphere_centers_1 = &(gsr->link_body_decompositions_[i]->getSphereCenters());
265  }
266  else
267  {
268  collision_spheres_1 =
269  &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getCollisionSpheres());
270  sphere_centers_1 = &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters());
271  }
272 
273  if (req.contacts)
274  {
275  std::vector<unsigned int> colls;
276  bool coll =
277  getCollisionSphereCollision(gsr->dfce_->distance_field_.get(), *collision_spheres_1, *sphere_centers_1,
279  std::min(req.max_contacts_per_pair, req.max_contacts - res.contact_count), colls);
280  if (coll)
281  {
282  res.collision = true;
283  for (unsigned int j = 0; j < colls.size(); j++)
284  {
286  if (is_link)
287  {
288  con.pos = gsr->link_body_decompositions_[i]->getSphereCenters()[colls[j]];
290  con.body_name_1 = gsr->dfce_->link_names_[i];
291  }
292  else
293  {
294  con.pos =
295  gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters()[colls[j]];
297  con.body_name_1 = gsr->dfce_->attached_body_names_[i - gsr->dfce_->link_names_.size()];
298  }
299 
300  ROS_DEBUG_STREAM("Self collision detected for link " << con.body_name_1);
301 
303  con.body_name_2 = "self";
304  res.contact_count++;
305  res.contacts[std::pair<std::string, std::string>(con.body_name_1, con.body_name_2)].push_back(con);
306  gsr->gradients_[i].types[colls[j]] = SELF;
307  }
308  gsr->gradients_[i].collision = true;
309 
310  if (res.contact_count >= req.max_contacts)
311  {
312  return true;
313  }
314  }
315  }
316  else
317  {
318  bool coll = getCollisionSphereCollision(gsr->dfce_->distance_field_.get(), *collision_spheres_1,
319  *sphere_centers_1, max_propogation_distance_, collision_tolerance_);
320  if (coll)
321  {
322  ROS_DEBUG("Link %s in self collision", gsr->dfce_->link_names_[i].c_str());
323  res.collision = true;
324  return true;
325  }
326  }
327  }
328  return (res.contact_count >= req.max_contacts);
329 }
330 
331 bool CollisionRobotDistanceField::getSelfProximityGradients(GroupStateRepresentationPtr& gsr) const
332 {
333  bool in_collision = false;
334 
335  for (unsigned int i = 0; i < gsr->dfce_->link_names_.size(); i++)
336  {
337  const std::string& link_name = gsr->dfce_->link_names_[i];
338  bool is_link = i < gsr->dfce_->link_names_.size();
339  if ((is_link && !gsr->dfce_->link_has_geometry_[i]) || !gsr->dfce_->self_collision_enabled_[i])
340  {
341  continue;
342  }
343 
344  const std::vector<CollisionSphere>* collision_spheres_1;
345  const EigenSTL::vector_Vector3d* sphere_centers_1;
346  if (is_link)
347  {
348  collision_spheres_1 = &(gsr->link_body_decompositions_[i]->getCollisionSpheres());
349  sphere_centers_1 = &(gsr->link_body_decompositions_[i]->getSphereCenters());
350  }
351  else
352  {
353  collision_spheres_1 =
354  &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getCollisionSpheres());
355  sphere_centers_1 = &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters());
356  }
357 
358  // computing distance gradients by checking collisions against other mobile
359  // links as indicated by the AllowedCollisionMatrix
360  bool coll = false;
361  if (gsr->dfce_->acm_.getSize() > 0)
362  {
363  AllowedCollision::Type col_type;
364  for (unsigned int j = 0; j < gsr->dfce_->link_names_.size(); j++)
365  {
366  // on self collisions skip
367  if (link_name == gsr->dfce_->link_names_[j])
368  {
369  continue;
370  }
371 
372  // on collision exceptions skip
373  if (gsr->dfce_->acm_.getEntry(link_name, gsr->dfce_->link_names_[j], col_type) &&
374  col_type != AllowedCollision::NEVER)
375  {
376  continue;
377  }
378 
379  if (gsr->link_distance_fields_[j])
380  {
381  coll = gsr->link_distance_fields_[j]->getCollisionSphereGradients(
382  *collision_spheres_1, *sphere_centers_1, gsr->gradients_[i], collision_detection::SELF,
384 
385  if (coll)
386  {
387  in_collision = true;
388  }
389  }
390  }
391  }
392 
393  coll = getCollisionSphereGradients(gsr->dfce_->distance_field_.get(), *collision_spheres_1, *sphere_centers_1,
394  gsr->gradients_[i], collision_detection::SELF, collision_tolerance_, false,
396 
397  if (coll)
398  {
399  in_collision = true;
400  }
401  }
402 
403  return in_collision;
404 }
405 
408  GroupStateRepresentationPtr& gsr) const
409 {
410  unsigned int num_links = gsr->dfce_->link_names_.size();
411  unsigned int num_attached_bodies = gsr->dfce_->attached_body_names_.size();
412 
413  for (unsigned int i = 0; i < num_links + num_attached_bodies; i++)
414  {
415  for (unsigned int j = i + 1; j < num_links + num_attached_bodies; j++)
416  {
417  if (i == j)
418  continue;
419  bool i_is_link = i < num_links;
420  bool j_is_link = j < num_links;
421 
422  if ((i_is_link && !gsr->dfce_->link_has_geometry_[i]) || (j_is_link && !gsr->dfce_->link_has_geometry_[j]))
423  continue;
424 
425  if (!gsr->dfce_->intra_group_collision_enabled_[i][j])
426  continue;
427 
428  if (i_is_link && j_is_link &&
429  !doBoundingSpheresIntersect(gsr->link_body_decompositions_[i], gsr->link_body_decompositions_[j]))
430  {
431  // ROS_DEBUG_STREAM("Bounding spheres for " <<
432  // gsr->dfce_->link_names_[i] << " and " << gsr->dfce_->link_names_[j]
433  //<< " don't intersect");
434  continue;
435  }
436  else if (!i_is_link || !j_is_link)
437  {
438  bool all_ok = true;
439  if (!i_is_link && j_is_link)
440  {
441  for (unsigned int k = 0; k < gsr->attached_body_decompositions_[i - num_links]->getSize(); k++)
442  {
444  gsr->link_body_decompositions_[j],
445  gsr->attached_body_decompositions_[i - num_links]->getPosedBodySphereDecomposition(k)))
446  {
447  all_ok = false;
448  break;
449  }
450  }
451  }
452  else if (i_is_link && !j_is_link)
453  {
454  for (unsigned int k = 0; k < gsr->attached_body_decompositions_[j - num_links]->getSize(); k++)
455  {
457  gsr->link_body_decompositions_[i],
458  gsr->attached_body_decompositions_[j - num_links]->getPosedBodySphereDecomposition(k)))
459  {
460  all_ok = false;
461  break;
462  }
463  }
464  }
465  else
466  {
467  for (unsigned int k = 0; k < gsr->attached_body_decompositions_[i - num_links]->getSize() && all_ok; k++)
468  {
469  for (unsigned int l = 0; l < gsr->attached_body_decompositions_[j - num_links]->getSize(); l++)
470  {
472  gsr->attached_body_decompositions_[i - num_links]->getPosedBodySphereDecomposition(k),
473  gsr->attached_body_decompositions_[j - num_links]->getPosedBodySphereDecomposition(l)))
474  {
475  all_ok = false;
476  break;
477  }
478  }
479  }
480  }
481  if (all_ok)
482  {
483  continue;
484  }
485  // std::cerr << "Bounding spheres for " << gsr->dfce_->link_names_[i] <<
486  // " and " << gsr->dfce_->link_names_[j]
487  // << " intersect" << std::endl;
488  }
489  int num_pair = -1;
490  std::string name_1;
491  std::string name_2;
492  if (i_is_link)
493  {
494  name_1 = gsr->dfce_->link_names_[i];
495  }
496  else
497  {
498  name_1 = gsr->dfce_->attached_body_names_[i - num_links];
499  }
500 
501  if (j_is_link)
502  {
503  name_2 = gsr->dfce_->link_names_[j];
504  }
505  else
506  {
507  name_2 = gsr->dfce_->attached_body_names_[j - num_links];
508  }
509  if (req.contacts)
510  {
511  collision_detection::CollisionResult::ContactMap::iterator it =
512  res.contacts.find(std::pair<std::string, std::string>(name_1, name_2));
513  if (it == res.contacts.end())
514  {
515  num_pair = 0;
516  }
517  else
518  {
519  num_pair = it->second.size();
520  }
521  }
522  const std::vector<CollisionSphere>* collision_spheres_1;
523  const std::vector<CollisionSphere>* collision_spheres_2;
524  const EigenSTL::vector_Vector3d* sphere_centers_1;
525  const EigenSTL::vector_Vector3d* sphere_centers_2;
526  if (i_is_link)
527  {
528  collision_spheres_1 = &(gsr->link_body_decompositions_[i]->getCollisionSpheres());
529  sphere_centers_1 = &(gsr->link_body_decompositions_[i]->getSphereCenters());
530  }
531  else
532  {
533  collision_spheres_1 = &(gsr->attached_body_decompositions_[i - num_links]->getCollisionSpheres());
534  sphere_centers_1 = &(gsr->attached_body_decompositions_[i - num_links]->getSphereCenters());
535  }
536  if (j_is_link)
537  {
538  collision_spheres_2 = &(gsr->link_body_decompositions_[j]->getCollisionSpheres());
539  sphere_centers_2 = &(gsr->link_body_decompositions_[j]->getSphereCenters());
540  }
541  else
542  {
543  collision_spheres_2 = &(gsr->attached_body_decompositions_[j - num_links]->getCollisionSpheres());
544  sphere_centers_2 = &(gsr->attached_body_decompositions_[j - num_links]->getSphereCenters());
545  }
546 
547  for (unsigned int k = 0; k < collision_spheres_1->size() && num_pair < (int)req.max_contacts_per_pair; k++)
548  {
549  for (unsigned int l = 0; l < collision_spheres_2->size() && num_pair < (int)req.max_contacts_per_pair; l++)
550  {
551  Eigen::Vector3d gradient = (*sphere_centers_1)[k] - (*sphere_centers_2)[l];
552  double dist = gradient.norm();
553  // std::cerr << "Dist is " << dist << " rad " <<
554  // (*collision_spheres_1)[k].radius_+(*collision_spheres_2)[l].radius_
555  // << std::endl;
556 
557  if (dist < (*collision_spheres_1)[k].radius_ + (*collision_spheres_2)[l].radius_)
558  {
559  // ROS_DEBUG("Intra-group contact between %s and %s, d =
560  // %f < r1 = %f + r2 = %f", name_1.c_str(),
561  // name_2.c_str(),
562  // dist ,(*collision_spheres_1)[k].radius_
563  // ,(*collision_spheres_2)[l].radius_);
564  // Eigen::Vector3d sc1 = (*sphere_centers_1)[k];
565  // Eigen::Vector3d sc2 = (*sphere_centers_2)[l];
566  // ROS_DEBUG("sphere center 1:[ %f, %f, %f ], sphere
567  // center 2: [%f, %f,%f ], lbdc size =
568  // %i",sc1[0],sc1[1],sc1[2],
569  // sc2[0],sc2[1],sc2[2],int(gsr->link_body_decompositions_.size()));
570  res.collision = true;
571 
572  if (req.contacts)
573  {
575  con.pos = gsr->link_body_decompositions_[i]->getSphereCenters()[k];
576  con.body_name_1 = name_1;
577  con.body_name_2 = name_2;
578  if (i_is_link)
579  {
581  }
582  else
583  {
585  }
586  if (j_is_link)
587  {
589  }
590  else
591  {
593  }
594  res.contact_count++;
595  res.contacts[std::pair<std::string, std::string>(con.body_name_1, con.body_name_2)].push_back(con);
596  num_pair++;
597  // std::cerr << "Pushing back intra " << con.body_name_1 << " and
598  // " << con.body_name_2 << std::endl;
599  gsr->gradients_[i].types[k] = INTRA;
600  gsr->gradients_[i].collision = true;
601  gsr->gradients_[j].types[l] = INTRA;
602  gsr->gradients_[j].collision = true;
603  // ROS_INFO_STREAM("Sphere 1 " << (*sphere_centers_1)[k]);
604  // ROS_INFO_STREAM("Sphere 2 " << (*sphere_centers_2)[l]);
605  // ROS_INFO_STREAM("Norm " << gradient.norm());
606  // ROS_INFO_STREAM("Dist is " << dist
607  // << " radius 1 " <<
608  // (*collision_spheres_1)[k].radius_
609  // << " radius 2 " <<
610  // (*collision_spheres_2)[l].radius_);
611  // ROS_INFO_STREAM("Gradient " << gradient);
612  // ROS_INFO_STREAM("Spheres intersect for " <<
613  // gsr->dfce_->link_names_[i] << " and " <<
614  // gsr->dfce_->link_names_[j]);
615  // std::cerr << "Spheres intersect for " <<
616  // gsr->dfce_->link_names_[i] << " and " <<
617  // gsr->dfce_->link_names_[j] << std::cerr;
618  if (res.contact_count >= req.max_contacts)
619  {
620  return true;
621  }
622  }
623  else
624  {
625  return true;
626  }
627  }
628  }
629  }
630  }
631  }
632  return false;
633 }
634 
635 bool CollisionRobotDistanceField::getIntraGroupProximityGradients(GroupStateRepresentationPtr& gsr) const
636 {
637  bool in_collision = false;
638  unsigned int num_links = gsr->dfce_->link_names_.size();
639  unsigned int num_attached_bodies = gsr->dfce_->attached_body_names_.size();
640  // TODO - deal with attached bodies
641  for (unsigned int i = 0; i < num_links + num_attached_bodies; i++)
642  {
643  for (unsigned int j = i + 1; j < num_links + num_attached_bodies; j++)
644  {
645  if (i == j)
646  continue;
647  bool i_is_link = i < num_links;
648  bool j_is_link = j < num_links;
649  if ((i_is_link && !gsr->dfce_->link_has_geometry_[i]) || (j_is_link && !gsr->dfce_->link_has_geometry_[j]))
650  continue;
651  if (!gsr->dfce_->intra_group_collision_enabled_[i][j])
652  continue;
653  const std::vector<CollisionSphere>* collision_spheres_1;
654  const std::vector<CollisionSphere>* collision_spheres_2;
655  const EigenSTL::vector_Vector3d* sphere_centers_1;
656  const EigenSTL::vector_Vector3d* sphere_centers_2;
657  if (i_is_link)
658  {
659  collision_spheres_1 = &(gsr->link_body_decompositions_[i]->getCollisionSpheres());
660  sphere_centers_1 = &(gsr->link_body_decompositions_[i]->getSphereCenters());
661  }
662  else
663  {
664  collision_spheres_1 = &(gsr->attached_body_decompositions_[i - num_links]->getCollisionSpheres());
665  sphere_centers_1 = &(gsr->attached_body_decompositions_[i - num_links]->getSphereCenters());
666  }
667  if (j_is_link)
668  {
669  collision_spheres_2 = &(gsr->link_body_decompositions_[j]->getCollisionSpheres());
670  sphere_centers_2 = &(gsr->link_body_decompositions_[j]->getSphereCenters());
671  }
672  else
673  {
674  collision_spheres_2 = &(gsr->attached_body_decompositions_[j - num_links]->getCollisionSpheres());
675  sphere_centers_2 = &(gsr->attached_body_decompositions_[j - num_links]->getSphereCenters());
676  }
677  for (unsigned int k = 0; k < collision_spheres_1->size(); k++)
678  {
679  for (unsigned int l = 0; l < collision_spheres_2->size(); l++)
680  {
681  Eigen::Vector3d gradient = (*sphere_centers_1)[k] - (*sphere_centers_2)[l];
682  double dist = gradient.norm();
683  if (dist < gsr->gradients_[i].distances[k])
684  {
685  gsr->gradients_[i].distances[k] = dist;
686  gsr->gradients_[i].gradients[k] = gradient;
687  gsr->gradients_[i].types[k] = INTRA;
688  }
689  if (dist < gsr->gradients_[j].distances[l])
690  {
691  gsr->gradients_[j].distances[l] = dist;
692  gsr->gradients_[j].gradients[l] = -gradient;
693  gsr->gradients_[j].types[l] = INTRA;
694  }
695  }
696  }
697  }
698  }
699  return in_collision;
700 }
702  const std::string& group_name, const moveit::core::RobotState& state,
703  const collision_detection::AllowedCollisionMatrix* acm, bool generate_distance_field) const
704 {
705  DistanceFieldCacheEntryPtr dfce(new DistanceFieldCacheEntry());
706 
707  if (robot_model_->getJointModelGroup(group_name) == nullptr)
708  {
709  ROS_WARN("No group %s", group_name.c_str());
710  return dfce;
711  }
712 
713  dfce->group_name_ = group_name;
714  dfce->state_.reset(new moveit::core::RobotState(state));
715  if (acm)
716  {
717  dfce->acm_ = *acm;
718  }
719  else
720  {
721  ROS_WARN_STREAM("Allowed Collision Matrix is null, enabling all collisions "
722  "in the DistanceFieldCacheEntry");
723  }
724 
725  // generateAllowedCollisionInformation(dfce);
726  dfce->link_names_ = robot_model_->getJointModelGroup(group_name)->getUpdatedLinkModelNames();
727  std::vector<const moveit::core::AttachedBody*> all_attached_bodies;
728  dfce->state_->getAttachedBodies(all_attached_bodies);
729  unsigned int att_count = 0;
730 
731  // may be bigger than necessary
732  std::vector<bool> all_true(dfce->link_names_.size() + all_attached_bodies.size(), true);
733  std::vector<bool> all_false(dfce->link_names_.size() + all_attached_bodies.size(), false);
734 
735  const std::vector<const moveit::core::LinkModel*>& lsv = state.getJointModelGroup(group_name)->getUpdatedLinkModels();
736  dfce->self_collision_enabled_.resize(dfce->link_names_.size() + all_attached_bodies.size(), true);
737  dfce->intra_group_collision_enabled_.resize(dfce->link_names_.size() + all_attached_bodies.size());
738 
739  for (unsigned int i = 0; i < dfce->link_names_.size(); i++)
740  {
741  std::string link_name = dfce->link_names_[i];
742  const moveit::core::LinkModel* link_state = dfce->state_->getLinkModel(link_name);
743  bool found = false;
744 
745  for (unsigned int j = 0; j < lsv.size(); j++)
746  {
747  if (lsv[j]->getName() == link_name)
748  {
749  dfce->link_state_indices_.push_back(j);
750  found = true;
751  break;
752  }
753  }
754 
755  if (!found)
756  {
757  ROS_DEBUG("No link state found for link %s", dfce->link_names_[i].c_str());
758  continue;
759  }
760 
761  if (!link_state->getShapes().empty())
762  {
763  dfce->link_has_geometry_.push_back(true);
764  dfce->link_body_indices_.push_back(link_body_decomposition_index_map_.find(link_name)->second);
765 
766  if (acm)
767  {
769  if (acm->getEntry(link_name, link_name, t) && t == collision_detection::AllowedCollision::ALWAYS)
770  {
771  dfce->self_collision_enabled_[i] = false;
772  }
773 
774  dfce->intra_group_collision_enabled_[i] = all_true;
775  for (unsigned int j = i + 1; j < dfce->link_names_.size(); j++)
776  {
777  if (link_name == dfce->link_names_[j])
778  {
779  dfce->intra_group_collision_enabled_[i][j] = false;
780  continue;
781  }
782  if (acm->getEntry(link_name, dfce->link_names_[j], t) && t == collision_detection::AllowedCollision::ALWAYS)
783  {
784  dfce->intra_group_collision_enabled_[i][j] = false;
785  }
786  }
787 
788  std::vector<const moveit::core::AttachedBody*> link_attached_bodies;
789  state.getAttachedBodies(link_attached_bodies, link_state);
790  for (unsigned int j = 0; j < link_attached_bodies.size(); j++, att_count++)
791  {
792  dfce->attached_body_names_.push_back(link_attached_bodies[j]->getName());
793  dfce->attached_body_link_state_indices_.push_back(dfce->link_state_indices_[i]);
794 
795  if (acm->getEntry(link_name, link_attached_bodies[j]->getName(), t))
796  {
798  {
799  dfce->intra_group_collision_enabled_[i][att_count + dfce->link_names_.size()] = false;
800  }
801  }
802  // std::cerr << "Checking touch links for " << link_name << " and " <<
803  // attached_bodies[j]->getName()
804  // << " num " << attached_bodies[j]->getTouchLinks().size()
805  // << std::endl;
806  // touch links take priority
807  if (link_attached_bodies[j]->getTouchLinks().find(link_name) != link_attached_bodies[j]->getTouchLinks().end())
808  {
809  dfce->intra_group_collision_enabled_[i][att_count + dfce->link_names_.size()] = false;
810  // std::cerr << "Setting intra group for " << link_name << " and
811  // attached body " << link_attached_bodies[j]->getName() << " to
812  // false" << std::endl;
813  }
814  }
815  }
816  else
817  {
818  dfce->self_collision_enabled_[i] = true;
819  dfce->intra_group_collision_enabled_[i] = all_true;
820  }
821  }
822  else
823  {
824  dfce->link_has_geometry_.push_back(false);
825  dfce->link_body_indices_.push_back(0);
826  dfce->self_collision_enabled_[i] = false;
827  dfce->intra_group_collision_enabled_[i] = all_false;
828  }
829  }
830 
831  for (unsigned int i = 0; i < dfce->attached_body_names_.size(); i++)
832  {
833  dfce->intra_group_collision_enabled_[i + dfce->link_names_.size()] = all_true;
834  if (acm)
835  {
837  if (acm->getEntry(dfce->attached_body_names_[i], dfce->attached_body_names_[i], t) &&
839  {
840  dfce->self_collision_enabled_[i + dfce->link_names_.size()] = false;
841  }
842  for (unsigned int j = i + 1; j < dfce->attached_body_names_.size(); j++)
843  {
844  if (acm->getEntry(dfce->attached_body_names_[i], dfce->attached_body_names_[j], t) &&
846  {
847  dfce->intra_group_collision_enabled_[i + dfce->link_names_.size()][j + dfce->link_names_.size()] = false;
848  }
849  // TODO - allow for touch links to be attached bodies?
850  // else {
851  // std::cerr << "Setting not allowed for " << link_name << " and " <<
852  // dfce->link_names_[j] << std::endl;
853  //}
854  }
855  }
856  }
857 
858  std::map<std::string, GroupStateRepresentationPtr>::const_iterator it =
859  pregenerated_group_state_representation_map_.find(dfce->group_name_);
861  {
862  dfce->pregenerated_group_state_representation_ = it->second;
863  }
864 
865  std::map<std::string, bool> updated_map;
866  if (!dfce->link_names_.empty())
867  {
868  const std::vector<const moveit::core::JointModel*>& child_joint_models =
869  dfce->state_->getJointModelGroup(dfce->group_name_)->getActiveJointModels();
870  for (unsigned int i = 0; i < child_joint_models.size(); i++)
871  {
872  updated_map[child_joint_models[i]->getName()] = true;
873  ROS_DEBUG_STREAM("Joint " << child_joint_models[i]->getName() << " has been added to updated list");
874  }
875  }
876 
877  const std::vector<std::string>& state_variable_names = state.getVariableNames();
878  for (std::vector<std::string>::const_iterator name_iter = state_variable_names.begin();
879  name_iter != state_variable_names.end(); name_iter++)
880  {
881  double val = state.getVariablePosition(*name_iter);
882  dfce->state_values_.push_back(val);
883  if (updated_map.count(*name_iter) == 0)
884  {
885  dfce->state_check_indices_.push_back(dfce->state_values_.size() - 1);
886  ROS_DEBUG_STREAM("Non-group joint " << *name_iter << " will be checked for state changes");
887  }
888  }
889 
890  if (generate_distance_field)
891  {
892  if (dfce->distance_field_)
893  {
894  ROS_DEBUG_STREAM("CollisionRobot skipping distance field generation, "
895  "will use existing one");
896  }
897  else
898  {
899  std::vector<PosedBodyPointDecompositionPtr> non_group_link_decompositions;
900  std::vector<PosedBodyPointDecompositionVectorPtr> non_group_attached_body_decompositions;
901  const std::map<std::string, bool>& updated_group_map = in_group_update_map_.find(group_name)->second;
902  for (unsigned int i = 0; i < robot_model_->getLinkModelsWithCollisionGeometry().size(); i++)
903  {
904  std::string link_name = robot_model_->getLinkModelsWithCollisionGeometry()[i]->getName();
905  const moveit::core::LinkModel* link_state = dfce->state_->getLinkModel(link_name);
906  if (updated_group_map.find(link_name) != updated_group_map.end())
907  {
908  continue;
909  }
910 
911  // populating array with link that are not part of the planning group
912  non_group_link_decompositions.push_back(getPosedLinkBodyPointDecomposition(link_state));
913  non_group_link_decompositions.back()->updatePose(dfce->state_->getFrameTransform(link_state->getName()));
914 
915  std::vector<const moveit::core::AttachedBody*> attached_bodies;
916  dfce->state_->getAttachedBodies(attached_bodies, link_state);
917  for (unsigned int j = 0; j < attached_bodies.size(); j++)
918  {
919  non_group_attached_body_decompositions.push_back(
920  getAttachedBodyPointDecomposition(attached_bodies[j], resolution_));
921  }
922  }
923  dfce->distance_field_.reset(new distance_field::PropagationDistanceField(
924  size_.x(), size_.y(), size_.z(), resolution_, origin_.x() - 0.5 * size_.x(), origin_.y() - 0.5 * size_.y(),
926 
927  // ROS_INFO_STREAM("Creation took " <<
928  // (ros::WallTime::now()-before_create).toSec());
929  // TODO - deal with AllowedCollisionMatrix
930  // now we need to actually set the points
931  // TODO - deal with shifted robot
932  EigenSTL::vector_Vector3d all_points;
933  for (unsigned int i = 0; i < non_group_link_decompositions.size(); i++)
934  {
935  all_points.insert(all_points.end(), non_group_link_decompositions[i]->getCollisionPoints().begin(),
936  non_group_link_decompositions[i]->getCollisionPoints().end());
937  }
938 
939  for (unsigned int i = 0; i < non_group_attached_body_decompositions.size(); i++)
940  {
941  all_points.insert(all_points.end(), non_group_attached_body_decompositions[i]->getCollisionPoints().begin(),
942  non_group_attached_body_decompositions[i]->getCollisionPoints().end());
943  }
944 
945  dfce->distance_field_->addPointsToField(all_points);
946  ROS_DEBUG_STREAM("CollisionRobot distance field has been initialized with " << all_points.size() << " points.");
947  }
948  }
949  return dfce;
950 }
951 
953 {
954  const std::vector<const moveit::core::LinkModel*>& link_models = robot_model_->getLinkModelsWithCollisionGeometry();
955  for (unsigned int i = 0; i < link_models.size(); i++)
956  {
957  if (link_models[i]->getShapes().empty())
958  {
959  ROS_WARN("No collision geometry for link model %s though there should be", link_models[i]->getName().c_str());
960  continue;
961  }
962 
963  ROS_DEBUG("Generating model for %s", link_models[i]->getName().c_str());
964  BodyDecompositionConstPtr bd(new BodyDecomposition(link_models[i]->getShapes(),
965  link_models[i]->getCollisionOriginTransforms(), resolution,
966  getLinkPadding(link_models[i]->getName())));
967  link_body_decomposition_vector_.push_back(bd);
968  link_body_decomposition_index_map_[link_models[i]->getName()] = link_body_decomposition_vector_.size() - 1;
969  }
970 }
971 
973  visualization_msgs::MarkerArray& model_markers) const
974 {
975  // creating colors
976  std_msgs::ColorRGBA robot_color;
977  robot_color.r = 0;
978  robot_color.b = 0.8f;
979  robot_color.g = 0;
980  robot_color.a = 0.5;
981 
982  std_msgs::ColorRGBA world_links_color;
983  world_links_color.r = 1;
984  world_links_color.g = 1;
985  world_links_color.b = 0;
986  world_links_color.a = 0.5;
987 
988  // creating sphere marker
989  visualization_msgs::Marker sphere_marker;
990  sphere_marker.header.frame_id = robot_model_->getRootLinkName();
991  sphere_marker.header.stamp = ros::Time(0);
992  sphere_marker.ns = distance_field_cache_entry_->group_name_ + "_sphere_decomposition";
993  sphere_marker.id = 0;
994  sphere_marker.type = visualization_msgs::Marker::SPHERE;
995  sphere_marker.action = visualization_msgs::Marker::ADD;
996  sphere_marker.pose.orientation.x = 0;
997  sphere_marker.pose.orientation.y = 0;
998  sphere_marker.pose.orientation.z = 0;
999  sphere_marker.pose.orientation.w = 1;
1000  sphere_marker.color = robot_color;
1001  sphere_marker.lifetime = ros::Duration(0);
1002 
1003  unsigned int id = 0;
1004  const moveit::core::JointModelGroup* joint_group = state.getJointModelGroup(distance_field_cache_entry_->group_name_);
1005  const std::vector<std::string>& group_link_names = joint_group->getUpdatedLinkModelNames();
1006 
1007  std::map<std::string, unsigned int>::const_iterator map_iter;
1008  for (map_iter = link_body_decomposition_index_map_.begin(); map_iter != link_body_decomposition_index_map_.end();
1009  map_iter++)
1010  {
1011  const std::string& link_name = map_iter->first;
1012  unsigned int link_index = map_iter->second;
1013 
1014  // selecting color
1015  if (std::find(group_link_names.begin(), group_link_names.end(), link_name) != group_link_names.end())
1016  {
1017  sphere_marker.color = robot_color;
1018  }
1019  else
1020  {
1021  sphere_marker.color = world_links_color;
1022  }
1023 
1024  collision_detection::PosedBodySphereDecompositionPtr sphere_representation(
1026  sphere_representation->updatePose(state.getGlobalLinkTransform(link_name));
1027  for (unsigned int j = 0; j < sphere_representation->getCollisionSpheres().size(); j++)
1028  {
1029  sphere_marker.pose.position = tf2::toMsg(sphere_representation->getSphereCenters()[j]);
1030  sphere_marker.scale.x = sphere_marker.scale.y = sphere_marker.scale.z =
1031  2 * sphere_representation->getCollisionSpheres()[j].radius_;
1032  sphere_marker.id = id;
1033  id++;
1034 
1035  model_markers.markers.push_back(sphere_marker);
1036  }
1037  }
1038 }
1039 
1041  double resolution, const std::map<std::string, std::vector<CollisionSphere>>& link_spheres)
1042 {
1043  ROS_ASSERT_MSG(robot_model_, "RobotModelPtr is invalid");
1044  const std::vector<const moveit::core::LinkModel*>& link_models = robot_model_->getLinkModelsWithCollisionGeometry();
1045 
1046  for (unsigned int i = 0; i < link_models.size(); i++)
1047  {
1048  if (link_models[i]->getShapes().empty())
1049  {
1050  ROS_WARN_STREAM("Skipping model generation for link " << link_models[i]->getName()
1051  << " since it contains no geometries");
1052  continue;
1053  }
1054 
1055  BodyDecompositionPtr bd(new BodyDecomposition(link_models[i]->getShapes(),
1056  link_models[i]->getCollisionOriginTransforms(), resolution,
1057  getLinkPadding(link_models[i]->getName())));
1058 
1059  ROS_DEBUG("Generated model for %s", link_models[i]->getName().c_str());
1060 
1061  if (link_spheres.find(link_models[i]->getName()) != link_spheres.end())
1062  {
1063  bd->replaceCollisionSpheres(link_spheres.find(link_models[i]->getName())->second, Eigen::Isometry3d::Identity());
1064  }
1065  link_body_decomposition_vector_.push_back(bd);
1066  link_body_decomposition_index_map_[link_models[i]->getName()] = link_body_decomposition_vector_.size() - 1;
1067  }
1068  ROS_DEBUG_STREAM(__FUNCTION__ << " Finished ");
1069 }
1070 
1071 PosedBodySphereDecompositionPtr
1073  unsigned int ind) const
1074 {
1075  PosedBodySphereDecompositionPtr ret;
1077  return ret;
1078 }
1079 
1080 PosedBodyPointDecompositionPtr
1082 {
1083  PosedBodyPointDecompositionPtr ret;
1084  std::map<std::string, unsigned int>::const_iterator it = link_body_decomposition_index_map_.find(ls->getName());
1085  if (it == link_body_decomposition_index_map_.end())
1086  {
1087  ROS_ERROR_NAMED("collision_distance_field", "No link body decomposition for link %s.", ls->getName().c_str());
1088  return ret;
1089  }
1091  return ret;
1092 }
1093 
1095  GroupStateRepresentationPtr& gsr) const
1096 {
1097  for (unsigned int i = 0; i < gsr->dfce_->link_names_.size(); i++)
1098  {
1099  const moveit::core::LinkModel* ls = state.getLinkModel(gsr->dfce_->link_names_[i]);
1100  if (gsr->dfce_->link_has_geometry_[i])
1101  {
1102  gsr->link_body_decompositions_[i]->updatePose(state.getFrameTransform(ls->getName()));
1103  gsr->link_distance_fields_[i]->updatePose(state.getFrameTransform(ls->getName()));
1104  gsr->gradients_[i].closest_distance = DBL_MAX;
1105  gsr->gradients_[i].collision = false;
1106  gsr->gradients_[i].types.assign(gsr->link_body_decompositions_[i]->getCollisionSpheres().size(), NONE);
1107  gsr->gradients_[i].distances.assign(gsr->link_body_decompositions_[i]->getCollisionSpheres().size(), DBL_MAX);
1108  gsr->gradients_[i].gradients.assign(gsr->link_body_decompositions_[i]->getCollisionSpheres().size(),
1109  Eigen::Vector3d(0.0, 0.0, 0.0));
1110  gsr->gradients_[i].sphere_locations = gsr->link_body_decompositions_[i]->getSphereCenters();
1111  }
1112  }
1113 
1114  for (unsigned int i = 0; i < gsr->dfce_->attached_body_names_.size(); i++)
1115  {
1116  const moveit::core::AttachedBody* att = state.getAttachedBody(gsr->dfce_->attached_body_names_[i]);
1117  if (!att)
1118  {
1119  ROS_WARN("Attached body discrepancy");
1120  continue;
1121  }
1122 
1123  // TODO: This logic for checking attached body count might be incorrect
1124  if (gsr->attached_body_decompositions_.size() != att->getShapes().size())
1125  {
1126  ROS_WARN("Attached body size discrepancy");
1127  continue;
1128  }
1129 
1130  for (unsigned int j = 0; j < att->getShapes().size(); j++)
1131  {
1132  gsr->attached_body_decompositions_[i]->updatePose(j, att->getGlobalCollisionBodyTransforms()[j]);
1133  }
1134 
1135  gsr->gradients_[i + gsr->dfce_->link_names_.size()].closest_distance = DBL_MAX;
1136  gsr->gradients_[i + gsr->dfce_->link_names_.size()].collision = false;
1137  gsr->gradients_[i + gsr->dfce_->link_names_.size()].types.assign(
1138  gsr->attached_body_decompositions_[i]->getCollisionSpheres().size(), NONE);
1139  gsr->gradients_[i + gsr->dfce_->link_names_.size()].distances.assign(
1140  gsr->attached_body_decompositions_[i]->getCollisionSpheres().size(), DBL_MAX);
1141  gsr->gradients_[i + gsr->dfce_->link_names_.size()].gradients.assign(
1142  gsr->attached_body_decompositions_[i]->getCollisionSpheres().size(), Eigen::Vector3d(0.0, 0.0, 0.0));
1143  gsr->gradients_[i + gsr->dfce_->link_names_.size()].sphere_locations =
1144  gsr->attached_body_decompositions_[i]->getSphereCenters();
1145  }
1146 }
1147 
1148 void CollisionRobotDistanceField::getGroupStateRepresentation(const DistanceFieldCacheEntryConstPtr& dfce,
1149  const moveit::core::RobotState& state,
1150  GroupStateRepresentationPtr& gsr) const
1151 {
1152  if (!dfce->pregenerated_group_state_representation_)
1153  {
1154  ROS_DEBUG_STREAM("Creating GroupStateRepresentation");
1155 
1156  // unsigned int count = 0;
1157  gsr.reset(new GroupStateRepresentation());
1158  gsr->dfce_ = dfce;
1159  gsr->gradients_.resize(dfce->link_names_.size() + dfce->attached_body_names_.size());
1160 
1161  Eigen::Vector3d link_size;
1162  Eigen::Vector3d link_origin;
1163  for (unsigned int i = 0; i < dfce->link_names_.size(); i++)
1164  {
1165  const moveit::core::LinkModel* ls = state.getLinkModel(dfce->link_names_[i]);
1166  if (dfce->link_has_geometry_[i])
1167  {
1168  // create link body geometric decomposition
1169  gsr->link_body_decompositions_.push_back(getPosedLinkBodySphereDecomposition(ls, dfce->link_body_indices_[i]));
1170 
1171  // create and fill link distance field
1172  PosedBodySphereDecompositionPtr& link_bd = gsr->link_body_decompositions_.back();
1173  double diameter = 2 * link_bd->getBoundingSphereRadius();
1174  link_size = Eigen::Vector3d(diameter, diameter, diameter);
1175  link_origin = link_bd->getBoundingSphereCenter() - 0.5 * link_size;
1176 
1177  ROS_DEBUG_STREAM("Creating PosedDistanceField for link "
1178  << dfce->link_names_[i] << " with size [" << link_size.x() << ", " << link_size.y() << ", "
1179  << link_size.z() << "] and origin " << link_origin.x() << ", " << link_origin.y() << ", "
1180  << link_origin.z());
1181 
1182  gsr->link_distance_fields_.push_back(PosedDistanceFieldPtr(new PosedDistanceField(
1184  gsr->link_distance_fields_.back()->addPointsToField(link_bd->getCollisionPoints());
1185  ROS_DEBUG_STREAM("Created PosedDistanceField for link " << dfce->link_names_[i] << " with "
1186  << link_bd->getCollisionPoints().size() << " points");
1187 
1188  gsr->link_body_decompositions_.back()->updatePose(state.getFrameTransform(ls->getName()));
1189  gsr->link_distance_fields_.back()->updatePose(state.getFrameTransform(ls->getName()));
1190  gsr->gradients_[i].types.resize(gsr->link_body_decompositions_.back()->getCollisionSpheres().size(), NONE);
1191  gsr->gradients_[i].distances.resize(gsr->link_body_decompositions_.back()->getCollisionSpheres().size(),
1192  DBL_MAX);
1193  gsr->gradients_[i].gradients.resize(gsr->link_body_decompositions_.back()->getCollisionSpheres().size());
1194  gsr->gradients_[i].sphere_radii = gsr->link_body_decompositions_.back()->getSphereRadii();
1195  gsr->gradients_[i].joint_name = ls->getParentJointModel()->getName();
1196  }
1197  else
1198  {
1199  gsr->link_body_decompositions_.push_back(PosedBodySphereDecompositionPtr());
1200  gsr->link_distance_fields_.push_back(PosedDistanceFieldPtr());
1201  }
1202  }
1203  }
1204  else
1205  {
1206  gsr.reset(new GroupStateRepresentation(*(dfce->pregenerated_group_state_representation_)));
1207  gsr->dfce_ = dfce;
1208  gsr->gradients_.resize(dfce->link_names_.size() + dfce->attached_body_names_.size());
1209  for (unsigned int i = 0; i < dfce->link_names_.size(); i++)
1210  {
1211  const moveit::core::LinkModel* ls = state.getLinkModel(dfce->link_names_[i]);
1212  if (dfce->link_has_geometry_[i])
1213  {
1214  gsr->link_body_decompositions_[i]->updatePose(state.getFrameTransform(ls->getName()));
1215  gsr->link_distance_fields_[i]->updatePose(state.getFrameTransform(ls->getName()));
1216  gsr->gradients_[i].sphere_locations = gsr->link_body_decompositions_[i]->getSphereCenters();
1217  }
1218  }
1219  }
1220 
1221  for (unsigned int i = 0; i < dfce->attached_body_names_.size(); i++)
1222  {
1223  int link_index = dfce->attached_body_link_state_indices_[i];
1224  const moveit::core::LinkModel* ls =
1225  state.getJointModelGroup(gsr->dfce_->group_name_)->getUpdatedLinkModels()[link_index];
1226  // const moveit::core::LinkModel* ls =
1227  // state.getLinkStateVector()[dfce->attached_body_link_state_indices_[i]];
1230  gsr->attached_body_decompositions_.push_back(
1231  getAttachedBodySphereDecomposition(state.getAttachedBody(dfce->attached_body_names_[i]), resolution_));
1232  gsr->gradients_[i + dfce->link_names_.size()].types.resize(
1233  gsr->attached_body_decompositions_.back()->getCollisionSpheres().size(), NONE);
1234  gsr->gradients_[i + dfce->link_names_.size()].distances.resize(
1235  gsr->attached_body_decompositions_.back()->getCollisionSpheres().size(), DBL_MAX);
1236  gsr->gradients_[i + dfce->link_names_.size()].gradients.resize(
1237  gsr->attached_body_decompositions_.back()->getCollisionSpheres().size());
1238  gsr->gradients_[i + dfce->link_names_.size()].sphere_locations =
1239  gsr->attached_body_decompositions_.back()->getSphereCenters();
1240  gsr->gradients_[i + dfce->link_names_.size()].sphere_radii =
1241  gsr->attached_body_decompositions_.back()->getSphereRadii();
1242  gsr->gradients_[i + dfce->link_names_.size()].joint_name = ls->getParentJointModel()->getName();
1243  }
1244 }
1245 
1246 bool CollisionRobotDistanceField::compareCacheEntryToState(const DistanceFieldCacheEntryConstPtr& dfce,
1247  const moveit::core::RobotState& state) const
1248 {
1249  std::vector<double> new_state_values(state.getVariableCount());
1250  for (unsigned int i = 0; i < new_state_values.size(); i++)
1251  {
1252  new_state_values[i] = state.getVariablePosition(i);
1253  }
1254 
1255  if (dfce->state_values_.size() != new_state_values.size())
1256  {
1257  ROS_ERROR("State value size mismatch");
1258  return false;
1259  }
1260 
1261  for (unsigned int i = 0; i < dfce->state_check_indices_.size(); i++)
1262  {
1263  double diff =
1264  fabs(dfce->state_values_[dfce->state_check_indices_[i]] - new_state_values[dfce->state_check_indices_[i]]);
1265  if (diff > EPSILON)
1266  {
1267  ROS_WARN_STREAM("State for Variable " << state.getVariableNames()[dfce->state_check_indices_[i]]
1268  << " has changed by " << diff << " radians");
1269  return false;
1270  }
1271  }
1272  std::vector<const moveit::core::AttachedBody*> attached_bodies_dfce;
1273  std::vector<const moveit::core::AttachedBody*> attached_bodies_state;
1274  dfce->state_->getAttachedBodies(attached_bodies_dfce);
1275  state.getAttachedBodies(attached_bodies_state);
1276  if (attached_bodies_dfce.size() != attached_bodies_state.size())
1277  {
1278  return false;
1279  }
1280  // TODO - figure all the things that can change
1281  for (unsigned int i = 0; i < attached_bodies_dfce.size(); i++)
1282  {
1283  if (attached_bodies_dfce[i]->getName() != attached_bodies_state[i]->getName())
1284  {
1285  return false;
1286  }
1287  if (attached_bodies_dfce[i]->getTouchLinks() != attached_bodies_state[i]->getTouchLinks())
1288  {
1289  return false;
1290  }
1291  if (attached_bodies_dfce[i]->getShapes().size() != attached_bodies_state[i]->getShapes().size())
1292  {
1293  return false;
1294  }
1295  for (unsigned int j = 0; j < attached_bodies_dfce[i]->getShapes().size(); j++)
1296  {
1297  if (attached_bodies_dfce[i]->getShapes()[j] != attached_bodies_state[i]->getShapes()[j])
1298  {
1299  return false;
1300  }
1301  }
1302  }
1303  return true;
1304 }
1305 
1307  const DistanceFieldCacheEntryConstPtr& dfce, const collision_detection::AllowedCollisionMatrix& acm) const
1308 {
1309  if (dfce->acm_.getSize() != acm.getSize())
1310  {
1311  ROS_DEBUG("Allowed collision matrix size mismatch");
1312  return false;
1313  }
1314  std::vector<const moveit::core::AttachedBody*> attached_bodies;
1315  dfce->state_->getAttachedBodies(attached_bodies);
1316  for (unsigned int i = 0; i < dfce->link_names_.size(); i++)
1317  {
1318  std::string link_name = dfce->link_names_[i];
1319  if (dfce->link_has_geometry_[i])
1320  {
1321  bool self_collision_enabled = true;
1323  if (acm.getEntry(link_name, link_name, t))
1324  {
1326  {
1327  self_collision_enabled = false;
1328  }
1329  }
1330  if (self_collision_enabled != dfce->self_collision_enabled_[i])
1331  {
1332  // ROS_INFO_STREAM("Self collision for " << link_name << " went from "
1333  // << dfce->self_collision_enabled_[i] << " to " <<
1334  // self_collision_enabled);
1335  return false;
1336  }
1337  for (unsigned int j = i; j < dfce->link_names_.size(); j++)
1338  {
1339  if (i == j)
1340  continue;
1341  if (dfce->link_has_geometry_[j])
1342  {
1343  bool intra_collision_enabled = true;
1344  if (acm.getEntry(link_name, dfce->link_names_[j], t))
1345  {
1347  {
1348  intra_collision_enabled = false;
1349  }
1350  }
1351  if (dfce->intra_group_collision_enabled_[i][j] != intra_collision_enabled)
1352  {
1353  // std::cerr << "Intra collision for " << dfce->link_names_[i] << "
1354  // " << dfce->link_names_[j]
1355  // << " went from " <<
1356  // dfce->intra_group_collision_enabled_[i][j] << " to " <<
1357  // intra_collision_enabled << std::endl;
1358  return false;
1359  }
1360  }
1361  }
1362  }
1363  }
1364  return true;
1365 }
1366 
1367 // void
1368 // CollisionRobotDistanceField::generateAllowedCollisionInformation(CollisionRobotDistanceField::DistanceFieldCacheEntryPtr&
1369 // dfce)
1370 // {
1371 // for(unsigned int i = 0; i < dfce.link_names_.size(); i++) {
1372 // for(unsigned int j = 0; j <
1373 // if(dfce->acm.find
1374 // }
1375 // }
1376 } // namespace collision_detection
void checkSelfCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const moveit::core::RobotState &state) const override
Check for self collision. Any collision between any pair of links is checked for, NO collisions are i...
PosedBodySphereDecompositionVectorPtr getAttachedBodySphereDecomposition(const robot_state::AttachedBody *att, double resolution)
void checkSelfCollisionHelper(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix *acm, GroupStateRepresentationPtr &gsr) const
const EigenSTL::vector_Isometry3d & getGlobalCollisionBodyTransforms() const
Get the global transforms for the collision bodies.
Core components of MoveIt!
const std::vector< shapes::ShapeConstPtr > & getShapes() const
Get shape associated to the collision geometry for this link.
Definition: link_model.h:174
static const double DEFAULT_RESOLUTION
bool getEntry(const std::string &name1, const std::string &name2, AllowedCollision::Type &allowed_collision_type) const
Get the type of the allowed collision between two elements. Return true if the entry is included in t...
double getVariablePosition(const std::string &variable) const
Get the position of a particular variable. An exception is thrown if the variable is not known...
Definition: robot_state.h:240
Collisions between the pair of bodies is never ok, i.e., if two bodies are in contact in a particular...
bool doBoundingSpheresIntersect(const PosedBodySphereDecompositionConstPtr &p1, const PosedBodySphereDecompositionConstPtr &p2)
bool compareCacheEntryToState(const DistanceFieldCacheEntryConstPtr &dfce, const moveit::core::RobotState &state) const
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:90
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
BodyType body_type_2
The type of the second body involved in the contact.
const JointModel * getParentJointModel() const
Get the joint model whose child this link is. There will always be a parent joint.
Definition: link_model.h:108
bool getCollisionSphereGradients(const distance_field::DistanceField *distance_field, const std::vector< CollisionSphere > &sphere_list, const EigenSTL::vector_Vector3d &sphere_centers, GradientInfo &gradient, const CollisionType &type, double tolerance, bool subtract_radii, double maximum_value, bool stop_at_first_collision)
bool getSelfCollisions(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, GroupStateRepresentationPtr &gsr) const
ContactMap contacts
A map returning the pairs of body ids in contact, plus their contact details.
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
void createCollisionModelMarker(const moveit::core::RobotState &state, visualization_msgs::MarkerArray &model_markers) const
void setPadding(double padding)
Set the link padding (for every link)
ROSCONSOLE_CONSOLE_IMPL_DECL std::string getName(void *handle)
const AttachedBody * getAttachedBody(const std::string &name) const
Get the attached body named name. Return NULL if not found.
void initialize(const std::map< std::string, std::vector< CollisionSphere >> &link_body_decompositions, const Eigen::Vector3d &size, const Eigen::Vector3d &origin, bool use_signed_distance_field, double resolution, double collision_tolerance, double max_propogation_distance)
#define ROS_ERROR(...)
std::map< std::string, unsigned int > link_body_decomposition_index_map_
#define ROS_WARN(...)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW CollisionRobotDistanceField(const robot_model::RobotModelConstPtr &robot_model)
PosedBodyPointDecompositionPtr getPosedLinkBodyPointDecomposition(const moveit::core::LinkModel *ls) const
geometry_msgs::TransformStamped t
DistanceFieldCacheEntryPtr generateDistanceFieldCacheEntry(const std::string &group_name, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix *acm, bool generate_distance_field) const
static const double DEFAULT_MAX_PROPOGATION_DISTANCE
Generic interface to collision detection.
#define ROS_DEBUG(...)
const JointModelGroup * getJointModelGroup(const std::string &group) const
Get the model of a particular joint group.
Definition: robot_state.h:167
bool collision
True if collision was found, false otherwise.
robot_model::RobotModelConstPtr robot_model_
The kinematic model corresponding to this collision model.
PosedBodySphereDecompositionPtr getPosedLinkBodySphereDecomposition(const moveit::core::LinkModel *ls, unsigned int ind) const
void updateGroupStateRepresentationState(const moveit::core::RobotState &state, GroupStateRepresentationPtr &gsr) const
void getAttachedBodies(std::vector< const AttachedBody *> &attached_bodies) const
Get all bodies attached to the model corresponding to this state.
std::size_t max_contacts
Overall maximum number of contacts to compute.
PosedBodyPointDecompositionVectorPtr getAttachedBodyPointDecomposition(const robot_state::AttachedBody *att, double resolution)
static const double DEFAULT_COLLISION_TOLERANCE
bool getIntraGroupProximityGradients(GroupStateRepresentationPtr &gsr) const
std::map< std::string, std::map< std::string, bool > > in_group_update_map_
This class maintains the representation of the environment as seen by a planning instance. The environment geometry, the robot geometry and state are maintained.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Vector3d pos
contact position
const std::vector< std::string > & getUpdatedLinkModelNames() const
Get the names of the links returned by getUpdatedLinkModels()
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
#define ROS_ASSERT_MSG(cond,...)
This class represents a collision model of the robot and can be used for self collision checks (to ch...
const std::string & getName() const
Get the name of the joint group.
std::string group_name
The group name to check collisions for (optional; if empty, assume the complete robot) ...
bool getIntraGroupCollisions(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, GroupStateRepresentationPtr &gsr) const
std::string body_name_2
The id of the second body involved in the contact.
std::size_t getSize() const
Get the size of the allowed collision matrix (number of specified entries)
bool contacts
If true, compute contacts. Otherwise only a binary collision yes/no is reported.
bool compareCacheEntryToAllowedCollisionMatrix(const DistanceFieldCacheEntryConstPtr &dfce, const collision_detection::AllowedCollisionMatrix &acm) const
const std::string & getName() const
The name of this link.
Definition: link_model.h:81
#define ROS_WARN_STREAM(args)
#define ROS_DEBUG_STREAM(args)
bool getSelfProximityGradients(GroupStateRepresentationPtr &gsr) const
const Eigen::Isometry3d & getFrameTransform(const std::string &frame_id)
Get the transformation matrix from the model frame to the frame identified by frame_id.
#define ROS_DEBUG_COND(cond,...)
B toMsg(const A &a)
std::string body_name_1
The id of the first body involved in the contact.
const LinkModel * getLinkModel(const std::string &link) const
Get the model of a particular link.
Definition: robot_state.h:155
std::size_t max_contacts_per_pair
Maximum number of contacts to compute per pair of bodies (multiple bodies may be in contact at differ...
std::size_t getVariableCount() const
Get the number of variables that make up this state.
Definition: robot_state.h:143
std::vector< BodyDecompositionConstPtr > link_body_decomposition_vector_
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
Definition: robot_state.h:1441
const std::vector< std::string > & getUpdatedLinkModelsWithGeometryNames() const
Get the names of the links returned by getUpdatedLinkModels()
static const bool DEFAULT_USE_SIGNED_DISTANCE_FIELD
#define ROS_ERROR_NAMED(name,...)
const std::map< std::string, double > & getLinkPadding() const
Get the link paddings as a map (from link names to padding value)
const std::vector< const LinkModel * > & getUpdatedLinkModels() const
Get the names of the links that are to be updated when the state of this group changes. This includes links that are in the kinematic model but outside this group, if those links are descendants of joints in this group that have their values updated. The order is the correct order for updating the corresponding states.
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:122
Collisions between a particular pair of bodies does not imply that the robot configuration is in coll...
BodyType body_type_1
The type of the first body involved in the contact.
void updateLinkTransforms()
Update the reference frame transforms for links. This call is needed before using the transforms of l...
const std::vector< shapes::ShapeConstPtr > & getShapes() const
Get the shapes that make up this attached body.
Definition: attached_body.h:88
A link from the robot. Contains the constant transform applied to the link and its geometry...
Definition: link_model.h:72
A DistanceField implementation that uses a vector propagation method. Distances propagate outward fro...
std::map< std::string, GroupStateRepresentationPtr > pregenerated_group_state_representation_map_
void generateCollisionCheckingStructures(const std::string &group_name, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix *acm, GroupStateRepresentationPtr &gsr, bool generate_distance_field) const
Object defining bodies that can be attached to robot links. This is useful when handling objects pick...
Definition: attached_body.h:56
const std::vector< std::string > & getVariableNames() const
Get the names of the variables that make up this state, in the order they are stored in memory...
Definition: robot_state.h:149
bool getCollisionSphereCollision(const distance_field::DistanceField *distance_field, const std::vector< CollisionSphere > &sphere_list, const EigenSTL::vector_Vector3d &sphere_centers, double maximum_value, double tolerance)
DistanceFieldCacheEntryConstPtr getDistanceFieldCacheEntry(const std::string &group_name, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix *acm) const
const std::string & getName() const
Get the name of the joint.
Definition: joint_model.h:131
void getGroupStateRepresentation(const DistanceFieldCacheEntryConstPtr &dfce, const moveit::core::RobotState &state, GroupStateRepresentationPtr &gsr) const


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Tue Sep 8 2020 04:12:44