feat(franka): add droid config

examples/inference/franka.py

@@ -432,10 +432,15 @@ def main():
 
     controller = ModelInference(env_rel, cfg)
     with env_rel:
-        worker = threading.Thread(target=controller.loop, name="model-inference", daemon=True)
-        worker.start()
-        command_loop(controller)
-        worker.join()
+        if ROBOT_INSTANCE == RobotPlatform.SIMULATION:
+            # sim doesnt allow threads
+            controller.submit_command("e")  # use "r" to record the sim rollout
+            controller.loop()
+        else:
+            worker = threading.Thread(target=controller.loop, name="model-inference", daemon=True)
+            worker.start()
+            command_loop(controller)
+            worker.join()
 
 
 if __name__ == "__main__":

extensions/rcs_fr3/src/rcs_fr3/configs.py

@@ -103,7 +103,51 @@ def config(self) -> FR3HardwareEnvCreatorConfig:
         )
 
 
-class DefaultFR3MultiHardwareEnv(RCSFR3MultiConfigEnvCreator):
+class DROIDEnv(RCSFR3MultiConfigEnvCreator):
+    robot_ip = "192.168.101.1"
+    gripper_serial_number = "DAAQMPDC"
+
+    def config(self) -> FR3MultiHardwareEnvCreatorConfig:
+        try:
+            from rcs_robotiq2f85.hw import RobotiQ2F85GripperConfig
+        except ImportError as e:
+            msg = "Robotiq gripper support requires the `rcs_robotiq2f85` extension to be installed."
+            raise ImportError(msg) from e
+
+        base = DefaultFR3HardwareEnv()
+
+        cfg = base.config()
+        cfg.robot_cfg.async_control = True
+        cfg.robot_cfg.ip = self.robot_ip
+        cfg.robot_cfg.tcp_offset = rcs.GRIPPER_OFFSETS[common.GripperType("Robotiq2F85")]
+        cfg.robot_cfg.q_home = rcs.HOME_POSITIONS["FR3_DROID"]
+
+        return FR3MultiHardwareEnvCreatorConfig(
+            control_mode=ControlMode.CARTESIAN_TRPY,
+            robot_cfgs={
+                "right": cfg.robot_cfg,
+            },
+            camera_cfgs=None,
+            max_relative_movement=(0.5, np.deg2rad(90)),
+            relative_to=RelativeTo.LAST_STEP,
+            gripper_cfgs={
+                "right": RobotiQ2F85GripperConfig(
+                    serial_number=self.gripper_serial_number,
+                    speed=100,
+                    force=50,
+                    async_control=True,
+                ),
+            },
+            robot_to_shared_base_frame={
+                "right": common.Pose(
+                    translation=np.array([0, 0, 0]),
+                    rpy_vector=np.array([0, 0, 0]),
+                ),
+            },
+        )
+
+
+class DefaultFR3DualMultiHardwareEnv(RCSFR3MultiConfigEnvCreator):
     left_ip = "192.168.101.1"
     right_ip = "192.168.102.1"
 
@@ -151,7 +195,7 @@ def config(self) -> FR3MultiHardwareEnvCreatorConfig:
         )
 
 
-class FrankaDuoEnv(DefaultFR3MultiHardwareEnv):
+class FrankaDuoEnv(DefaultFR3DualMultiHardwareEnv):
     # use `udevadm info -a -n /dev/ttyUSB0 | grep serial` to find out the serial numbers
     left_gripper_serial_number = "DAAQMJHX"
     right_gripper_serial_number = "DAAQMPDC"

extensions/rcs_panda/src/rcs_panda/configs.py

@@ -44,6 +44,50 @@ def config(self) -> PandaHardwareEnvCreatorConfig:
         )
 
 
+class DROIDEnv(RCSPandaMultiConfigEnvCreator):
+    robot_ip = "192.168.4.100"
+    gripper_serial_number = "DAAQMPDC"
+
+    def config(self) -> PandaMultiHardwareEnvCreatorConfig:
+        try:
+            from rcs_robotiq2f85.hw import RobotiQ2F85GripperConfig
+        except ImportError as e:
+            msg = "Robotiq gripper support requires the `rcs_robotiq2f85` extension to be installed."
+            raise ImportError(msg) from e
+
+        base = DefaultPandaHardwareEnv()
+
+        cfg = base.config()
+        cfg.robot_cfg.async_control = True
+        cfg.robot_cfg.ip = self.robot_ip
+        cfg.robot_cfg.tcp_offset = rcs.GRIPPER_OFFSETS[common.GripperType("Robotiq2F85")]
+        cfg.robot_cfg.q_home = rcs.ROBOTS[RobotType.Panda].q_home
+
+        return PandaMultiHardwareEnvCreatorConfig(
+            control_mode=ControlMode.CARTESIAN_TQuat,
+            robot_cfgs={
+                "right": cfg.robot_cfg,
+            },
+            camera_cfgs=None,
+            max_relative_movement=0.5,
+            relative_to=RelativeTo.LAST_STEP,
+            gripper_cfgs={
+                "right": RobotiQ2F85GripperConfig(
+                    serial_number=self.gripper_serial_number,
+                    speed=100,
+                    force=50,
+                    async_control=True,
+                ),
+            },
+            robot_to_shared_base_frame={
+                "right": common.Pose(
+                    translation=np.array([0, 0, 0]),
+                    rpy_vector=np.array([0, 0, 0]),
+                ),
+            },
+        )
+
+
 class DefaultPandaMultiHardwareEnv(RCSPandaMultiConfigEnvCreator):
     left_ip = "192.168.4.100"
     right_ip = "192.168.4.101"

