Enable collisions visualizations (#708)

Author: rhaschke

core/include/moveit/task_constructor/storage.h

@@ -335,7 +335,7 @@ class SolutionBase
 	// comment for this solution, e.g. explanation of failure
 	std::string comment_;
 	// markers for this solution, e.g. target frame or collision indicators
-	std::deque<visualization_msgs::Marker> markers_;
+	std::vector<visualization_msgs::Marker> markers_;
 
 	// begin and end InterfaceState of this solution/trajectory
 	const InterfaceState* start_ = nullptr;

core/include/moveit/task_constructor/utils.h

@@ -45,6 +45,9 @@
 #include <Eigen/Geometry>
 
 #include <moveit/macros/class_forward.h>
+#include <moveit/robot_trajectory/robot_trajectory.h>
+#include <moveit/collision_detection/collision_common.h>
+#include <moveit/task_constructor/solvers/planner_interface.h>
 
 namespace planning_scene {
 MOVEIT_CLASS_FORWARD(PlanningScene);
@@ -143,6 +146,19 @@ class Flags
 bool getRobotTipForFrame(const Property& tip_pose, const planning_scene::PlanningScene& scene,
                          const moveit::core::JointModelGroup* jmg, std::string& error_msg,
                          const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame);
+
+/** Add sphere markers to visualize given collisions */
+void addCollisionMarkers(std::vector<visualization_msgs::Marker>& markers, const std::string& frame_id,
+                         const collision_detection::CollisionResult::ContactMap& contacts, double radius = 0.035);
+
+/** Add sphere markers to visualize collisions within the trajectory */
+void addCollisionMarkers(std::vector<visualization_msgs::Marker>& markers,
+                         const robot_trajectory::RobotTrajectory& trajectory,
+                         const planning_scene::PlanningSceneConstPtr& planning_scene);
+
+/** Returns true if the result provides hints that planning failed due to collisions */
+bool hints_at_collisions(const solvers::PlannerInterface::Result& result);
+
 }  // namespace utils
 }  // namespace task_constructor
 }  // namespace moveit

core/src/stages/compute_ik.cpp

