Enable collisions visualizations (#708)
Some checks failed
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[CLANG_TIDY:true IMAGE:noble-ci-testing TARGET_CMAKE_… (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[DOCKER_RUN_OPTS:-e PRELOAD=libasan.so.8 -e LSAN_OPTI… (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:jammy-ci]) (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:noble-ci NAME:ccov TARGET_CMAKE_ARGS:-DCMAKE_B… (push) Has been cancelled
Format / pre-commit (push) Has been cancelled
CI / doc (push) Has been cancelled
CI / deploy (push) Has been cancelled

This commit is contained in:
Robert Haschke 2025-08-22 08:38:44 +02:00 committed by GitHub
parent 336ad4456c
commit e9eab62f5f
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
8 changed files with 113 additions and 21 deletions

View File

@ -335,7 +335,7 @@ private:
// 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;

View File

@ -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 @@ private:
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

View File

@ -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");
}

View File

@ -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);
}

View File

@ -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());

View File

@ -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;

View File

@ -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;

View File

@ -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