python/rcs/__init__.py

@@ -243,6 +243,7 @@ class RobotMetaConfig:
     # "right": {"xyzrpy": [0.61, -0.21, 0.40, -np.pi, np.deg2rad(-20), 0], "gripper": [0]},
     "FR3_DUO_LEFT": np.array([0.48797692, -0.57224476, -0.58536988, -2.57958827, 0.86400183, 2.0530809, -0.85965005]),
     "FR3_DUO_RIGHT": np.array([-0.48797676, -0.57224472, 0.58536959, -2.57958788, -0.86400148, 2.05308196, 0.85965057]),
+    "FR3_DROID": np.array([0.0, -np.pi / 4, 0.0, -3 * np.pi / 4, 0.0, np.pi / 2, 0.0]),
 }
 
 # Append RCS package prefix to all asset paths

python/rcs/envs/configs.py

@@ -31,7 +31,7 @@
 
 
 class EmptyWorldFR3(SimEnvCreator):
-    robot_prefix_template = "robot"
+    robot_prefix_template = "right"
     gripper_prefix_template = "gripper"
 
     def config(self) -> SimEnvCreatorConfig:
@@ -79,7 +79,7 @@ def config(self) -> SimEnvCreatorConfig:
             q_home=q_home,
         )
 
-        robot_cfgs: dict[str, SimRobotConfig] = {"robot": robot_cfg}
+        robot_cfgs: dict[str, SimRobotConfig] = {self.robot_prefix_template: robot_cfg}
         sim_cfg: SimConfig = SimConfig(async_control=False, realtime=True, frequency=1, max_convergence_steps=500)
 
         control_mode: ControlMode = ControlMode.CARTESIAN_TQuat
@@ -100,7 +100,7 @@ def config(self) -> SimEnvCreatorConfig:
             min_actuator_width=0.0,
             gripper_type=GripperType.FrankaHand,
         )
-        gripper_cfgs: dict[str, SimGripperConfig] = {"robot": gripper_cfg}
+        gripper_cfgs: dict[str, SimGripperConfig] = {self.robot_prefix_template: gripper_cfg}
         camera_cfgs: dict[str, SimCameraConfig] | None = {
             "bird_eye": SimCameraConfig(
                 identifier="bird_eye",
@@ -120,7 +120,7 @@ def config(self) -> SimEnvCreatorConfig:
         max_relative_movement: float | tuple[float, float] | None = None
         relative_to: RelativeTo = RelativeTo.LAST_STEP
 
-        robot_to_shared_base_frame: dict[str, rcs.common.Pose] | None = {"robot": rcs.common.Pose()}
+        robot_to_shared_base_frame: dict[str, rcs.common.Pose] | None = {self.robot_prefix_template: rcs.common.Pose()}
         wrapper_cfg: WrapperConfig = WrapperConfig(binary_gripper=True, home_on_reset=True)
         headless = False
         add_gravcomp = True
@@ -149,11 +149,13 @@ def config(self) -> SimEnvCreatorConfig:
                 * rcs.common.Pose(
                     translation=np.array([0.062, -0.009, 0.05245]), rpy_vector=np.array([0.0, np.pi, -np.pi / 2])
                 ),
-                robot_name="robot",
+                robot_name=self.robot_prefix_template,
             ),
         }
         gripper_offsets: dict[str, rcs.common.Pose] | None = {
-            "robot": rcs.common.Pose(rotation=FrankaHandTCPOffset()[:3, :3], translation=np.array([0.0, 0.0, 0.0]))
+            self.robot_prefix_template: rcs.common.Pose(
+                rotation=FrankaHandTCPOffset()[:3, :3], translation=np.array([0.0, 0.0, 0.0])
+            )
         }
         return SimEnvCreatorConfig(
             robot_cfgs=robot_cfgs,
@@ -444,7 +446,7 @@ def config(self) -> SimEnvCreatorConfig:
 
 
 class EmptyWorldSO101(EmptyWorldFR3):
-    gripper_prefix_template = "robot"
+    gripper_prefix_template = "gripper"
 
     def config(self) -> SimEnvCreatorConfig:
         rt = RobotType("SO101")

python/rcs/operator/interface.py

@@ -193,7 +193,8 @@ def environment_step_loop(self):
                         # TODO the following is a dict and can thus not easily be used like this
                         # and self.env.get_wrapper_attr("relative_to") == RelativeTo.CONFIGURED_ORIGIN
                     ), "both robot env and operator must be configured to relative_to.CONFIGURED_ORIGIN"
-                    self.env.get_wrapper_attr("envs")[robot].get_wrapper_attr("set_origin_to_current")()
+                    if robot in self.env.get_wrapper_attr("envs"):
+                        self.env.get_wrapper_attr("envs")[robot].get_wrapper_attr("set_origin_to_current")()
 
             # 2. Step the Environment
             actions = self.operator.consume_action()