Add set robot pose (#38)

* merge renderer.py: add texture to objects, ignore base

* merge Change kuka_kmr default position

* Add set_robot_pose function

* Fix mistake with old version initial_config

* Reformat black

* Add reinitialization of self.collision after robot pose  is changed

guided_tamp_benchmark/models/robots/kuka_kmr_iiwa_robot.py

@@ -28,7 +28,7 @@ def initial_configuration(self) -> List[float]:
         """Return the initial configuration of the robot."""
         return [
             0.2,
-            1.5,
+            2.2,
             -np.pi,
             0,
             -np.pi / 4,

guided_tamp_benchmark/tasks/base_task.py

@@ -60,6 +60,15 @@ def get_robot_pose(self) -> np.array:
         """Returns the robot base pose as a 4x4 numpy array"""
         return self.demo.robot_pose
 
+    def set_robot_pose(self, pose: np.array):
+        """Sets the robot base pose (4x4 numpy array)"""
+        self.demo.robot_pose = pose
+        if self.robot.robot_type == "fixed":
+            "Move (recreate) box under the robot"
+            box_pose, box_size = create_box(self)
+            self.furniture[-1] = Box(pose=box_pose, box_size=box_size)
+        self.collision = Collision(self)
+
     def get_furniture(self) -> list[FurnitureObject]:
         """Returns the list of furniture instances"""
         return self.furniture