mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
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
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:
parent
336ad4456c
commit
e9eab62f5f
@ -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;
|
||||
|
@ -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
|
||||
|
@ -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");
|
||||
}
|
||||
|
@ -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);
|
||||
}
|
||||
|
||||
|
@ -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());
|
||||
|
@ -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;
|
||||
|
@ -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;
|
||||
|
@ -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
|
||||
|
Loading…
Reference in New Issue
Block a user