diff --git a/src/mediator/mg_mediator.cpp b/src/mediator/mg_mediator.cpp index 216988e758df9ee72f39bb5db47970f62c78aa0d..6c277b462afab3a90a48cabb89596dd16b92bc82 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_;