@@ -89,7 +89,7 @@ void ComputeIK::setTargetPose(const Eigen::Isometry3d& pose, const std::string&
 struct IKSolution
 {
 	std::vector<double> joint_positions;
-	collision_detection::Contact contact;
+	collision_detection::CollisionResult::ContactMap contacts;
 	bool collision_free;
 	bool satisfies_constraints;
 };
@@ -141,7 +141,7 @@ bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& sce
 }
 
 std::string listCollisionPairs(const collision_detection::CollisionResult::ContactMap& contacts,
-                               const std::string& separator) {
+                               const std::string& separator = ", ") {
 	std::string result;
 	for (const auto& contact : contacts) {
 		if (!result.empty())
@@ -345,8 +345,8 @@ void ComputeIK::compute() {
 		generateCollisionMarkers(sandbox_state, appender, links_to_visualize);
 		std::copy(eef_markers.begin(), eef_markers.end(), std::back_inserter(solution.markers()));
 		solution.markAsFailure();
-		// TODO: visualize collisions
-		solution.setComment(s.comment() + " eef in collision: " + listCollisionPairs(collisions.contacts, ", "));
+		solution.setComment(s.comment() + " eef in collision: " + listCollisionPairs(collisions.contacts));
+		utils::addCollisionMarkers(solution.markers(), scene->getPlanningFrame(), collisions.contacts);
 		auto colliding_scene{ scene->diff() };
 		colliding_scene->setCurrentState(sandbox_state);
 		spawn(InterfaceState(colliding_scene), std::move(solution));
@@ -395,9 +395,7 @@ void ComputeIK::compute() {
 		req.group_name = jmg->getName();
 		scene->checkCollision(req, res, *state);
 		solution.collision_free = ignore_collisions || !res.collision;
-		if (!res.contacts.empty()) {
-			solution.contact = res.contacts.begin()->second.front();
-		}
+		solution.contacts = std::move(res.contacts);
 
 		return solution.satisfies_constraints && solution.collision_free;
 	};
@@ -433,10 +431,8 @@ void ComputeIK::compute() {
 				// compute cost as distance to compare_pose
 				solution.setCost(s.cost() + jmg->distance(ik_solutions[i].joint_positions.data(), compare_pose.data()));
 			else if (!ik_solutions[i].collision_free) {  // solution was in collision
-				std::stringstream ss;
-				ss << "Collision between '" << ik_solutions[i].contact.body_name_1 << "' and '"
-				   << ik_solutions[i].contact.body_name_2 << "'";
-				solution.markAsFailure(ss.str());
+				solution.markAsFailure("Collision between " + listCollisionPairs(ik_solutions[i].contacts));
+				utils::addCollisionMarkers(solution.markers(), scene->getPlanningFrame(), ik_solutions[i].contacts);
 			} else if (!ik_solutions[i].satisfies_constraints) {  // solution was violating constraints
 				solution.markAsFailure("Constraints violated");
 			}

core/src/stages/connect.cpp

@@ -150,6 +150,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
 	intermediate_scenes.push_back(start);
 
 	bool success = false;
+	bool has_potential_collisions = false;
 	std::string comment = "No planners specified";
 	std::vector<double> positions;
 	for (const GroupPlannerVector::value_type& pair : planner_) {
@@ -169,6 +170,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
 
 		if (!success) {
 			comment = result.message;
+			has_potential_collisions = trajectory && utils::hints_at_collisions(result);
 			break;
 		}
 
@@ -187,8 +189,15 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
 		solution = merge(sub_trajectories, intermediate_scenes, from.scene()->getCurrentState());
 	if (!solution)  // success == false or merging failed: store sequentially
 		solution = makeSequential(sub_trajectories, intermediate_scenes, from, to);
-	if (!success)  // error during sequential planning
+	if (!success) {  // error already during sequential planning
 		solution->markAsFailure(comment);
+		if (has_potential_collisions) {
+			// add collision markers for last (failed) trajectory segment
+			auto sequence = std::dynamic_pointer_cast<SolutionSequence>(solution);
+			auto trajectory = dynamic_cast<const SubTrajectory*>(sequence->solutions().back())->trajectory();
+			utils::addCollisionMarkers(solution->markers(), *trajectory, start);
+		}
+	}
 	connect(from, to, solution);
 }
 

core/src/stages/modify_planning_scene.cpp

@@ -163,6 +163,7 @@ std::pair<InterfaceState, SubTrajectory> ModifyPlanningScene::apply(const Interf
 		if (res.collision) {
 			const auto contact = res.contacts.begin()->second.front();
 			traj.markAsFailure(contact.body_name_1 + " colliding with " + contact.body_name_2);
+			utils::addCollisionMarkers(traj.markers(), scene->getPlanningFrame(), res.contacts);
 		}
 	} catch (const std::exception& e) {
 		traj.markAsFailure(e.what());

core/src/stages/move_relative.cpp

@@ -105,7 +105,7 @@ static bool getJointStateFromOffset(const boost::any& direction, const Interface
 }
 
 // Create an arrow marker from start_pose to reached_pose, split into a red and green part based on achieved distance
-static void visualizePlan(std::deque<visualization_msgs::Marker>& markers, Interface::Direction dir, bool success,
+static void visualizePlan(std::vector<visualization_msgs::Marker>& markers, Interface::Direction dir, bool success,
                           const std::string& ns, const std::string& frame_id, const Eigen::Isometry3d& start_pose,
                           const Eigen::Isometry3d& reached_pose, const Eigen::Vector3d& linear, double distance) {
 	double linear_norm = linear.norm();
@@ -193,14 +193,17 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
 
 	robot_trajectory::RobotTrajectoryPtr robot_trajectory;
 	bool success = false;
+	bool has_potential_collisions = false;
 	std::string comment = "";
 
 	if (getJointStateFromOffset(direction, dir, jmg, scene->getCurrentStateNonConst())) {
 		// plan to joint-space target
 		auto result = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
 		success = bool(result);
-		if (!success)
+		if (!success) {
 			comment = result.message;
+			has_potential_collisions = robot_trajectory && utils::hints_at_collisions(result);
+		}
 	} else {
 		// Cartesian targets require an IK reference frame
 		const moveit::core::LinkModel* link;
@@ -292,8 +295,10 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
 		auto result =
 		    planner_->plan(state.scene(), *link, offset, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
 		success = bool(result);
-		if (!success)
+		if (!success) {
 			comment = result.message;
+			has_potential_collisions = robot_trajectory && utils::hints_at_collisions(result);
+		}
 
 		if (robot_trajectory && robot_trajectory->getWayPointCount() > 0) {  // the following requires a robot_trajectory
 			                                                                  // returned from planning
@@ -337,8 +342,11 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
 			robot_trajectory->reverse();
 		solution.setTrajectory(robot_trajectory);
 
-		if (!success)
+		if (!success) {
 			solution.markAsFailure(comment);
+			if (has_potential_collisions)
+				utils::addCollisionMarkers(solution.markers(), *robot_trajectory, scene);
+		}
 		return true;
 	}
 	return false;

core/src/stages/move_to.cpp

@@ -197,14 +197,17 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
 	const auto& path_constraints = props.get<moveit_msgs::Constraints>("path_constraints");
 	robot_trajectory::RobotTrajectoryPtr robot_trajectory;
 	bool success = false;
+	bool has_potential_collisions = false;
 	std::string comment = "";
 
 	if (getJointStateGoal(goal, jmg, scene->getCurrentStateNonConst())) {
 		// plan to joint-space target
 		auto result = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
 		success = bool(result);
-		if (!success)
+		if (!success) {
 			comment = result.message;
+			has_potential_collisions = robot_trajectory && utils::hints_at_collisions(result);
+		}
 	} else {  // Cartesian goal
 		// Where to go?
 		Eigen::Isometry3d target;
@@ -243,8 +246,10 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
 		const auto result =
 		    planner_->plan(state.scene(), *link, offset, target, jmg, timeout, robot_trajectory, path_constraints);
 		success = bool(result);
-		if (!success)
+		if (!success) {
 			comment = result.message;
+			has_potential_collisions = robot_trajectory && utils::hints_at_collisions(result);
+		}
 	}
 
 	// store result
@@ -259,9 +264,11 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
 			robot_trajectory->reverse();
 		solution.setTrajectory(robot_trajectory);
 
-		if (!success)
+		if (!success) {
 			solution.markAsFailure(comment);
-
+			if (has_potential_collisions)
+				utils::addCollisionMarkers(solution.markers(), *robot_trajectory, scene);
+		}
 		return true;
 	}
 	return false;

core/src/utils.cpp

@@ -39,6 +39,8 @@
 
 #include <moveit/robot_state/robot_state.h>
 #include <moveit/planning_scene/planning_scene.h>
+#include <moveit/robot_trajectory/robot_trajectory.h>
+#include <moveit/utils/moveit_error_code.h>
 
 #include <moveit/task_constructor/properties.h>
 #include <moveit/task_constructor/storage.h>
@@ -97,6 +99,59 @@ bool getRobotTipForFrame(const Property& tip_pose, const planning_scene::Plannin
 	return true;
 }
 
+void addCollisionMarkers(std::vector<visualization_msgs::Marker>& markers, const std::string& frame_id,
+                         const collision_detection::CollisionResult::ContactMap& contacts, double radius) {
+	visualization_msgs::Marker m;
+	m.header.frame_id = frame_id;
+	m.ns = "collisions";
+	m.type = visualization_msgs::Marker::SPHERE;
+	m.action = visualization_msgs::Marker::ADD;
+	m.pose.orientation.x = 0.0;
+	m.pose.orientation.y = 0.0;
+	m.pose.orientation.z = 0.0;
+	m.pose.orientation.w = 1.0;
+	m.scale.x = m.scale.y = m.scale.z = radius * 2.0;
+	m.color.r = m.color.a = 1.0;
+
+	for (const auto& collision : contacts) {
+		for (const auto& contact : collision.second) {
+			m.pose.position.x = contact.pos.x();
+			m.pose.position.y = contact.pos.y();
+			m.pose.position.z = contact.pos.z();
+			markers.push_back(m);
+		}
+	}
+}
+
+void addCollisionMarkers(std::vector<visualization_msgs::Marker>& markers,
+                         const robot_trajectory::RobotTrajectory& trajectory,
+                         const planning_scene::PlanningSceneConstPtr& planning_scene) {
+	std::size_t n_wp = trajectory.getWayPointCount();
+	for (std::size_t it = 0; it < n_wp; ++it) {
+		const moveit::core::RobotState& robot_state = trajectory.getWayPoint(it);
+
+		// Collision contact check
+		collision_detection::CollisionRequest req;
+		collision_detection::CollisionResult res;
+		req.contacts = true;
+		req.max_contacts = 10;
+
+		planning_scene->checkCollision(req, res, robot_state);
+
+		if (res.contact_count > 0)
+			addCollisionMarkers(markers, planning_scene->getPlanningFrame(), res.contacts);
+	}
+}
+
+bool hints_at_collisions(const solvers::PlannerInterface::Result& result) {
+	static const std::string INVALID_MOTION_PLAN_MSG = []() {
+		moveit_msgs::MoveItErrorCodes err;
+		err.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
+		return moveit::core::MoveItErrorCode::toString(err);
+	}();
+	return result.message == INVALID_MOTION_PLAN_MSG;
+}
+
 }  // namespace utils
 }  // namespace task_constructor
 }  // namespace moveit