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>
44 
45 namespace collision_detection
46 {
47 const double EPSILON = 0.001f;
48 
49 CollisionRobotDistanceField::CollisionRobotDistanceField(const robot_model::RobotModelConstPtr& kmodel)
50  : CollisionRobot(kmodel)
51 {
52  // planning_scene_.reset(new planning_scene::PlanningScene(robot_model_));
53 
54  std::map<std::string, std::vector<CollisionSphere>> link_body_decompositions;
55  Eigen::Vector3d size(DEFAULT_SIZE_X, DEFAULT_SIZE_Y, DEFAULT_SIZE_Z);
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& kmodel,
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(kmodel, 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 
72 CollisionRobotDistanceField::CollisionRobotDistanceField(const CollisionRobot& col_robot, const Eigen::Vector3d& size,
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;
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, NULL, gsr);
216 }
217 
220  const moveit::core::RobotState& state,
221  GroupStateRepresentationPtr& gsr) const
222 {
223  checkSelfCollisionHelper(req, res, state, NULL, 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 = getCollisionSphereCollision(
277  gsr->dfce_->distance_field_.get(), *collision_spheres_1, *sphere_centers_1, max_propogation_distance_,
278  collision_tolerance_, std::min(req.max_contacts_per_pair, req.max_contacts - res.contact_count), colls);
279  if (coll)
280  {
281  res.collision = true;
282  for (unsigned int j = 0; j < colls.size(); j++)
283  {
285  if (is_link)
286  {
287  con.pos = gsr->link_body_decompositions_[i]->getSphereCenters()[colls[j]];
289  con.body_name_1 = gsr->dfce_->link_names_[i];
290  }
291  else
292  {
293  con.pos =
294  gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters()[colls[j]];
296  con.body_name_1 = gsr->dfce_->attached_body_names_[i - gsr->dfce_->link_names_.size()];
297  }
298 
299  ROS_DEBUG_STREAM("Self collision detected for link " << con.body_name_1);
300 
302  con.body_name_2 = "self";
303  res.contact_count++;
304  res.contacts[std::pair<std::string, std::string>(con.body_name_1, con.body_name_2)].push_back(con);
305  gsr->gradients_[i].types[colls[j]] = SELF;
306  }
307  gsr->gradients_[i].collision = true;
308 
309  if (res.contact_count >= req.max_contacts)
310  {
311  return true;
312  }
313  }
314  }
315  else
316  {
317  bool coll = getCollisionSphereCollision(gsr->dfce_->distance_field_.get(), *collision_spheres_1,
318  *sphere_centers_1, max_propogation_distance_, collision_tolerance_);
319  if (coll)
320  {
321  ROS_DEBUG("Link %s in self collision", gsr->dfce_->link_names_[i].c_str());
322  res.collision = true;
323  return true;
324  }
325  }
326  }
327  return (res.contact_count >= req.max_contacts);
328 }
329 
330 bool CollisionRobotDistanceField::getSelfProximityGradients(GroupStateRepresentationPtr& gsr) const
331 {
332  bool in_collision = false;
333 
334  for (unsigned int i = 0; i < gsr->dfce_->link_names_.size(); i++)
335  {
336  const std::string& link_name = gsr->dfce_->link_names_[i];
337  bool is_link = i < gsr->dfce_->link_names_.size();
338  if ((is_link && !gsr->dfce_->link_has_geometry_[i]) || !gsr->dfce_->self_collision_enabled_[i])
339  {
340  continue;
341  }
342 
343  const std::vector<CollisionSphere>* collision_spheres_1;
344  const EigenSTL::vector_Vector3d* sphere_centers_1;
345  if (is_link)
346  {
347  collision_spheres_1 = &(gsr->link_body_decompositions_[i]->getCollisionSpheres());
348  sphere_centers_1 = &(gsr->link_body_decompositions_[i]->getSphereCenters());
349  }
350  else
351  {
352  collision_spheres_1 =
353  &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getCollisionSpheres());
354  sphere_centers_1 = &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters());
355  }
356 
357  // computing distance gradients by checking collisions against other mobile
358  // links as indicated by the AllowedCollisionMatrix
359  bool coll = false;
360  if (gsr->dfce_->acm_.getSize() > 0)
361  {
362  AllowedCollision::Type col_type;
363  for (unsigned int j = 0; j < gsr->dfce_->link_names_.size(); j++)
364  {
365  // on self collisions skip
366  if (link_name == gsr->dfce_->link_names_[j])
367  {
368  continue;
369  }
370 
371  // on collision exceptions skip
372  if (gsr->dfce_->acm_.getEntry(link_name, gsr->dfce_->link_names_[j], col_type) &&
373  col_type != AllowedCollision::NEVER)
374  {
375  continue;
376  }
377 
378  if (gsr->link_distance_fields_[j])
379  {
380  coll = gsr->link_distance_fields_[j]->getCollisionSphereGradients(
381  *collision_spheres_1, *sphere_centers_1, gsr->gradients_[i], collision_detection::SELF,
383 
384  if (coll)
385  {
386  in_collision = true;
387  }
388  }
389  }
390  }
391 
392  coll = getCollisionSphereGradients(gsr->dfce_->distance_field_.get(), *collision_spheres_1, *sphere_centers_1,
393  gsr->gradients_[i], collision_detection::SELF, collision_tolerance_, false,
395 
396  if (coll)
397  {
398  in_collision = true;
399  }
400  }
401 
402  return in_collision;
403 }
404 
407  GroupStateRepresentationPtr& gsr) const
408 {
409  unsigned int num_links = gsr->dfce_->link_names_.size();
410  unsigned int num_attached_bodies = gsr->dfce_->attached_body_names_.size();
411 
412  for (unsigned int i = 0; i < num_links + num_attached_bodies; i++)
413  {
414  for (unsigned int j = i + 1; j < num_links + num_attached_bodies; j++)
415  {
416  if (i == j)
417  continue;
418  bool i_is_link = i < num_links;
419  bool j_is_link = j < num_links;
420 
421  if ((i_is_link && !gsr->dfce_->link_has_geometry_[i]) || (j_is_link && !gsr->dfce_->link_has_geometry_[j]))
422  continue;
423 
424  if (!gsr->dfce_->intra_group_collision_enabled_[i][j])
425  continue;
426 
427  if (i_is_link && j_is_link &&
428  !doBoundingSpheresIntersect(gsr->link_body_decompositions_[i], gsr->link_body_decompositions_[j]))
429  {
430  // ROS_DEBUG_STREAM("Bounding spheres for " <<
431  // gsr->dfce_->link_names_[i] << " and " << gsr->dfce_->link_names_[j]
432  //<< " don't intersect");
433  continue;
434  }
435  else if (!i_is_link || !j_is_link)
436  {
437  bool all_ok = true;
438  if (!i_is_link && j_is_link)
439  {
440  for (unsigned int k = 0; k < gsr->attached_body_decompositions_[i - num_links]->getSize(); k++)
441  {
443  gsr->link_body_decompositions_[j],
444  gsr->attached_body_decompositions_[i - num_links]->getPosedBodySphereDecomposition(k)))
445  {
446  all_ok = false;
447  break;
448  }
449  }
450  }
451  else if (i_is_link && !j_is_link)
452  {
453  for (unsigned int k = 0; k < gsr->attached_body_decompositions_[j - num_links]->getSize(); k++)
454  {
456  gsr->link_body_decompositions_[i],
457  gsr->attached_body_decompositions_[j - num_links]->getPosedBodySphereDecomposition(k)))
458  {
459  all_ok = false;
460  break;
461  }
462  }
463  }
464  else
465  {
466  for (unsigned int k = 0; k < gsr->attached_body_decompositions_[i - num_links]->getSize() && all_ok; k++)
467  {
468  for (unsigned int l = 0; l < gsr->attached_body_decompositions_[j - num_links]->getSize(); l++)
469  {
471  gsr->attached_body_decompositions_[i - num_links]->getPosedBodySphereDecomposition(k),
472  gsr->attached_body_decompositions_[j - num_links]->getPosedBodySphereDecomposition(l)))
473  {
474  all_ok = false;
475  break;
476  }
477  }
478  }
479  }
480  if (all_ok)
481  {
482  continue;
483  }
484  // std::cerr << "Bounding spheres for " << gsr->dfce_->link_names_[i] <<
485  // " and " << gsr->dfce_->link_names_[j]
486  // << " intersect" << std::endl;
487  }
488  int num_pair = -1;
489  std::string name_1;
490  std::string name_2;
491  if (i_is_link)
492  {
493  name_1 = gsr->dfce_->link_names_[i];
494  }
495  else
496  {
497  name_1 = gsr->dfce_->attached_body_names_[i - num_links];
498  }
499 
500  if (j_is_link)
501  {
502  name_2 = gsr->dfce_->link_names_[j];
503  }
504  else
505  {
506  name_2 = gsr->dfce_->attached_body_names_[j - num_links];
507  }
508  if (req.contacts)
509  {
510  collision_detection::CollisionResult::ContactMap::iterator it =
511  res.contacts.find(std::pair<std::string, std::string>(name_1, name_2));
512  if (it == res.contacts.end())
513  {
514  num_pair = 0;
515  }
516  else
517  {
518  num_pair = it->second.size();
519  }
520  }
521  const std::vector<CollisionSphere>* collision_spheres_1;
522  const std::vector<CollisionSphere>* collision_spheres_2;
523  const EigenSTL::vector_Vector3d* sphere_centers_1;
524  const EigenSTL::vector_Vector3d* sphere_centers_2;
525  if (i_is_link)
526  {
527  collision_spheres_1 = &(gsr->link_body_decompositions_[i]->getCollisionSpheres());
528  sphere_centers_1 = &(gsr->link_body_decompositions_[i]->getSphereCenters());
529  }
530  else
531  {
532  collision_spheres_1 = &(gsr->attached_body_decompositions_[i - num_links]->getCollisionSpheres());
533  sphere_centers_1 = &(gsr->attached_body_decompositions_[i - num_links]->getSphereCenters());
534  }
535  if (j_is_link)
536  {
537  collision_spheres_2 = &(gsr->link_body_decompositions_[j]->getCollisionSpheres());
538  sphere_centers_2 = &(gsr->link_body_decompositions_[j]->getSphereCenters());
539  }
540  else
541  {
542  collision_spheres_2 = &(gsr->attached_body_decompositions_[j - num_links]->getCollisionSpheres());
543  sphere_centers_2 = &(gsr->attached_body_decompositions_[j - num_links]->getSphereCenters());
544  }
545 
546  for (unsigned int k = 0; k < collision_spheres_1->size() && num_pair < (int)req.max_contacts_per_pair; k++)
547  {
548  for (unsigned int l = 0; l < collision_spheres_2->size() && num_pair < (int)req.max_contacts_per_pair; l++)
549  {
550  Eigen::Vector3d gradient = (*sphere_centers_1)[k] - (*sphere_centers_2)[l];
551  double dist = gradient.norm();
552  // std::cerr << "Dist is " << dist << " rad " <<
553  // (*collision_spheres_1)[k].radius_+(*collision_spheres_2)[l].radius_
554  // << std::endl;
555 
556  if (dist < (*collision_spheres_1)[k].radius_ + (*collision_spheres_2)[l].radius_)
557  {
558  // ROS_DEBUG("Intra-group contact between %s and %s, d =
559  // %f < r1 = %f + r2 = %f", name_1.c_str(),
560  // name_2.c_str(),
561  // dist ,(*collision_spheres_1)[k].radius_
562  // ,(*collision_spheres_2)[l].radius_);
563  // Eigen::Vector3d sc1 = (*sphere_centers_1)[k];
564  // Eigen::Vector3d sc2 = (*sphere_centers_2)[l];
565  // ROS_DEBUG("sphere center 1:[ %f, %f, %f ], sphere
566  // center 2: [%f, %f,%f ], lbdc size =
567  // %i",sc1[0],sc1[1],sc1[2],
568  // sc2[0],sc2[1],sc2[2],int(gsr->link_body_decompositions_.size()));
569  res.collision = true;
570 
571  if (req.contacts)
572  {
574  con.pos = gsr->link_body_decompositions_[i]->getSphereCenters()[k];
575  con.body_name_1 = name_1;
576  con.body_name_2 = name_2;
577  if (i_is_link)
578  {
580  }
581  else
582  {
584  }
585  if (j_is_link)
586  {
588  }
589  else
590  {
592  }
593  res.contact_count++;
594  res.contacts[std::pair<std::string, std::string>(con.body_name_1, con.body_name_2)].push_back(con);
595  num_pair++;
596  // std::cerr << "Pushing back intra " << con.body_name_1 << " and
597  // " << con.body_name_2 << std::endl;
598  gsr->gradients_[i].types[k] = INTRA;
599  gsr->gradients_[i].collision = true;
600  gsr->gradients_[j].types[l] = INTRA;
601  gsr->gradients_[j].collision = true;
602  // ROS_INFO_STREAM("Sphere 1 " << (*sphere_centers_1)[k]);
603  // ROS_INFO_STREAM("Sphere 2 " << (*sphere_centers_2)[l]);
604  // ROS_INFO_STREAM("Norm " << gradient.norm());
605  // ROS_INFO_STREAM("Dist is " << dist
606  // << " radius 1 " <<
607  // (*collision_spheres_1)[k].radius_
608  // << " radius 2 " <<
609  // (*collision_spheres_2)[l].radius_);
610  // ROS_INFO_STREAM("Gradient " << gradient);
611  // ROS_INFO_STREAM("Spheres intersect for " <<
612  // gsr->dfce_->link_names_[i] << " and " <<
613  // gsr->dfce_->link_names_[j]);
614  // std::cerr << "Spheres intersect for " <<
615  // gsr->dfce_->link_names_[i] << " and " <<
616  // gsr->dfce_->link_names_[j] << std::cerr;
617  if (res.contact_count >= req.max_contacts)
618  {
619  return true;
620  }
621  }
622  else
623  {
624  return true;
625  }
626  }
627  }
628  }
629  }
630  }
631  return false;
632 }
633 
634 bool CollisionRobotDistanceField::getIntraGroupProximityGradients(GroupStateRepresentationPtr& gsr) const
635 {
636  bool in_collision = false;
637  unsigned int num_links = gsr->dfce_->link_names_.size();
638  unsigned int num_attached_bodies = gsr->dfce_->attached_body_names_.size();
639  // TODO - deal with attached bodies
640  for (unsigned int i = 0; i < num_links + num_attached_bodies; i++)
641  {
642  for (unsigned int j = i + 1; j < num_links + num_attached_bodies; j++)
643  {
644  if (i == j)
645  continue;
646  bool i_is_link = i < num_links;
647  bool j_is_link = j < num_links;
648  if ((i_is_link && !gsr->dfce_->link_has_geometry_[i]) || (j_is_link && !gsr->dfce_->link_has_geometry_[j]))
649  continue;
650  if (!gsr->dfce_->intra_group_collision_enabled_[i][j])
651  continue;
652  const std::vector<CollisionSphere>* collision_spheres_1;
653  const std::vector<CollisionSphere>* collision_spheres_2;
654  const EigenSTL::vector_Vector3d* sphere_centers_1;
655  const EigenSTL::vector_Vector3d* sphere_centers_2;
656  if (i_is_link)
657  {
658  collision_spheres_1 = &(gsr->link_body_decompositions_[i]->getCollisionSpheres());
659  sphere_centers_1 = &(gsr->link_body_decompositions_[i]->getSphereCenters());
660  }
661  else
662  {
663  collision_spheres_1 = &(gsr->attached_body_decompositions_[i - num_links]->getCollisionSpheres());
664  sphere_centers_1 = &(gsr->attached_body_decompositions_[i - num_links]->getSphereCenters());
665  }
666  if (j_is_link)
667  {
668  collision_spheres_2 = &(gsr->link_body_decompositions_[j]->getCollisionSpheres());
669  sphere_centers_2 = &(gsr->link_body_decompositions_[j]->getSphereCenters());
670  }
671  else
672  {
673  collision_spheres_2 = &(gsr->attached_body_decompositions_[j - num_links]->getCollisionSpheres());
674  sphere_centers_2 = &(gsr->attached_body_decompositions_[j - num_links]->getSphereCenters());
675  }
676  for (unsigned int k = 0; k < collision_spheres_1->size(); k++)
677  {
678  for (unsigned int l = 0; l < collision_spheres_2->size(); l++)
679  {
680  Eigen::Vector3d gradient = (*sphere_centers_1)[k] - (*sphere_centers_2)[l];
681  double dist = gradient.norm();
682  if (dist < gsr->gradients_[i].distances[k])
683  {
684  gsr->gradients_[i].distances[k] = dist;
685  gsr->gradients_[i].gradients[k] = gradient;
686  gsr->gradients_[i].types[k] = INTRA;
687  }
688  if (dist < gsr->gradients_[j].distances[l])
689  {
690  gsr->gradients_[j].distances[l] = dist;
691  gsr->gradients_[j].gradients[l] = -gradient;
692  gsr->gradients_[j].types[l] = INTRA;
693  }
694  }
695  }
696  }
697  }
698  return in_collision;
699 }
701  const std::string& group_name, const moveit::core::RobotState& state,
702  const collision_detection::AllowedCollisionMatrix* acm, bool generate_distance_field) const
703 {
704  DistanceFieldCacheEntryPtr dfce(new DistanceFieldCacheEntry());
705 
706  if (robot_model_->getJointModelGroup(group_name) == NULL)
707  {
708  ROS_WARN("No group %s", group_name.c_str());
709  return dfce;
710  }
711 
712  dfce->group_name_ = group_name;
713  dfce->state_.reset(new moveit::core::RobotState(state));
714  if (acm)
715  {
716  dfce->acm_ = *acm;
717  }
718  else
719  {
720  ROS_WARN_STREAM("Allowed Collision Matrix is null, enabling all collisions "
721  "in the DistanceFieldCacheEntry");
722  }
723 
724  // generateAllowedCollisionInformation(dfce);
725  dfce->link_names_ = robot_model_->getJointModelGroup(group_name)->getUpdatedLinkModelNames();
726  std::vector<const moveit::core::AttachedBody*> all_attached_bodies;
727  dfce->state_->getAttachedBodies(all_attached_bodies);
728  unsigned int att_count = 0;
729 
730  // may be bigger than necessary
731  std::vector<bool> all_true(dfce->link_names_.size() + all_attached_bodies.size(), true);
732  std::vector<bool> all_false(dfce->link_names_.size() + all_attached_bodies.size(), false);
733 
734  const std::vector<const moveit::core::LinkModel*>& lsv = state.getJointModelGroup(group_name)->getUpdatedLinkModels();
735  dfce->self_collision_enabled_.resize(dfce->link_names_.size() + all_attached_bodies.size(), true);
736  dfce->intra_group_collision_enabled_.resize(dfce->link_names_.size() + all_attached_bodies.size());
737 
738  for (unsigned int i = 0; i < dfce->link_names_.size(); i++)
739  {
740  std::string link_name = dfce->link_names_[i];
741  const moveit::core::LinkModel* link_state = dfce->state_->getLinkModel(link_name);
742  bool found = false;
743 
744  for (unsigned int j = 0; j < lsv.size(); j++)
745  {
746  if (lsv[j]->getName() == link_name)
747  {
748  dfce->link_state_indices_.push_back(j);
749  found = true;
750  break;
751  }
752  }
753 
754  if (!found)
755  {
756  ROS_DEBUG("No link state found for link %s", dfce->link_names_[i].c_str());
757  continue;
758  }
759 
760  if (link_state->getShapes().size() > 0)
761  {
762  dfce->link_has_geometry_.push_back(true);
763  dfce->link_body_indices_.push_back(link_body_decomposition_index_map_.find(link_name)->second);
764 
765  if (acm)
766  {
768  if (acm->getEntry(link_name, link_name, t) && t == collision_detection::AllowedCollision::ALWAYS)
769  {
770  dfce->self_collision_enabled_[i] = false;
771  }
772 
773  dfce->intra_group_collision_enabled_[i] = all_true;
774  for (unsigned int j = i + 1; j < dfce->link_names_.size(); j++)
775  {
776  if (link_name == dfce->link_names_[j])
777  {
778  dfce->intra_group_collision_enabled_[i][j] = false;
779  continue;
780  }
781  if (acm->getEntry(link_name, dfce->link_names_[j], t) && t == collision_detection::AllowedCollision::ALWAYS)
782  {
783  dfce->intra_group_collision_enabled_[i][j] = false;
784  }
785  }
786 
787  std::vector<const moveit::core::AttachedBody*> link_attached_bodies;
788  state.getAttachedBodies(link_attached_bodies, link_state);
789  for (unsigned int j = 0; j < link_attached_bodies.size(); j++, att_count++)
790  {
791  dfce->attached_body_names_.push_back(link_attached_bodies[j]->getName());
792  dfce->attached_body_link_state_indices_.push_back(dfce->link_state_indices_[i]);
793 
794  if (acm->getEntry(link_name, link_attached_bodies[j]->getName(), t))
795  {
797  {
798  dfce->intra_group_collision_enabled_[i][att_count + dfce->link_names_.size()] = false;
799  }
800  }
801  // std::cerr << "Checking touch links for " << link_name << " and " <<
802  // attached_bodies[j]->getName()
803  // << " num " << attached_bodies[j]->getTouchLinks().size()
804  // << std::endl;
805  // touch links take priority
806  if (link_attached_bodies[j]->getTouchLinks().find(link_name) !=
807  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  tf::pointEigenToMsg(sphere_representation->getSphereCenters()[j], sphere_marker.pose.position);
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::Affine3d::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 
1072  const moveit::core::LinkModel* ls, unsigned int ind) const
1073 {
1074  PosedBodySphereDecompositionPtr ret;
1076  return ret;
1077 }
1078 
1079 PosedBodyPointDecompositionPtr
1081 {
1082  PosedBodyPointDecompositionPtr ret;
1083  std::map<std::string, unsigned int>::const_iterator it = link_body_decomposition_index_map_.find(ls->getName());
1084  if (it == link_body_decomposition_index_map_.end())
1085  {
1086  ROS_ERROR_NAMED("collision_distance_field", "No link body decomposition for link %s.", ls->getName().c_str());
1087  return ret;
1088  }
1090  return ret;
1091 }
1092 
1094  GroupStateRepresentationPtr& gsr) const
1095 {
1096  for (unsigned int i = 0; i < gsr->dfce_->link_names_.size(); i++)
1097  {
1098  const moveit::core::LinkModel* ls = state.getLinkModel(gsr->dfce_->link_names_[i]);
1099  if (gsr->dfce_->link_has_geometry_[i])
1100  {
1101  gsr->link_body_decompositions_[i]->updatePose(state.getFrameTransform(ls->getName()));
1102  gsr->link_distance_fields_[i]->updatePose(state.getFrameTransform(ls->getName()));
1103  gsr->gradients_[i].closest_distance = DBL_MAX;
1104  gsr->gradients_[i].collision = false;
1105  gsr->gradients_[i].types.assign(gsr->link_body_decompositions_[i]->getCollisionSpheres().size(), NONE);
1106  gsr->gradients_[i].distances.assign(gsr->link_body_decompositions_[i]->getCollisionSpheres().size(), DBL_MAX);
1107  gsr->gradients_[i].gradients.assign(gsr->link_body_decompositions_[i]->getCollisionSpheres().size(),
1108  Eigen::Vector3d(0.0, 0.0, 0.0));
1109  gsr->gradients_[i].sphere_locations = gsr->link_body_decompositions_[i]->getSphereCenters();
1110  }
1111  }
1112 
1113  for (unsigned int i = 0; i < gsr->dfce_->attached_body_names_.size(); i++)
1114  {
1115  const moveit::core::AttachedBody* att = state.getAttachedBody(gsr->dfce_->attached_body_names_[i]);
1116  if (!att)
1117  {
1118  ROS_WARN("Attached body discrepancy");
1119  continue;
1120  }
1121 
1122  // TODO: This logic for checking attached body count might be incorrect
1123  if (gsr->attached_body_decompositions_.size() != att->getShapes().size())
1124  {
1125  ROS_WARN("Attached body size discrepancy");
1126  continue;
1127  }
1128 
1129  for (unsigned int j = 0; j < att->getShapes().size(); j++)
1130  {
1131  gsr->attached_body_decompositions_[i]->updatePose(j, att->getGlobalCollisionBodyTransforms()[j]);
1132  }
1133 
1134  gsr->gradients_[i + gsr->dfce_->link_names_.size()].closest_distance = DBL_MAX;
1135  gsr->gradients_[i + gsr->dfce_->link_names_.size()].collision = false;
1136  gsr->gradients_[i + gsr->dfce_->link_names_.size()].types.assign(
1137  gsr->attached_body_decompositions_[i]->getCollisionSpheres().size(), NONE);
1138  gsr->gradients_[i + gsr->dfce_->link_names_.size()].distances.assign(
1139  gsr->attached_body_decompositions_[i]->getCollisionSpheres().size(), DBL_MAX);
1140  gsr->gradients_[i + gsr->dfce_->link_names_.size()].gradients.assign(
1141  gsr->attached_body_decompositions_[i]->getCollisionSpheres().size(), Eigen::Vector3d(0.0, 0.0, 0.0));
1142  gsr->gradients_[i + gsr->dfce_->link_names_.size()].sphere_locations =
1143  gsr->attached_body_decompositions_[i]->getSphereCenters();
1144  }
1145 }
1146 
1147 void CollisionRobotDistanceField::getGroupStateRepresentation(const DistanceFieldCacheEntryConstPtr& dfce,
1148  const moveit::core::RobotState& state,
1149  GroupStateRepresentationPtr& gsr) const
1150 {
1151  if (!dfce->pregenerated_group_state_representation_)
1152  {
1153  ROS_DEBUG_STREAM("Creating GroupStateRepresentation");
1154 
1155  // unsigned int count = 0;
1156  gsr.reset(new GroupStateRepresentation());
1157  gsr->dfce_ = dfce;
1158  gsr->gradients_.resize(dfce->link_names_.size() + dfce->attached_body_names_.size());
1159 
1160  Eigen::Vector3d link_size;
1161  Eigen::Vector3d link_origin;
1162  for (unsigned int i = 0; i < dfce->link_names_.size(); i++)
1163  {
1164  const moveit::core::LinkModel* ls = state.getLinkModel(dfce->link_names_[i]);
1165  if (dfce->link_has_geometry_[i])
1166  {
1167  // create link body geometric decomposition
1168  gsr->link_body_decompositions_.push_back(getPosedLinkBodySphereDecomposition(ls, dfce->link_body_indices_[i]));
1169 
1170  // create and fill link distance field
1171  PosedBodySphereDecompositionPtr& link_bd = gsr->link_body_decompositions_.back();
1172  double diameter = 2 * link_bd->getBoundingSphereRadius();
1173  link_size = Eigen::Vector3d(diameter, diameter, diameter);
1174  link_origin = link_bd->getBoundingSphereCenter() - 0.5 * link_size;
1175 
1176  ROS_DEBUG_STREAM("Creating PosedDistanceField for link "
1177  << dfce->link_names_[i] << " with size [" << link_size.x() << ", " << link_size.y() << ", "
1178  << link_size.z() << "] and origin " << link_origin.x() << ", " << link_origin.y() << ", "
1179  << link_origin.z());
1180 
1181  gsr->link_distance_fields_.push_back(PosedDistanceFieldPtr(new PosedDistanceField(
1183  gsr->link_distance_fields_.back()->addPointsToField(link_bd->getCollisionPoints());
1184  ROS_DEBUG_STREAM("Created PosedDistanceField for link " << dfce->link_names_[i] << " with "
1185  << link_bd->getCollisionPoints().size() << " points");
1186 
1187  gsr->link_body_decompositions_.back()->updatePose(state.getFrameTransform(ls->getName()));
1188  gsr->link_distance_fields_.back()->updatePose(state.getFrameTransform(ls->getName()));
1189  gsr->gradients_[i].types.resize(gsr->link_body_decompositions_.back()->getCollisionSpheres().size(), NONE);
1190  gsr->gradients_[i].distances.resize(gsr->link_body_decompositions_.back()->getCollisionSpheres().size(),
1191  DBL_MAX);
1192  gsr->gradients_[i].gradients.resize(gsr->link_body_decompositions_.back()->getCollisionSpheres().size());
1193  gsr->gradients_[i].sphere_radii = gsr->link_body_decompositions_.back()->getSphereRadii();
1194  gsr->gradients_[i].joint_name = ls->getParentJointModel()->getName();
1195  }
1196  else
1197  {
1198  gsr->link_body_decompositions_.push_back(PosedBodySphereDecompositionPtr());
1199  gsr->link_distance_fields_.push_back(PosedDistanceFieldPtr());
1200  }
1201  }
1202  }
1203  else
1204  {
1205  gsr.reset(new GroupStateRepresentation(*(dfce->pregenerated_group_state_representation_)));
1206  gsr->dfce_ = dfce;
1207  gsr->gradients_.resize(dfce->link_names_.size() + dfce->attached_body_names_.size());
1208  for (unsigned int i = 0; i < dfce->link_names_.size(); i++)
1209  {
1210  const moveit::core::LinkModel* ls = state.getLinkModel(dfce->link_names_[i]);
1211  if (dfce->link_has_geometry_[i])
1212  {
1213  gsr->link_body_decompositions_[i]->updatePose(state.getFrameTransform(ls->getName()));
1214  gsr->link_distance_fields_[i]->updatePose(state.getFrameTransform(ls->getName()));
1215  gsr->gradients_[i].sphere_locations = gsr->link_body_decompositions_[i]->getSphereCenters();
1216  }
1217  }
1218  }
1219 
1220  for (unsigned int i = 0; i < dfce->attached_body_names_.size(); i++)
1221  {
1222  int link_index = dfce->attached_body_link_state_indices_[i];
1223  const moveit::core::LinkModel* ls =
1224  state.getJointModelGroup(gsr->dfce_->group_name_)->getUpdatedLinkModels()[link_index];
1225  // const moveit::core::LinkModel* ls =
1226  // state.getLinkStateVector()[dfce->attached_body_link_state_indices_[i]];
1229  gsr->attached_body_decompositions_.push_back(
1230  getAttachedBodySphereDecomposition(state.getAttachedBody(dfce->attached_body_names_[i]), resolution_));
1231  gsr->gradients_[i + dfce->link_names_.size()].types.resize(
1232  gsr->attached_body_decompositions_.back()->getCollisionSpheres().size(), NONE);
1233  gsr->gradients_[i + dfce->link_names_.size()].distances.resize(
1234  gsr->attached_body_decompositions_.back()->getCollisionSpheres().size(), DBL_MAX);
1235  gsr->gradients_[i + dfce->link_names_.size()].gradients.resize(
1236  gsr->attached_body_decompositions_.back()->getCollisionSpheres().size());
1237  gsr->gradients_[i + dfce->link_names_.size()].sphere_locations =
1238  gsr->attached_body_decompositions_.back()->getSphereCenters();
1239  gsr->gradients_[i + dfce->link_names_.size()].sphere_radii =
1240  gsr->attached_body_decompositions_.back()->getSphereRadii();
1241  gsr->gradients_[i + dfce->link_names_.size()].joint_name = ls->getParentJointModel()->getName();
1242  }
1243 }
1244 
1245 bool CollisionRobotDistanceField::compareCacheEntryToState(const DistanceFieldCacheEntryConstPtr& dfce,
1246  const moveit::core::RobotState& state) const
1247 {
1248  std::vector<double> new_state_values(state.getVariableCount());
1249  for (unsigned int i = 0; i < new_state_values.size(); i++)
1250  {
1251  new_state_values[i] = state.getVariablePosition(i);
1252  }
1253 
1254  if (dfce->state_values_.size() != new_state_values.size())
1255  {
1256  ROS_ERROR("State value size mismatch");
1257  return false;
1258  }
1259 
1260  for (unsigned int i = 0; i < dfce->state_check_indices_.size(); i++)
1261  {
1262  double diff =
1263  fabs(dfce->state_values_[dfce->state_check_indices_[i]] - new_state_values[dfce->state_check_indices_[i]]);
1264  if (diff > EPSILON)
1265  {
1266  ROS_WARN_STREAM("State for Variable " << state.getVariableNames()[dfce->state_check_indices_[i]]
1267  << " has changed by " << diff << " radians");
1268  return false;
1269  }
1270  }
1271  std::vector<const moveit::core::AttachedBody*> attached_bodies_dfce;
1272  std::vector<const moveit::core::AttachedBody*> attached_bodies_state;
1273  dfce->state_->getAttachedBodies(attached_bodies_dfce);
1274  state.getAttachedBodies(attached_bodies_state);
1275  if (attached_bodies_dfce.size() != attached_bodies_state.size())
1276  {
1277  return false;
1278  }
1279  // TODO - figure all the things that can change
1280  for (unsigned int i = 0; i < attached_bodies_dfce.size(); i++)
1281  {
1282  if (attached_bodies_dfce[i]->getName() != attached_bodies_state[i]->getName())
1283  {
1284  return false;
1285  }
1286  if (attached_bodies_dfce[i]->getTouchLinks() != attached_bodies_state[i]->getTouchLinks())
1287  {
1288  return false;
1289  }
1290  if (attached_bodies_dfce[i]->getShapes().size() != attached_bodies_state[i]->getShapes().size())
1291  {
1292  return false;
1293  }
1294  for (unsigned int j = 0; j < attached_bodies_dfce[i]->getShapes().size(); j++)
1295  {
1296  if (attached_bodies_dfce[i]->getShapes()[j] != attached_bodies_state[i]->getShapes()[j])
1297  {
1298  return false;
1299  }
1300  }
1301  }
1302  return true;
1303 }
1304 
1306  const DistanceFieldCacheEntryConstPtr& dfce, const collision_detection::AllowedCollisionMatrix& acm) const
1307 {
1308  if (dfce->acm_.getSize() != acm.getSize())
1309  {
1310  ROS_DEBUG("Allowed collision matrix size mismatch");
1311  return false;
1312  }
1313  std::vector<const moveit::core::AttachedBody*> attached_bodies;
1314  dfce->state_->getAttachedBodies(attached_bodies);
1315  for (unsigned int i = 0; i < dfce->link_names_.size(); i++)
1316  {
1317  std::string link_name = dfce->link_names_[i];
1318  if (dfce->link_has_geometry_[i])
1319  {
1320  bool self_collision_enabled = true;
1322  if (acm.getEntry(link_name, link_name, t))
1323  {
1325  {
1326  self_collision_enabled = false;
1327  }
1328  }
1329  if (self_collision_enabled != dfce->self_collision_enabled_[i])
1330  {
1331  // ROS_INFO_STREAM("Self collision for " << link_name << " went from "
1332  // << dfce->self_collision_enabled_[i] << " to " <<
1333  // self_collision_enabled);
1334  return false;
1335  }
1336  for (unsigned int j = i; j < dfce->link_names_.size(); j++)
1337  {
1338  if (i == j)
1339  continue;
1340  if (dfce->link_has_geometry_[j])
1341  {
1342  bool intra_collision_enabled = true;
1343  if (acm.getEntry(link_name, dfce->link_names_[j], t))
1344  {
1346  {
1347  intra_collision_enabled = false;
1348  }
1349  }
1350  if (dfce->intra_group_collision_enabled_[i][j] != intra_collision_enabled)
1351  {
1352  // std::cerr << "Intra collision for " << dfce->link_names_[i] << "
1353  // " << dfce->link_names_[j]
1354  // << " went from " <<
1355  // dfce->intra_group_collision_enabled_[i][j] << " to " <<
1356  // intra_collision_enabled << std::endl;
1357  return false;
1358  }
1359  }
1360  }
1361  }
1362  }
1363  return true;
1364 }
1365 
1366 // void
1367 // CollisionRobotDistanceField::generateAllowedCollisionInformation(CollisionRobotDistanceField::DistanceFieldCacheEntryPtr&
1368 // dfce)
1369 // {
1370 // for(unsigned int i = 0; i < dfce.link_names_.size(); i++) {
1371 // for(unsigned int j = 0; j <
1372 // if(dfce->acm.find
1373 // }
1374 // }
1375 }
const std::string & getName() const
The name of this link.
Definition: link_model.h:81
bool getIntraGroupProximityGradients(GroupStateRepresentationPtr &gsr) const
void getAttachedBodies(std::vector< const AttachedBody * > &attached_bodies) const
Get all bodies attached to the model corresponding to this state.
PosedBodySphereDecompositionVectorPtr getAttachedBodySphereDecomposition(const robot_state::AttachedBody *att, double resolution)
bool compareCacheEntryToState(const DistanceFieldCacheEntryConstPtr &dfce, const moveit::core::RobotState &state) const
const std::string & getName() const
Get the name of the joint.
Definition: joint_model.h:131
void generateCollisionCheckingStructures(const std::string &group_name, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix *acm, GroupStateRepresentationPtr &gsr, bool generate_distance_field) const
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.
static const double DEFAULT_RESOLUTION
Collisions between the pair of bodies is never ok, i.e., if two bodies are in contact in a particular...
void createCollisionModelMarker(const moveit::core::RobotState &state, visualization_msgs::MarkerArray &model_markers) const
bool doBoundingSpheresIntersect(const PosedBodySphereDecompositionConstPtr &p1, const PosedBodySphereDecompositionConstPtr &p2)
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
const Eigen::Affine3d & getGlobalLinkTransform(const std::string &link_name)
Definition: robot_state.h:1368
BodyType body_type_2
The type of the second body involved in the contact.
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)
const JointModelGroup * getJointModelGroup(const std::string &group) const
Get the model of a particular joint group.
Definition: robot_state.h:168
const std::string & getName() const
Get the name of the joint group.
ContactMap contacts
A map returning the pairs of ids of the bodies in contact, plus information about the contacts themse...
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
void setPadding(double padding)
Set the link padding (for every link)
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)
const std::map< std::string, double > & getLinkPadding() const
Get the link paddings as a map (from link names to padding value)
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)
std::map< std::string, unsigned int > link_body_decomposition_index_map_
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...
EIGEN_MAKE_ALIGNED_OPERATOR_NEW CollisionRobotDistanceField(const robot_model::RobotModelConstPtr &kmodel)
#define ROS_WARN(...)
static const double DEFAULT_MAX_PROPOGATION_DISTANCE
Generic interface to collision detection.
const AttachedBody * getAttachedBody(const std::string &name) const
Get the attached body named name. Return NULL if not found.
std::size_t getSize() const
Get the size of the allowed collision matrix (number of specified entries)
bool collision
True if collision was found, false otherwise.
robot_model::RobotModelConstPtr robot_model_
The kinematic model corresponding to this collision model.
const JointModel * getParentJointModel() const
Get the joint model whose child this link is. There will always be a parent joint.
Definition: link_model.h:108
const std::vector< shapes::ShapeConstPtr > & getShapes() const
Get the shapes that make up this attached body.
Definition: attached_body.h:88
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 compareCacheEntryToAllowedCollisionMatrix(const DistanceFieldCacheEntryConstPtr &dfce, const collision_detection::AllowedCollisionMatrix &acm) const
static const double resolution
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.
void updateGroupStateRepresentationState(const moveit::core::RobotState &state, GroupStateRepresentationPtr &gsr) const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Vector3d pos
contact position
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
PosedBodySphereDecompositionPtr getPosedLinkBodySphereDecomposition(const moveit::core::LinkModel *ls, unsigned int ind) const
#define ROS_ASSERT_MSG(cond,...)
This class represents a collision model of the robot and can be used for self collision checks (to ch...
std::string group_name
The group name to check collisions for (optional; if empty, assume the complete robot) ...
std::string body_name_2
The id of the second body involved in the contact.
const std::vector< shapes::ShapeConstPtr > & getShapes() const
Get shape associated to the collision geometry for this link.
Definition: link_model.h:174
bool getIntraGroupCollisions(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, GroupStateRepresentationPtr &gsr) const
#define ROS_WARN_STREAM(args)
#define ROS_DEBUG_STREAM(args)
const EigenSTL::vector_Affine3d & getGlobalCollisionBodyTransforms() const
Get the global transforms for the collision bodies.
DistanceFieldCacheEntryPtr generateDistanceFieldCacheEntry(const std::string &group_name, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix *acm, bool generate_distance_field) const
void pointEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Point &m)
#define ROS_DEBUG_COND(cond,...)
const std::vector< std::string > & getUpdatedLinkModelNames() const
Get the names of the links returned by getUpdatedLinkModels()
std::string body_name_1
The id of the first body involved in the contact.
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...
void getGroupStateRepresentation(const DistanceFieldCacheEntryConstPtr &dfce, const moveit::core::RobotState &state, GroupStateRepresentationPtr &gsr) const
std::vector< BodyDecompositionConstPtr > link_body_decomposition_vector_
const LinkModel * getLinkModel(const std::string &link) const
Get the model of a particular link.
Definition: robot_state.h:156
static const bool DEFAULT_USE_SIGNED_DISTANCE_FIELD
#define ROS_ERROR_NAMED(name,...)
DistanceFieldCacheEntryConstPtr getDistanceFieldCacheEntry(const std::string &group_name, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix *acm) const
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:123
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...
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:241
A link from the robot. Contains the constant transform applied to the link and its geometry...
Definition: link_model.h:72
const std::vector< std::string > & getUpdatedLinkModelsWithGeometryNames() const
Get the names of the links returned by getUpdatedLinkModels()
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:150
A DistanceField implementation that uses a vector propagation method. Distances propagate outward fro...
std::map< std::string, GroupStateRepresentationPtr > pregenerated_group_state_representation_map_
bool getSelfProximityGradients(GroupStateRepresentationPtr &gsr) const
std::size_t getVariableCount() const
Get the number of variables that make up this state.
Definition: robot_state.h:144
Object defining bodies that can be attached to robot links. This is useful when handling objects pick...
Definition: attached_body.h:56
void checkSelfCollisionHelper(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix *acm, GroupStateRepresentationPtr &gsr) const
#define ROS_ERROR(...)
PosedBodyPointDecompositionPtr getPosedLinkBodyPointDecomposition(const moveit::core::LinkModel *ls) const
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)
virtual void checkSelfCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const moveit::core::RobotState &state) const
Check for self collision. Any collision between any pair of links is checked for, NO collisions are i...
bool getSelfCollisions(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, GroupStateRepresentationPtr &gsr) const
#define ROS_DEBUG(...)


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Sep 15 2019 03:57:56