mini change in drop task planning. Dropped Objects vanish, no further planning scene config needed

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