diff --git a/examples/inference/franka.py b/examples/inference/franka.py index beccd8b3..049b48ba 100644 --- a/examples/inference/franka.py +++ b/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__": diff --git a/extensions/rcs_fr3/src/rcs_fr3/configs.py b/extensions/rcs_fr3/src/rcs_fr3/configs.py index baf47eaf..f55ff42a 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/configs.py +++ b/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" diff --git a/extensions/rcs_panda/src/rcs_panda/configs.py b/extensions/rcs_panda/src/rcs_panda/configs.py index a5d1eebe..368b5d98 100644 --- a/extensions/rcs_panda/src/rcs_panda/configs.py +++ b/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" diff --git a/python/rcs/__init__.py b/python/rcs/__init__.py index 106ed229..428ee4c3 100644 --- a/python/rcs/__init__.py +++ b/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 diff --git a/python/rcs/envs/configs.py b/python/rcs/envs/configs.py index 6601d35e..2139c7d0 100644 --- a/python/rcs/envs/configs.py +++ b/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") diff --git a/python/rcs/operator/interface.py b/python/rcs/operator/interface.py index ee58f920..b8d454bc 100644 --- a/python/rcs/operator/interface.py +++ b/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()