diff --git a/src/moveit_sorting_controller.cpp b/src/moveit_sorting_controller.cpp
index b475dfaf878df616bcefe456f1a859c97ccd9ac9..8add2aca87583a72158b4a852babbc02a17b0d56 100644
--- a/src/moveit_sorting_controller.cpp
+++ b/src/moveit_sorting_controller.cpp
@@ -56,6 +56,12 @@ int main(int argc, char** argv)
     controller.initScene(scene, false);
   });
 
+  ros::WallDuration(5).sleep();  // wait 5 secs to init scene (to give moveit time to load)
+
+  controller.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") +
+      "/config/config_scene_ceti-table-1-empty.json"));
+
+  // only add the scene observer after the base scene has been loaded
   controller.addCallback(scene_observer_topic, [&controller](auto msg) {
     ROS_INFO_STREAM("Updating scene from observer.");
     Scene scene;
@@ -63,13 +69,6 @@ int main(int argc, char** argv)
     controller.initScene(scene, true);
   });
 
-  ros::WallDuration(5).sleep();  // wait 5 secs to init scene (to give moveit time to load)
-
-  controller.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") +
-      "/config/config_scene_ceti-table-placeworld.json"));
-
-  std::optional<std::string> currentlyPickedBox{};
-
   ros::Subscriber sub =
       n.subscribe<std_msgs::Empty>("send_scene", 1000, [&controller](const std_msgs::EmptyConstPtr& msg) {
         ROS_INFO_STREAM("Sending scene manually (triggered by a message to the 'send_scene' topic)");
@@ -194,25 +193,65 @@ int main(int argc, char** argv)
       }
       controller.configureCollaborationZone(*zone, command.configchange().idrobotnewowner());
     }
-    else
+    else if (command.has_pick())
     {
-      ROS_WARN_STREAM("[COMMAND] IGNORED! Ignoring command of unknown type " << command.msg_case());
-    }
-  };
-  controller.reactToCommandMessage(pick_place_callback);
 
-  auto sceneUpdateCallback = [&currentlyPickedBox, &controller]() {
-    if (currentlyPickedBox.has_value())
+      Object* robot = controller.resolveObject(command.pick().idrobot());
+      Object* object = controller.resolveObject(command.pick().idpick());
+      ROS_INFO_STREAM("[COMMAND] Pick " << object->id() << " using robot " << robot->id());
+      if (!robot)
+      {
+        ROS_ERROR_STREAM("[COMMAND] FAILED! Unable to pick using unknown robot '" << command.pick().idrobot() << "'.");
+        return;
+      }
+      if (!object)
+      {
+        ROS_ERROR_STREAM("[COMMAND] FAILED! Unable to pick unknown object '" << command.pick().idpick() << "'.");
+        return;
+      }
+      controller.pick(command.pick().idrobot(), command.pick().idpick(), false);
+    }
+    else if (command.has_place())
     {
-      auto resolved = controller.resolveObject(currentlyPickedBox.value());
-      if (!resolved)
+      Object* robot = controller.resolveObject(command.place().idrobot());
+      Object* object = controller.resolveObject(command.place().idplace());
+      ROS_INFO_STREAM("[COMMAND] Place object at " << object->id() << " using robot " << robot->id());
+      if (!robot)
+      {
+        ROS_ERROR_STREAM("[COMMAND] FAILED! Unable to place using unknown robot '" << command.place().idrobot() << "'.");
+        return;
+      }
+      if (!object)
       {
-        ROS_INFO_STREAM("box " << currentlyPickedBox.value() << " has been removed from the scene!");
-        currentlyPickedBox.reset();
+        ROS_ERROR_STREAM("[COMMAND] FAILED! Unable to place at unknown location '" << command.place().idplace() << "'.");
+        return;
       }
+      controller.place(command.place().idrobot(), command.place().idplace(), false);
+    }
+    else if (command.has_drop())
+    {
+
+      Object* robot = controller.resolveObject(command.drop().idrobot());
+      Object* bin = controller.resolveObject(command.drop().idbin());
+      ROS_INFO_STREAM("[COMMAND] drop object into bin " << bin->id() << " using robot " << robot->id());
+      if (!robot)
+      {
+        ROS_ERROR_STREAM("[COMMAND] FAILED! Unable to drop using unknown robot '" << command.drop().idrobot() << "'.");
+        return;
+      }
+      if (!bin)
+      {
+        ROS_ERROR_STREAM("[COMMAND] FAILED! Unable to drop into unknown bin '" << command.drop().idbin() << "'.");
+        return;
+      }
+      controller.drop(command.drop().idrobot(), command.drop().idbin(), false);
+    }
+    else
+    {
+      ROS_WARN_STREAM("[COMMAND] IGNORED! Ignoring command of unknown type " << command.msg_case());
     }
   };
-  controller.reactToSceneUpdateMessage(sceneUpdateCallback);
+  controller.reactToCommandMessage(pick_place_callback);
 
   ros::spin();