mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
ComputeIK: report collision pairs
This commit is contained in:
parent
19fa7349d3
commit
ce10d96c5c
@ -117,9 +117,10 @@ typedef std::vector<std::vector<double>> IKSolutions;
|
|||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
// ??? TODO: provide callback methods in PlanningScene class / probably not very useful here though...
|
// ??? 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,
|
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();
|
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
|
// check collision with the world using the padded version
|
||||||
collision_detection::CollisionRequest req;
|
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);
|
scene->checkCollision(req, res, robot_state, acm);
|
||||||
return res.collision;
|
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,
|
bool validateEEF(const PropertyMap& props, const moveit::core::RobotModelConstPtr& robot_model,
|
||||||
const moveit::core::JointModelGroup*& jmg, std::string* msg)
|
const moveit::core::JointModelGroup*& jmg, std::string* msg)
|
||||||
{
|
{
|
||||||
@ -283,9 +298,10 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
|||||||
}
|
}
|
||||||
|
|
||||||
// validate placed link for collisions
|
// 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()) {
|
if (colliding && !storeFailures()) {
|
||||||
ROS_ERROR("eef in collision");
|
ROS_WARN_STREAM("eef in collision: " << listCollisionPairs(collisions.contacts, "\n"));
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
robot_state::RobotState& sandbox_state = sandbox_scene->getCurrentStateNonConst();
|
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);
|
generateCollisionMarkers(sandbox_state, appender, link_to_visualize);
|
||||||
std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution.markers()));
|
std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution.markers()));
|
||||||
solution.setCost(std::numeric_limits<double>::infinity()); // mark solution as failure
|
solution.setCost(std::numeric_limits<double>::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));
|
spawn(InterfaceState(sandbox_scene), std::move(solution));
|
||||||
return;
|
return;
|
||||||
} else
|
} else
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user