From bc0471c5e05cc80983462bfd3febfc828d796f04 Mon Sep 17 00:00:00 2001
From: KingMaZito <matteo.aneddama@icloud.com>
Date: Tue, 25 Jul 2023 13:41:04 +0200
Subject: [PATCH] mini change in drop task planning. Dropped Objects vanish, no
further planning scene config needed
---
src/mediator/mg_mediator.cpp | 26 +++++++++++++++++++-------
1 file changed, 19 insertions(+), 7 deletions(-)
diff --git a/src/mediator/mg_mediator.cpp b/src/mediator/mg_mediator.cpp
index 216988e..6c277b4 100644
--- a/src/mediator/mg_mediator.cpp
+++ b/src/mediator/mg_mediator.cpp
@@ -714,6 +714,24 @@ moveit::task_constructor::Task MGMediator::drop(const std::string& obj, const st
place->insert(std::move(stage));
}
+ {
+ /*
+ The object should fall, but animating it is not my objective.... let's just make it disappear
+ TODO add a Callback to a working thread who does the animation... that would be so cool
+ */
+ auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("remove object");
+ stage->removeObject(object);
+ place->insert(std::move(stage));
+ }
+
+ {
+ auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (hand,object)");
+ stage->allowCollisions(
+ object, task_.getRobotModel()->getJointModelGroup(mr->map()["eef_name"])->getLinkModelNamesWithCollisionGeometry(),
+ false);
+ task_.add(std::move(stage));
+ }
+
{
auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("close hand", sampling_planner_);
stage->setGroup(mr->map()["eef_name"]);
@@ -732,13 +750,7 @@ moveit::task_constructor::Task MGMediator::drop(const std::string& obj, const st
stage->restrictDirection(moveit::task_constructor::stages::MoveTo::FORWARD);
task_.add(std::move(stage));
- {
- auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (hand,object)");
- stage->allowCollisions(
- object, task_.getRobotModel()->getJointModelGroup(mr->map()["eef_name"])->getLinkModelNamesWithCollisionGeometry(),
- false);
- task_.add(std::move(stage));
- }
+
}
return task_;
--
GitLab