38 #include <rclcpp/clock.hpp>
39 #include <rclcpp/duration.hpp>
40 #include <rclcpp/logger.hpp>
41 #include <rclcpp/logging.hpp>
42 #include <rclcpp/time.hpp>
43 #include <tf2_eigen/tf2_eigen.hpp>
60 const moveit::core::RobotModelConstPtr& robot_model,
61 const std::map<std::string, std::vector<CollisionSphere>>& link_body_decompositions,
double size_x,
double size_y,
62 double size_z,
const Eigen::Vector3d& origin,
bool use_signed_distance_field,
double resolution,
63 double collision_tolerance,
double max_propogation_distance,
double ,
double )
67 resolution, collision_tolerance, max_propogation_distance);
79 const moveit::core::RobotModelConstPtr& robot_model,
const WorldPtr& world,
80 const std::map<std::string, std::vector<CollisionSphere>>& link_body_decompositions,
double size_x,
double size_y,
81 double size_z,
const Eigen::Vector3d& origin,
bool use_signed_distance_field,
double resolution,
82 double collision_tolerance,
double max_propogation_distance,
double padding,
double scale)
86 resolution, collision_tolerance, max_propogation_distance);
126 const std::map<std::string, std::vector<CollisionSphere>>& link_body_decompositions,
const Eigen::Vector3d& size,
127 const Eigen::Vector3d& origin,
bool use_signed_distance_field,
double resolution,
double collision_tolerance,
128 double max_propogation_distance)
140 const std::vector<const moveit::core::JointModelGroup*>& jmg =
robot_model_->getJointModelGroups();
143 std::map<std::string, bool> updated_group_entry;
144 std::vector<std::string> links = jm->getUpdatedLinkModelsWithGeometryNames();
145 for (
const std::string& link : links)
147 updated_group_entry[link] =
true;
151 DistanceFieldCacheEntryPtr dfce =
160 bool generate_distance_field)
const
163 if (!dfce || (generate_distance_field && !dfce->distance_field_))
167 DistanceFieldCacheEntryPtr new_dfce =
180 GroupStateRepresentationPtr& gsr)
const
197 RCLCPP_DEBUG(
logger_,
"Intra Group Collision found");
201 DistanceFieldCacheEntryConstPtr
206 DistanceFieldCacheEntryConstPtr ret;
209 RCLCPP_DEBUG(
logger_,
" No current Distance field cache entry");
213 if (group_name != cur->group_name_)
215 RCLCPP_DEBUG(
logger_,
"No cache entry as group name changed from %s to %s", cur->group_name_.c_str(),
228 RCLCPP_DEBUG(
logger_,
"Regenerating distance field as some relevant part of the acm changed");
238 GroupStateRepresentationPtr gsr;
245 GroupStateRepresentationPtr& gsr)
const
255 GroupStateRepresentationPtr gsr;
263 GroupStateRepresentationPtr& gsr)
const
267 RCLCPP_WARN(
logger_,
"Shouldn't be calling this function with initialized gsr - ACM "
275 GroupStateRepresentationPtr& gsr)
const
277 for (
unsigned int i{ 0 }; i < gsr->dfce_->link_names_.size() + gsr->dfce_->attached_body_names_.size(); ++i)
279 bool is_link{ i < gsr->dfce_->link_names_.size() };
280 if ((is_link && !gsr->dfce_->link_has_geometry_[i]) || !gsr->dfce_->self_collision_enabled_[i])
282 const std::vector<CollisionSphere>* collision_spheres_1;
283 const EigenSTL::vector_Vector3d* sphere_centers_1;
287 collision_spheres_1 = &(gsr->link_body_decompositions_[i]->getCollisionSpheres());
288 sphere_centers_1 = &(gsr->link_body_decompositions_[i]->getSphereCenters());
292 collision_spheres_1 =
293 &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getCollisionSpheres());
294 sphere_centers_1 = &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters());
299 std::vector<unsigned int> colls;
307 for (
unsigned int col : colls)
312 con.
pos = gsr->link_body_decompositions_[i]->getSphereCenters()[col];
318 con.
pos = gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters()[col];
320 con.
body_name_1 = gsr->dfce_->attached_body_names_[i - gsr->dfce_->link_names_.size()];
323 RCLCPP_DEBUG(
logger_,
"Self collision detected for link %s ", con.
body_name_1.c_str());
329 gsr->gradients_[i].types[col] =
SELF;
331 gsr->gradients_[i].collision =
true;
345 RCLCPP_DEBUG(
logger_,
"Link %s in self collision", gsr->dfce_->link_names_[i].c_str());
356 bool in_collision =
false;
358 for (
unsigned int i{ 0 }; i < gsr->dfce_->link_names_.size(); ++i)
360 const std::string& link_name = gsr->dfce_->link_names_[i];
361 bool is_link{ i < gsr->dfce_->link_names_.size() };
362 if ((is_link && !gsr->dfce_->link_has_geometry_[i]) || !gsr->dfce_->self_collision_enabled_[i])
367 const std::vector<CollisionSphere>* collision_spheres_1;
368 const EigenSTL::vector_Vector3d* sphere_centers_1;
371 collision_spheres_1 = &(gsr->link_body_decompositions_[i]->getCollisionSpheres());
372 sphere_centers_1 = &(gsr->link_body_decompositions_[i]->getSphereCenters());
376 collision_spheres_1 =
377 &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getCollisionSpheres());
378 sphere_centers_1 = &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters());
384 if (gsr->dfce_->acm_.getSize() > 0)
387 for (
unsigned int j{ 0 }; j < gsr->dfce_->link_names_.size(); ++j)
390 if (link_name == gsr->dfce_->link_names_[j])
396 if (gsr->dfce_->acm_.getEntry(link_name, gsr->dfce_->link_names_[j], col_type) &&
402 if (gsr->link_distance_fields_[j])
404 coll = gsr->link_distance_fields_[j]->getCollisionSphereGradients(
431 GroupStateRepresentationPtr& gsr)
const
433 unsigned int num_links = gsr->dfce_->link_names_.size();
434 unsigned int num_attached_bodies = gsr->dfce_->attached_body_names_.size();
436 for (
unsigned int i{ 0 }; i < num_links + num_attached_bodies; ++i)
438 for (
unsigned int j{ i + 1 }; j < num_links + num_attached_bodies; ++j)
442 bool i_is_link = i < num_links;
443 bool j_is_link = j < num_links;
445 if ((i_is_link && !gsr->dfce_->link_has_geometry_[i]) || (j_is_link && !gsr->dfce_->link_has_geometry_[j]))
448 if (!gsr->dfce_->intra_group_collision_enabled_[i][j])
451 if (i_is_link && j_is_link &&
459 else if (!i_is_link || !j_is_link)
462 if (!i_is_link && j_is_link)
464 for (
unsigned int k = 0; k < gsr->attached_body_decompositions_[i - num_links]->getSize(); ++k)
467 gsr->link_body_decompositions_[j],
468 gsr->attached_body_decompositions_[i - num_links]->getPosedBodySphereDecomposition(k)))
475 else if (i_is_link && !j_is_link)
477 for (
unsigned int k = 0; k < gsr->attached_body_decompositions_[j - num_links]->getSize(); ++k)
480 gsr->link_body_decompositions_[i],
481 gsr->attached_body_decompositions_[j - num_links]->getPosedBodySphereDecomposition(k)))
490 for (
unsigned int k{ 0 }; k < gsr->attached_body_decompositions_[i - num_links]->getSize() && all_ok; ++k)
492 for (
unsigned int l{ 0 }; l < gsr->attached_body_decompositions_[j - num_links]->getSize(); ++l)
495 gsr->attached_body_decompositions_[i - num_links]->getPosedBodySphereDecomposition(k),
496 gsr->attached_body_decompositions_[j - num_links]->getPosedBodySphereDecomposition(l)))
517 name_1 = gsr->dfce_->link_names_[i];
521 name_1 = gsr->dfce_->attached_body_names_[i - num_links];
526 name_2 = gsr->dfce_->link_names_[j];
530 name_2 = gsr->dfce_->attached_body_names_[j - num_links];
534 collision_detection::CollisionResult::ContactMap::iterator it =
535 res.
contacts.find(std::pair<std::string, std::string>(name_1, name_2));
542 num_pair = it->second.size();
545 const std::vector<CollisionSphere>* collision_spheres_1;
546 const std::vector<CollisionSphere>* collision_spheres_2;
547 const EigenSTL::vector_Vector3d* sphere_centers_1;
548 const EigenSTL::vector_Vector3d* sphere_centers_2;
551 collision_spheres_1 = &(gsr->link_body_decompositions_[i]->getCollisionSpheres());
552 sphere_centers_1 = &(gsr->link_body_decompositions_[i]->getSphereCenters());
556 collision_spheres_1 = &(gsr->attached_body_decompositions_[i - num_links]->getCollisionSpheres());
557 sphere_centers_1 = &(gsr->attached_body_decompositions_[i - num_links]->getSphereCenters());
561 collision_spheres_2 = &(gsr->link_body_decompositions_[j]->getCollisionSpheres());
562 sphere_centers_2 = &(gsr->link_body_decompositions_[j]->getSphereCenters());
566 collision_spheres_2 = &(gsr->attached_body_decompositions_[j - num_links]->getCollisionSpheres());
567 sphere_centers_2 = &(gsr->attached_body_decompositions_[j - num_links]->getSphereCenters());
570 for (
unsigned int k{ 0 };
573 for (
unsigned int l{ 0 };
576 Eigen::Vector3d gradient = (*sphere_centers_1)[k] - (*sphere_centers_2)[l];
577 double dist = gradient.norm();
582 if (dist < (*collision_spheres_1)[k].radius_ + (*collision_spheres_2)[l].radius_)
600 con.
pos = gsr->link_body_decompositions_[i]->getSphereCenters()[k];
622 gsr->gradients_[i].types[k] =
INTRA;
623 gsr->gradients_[i].collision =
true;
624 gsr->gradients_[j].types[l] =
INTRA;
625 gsr->gradients_[j].collision =
true;
645 bool in_collision{
false };
646 unsigned int num_links = gsr->dfce_->link_names_.size();
647 unsigned int num_attached_bodies = gsr->dfce_->attached_body_names_.size();
649 for (
unsigned int i{ 0 }; i < num_links + num_attached_bodies; ++i)
651 for (
unsigned int j{ i + 1 }; j < num_links + num_attached_bodies; ++j)
655 bool i_is_link = i < num_links;
656 bool j_is_link = j < num_links;
657 if ((i_is_link && !gsr->dfce_->link_has_geometry_[i]) || (j_is_link && !gsr->dfce_->link_has_geometry_[j]))
659 if (!gsr->dfce_->intra_group_collision_enabled_[i][j])
661 const std::vector<CollisionSphere>* collision_spheres_1;
662 const std::vector<CollisionSphere>* collision_spheres_2;
663 const EigenSTL::vector_Vector3d* sphere_centers_1;
664 const EigenSTL::vector_Vector3d* sphere_centers_2;
667 collision_spheres_1 = &(gsr->link_body_decompositions_[i]->getCollisionSpheres());
668 sphere_centers_1 = &(gsr->link_body_decompositions_[i]->getSphereCenters());
672 collision_spheres_1 = &(gsr->attached_body_decompositions_[i - num_links]->getCollisionSpheres());
673 sphere_centers_1 = &(gsr->attached_body_decompositions_[i - num_links]->getSphereCenters());
677 collision_spheres_2 = &(gsr->link_body_decompositions_[j]->getCollisionSpheres());
678 sphere_centers_2 = &(gsr->link_body_decompositions_[j]->getSphereCenters());
682 collision_spheres_2 = &(gsr->attached_body_decompositions_[j - num_links]->getCollisionSpheres());
683 sphere_centers_2 = &(gsr->attached_body_decompositions_[j - num_links]->getSphereCenters());
685 for (
unsigned int k{ 0 }; k < collision_spheres_1->size(); ++k)
687 for (
unsigned int l{ 0 }; l < collision_spheres_2->size(); ++l)
689 Eigen::Vector3d gradient = (*sphere_centers_1)[k] - (*sphere_centers_2)[l];
690 double dist = gradient.norm();
691 if (dist < gsr->gradients_[i].distances[k])
693 gsr->gradients_[i].distances[k] = dist;
694 gsr->gradients_[i].gradients[k] = gradient;
695 gsr->gradients_[i].types[k] =
INTRA;
697 if (dist < gsr->gradients_[j].distances[l])
699 gsr->gradients_[j].distances[l] = dist;
700 gsr->gradients_[j].gradients[l] = -gradient;
701 gsr->gradients_[j].types[l] =
INTRA;
713 DistanceFieldCacheEntryPtr dfce = std::make_shared<DistanceFieldCacheEntry>();
715 if (
robot_model_->getJointModelGroup(group_name) ==
nullptr)
717 RCLCPP_WARN(
logger_,
"No group %s", group_name.c_str());
721 dfce->group_name_ = group_name;
722 dfce->state_ = std::make_shared<moveit::core::RobotState>(state);
729 RCLCPP_WARN(
logger_,
"Allowed Collision Matrix is null, enabling all collisions "
730 "in the DistanceFieldCacheEntry");
734 dfce->link_names_ =
robot_model_->getJointModelGroup(group_name)->getUpdatedLinkModelNames();
735 std::vector<const moveit::core::AttachedBody*> all_attached_bodies;
736 dfce->state_->getAttachedBodies(all_attached_bodies);
737 unsigned int att_count = 0;
740 std::vector<bool> all_true(dfce->link_names_.size() + all_attached_bodies.size(),
true);
741 std::vector<bool> all_false(dfce->link_names_.size() + all_attached_bodies.size(),
false);
744 dfce->self_collision_enabled_.resize(dfce->link_names_.size() + all_attached_bodies.size(),
true);
745 dfce->intra_group_collision_enabled_.resize(dfce->link_names_.size() + all_attached_bodies.size());
747 for (
unsigned int i{ 0 }; i < dfce->link_names_.size(); ++i)
749 std::string link_name = dfce->link_names_[i];
753 for (
unsigned int j{ 0 }; j < lsv.size(); ++j)
755 if (lsv[j]->getName() == link_name)
757 dfce->link_state_indices_.push_back(j);
765 RCLCPP_DEBUG(
logger_,
"No link state found for link %s", dfce->link_names_[i].c_str());
771 dfce->link_has_geometry_.push_back(
true);
779 dfce->self_collision_enabled_[i] =
false;
782 dfce->intra_group_collision_enabled_[i] = all_true;
783 for (
unsigned int j{ i + 1 }; j < dfce->link_names_.size(); ++j)
785 if (link_name == dfce->link_names_[j])
787 dfce->intra_group_collision_enabled_[i][j] =
false;
792 dfce->intra_group_collision_enabled_[i][j] =
false;
796 std::vector<const moveit::core::AttachedBody*> link_attached_bodies;
798 for (
unsigned int j{ 0 }; j < link_attached_bodies.size(); ++j, ++att_count)
800 dfce->attached_body_names_.push_back(link_attached_bodies[j]->getName());
801 dfce->attached_body_link_state_indices_.push_back(dfce->link_state_indices_[i]);
803 if (acm->
getEntry(link_name, link_attached_bodies[j]->getName(), t))
807 dfce->intra_group_collision_enabled_[i][att_count + dfce->link_names_.size()] =
false;
815 if (link_attached_bodies[j]->getTouchLinks().find(link_name) != link_attached_bodies[j]->getTouchLinks().end())
817 dfce->intra_group_collision_enabled_[i][att_count + dfce->link_names_.size()] =
false;
826 dfce->self_collision_enabled_[i] =
true;
827 dfce->intra_group_collision_enabled_[i] = all_true;
832 dfce->link_has_geometry_.push_back(
false);
833 dfce->link_body_indices_.push_back(0);
834 dfce->self_collision_enabled_[i] =
false;
835 dfce->intra_group_collision_enabled_[i] = all_false;
839 for (
unsigned int i = 0; i < dfce->attached_body_names_.size(); ++i)
841 dfce->intra_group_collision_enabled_[i + dfce->link_names_.size()] = all_true;
845 if (acm->
getEntry(dfce->attached_body_names_[i], dfce->attached_body_names_[i], t) &&
848 dfce->self_collision_enabled_[i + dfce->link_names_.size()] =
false;
850 for (
unsigned int j{ i + 1 }; j < dfce->attached_body_names_.size(); ++j)
852 if (acm->
getEntry(dfce->attached_body_names_[i], dfce->attached_body_names_[j], t) &&
855 dfce->intra_group_collision_enabled_[i + dfce->link_names_.size()][j + dfce->link_names_.size()] =
false;
866 std::map<std::string, GroupStateRepresentationPtr>::const_iterator it =
870 dfce->pregenerated_group_state_representation_ = it->second;
873 std::map<std::string, bool> updated_map;
874 if (!dfce->link_names_.empty())
876 const std::vector<const moveit::core::JointModel*>& child_joint_models =
877 dfce->state_->getJointModelGroup(dfce->group_name_)->getActiveJointModels();
880 updated_map[child_joint_model->getName()] =
true;
881 RCLCPP_DEBUG(
logger_,
"Joint %s has been added to updated list ", child_joint_model->getName().c_str());
885 const std::vector<std::string>& state_variable_names = state.
getVariableNames();
886 for (
const std::string& state_variable_name : state_variable_names)
889 dfce->state_values_.push_back(val);
890 if (updated_map.count(state_variable_name) == 0)
892 dfce->state_check_indices_.push_back(dfce->state_values_.size() - 1);
893 RCLCPP_DEBUG(
logger_,
"Non-group joint %p will be checked for state changes", state_variable_name.c_str());
897 if (generate_distance_field)
899 if (dfce->distance_field_)
901 RCLCPP_DEBUG(
logger_,
"CollisionRobot skipping distance field generation, "
902 "will use existing one");
906 std::vector<PosedBodyPointDecompositionPtr> non_group_link_decompositions;
907 std::vector<PosedBodyPointDecompositionVectorPtr> non_group_attached_body_decompositions;
908 const std::map<std::string, bool>& updated_group_map =
in_group_update_map_.find(group_name)->second;
911 const std::string& link_name = link_model->getName();
913 if (updated_group_map.find(link_name) != updated_group_map.end())
920 non_group_link_decompositions.back()->updatePose(dfce->state_->getFrameTransform(link_state->
getName()));
922 std::vector<const moveit::core::AttachedBody*> attached_bodies;
923 dfce->state_->getAttachedBodies(attached_bodies, link_state);
926 non_group_attached_body_decompositions.push_back(
930 dfce->distance_field_ = std::make_shared<distance_field::PropagationDistanceField>(
937 EigenSTL::vector_Vector3d all_points;
938 for (collision_detection::PosedBodyPointDecompositionPtr& non_group_link_decomposition :
939 non_group_link_decompositions)
941 all_points.insert(all_points.end(), non_group_link_decomposition->getCollisionPoints().begin(),
942 non_group_link_decomposition->getCollisionPoints().end());
945 for (collision_detection::PosedBodyPointDecompositionVectorPtr& non_group_attached_body_decomposition :
946 non_group_attached_body_decompositions)
948 const EigenSTL::vector_Vector3d collision_points = non_group_attached_body_decomposition->getCollisionPoints();
949 all_points.insert(all_points.end(), collision_points.begin(), collision_points.end());
952 dfce->distance_field_->addPointsToField(all_points);
953 RCLCPP_DEBUG(
logger_,
"CollisionRobot distance field has been initialized with %zu points.", all_points.size());
961 const std::vector<const moveit::core::LinkModel*>& link_models =
robot_model_->getLinkModelsWithCollisionGeometry();
964 if (link_model->getShapes().empty())
966 RCLCPP_WARN(
logger_,
"No collision geometry for link model %s though there should be",
967 link_model->getName().c_str());
971 RCLCPP_DEBUG(
logger_,
"Generating model for %s", link_model->getName().c_str());
972 BodyDecompositionConstPtr bd =
973 std::make_shared<const BodyDecomposition>(link_model->getShapes(), link_model->getCollisionOriginTransforms(),
981 visualization_msgs::msg::MarkerArray& model_markers)
const
984 std_msgs::msg::ColorRGBA robot_color;
986 robot_color.b = 0.8f;
990 std_msgs::msg::ColorRGBA world_links_color;
991 world_links_color.r = 1;
992 world_links_color.g = 1;
993 world_links_color.b = 0;
994 world_links_color.a = 0.5;
997 visualization_msgs::msg::Marker sphere_marker;
998 sphere_marker.header.frame_id =
robot_model_->getRootLinkName();
999 sphere_marker.header.stamp = rclcpp::Time(0);
1001 sphere_marker.id = 0;
1002 sphere_marker.type = visualization_msgs::msg::Marker::SPHERE;
1003 sphere_marker.action = visualization_msgs::msg::Marker::ADD;
1004 sphere_marker.pose.orientation.x = 0;
1005 sphere_marker.pose.orientation.y = 0;
1006 sphere_marker.pose.orientation.z = 0;
1007 sphere_marker.pose.orientation.w = 1;
1008 sphere_marker.color = robot_color;
1009 sphere_marker.lifetime = rclcpp::Duration::from_seconds(0);
1011 unsigned int id{ 0 };
1015 std::map<std::string, unsigned int>::const_iterator map_iter;
1019 const std::string& link_name = map_iter->first;
1020 unsigned int link_index = map_iter->second;
1023 if (std::find(group_link_names.begin(), group_link_names.end(), link_name) != group_link_names.end())
1025 sphere_marker.color = robot_color;
1029 sphere_marker.color = world_links_color;
1032 collision_detection::PosedBodySphereDecompositionPtr sphere_representation(
1035 for (
unsigned int j{ 0 }; j < sphere_representation->getCollisionSpheres().size(); ++j)
1037 sphere_marker.pose.position = tf2::toMsg(sphere_representation->getSphereCenters()[j]);
1038 sphere_marker.scale.x = sphere_marker.scale.y = sphere_marker.scale.z =
1039 2 * sphere_representation->getCollisionSpheres()[j].radius_;
1040 sphere_marker.id = id;
1043 model_markers.markers.push_back(sphere_marker);
1049 double resolution,
const std::map<std::string, std::vector<CollisionSphere>>& link_spheres)
1052 const std::vector<const moveit::core::LinkModel*>& link_models =
robot_model_->getLinkModelsWithCollisionGeometry();
1056 if (link_model->getShapes().empty())
1058 RCLCPP_WARN(
logger_,
"Skipping model generation for link %s since it contains no geometries",
1059 link_model->getName().c_str());
1063 BodyDecompositionPtr bd =
1064 std::make_shared<BodyDecomposition>(link_model->getShapes(), link_model->getCollisionOriginTransforms(),
1067 RCLCPP_DEBUG(
logger_,
"Generated model for %s", link_model->getName().c_str());
1069 if (link_spheres.find(link_model->getName()) != link_spheres.end())
1071 bd->replaceCollisionSpheres(link_spheres.find(link_model->getName())->second, Eigen::Isometry3d::Identity());
1076 RCLCPP_DEBUG(
logger_,
" Finished ");
1079 PosedBodySphereDecompositionPtr
1081 unsigned int ind)
const
1083 PosedBodySphereDecompositionPtr ret;
1088 PosedBodyPointDecompositionPtr
1091 PosedBodyPointDecompositionPtr ret;
1095 RCLCPP_ERROR(
logger_,
"No link body decomposition for link %s.", ls->
getName().c_str());
1103 GroupStateRepresentationPtr& gsr)
const
1105 for (
unsigned int i{ 0 }; i < gsr->dfce_->link_names_.size(); ++i)
1108 if (gsr->dfce_->link_has_geometry_[i])
1112 gsr->gradients_[i].closest_distance = DBL_MAX;
1113 gsr->gradients_[i].collision =
false;
1114 gsr->gradients_[i].types.assign(gsr->link_body_decompositions_[i]->getCollisionSpheres().size(),
NONE);
1115 gsr->gradients_[i].distances.assign(gsr->link_body_decompositions_[i]->getCollisionSpheres().size(), DBL_MAX);
1116 gsr->gradients_[i].gradients.assign(gsr->link_body_decompositions_[i]->getCollisionSpheres().size(),
1118 gsr->gradients_[i].sphere_locations = gsr->link_body_decompositions_[i]->getSphereCenters();
1122 for (
unsigned int i{ 0 }; i < gsr->dfce_->attached_body_names_.size(); ++i)
1127 RCLCPP_WARN(
logger_,
"Attached body discrepancy");
1132 if (gsr->attached_body_decompositions_.size() != att->
getShapes().size())
1134 RCLCPP_WARN(
logger_,
"Attached body size discrepancy");
1138 for (
unsigned int j{ 0 }; j < att->
getShapes().size(); ++j)
1143 gsr->gradients_[i + gsr->dfce_->link_names_.size()].closest_distance = DBL_MAX;
1144 gsr->gradients_[i + gsr->dfce_->link_names_.size()].collision =
false;
1145 gsr->gradients_[i + gsr->dfce_->link_names_.size()].types.assign(
1146 gsr->attached_body_decompositions_[i]->getCollisionSpheres().size(),
NONE);
1147 gsr->gradients_[i + gsr->dfce_->link_names_.size()].distances.assign(
1148 gsr->attached_body_decompositions_[i]->getCollisionSpheres().size(), DBL_MAX);
1149 gsr->gradients_[i + gsr->dfce_->link_names_.size()].gradients.assign(
1150 gsr->attached_body_decompositions_[i]->getCollisionSpheres().size(),
Eigen::Vector3d(0.0, 0.0, 0.0));
1151 gsr->gradients_[i + gsr->dfce_->link_names_.size()].sphere_locations =
1152 gsr->attached_body_decompositions_[i]->getSphereCenters();
1158 GroupStateRepresentationPtr& gsr)
const
1160 if (!dfce->pregenerated_group_state_representation_)
1162 RCLCPP_DEBUG(
logger_,
"Creating GroupStateRepresentation");
1165 gsr = std::make_shared<GroupStateRepresentation>();
1167 gsr->gradients_.resize(dfce->link_names_.size() + dfce->attached_body_names_.size());
1171 for (
unsigned int i{ 0 }; i < dfce->link_names_.size(); ++i)
1174 if (dfce->link_has_geometry_[i])
1180 PosedBodySphereDecompositionPtr& link_bd = gsr->link_body_decompositions_.back();
1181 double diameter = 2 * link_bd->getBoundingSphereRadius();
1183 link_origin = link_bd->getBoundingSphereCenter() - 0.5 * link_size;
1185 RCLCPP_DEBUG(
logger_,
"Creating PosedDistanceField for link %s with size [%f, %f , %f] and origin %f %f %f ",
1186 dfce->link_names_[i].c_str(), link_size.x(), link_size.y(), link_size.z(), link_origin.x(),
1187 link_origin.y(), link_origin.z());
1189 gsr->link_distance_fields_.push_back(std::make_shared<PosedDistanceField>(
1191 gsr->link_distance_fields_.back()->addPointsToField(link_bd->getCollisionPoints());
1192 RCLCPP_DEBUG(
logger_,
"Created PosedDistanceField for link %s with %zu points", dfce->link_names_[i].c_str(),
1193 link_bd->getCollisionPoints().size());
1197 gsr->gradients_[i].types.resize(gsr->link_body_decompositions_.back()->getCollisionSpheres().size(),
NONE);
1198 gsr->gradients_[i].distances.resize(gsr->link_body_decompositions_.back()->getCollisionSpheres().size(),
1200 gsr->gradients_[i].gradients.resize(gsr->link_body_decompositions_.back()->getCollisionSpheres().size());
1201 gsr->gradients_[i].sphere_radii = gsr->link_body_decompositions_.back()->getSphereRadii();
1206 gsr->link_body_decompositions_.push_back(PosedBodySphereDecompositionPtr());
1207 gsr->link_distance_fields_.push_back(PosedDistanceFieldPtr());
1213 gsr = std::make_shared<GroupStateRepresentation>(*(dfce->pregenerated_group_state_representation_));
1215 gsr->gradients_.resize(dfce->link_names_.size() + dfce->attached_body_names_.size());
1216 for (
unsigned int i{ 0 }; i < dfce->link_names_.size(); ++i)
1219 if (dfce->link_has_geometry_[i])
1223 gsr->gradients_[i].sphere_locations = gsr->link_body_decompositions_[i]->getSphereCenters();
1228 for (
unsigned int i{ 0 }; i < dfce->attached_body_names_.size(); ++i)
1230 int link_index = dfce->attached_body_link_state_indices_[i];
1237 gsr->attached_body_decompositions_.push_back(
1239 gsr->gradients_[i + dfce->link_names_.size()].types.resize(
1240 gsr->attached_body_decompositions_.back()->getCollisionSpheres().size(),
NONE);
1241 gsr->gradients_[i + dfce->link_names_.size()].distances.resize(
1242 gsr->attached_body_decompositions_.back()->getCollisionSpheres().size(), DBL_MAX);
1243 gsr->gradients_[i + dfce->link_names_.size()].gradients.resize(
1244 gsr->attached_body_decompositions_.back()->getCollisionSpheres().size());
1245 gsr->gradients_[i + dfce->link_names_.size()].sphere_locations =
1246 gsr->attached_body_decompositions_.back()->getSphereCenters();
1247 gsr->gradients_[i + dfce->link_names_.size()].sphere_radii =
1248 gsr->attached_body_decompositions_.back()->getSphereRadii();
1257 for (
unsigned int i{ 0 }; i < new_state_values.size(); ++i)
1262 if (dfce->state_values_.size() != new_state_values.size())
1264 RCLCPP_ERROR(
logger_,
" State value size mismatch");
1268 for (
unsigned int i{ 0 }; i < dfce->state_check_indices_.size(); ++i)
1271 fabs(dfce->state_values_[dfce->state_check_indices_[i]] - new_state_values[dfce->state_check_indices_[i]]);
1274 RCLCPP_WARN(
logger_,
"State for Variable %s has changed by %f radians",
1279 std::vector<const moveit::core::AttachedBody*> attached_bodies_dfce;
1280 std::vector<const moveit::core::AttachedBody*> attached_bodies_state;
1281 dfce->state_->getAttachedBodies(attached_bodies_dfce);
1283 if (attached_bodies_dfce.size() != attached_bodies_state.size())
1288 for (
unsigned int i{ 0 }; i < attached_bodies_dfce.size(); ++i)
1290 if (attached_bodies_dfce[i]->getName() != attached_bodies_state[i]->getName())
1294 if (attached_bodies_dfce[i]->getTouchLinks() != attached_bodies_state[i]->getTouchLinks())
1298 if (attached_bodies_dfce[i]->getShapes().size() != attached_bodies_state[i]->getShapes().size())
1302 for (
unsigned int j = 0; j < attached_bodies_dfce[i]->getShapes().size(); ++j)
1304 if (attached_bodies_dfce[i]->getShapes()[j] != attached_bodies_state[i]->getShapes()[j])
1316 if (dfce->acm_.getSize() != acm.
getSize())
1318 RCLCPP_DEBUG(
logger_,
"Allowed collision matrix size mismatch");
1321 std::vector<const moveit::core::AttachedBody*> attached_bodies;
1322 dfce->state_->getAttachedBodies(attached_bodies);
1323 for (
unsigned int i{ 0 }; i < dfce->link_names_.size(); ++i)
1325 std::string link_name = dfce->link_names_[i];
1326 if (dfce->link_has_geometry_[i])
1328 bool self_collision_enabled{
true };
1330 if (acm.
getEntry(link_name, link_name, t))
1334 self_collision_enabled =
false;
1337 if (self_collision_enabled != dfce->self_collision_enabled_[i])
1341 for (
unsigned int j{ i }; j < dfce->link_names_.size(); ++j)
1345 if (dfce->link_has_geometry_[j])
1347 bool intra_collision_enabled =
true;
1348 if (acm.
getEntry(link_name, dfce->link_names_[j], t))
1352 intra_collision_enabled =
false;
1355 if (dfce->intra_group_collision_enabled_[i][j] != intra_collision_enabled)
1384 GroupStateRepresentationPtr gsr;
1390 GroupStateRepresentationPtr& gsr)
const
1417 GroupStateRepresentationPtr gsr;
1423 GroupStateRepresentationPtr& gsr)
const
1449 GroupStateRepresentationPtr gsr;
1455 GroupStateRepresentationPtr& gsr)
const
1476 GroupStateRepresentationPtr gsr;
1483 GroupStateRepresentationPtr& gsr)
const
1506 RCLCPP_ERROR(
logger_,
"Continuous collision checking not implemented");
1513 RCLCPP_ERROR(
logger_,
"Continuous collision checking not implemented");
1519 GroupStateRepresentationPtr& gsr)
const
1542 GroupStateRepresentationPtr& gsr)
const
1561 const distance_field::DistanceFieldConstPtr& env_distance_field,
1562 GroupStateRepresentationPtr& gsr)
const
1564 for (
unsigned int i{ 0 }; i < gsr->dfce_->link_names_.size() + gsr->dfce_->attached_body_names_.size(); ++i)
1566 bool is_link = i < gsr->dfce_->link_names_.size();
1567 std::string link_name = i < gsr->dfce_->link_names_.size() ? gsr->dfce_->link_names_[i] :
"attached";
1568 if (is_link && !gsr->dfce_->link_has_geometry_[i])
1573 const std::vector<CollisionSphere>* collision_spheres_1;
1574 const EigenSTL::vector_Vector3d* sphere_centers_1;
1578 collision_spheres_1 = &(gsr->link_body_decompositions_[i]->getCollisionSpheres());
1579 sphere_centers_1 = &(gsr->link_body_decompositions_[i]->getSphereCenters());
1583 collision_spheres_1 =
1584 &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getCollisionSpheres());
1585 sphere_centers_1 = &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters());
1590 std::vector<unsigned int> colls;
1598 for (
unsigned int col : colls)
1603 con.
pos = gsr->link_body_decompositions_[i]->getSphereCenters()[col];
1609 con.
pos = gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters()[col];
1611 con.
body_name_1 = gsr->dfce_->attached_body_names_[i - gsr->dfce_->link_names_.size()];
1623 gsr->gradients_[i].collision =
true;
1645 const distance_field::DistanceFieldConstPtr& env_distance_field, GroupStateRepresentationPtr& gsr)
const
1647 bool in_collision =
false;
1648 for (
unsigned int i{ 0 }; i < gsr->dfce_->link_names_.size(); ++i)
1650 bool is_link = i < gsr->dfce_->link_names_.size();
1652 if (is_link && !gsr->dfce_->link_has_geometry_[i])
1657 const std::vector<CollisionSphere>* collision_spheres_1;
1658 const EigenSTL::vector_Vector3d* sphere_centers_1;
1661 collision_spheres_1 = &(gsr->link_body_decompositions_[i]->getCollisionSpheres());
1662 sphere_centers_1 = &(gsr->link_body_decompositions_[i]->getSphereCenters());
1666 collision_spheres_1 =
1667 &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getCollisionSpheres());
1668 sphere_centers_1 = &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters());
1676 in_collision =
true;
1679 return in_collision;
1705 rclcpp::Clock clock;
1706 rclcpp::Time start_time = clock.now();
1708 EigenSTL::vector_Vector3d add_points;
1709 EigenSTL::vector_Vector3d subtract_points;
1726 RCLCPP_DEBUG(
logger_,
"Modifying object %s took %lf s", obj->id_.c_str(), (clock.now() - start_time).seconds());
1730 EigenSTL::vector_Vector3d& add_points,
1731 EigenSTL::vector_Vector3d& subtract_points)
1733 std::map<std::string, std::vector<PosedBodyPointDecompositionPtr>>::iterator cur_it =
1734 dfce->posed_body_point_decompositions_.find(
id);
1735 if (cur_it != dfce->posed_body_point_decompositions_.end())
1737 for (collision_detection::PosedBodyPointDecompositionPtr& posed_body_point_decomposition : cur_it->second)
1739 subtract_points.insert(subtract_points.end(), posed_body_point_decomposition->getCollisionPoints().begin(),
1740 posed_body_point_decomposition->getCollisionPoints().end());
1744 World::ObjectConstPtr
object =
getWorld()->getObject(
id);
1747 RCLCPP_DEBUG(
logger_,
"Updating/Adding Object '%s' with %lu shapes to CollisionEnvDistanceField",
1748 object->id_.c_str(), object->shapes_.size());
1749 std::vector<PosedBodyPointDecompositionPtr> shape_points;
1750 for (
unsigned int i{ 0 }; i <
object->shapes_.size(); ++i)
1752 shapes::ShapeConstPtr shape =
object->shapes_[i];
1753 if (shape->type == shapes::OCTREE)
1755 const shapes::OcTree* octree_shape =
static_cast<const shapes::OcTree*
>(shape.get());
1756 std::shared_ptr<const octomap::OcTree> octree = octree_shape->octree;
1758 shape_points.push_back(std::make_shared<PosedBodyPointDecomposition>(octree));
1763 shape_points.push_back(
1764 std::make_shared<PosedBodyPointDecomposition>(bd,
getWorld()->getGlobalShapeTransform(
id, i)));
1767 add_points.insert(add_points.end(), shape_points.back()->getCollisionPoints().begin(),
1768 shape_points.back()->getCollisionPoints().end());
1771 dfce->posed_body_point_decompositions_[id] = shape_points;
1775 RCLCPP_DEBUG(
logger_,
"Removing Object '%s' from CollisionEnvDistanceField",
id.c_str());
1776 dfce->posed_body_point_decompositions_.erase(
id);
1780 CollisionEnvDistanceField::DistanceFieldCacheEntryWorldPtr
1783 DistanceFieldCacheEntryWorldPtr dfce = std::make_shared<DistanceFieldCacheEntryWorld>();
1784 dfce->distance_field_ = std::make_shared<distance_field::PropagationDistanceField>(
1788 EigenSTL::vector_Vector3d add_points;
1789 EigenSTL::vector_Vector3d subtract_points;
1790 for (
const std::pair<const std::string, ObjectPtr>&
object : *
getWorld())
1794 dfce->distance_field_->addPointsToField(add_points);
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
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...
std::size_t getSize() const
Get the size of the allowed collision matrix (number of specified entries)
static const std::string NAME
DistanceFieldCacheEntryWorldPtr distance_field_cache_entry_world_
void checkRobotCollision(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state) const override
Check whether the robot model is in collision with the world. Any collisions between a robot link and...
DistanceFieldCacheEntryConstPtr getDistanceFieldCacheEntry(const std::string &group_name, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix *acm) const
DistanceFieldCacheEntryPtr generateDistanceFieldCacheEntry(const std::string &group_name, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix *acm, bool generate_distance_field) const
std::map< std::string, std::map< std::string, bool > > in_group_update_map_
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)
double max_propogation_distance_
EIGEN_MAKE_ALIGNED_OPERATOR_NEW CollisionEnvDistanceField(const moveit::core::RobotModelConstPtr &robot_model, const std::map< std::string, std::vector< CollisionSphere >> &link_body_decompositions=std::map< std::string, std::vector< CollisionSphere >>(), double size_x=DEFAULT_SIZE_X, double size_y=DEFAULT_SIZE_Y, double size_z=DEFAULT_SIZE_Z, const Eigen::Vector3d &origin=Eigen::Vector3d(0, 0, 0), bool use_signed_distance_field=DEFAULT_USE_SIGNED_DISTANCE_FIELD, double resolution=DEFAULT_RESOLUTION, double collision_tolerance=DEFAULT_COLLISION_TOLERANCE, double max_propogation_distance=DEFAULT_MAX_PROPOGATION_DISTANCE, double padding=0.0, double scale=1.0)
DistanceFieldCacheEntryWorldPtr generateDistanceFieldCacheEntryWorld()
bool getSelfCollisions(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, GroupStateRepresentationPtr &gsr) const
bool compareCacheEntryToAllowedCollisionMatrix(const DistanceFieldCacheEntryConstPtr &dfce, const collision_detection::AllowedCollisionMatrix &acm) const
double collision_tolerance_
std::map< std::string, unsigned int > link_body_decomposition_index_map_
bool getIntraGroupCollisions(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, GroupStateRepresentationPtr &gsr) const
void checkSelfCollisionHelper(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix *acm, GroupStateRepresentationPtr &gsr) const
DistanceFieldCacheEntryPtr distance_field_cache_entry_
~CollisionEnvDistanceField() override
void generateCollisionCheckingStructures(const std::string &group_name, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix *acm, GroupStateRepresentationPtr &gsr, bool generate_distance_field) const
bool use_signed_distance_field_
bool compareCacheEntryToState(const DistanceFieldCacheEntryConstPtr &dfce, const moveit::core::RobotState &state) const
std::vector< BodyDecompositionConstPtr > link_body_decomposition_vector_
bool getEnvironmentProximityGradients(const distance_field::DistanceFieldConstPtr &env_distance_field, GroupStateRepresentationPtr &gsr) const
std::map< std::string, GroupStateRepresentationPtr > pregenerated_group_state_representation_map_
bool getSelfProximityGradients(GroupStateRepresentationPtr &gsr) const
bool getEnvironmentCollisions(const CollisionRequest &req, CollisionResult &res, const distance_field::DistanceFieldConstPtr &env_distance_field, GroupStateRepresentationPtr &gsr) const
void checkCollision(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state) const override
Check whether the robot model is in collision with itself or the world at a particular state....
PosedBodySphereDecompositionPtr getPosedLinkBodySphereDecomposition(const moveit::core::LinkModel *ls, unsigned int ind) const
void updateGroupStateRepresentationState(const moveit::core::RobotState &state, GroupStateRepresentationPtr &gsr) const
bool getIntraGroupProximityGradients(GroupStateRepresentationPtr &gsr) const
void setWorld(const WorldPtr &world) override
void updateDistanceObject(const std::string &id, CollisionEnvDistanceField::DistanceFieldCacheEntryWorldPtr &dfce, EigenSTL::vector_Vector3d &add_points, EigenSTL::vector_Vector3d &subtract_points)
std::mutex update_cache_lock_
void getAllCollisions(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, const AllowedCollisionMatrix *acm, GroupStateRepresentationPtr &gsr) const
void addLinkBodyDecompositions(double resolution)
planning_scene::PlanningScenePtr planning_scene_
PosedBodyPointDecompositionPtr getPosedLinkBodyPointDecomposition(const moveit::core::LinkModel *ls) const
void notifyObjectChange(const ObjectConstPtr &obj, World::Action action)
void createCollisionModelMarker(const moveit::core::RobotState &state, visualization_msgs::msg::MarkerArray &model_markers) const
void checkSelfCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const moveit::core::RobotState &state) const override
Check for robot self collision. Any collision between any pair of links is checked for,...
void getGroupStateRepresentation(const DistanceFieldCacheEntryConstPtr &dfce, const moveit::core::RobotState &state, GroupStateRepresentationPtr &gsr) const
void getCollisionGradients(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, const AllowedCollisionMatrix *acm, GroupStateRepresentationPtr &gsr) const
World::ObserverHandle observer_handle_
Provides the interface to the individual collision checking libraries.
virtual void setWorld(const WorldPtr &world)
moveit::core::RobotModelConstPtr robot_model_
The kinematic model corresponding to this collision model.
const WorldPtr & getWorld()
void setPadding(double padding)
Set the link padding (for every link)
World::ObjectConstPtr ObjectConstPtr
const std::map< std::string, double > & getLinkPadding() const
Get the link paddings as a map (from link names to padding value)
Represents an action that occurred on an object in the world. Several bits may be set indicating seve...
Object defining bodies that can be attached to robot links.
const EigenSTL::vector_Isometry3d & getGlobalCollisionBodyTransforms() const
Get the global transforms (in world frame) for the collision bodies. The returned transforms are guar...
const std::vector< shapes::ShapeConstPtr > & getShapes() const
Get the shapes that make up this attached body.
const std::vector< std::string > & getUpdatedLinkModelNames() const
Get the names of the links returned by getUpdatedLinkModels()
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....
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
const std::string & getName() const
Get the name of the joint.
A link from the robot. Contains the constant transform applied to the link and its geometry.
const std::vector< shapes::ShapeConstPtr > & getShapes() const
Get shape associated to the collision geometry for this link.
const JointModel * getParentJointModel() const
Get the joint model whose child this link is. There will always be a parent joint.
const std::string & getName() const
The name of this link.
Representation of a robot's state. This includes position, velocity, acceleration and effort.
const LinkModel * getLinkModel(const std::string &link) const
Get the model of a particular link.
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
Get the link transform w.r.t. the root link (model frame) of the RobotModel. This is typically the ro...
void getAttachedBodies(std::vector< const AttachedBody * > &attached_bodies) const
Get all bodies attached to the model corresponding to this state.
const JointModelGroup * getJointModelGroup(const std::string &group) const
Get the model of a particular joint group.
void updateLinkTransforms()
Update the reference frame transforms for links. This call is needed before using the transforms of l...
const Eigen::Isometry3d & getFrameTransform(const std::string &frame_id, bool *frame_found=nullptr)
Get the transformation matrix from the model frame (root of model) to the frame identified by frame_i...
double getVariablePosition(const std::string &variable) const
Get the position of a particular variable. An exception is thrown if the variable is not known.
const AttachedBody * getAttachedBody(const std::string &name) const
Get the attached body named name. Return nullptr if not found.
std::size_t getVariableCount() const
Get the number of variables that make up this state.
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.
@ NEVER
Collisions between the pair of bodies is never ok, i.e., if two bodies are in contact in a particular...
@ ALWAYS
Collisions between a particular pair of bodies does not imply that the robot configuration is in coll...
@ ROBOT_LINK
A link on the robot.
@ ROBOT_ATTACHED
A body attached to a robot link.
@ WORLD_OBJECT
A body in the environment.
rclcpp::Logger getLogger()
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)
PosedBodySphereDecompositionVectorPtr getAttachedBodySphereDecomposition(const moveit::core::AttachedBody *att, double resolution)
BodyDecompositionConstPtr getBodyDecompositionCacheEntry(const shapes::ShapeConstPtr &shape, double resolution)
bool doBoundingSpheresIntersect(const PosedBodySphereDecompositionConstPtr &p1, const PosedBodySphereDecompositionConstPtr &p2)
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)
PosedBodyPointDecompositionVectorPtr getAttachedBodyPointDecomposition(const moveit::core::AttachedBody *att, double resolution)
Vec3fX< details::Vec3Data< double > > Vector3d
Main namespace for MoveIt.
Representation of a collision checking request.
std::string group_name
The group name to check collisions for (optional; if empty, assume the complete robot)....
bool contacts
If true, compute contacts. Otherwise only a binary collision yes/no is reported.
std::size_t max_contacts
Overall maximum number of contacts to compute.
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...
Representation of a collision checking result.
bool collision
True if collision was found, false otherwise.
std::size_t contact_count
Number of contacts returned.