From ca7b643bbe7fbe240e1ba54578221a77de42eaec Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 30 Nov 2020 13:20:39 -0600 Subject: [PATCH 1/4] Do self collision checks --- .../src/bullet_collision_checker_tutorial.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/doc/bullet_collision_checker/src/bullet_collision_checker_tutorial.cpp b/doc/bullet_collision_checker/src/bullet_collision_checker_tutorial.cpp index bf50e5566..d14200f29 100644 --- a/doc/bullet_collision_checker/src/bullet_collision_checker_tutorial.cpp +++ b/doc/bullet_collision_checker/src/bullet_collision_checker_tutorial.cpp @@ -110,7 +110,7 @@ void computeCollisionContactPoints(InteractiveRobot& robot) c_req.max_contacts_per_pair = 5; c_req.verbose = false; - g_planning_scene->checkCollision(c_req, c_res, *robot.robotState()); + g_planning_scene->checkSelfCollision(c_req, c_res, *robot.robotState()); if (c_res.collision) { @@ -239,7 +239,7 @@ int main(int argc, char** argv) collision_detection::CollisionResult res; collision_detection::CollisionRequest req; req.contacts = true; - planning_scene->checkCollision(req, res); + planning_scene->checkSelfCollision(req, res); ROS_INFO_STREAM_NAMED("bullet_tutorial", (res.collision ? "In collision." : "Not in collision.")); // This code is repeated for the second robot configuration. // END_SUB_TUTORIAL @@ -283,7 +283,7 @@ int main(int argc, char** argv) visual_tools.publishRobotState(state); res.clear(); - planning_scene->checkCollision(req, res); + planning_scene->checkSelfCollision(req, res); ROS_INFO_STREAM_NAMED("bullet_tutorial", (res.collision ? "In collision." : "Not in collision.")); visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to perform a CCD check."); From b01a4f529e066e0800d87ffb30be4da06835c09e Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 21 Dec 2020 11:57:28 -0600 Subject: [PATCH 2/4] Add collision debug info --- .../src/bullet_collision_checker_tutorial.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/doc/bullet_collision_checker/src/bullet_collision_checker_tutorial.cpp b/doc/bullet_collision_checker/src/bullet_collision_checker_tutorial.cpp index d14200f29..b5487fe00 100644 --- a/doc/bullet_collision_checker/src/bullet_collision_checker_tutorial.cpp +++ b/doc/bullet_collision_checker/src/bullet_collision_checker_tutorial.cpp @@ -111,6 +111,9 @@ void computeCollisionContactPoints(InteractiveRobot& robot) c_req.verbose = false; g_planning_scene->checkSelfCollision(c_req, c_res, *robot.robotState()); + ROS_INFO_STREAM("Objects in collision (printing first collision pair of " + << c_res.contacts.size() << "): " << c_res.contacts.begin()->first.first << ", " + << c_res.contacts.begin()->first.second); if (c_res.collision) { @@ -241,6 +244,9 @@ int main(int argc, char** argv) req.contacts = true; planning_scene->checkSelfCollision(req, res); ROS_INFO_STREAM_NAMED("bullet_tutorial", (res.collision ? "In collision." : "Not in collision.")); + ROS_INFO_STREAM("Objects in collision (printing first collision pair of " + << res.contacts.size() << "): " << res.contacts.begin()->first.first << ", " + << res.contacts.begin()->first.second); // This code is repeated for the second robot configuration. // END_SUB_TUTORIAL @@ -285,6 +291,9 @@ int main(int argc, char** argv) res.clear(); planning_scene->checkSelfCollision(req, res); ROS_INFO_STREAM_NAMED("bullet_tutorial", (res.collision ? "In collision." : "Not in collision.")); + ROS_INFO_STREAM("Objects in collision (printing first collision pair of " + << res.contacts.size() << "): " << res.contacts.begin()->first.first << ", " + << res.contacts.begin()->first.second); visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to perform a CCD check."); @@ -298,6 +307,10 @@ int main(int argc, char** argv) res.clear(); planning_scene->getCollisionEnv()->checkRobotCollision(req, res, state, state_before); ROS_INFO_STREAM_NAMED("bullet_tutorial", (res.collision ? "In collision." : "Not in collision.")); + ROS_INFO_STREAM("Objects in collision (printing first collision pair of " + << res.contacts.size() << "): " << res.contacts.begin()->first.first << ", " + << res.contacts.begin()->first.second); + // Note that the terminal output displays "In collision.". // END_SUB_TUTORIAL visualization_msgs::MarkerArray markers; From cd5e45ce3cbac5fed854be804b312b78ab3e94dc Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 21 Dec 2020 12:11:45 -0600 Subject: [PATCH 3/4] Always visualize collision points --- .../src/bullet_collision_checker_tutorial.cpp | 41 ++++++++++++------- 1 file changed, 26 insertions(+), 15 deletions(-) diff --git a/doc/bullet_collision_checker/src/bullet_collision_checker_tutorial.cpp b/doc/bullet_collision_checker/src/bullet_collision_checker_tutorial.cpp index b5487fe00..f5333bf33 100644 --- a/doc/bullet_collision_checker/src/bullet_collision_checker_tutorial.cpp +++ b/doc/bullet_collision_checker/src/bullet_collision_checker_tutorial.cpp @@ -94,6 +94,28 @@ void publishMarkers(visualization_msgs::MarkerArray& markers) g_marker_array_publisher->publish(g_collision_points); } +void publishCollisionMarkers(collision_detection::CollisionResult& c_res) +{ + if (c_res.collision) + { + if (c_res.contact_count > 0) + { + std_msgs::ColorRGBA color; + color.r = 1.0; + color.g = 0.0; + color.b = 1.0; + color.a = 0.5; + visualization_msgs::MarkerArray markers; + + /* Get the contact ponts and display them as markers */ + collision_detection::getCollisionMarkersFromContacts(markers, "panda_link0", c_res.contacts, color, + ros::Duration(), // remain until deleted + 0.01); // radius + publishMarkers(markers); + } + } +} + void computeCollisionContactPoints(InteractiveRobot& robot) { // move the world geometry in the collision world @@ -118,21 +140,7 @@ void computeCollisionContactPoints(InteractiveRobot& robot) if (c_res.collision) { ROS_INFO("COLLIDING contact_point_count=%d", (int)c_res.contact_count); - if (c_res.contact_count > 0) - { - std_msgs::ColorRGBA color; - color.r = 1.0; - color.g = 0.0; - color.b = 1.0; - color.a = 0.5; - visualization_msgs::MarkerArray markers; - - /* Get the contact ponts and display them as markers */ - collision_detection::getCollisionMarkersFromContacts(markers, "panda_link0", c_res.contacts, color, - ros::Duration(), // remain until deleted - 0.01); // radius - publishMarkers(markers); - } + publishCollisionMarkers(c_res); } else { @@ -247,6 +255,7 @@ int main(int argc, char** argv) ROS_INFO_STREAM("Objects in collision (printing first collision pair of " << res.contacts.size() << "): " << res.contacts.begin()->first.first << ", " << res.contacts.begin()->first.second); + publishCollisionMarkers(res); // This code is repeated for the second robot configuration. // END_SUB_TUTORIAL @@ -294,6 +303,7 @@ int main(int argc, char** argv) ROS_INFO_STREAM("Objects in collision (printing first collision pair of " << res.contacts.size() << "): " << res.contacts.begin()->first.first << ", " << res.contacts.begin()->first.second); + publishCollisionMarkers(res); visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to perform a CCD check."); @@ -310,6 +320,7 @@ int main(int argc, char** argv) ROS_INFO_STREAM("Objects in collision (printing first collision pair of " << res.contacts.size() << "): " << res.contacts.begin()->first.first << ", " << res.contacts.begin()->first.second); + publishCollisionMarkers(res); // Note that the terminal output displays "In collision.". // END_SUB_TUTORIAL From d58829117f753916d6cd6741498a4331ef88ba3b Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 21 Dec 2020 12:29:24 -0600 Subject: [PATCH 4/4] Use a different collision detection method --- .../src/bullet_collision_checker_tutorial.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/doc/bullet_collision_checker/src/bullet_collision_checker_tutorial.cpp b/doc/bullet_collision_checker/src/bullet_collision_checker_tutorial.cpp index f5333bf33..33a9da526 100644 --- a/doc/bullet_collision_checker/src/bullet_collision_checker_tutorial.cpp +++ b/doc/bullet_collision_checker/src/bullet_collision_checker_tutorial.cpp @@ -250,7 +250,7 @@ int main(int argc, char** argv) collision_detection::CollisionResult res; collision_detection::CollisionRequest req; req.contacts = true; - planning_scene->checkSelfCollision(req, res); + planning_scene->getCollisionEnv()->checkSelfCollision(req, res, state); ROS_INFO_STREAM_NAMED("bullet_tutorial", (res.collision ? "In collision." : "Not in collision.")); ROS_INFO_STREAM("Objects in collision (printing first collision pair of " << res.contacts.size() << "): " << res.contacts.begin()->first.first << ", " @@ -298,7 +298,7 @@ int main(int argc, char** argv) visual_tools.publishRobotState(state); res.clear(); - planning_scene->checkSelfCollision(req, res); + planning_scene->getCollisionEnv()->checkSelfCollision(req, res, state); ROS_INFO_STREAM_NAMED("bullet_tutorial", (res.collision ? "In collision." : "Not in collision.")); ROS_INFO_STREAM("Objects in collision (printing first collision pair of " << res.contacts.size() << "): " << res.contacts.begin()->first.first << ", "