From ce10d96c5c7978d5037f2817e98d47c50613b352 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 8 Apr 2018 12:57:47 +0200 Subject: [PATCH] ComputeIK: report collision pairs --- core/src/stages/compute_ik.cpp | 29 +++++++++++++++++++++++------ 1 file changed, 23 insertions(+), 6 deletions(-) diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index bbd2ff81..0bacbf3e 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -117,9 +117,10 @@ typedef std::vector> IKSolutions; namespace { // ??? TODO: provide callback methods in PlanningScene class / probably not very useful here though... -// move into MoveIt! core +// TODO: move into MoveIt! core, lift active_components_only_ from fcl to common interface bool isTargetPoseColliding(const planning_scene::PlanningScenePtr& scene, - Eigen::Affine3d pose, const robot_model::LinkModel* link) + Eigen::Affine3d pose, const robot_model::LinkModel* link, + collision_detection::CollisionResult* collision_result = nullptr) { robot_state::RobotState& robot_state = scene->getCurrentStateNonConst(); @@ -150,11 +151,25 @@ bool isTargetPoseColliding(const planning_scene::PlanningScenePtr& scene, // check collision with the world using the padded version collision_detection::CollisionRequest req; - collision_detection::CollisionResult res; + collision_detection::CollisionResult result; + req.contacts = (collision_result != nullptr); + collision_detection::CollisionResult& res = collision_result ? *collision_result : result; scene->checkCollision(req, res, robot_state, acm); return res.collision; } +std::string listCollisionPairs(const collision_detection::CollisionResult::ContactMap &contacts, + const std::string& separator) +{ + std::string result; + for (const auto& contact : contacts) { + if (!result.empty()) + result.append(separator); + result.append(contact.first.first).append(" - ").append(contact.first.second); + } + return result; +} + bool validateEEF(const PropertyMap& props, const moveit::core::RobotModelConstPtr& robot_model, const moveit::core::JointModelGroup*& jmg, std::string* msg) { @@ -283,9 +298,10 @@ void ComputeIK::onNewSolution(const SolutionBase &s) } // validate placed link for collisions - bool colliding = isTargetPoseColliding(sandbox_scene, target_pose, link); + collision_detection::CollisionResult collisions; + bool colliding = isTargetPoseColliding(sandbox_scene, target_pose, link, &collisions); if (colliding && !storeFailures()) { - ROS_ERROR("eef in collision"); + ROS_WARN_STREAM("eef in collision: " << listCollisionPairs(collisions.contacts, "\n")); return; } robot_state::RobotState& sandbox_state = sandbox_scene->getCurrentStateNonConst(); @@ -308,7 +324,8 @@ void ComputeIK::onNewSolution(const SolutionBase &s) generateCollisionMarkers(sandbox_state, appender, link_to_visualize); std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution.markers())); solution.setCost(std::numeric_limits::infinity()); // mark solution as failure - solution.setName("eef in collision"); + // TODO: visualize collisions + solution.setName("eef in collision: " + listCollisionPairs(collisions.contacts, ", ")); spawn(InterfaceState(sandbox_scene), std::move(solution)); return; } else