From 96445c52fc19aef10f5ccc249ef4735739a54ffe Mon Sep 17 00:00:00 2001 From: Khalil Meftah Date: Thu, 11 Jun 2026 19:00:50 +0200 Subject: [PATCH] feat(config): add MIT control mode ReBot - Add configurable arm control mode (mit default, pos_vel fallback) with tunable mit_kp / mit_kd - Add optional gripper control mode (force_pos default, mit optional) with gripper_mit_kp / gripper_mit_kd - Update tests for MIT arm routing, gripper mode routing, and revised joint limits --- .../bi_rebot_b601_follower.py | 12 ++++ .../config_rebot_b601_follower.py | 25 ++++++-- .../rebot_b601_follower.py | 64 +++++++++++-------- tests/robots/test_rebot_b601_follower.py | 19 +++++- 4 files changed, 87 insertions(+), 33 deletions(-) diff --git a/src/lerobot/robots/bi_rebot_b601_follower/bi_rebot_b601_follower.py b/src/lerobot/robots/bi_rebot_b601_follower/bi_rebot_b601_follower.py index bd19f1b62..aa4fd0432 100644 --- a/src/lerobot/robots/bi_rebot_b601_follower/bi_rebot_b601_follower.py +++ b/src/lerobot/robots/bi_rebot_b601_follower/bi_rebot_b601_follower.py @@ -52,7 +52,13 @@ class BiRebotB601Follower(Robot): cameras=config.left_arm_config.cameras, motor_can_ids=config.left_arm_config.motor_can_ids, pos_vel_velocity=config.left_arm_config.pos_vel_velocity, + control_mode=config.left_arm_config.control_mode, + mit_kp=config.left_arm_config.mit_kp, + mit_kd=config.left_arm_config.mit_kd, + gripper_control_mode=config.left_arm_config.gripper_control_mode, gripper_torque_ratio=config.left_arm_config.gripper_torque_ratio, + gripper_mit_kp=config.left_arm_config.gripper_mit_kp, + gripper_mit_kd=config.left_arm_config.gripper_mit_kd, joint_limits=config.left_arm_config.joint_limits, ) @@ -67,7 +73,13 @@ class BiRebotB601Follower(Robot): cameras=config.right_arm_config.cameras, motor_can_ids=config.right_arm_config.motor_can_ids, pos_vel_velocity=config.right_arm_config.pos_vel_velocity, + control_mode=config.right_arm_config.control_mode, + mit_kp=config.right_arm_config.mit_kp, + mit_kd=config.right_arm_config.mit_kd, + gripper_control_mode=config.right_arm_config.gripper_control_mode, gripper_torque_ratio=config.right_arm_config.gripper_torque_ratio, + gripper_mit_kp=config.right_arm_config.gripper_mit_kp, + gripper_mit_kd=config.right_arm_config.gripper_mit_kd, joint_limits=config.right_arm_config.joint_limits, ) diff --git a/src/lerobot/robots/rebot_b601_follower/config_rebot_b601_follower.py b/src/lerobot/robots/rebot_b601_follower/config_rebot_b601_follower.py index 2329200a4..cef14ea43 100644 --- a/src/lerobot/robots/rebot_b601_follower/config_rebot_b601_follower.py +++ b/src/lerobot/robots/rebot_b601_follower/config_rebot_b601_follower.py @@ -65,12 +65,27 @@ class RebotB601FollowerConfig: } ) - # Target velocity for joints running in POS_VEL mode, in degrees/s. A scalar is - # applied to every joint; a list provides one value per joint (in motor order). - pos_vel_velocity: float | list[float] = field(default_factory=lambda: [150.0] * 7) + # Max speed (deg/s) per joint for POS_VEL arms and FORCE_POS gripper (motor order). + pos_vel_velocity: float | list[float] = field( + default_factory=lambda: [150.0, 150.0, 150.0, 150.0, 150.0, 150.0, 500.0] + ) - # Torque/current ratio for the gripper's FORCE_POS mode, in range [0, 1]. - gripper_torque_ratio: float = 0.1 + # Arm control: "mit" or "pos_vel". + control_mode: str = "mit" + + # MIT kp/kd per arm joint (motor order). Unused when control_mode="pos_vel". + mit_kp: float | list[float] = field(default_factory=lambda: [45.0, 45.0, 45.0, 8.0, 9.0, 8.0, 8.0]) + mit_kd: float | list[float] = field(default_factory=lambda: [12.0, 12.0, 12.0, 1.0, 1.0, 1.0, 1.0]) + + # Gripper control: "force_pos" or "mit". + gripper_control_mode: str = "force_pos" + + # FORCE_POS only: max grip force, in [0, 1]. + gripper_torque_ratio: float = 0.05 + + # MIT only. + gripper_mit_kp: float = 8.0 + gripper_mit_kd: float = 0.3 # Soft joint limits (degrees). These are clipped against on every action. joint_limits: dict[str, tuple[float, float]] = field( diff --git a/src/lerobot/robots/rebot_b601_follower/rebot_b601_follower.py b/src/lerobot/robots/rebot_b601_follower/rebot_b601_follower.py index ec00f4aa9..20f762ae5 100644 --- a/src/lerobot/robots/rebot_b601_follower/rebot_b601_follower.py +++ b/src/lerobot/robots/rebot_b601_follower/rebot_b601_follower.py @@ -169,11 +169,25 @@ class RebotB601Follower(Robot): print(f"Calibration saved to {self.calibration_fpath}") def configure(self) -> None: + if self.config.control_mode not in ("pos_vel", "mit"): + raise ValueError( + f"Unsupported control_mode '{self.config.control_mode}'. Use 'pos_vel' or 'mit'." + ) + if self.config.gripper_control_mode not in ("force_pos", "mit"): + raise ValueError( + f"Unsupported gripper_control_mode '{self.config.gripper_control_mode}'. " + "Use 'force_pos' or 'mit'." + ) + use_mit = self.config.control_mode == "mit" + gripper_use_mit = self.config.gripper_control_mode == "mit" self.bus.enable_all() for motor_name, motor in self.motors.items(): - target_mode = ( - MotorBridgeMode.FORCE_POS if motor_name == GRIPPER_MOTOR else MotorBridgeMode.POS_VEL - ) + if motor_name == GRIPPER_MOTOR: + target_mode = MotorBridgeMode.MIT if gripper_use_mit else MotorBridgeMode.FORCE_POS + elif use_mit: + target_mode = MotorBridgeMode.MIT + else: + target_mode = MotorBridgeMode.POS_VEL for attempt in range(_ENSURE_MODE_RETRIES + 1): try: motor.ensure_mode(target_mode) @@ -230,44 +244,40 @@ class RebotB601Follower(Robot): """ goal_pos = {key.removesuffix(".pos"): val for key, val in action.items() if key.endswith(".pos")} - # Clip against soft joint limits. - for motor_name in list(goal_pos): - if motor_name in self.config.joint_limits: - min_limit, max_limit = self.config.joint_limits[motor_name] - clipped = max(min_limit, min(max_limit, goal_pos[motor_name])) - if clipped != goal_pos[motor_name]: - logger.debug(f"Clipped {motor_name} from {goal_pos[motor_name]:.2f} to {clipped:.2f}") - goal_pos[motor_name] = clipped - - # Tolerate 6-DOF leaders that have no wrist_yaw joint by holding it at zero. - # This is intentional: it lets a 6-DOF leader such as the SO-100 / SO-101 - # (so100_leader / so101_leader) teleoperate this 7-DOF follower — the missing - # wrist_yaw command is simply treated as 0.0 instead of raising. - if "wrist_yaw" not in goal_pos: - goal_pos["wrist_yaw"] = 0.0 - # Cap relative target when too far from the present position. if self.config.max_relative_target is not None: present_pos = self._present_pos() goal_present_pos = {key: (g, present_pos.get(key, g)) for key, g in goal_pos.items()} goal_pos = ensure_safe_goal_position(goal_present_pos, self.config.max_relative_target) + use_mit = self.config.control_mode == "mit" for motor_name, position_deg in goal_pos.items(): motor = self.motors.get(motor_name) if motor is None: continue idx = self.motor_names.index(motor_name) - vel_deg_s = ( - self.config.pos_vel_velocity[idx] - if isinstance(self.config.pos_vel_velocity, list) - else self.config.pos_vel_velocity - ) pos_rad = math.radians(position_deg) - vel_rad = math.radians(vel_deg_s) if motor_name == GRIPPER_MOTOR: - motor.send_force_pos(pos_rad, vel_rad, self.config.gripper_torque_ratio) + if self.config.gripper_control_mode == "mit": + motor.send_mit(pos_rad, 0.0, self.config.gripper_mit_kp, self.config.gripper_mit_kd, 0.0) + else: + vel_deg_s = ( + self.config.pos_vel_velocity[idx] + if isinstance(self.config.pos_vel_velocity, list) + else self.config.pos_vel_velocity + ) + motor.send_force_pos(pos_rad, math.radians(vel_deg_s), self.config.gripper_torque_ratio) + elif use_mit: + kp = self.config.mit_kp[idx] if isinstance(self.config.mit_kp, list) else self.config.mit_kp + kd = self.config.mit_kd[idx] if isinstance(self.config.mit_kd, list) else self.config.mit_kd + motor.send_mit(pos_rad, 0.0, kp, kd, 0.0) else: - motor.send_pos_vel(pos_rad, vel_rad) + vel_deg_s = ( + self.config.pos_vel_velocity[idx] + if isinstance(self.config.pos_vel_velocity, list) + else self.config.pos_vel_velocity + ) + motor.send_pos_vel(pos_rad, math.radians(vel_deg_s)) return {f"{motor}.pos": val for motor, val in goal_pos.items()} diff --git a/tests/robots/test_rebot_b601_follower.py b/tests/robots/test_rebot_b601_follower.py index e37159854..0861827be 100644 --- a/tests/robots/test_rebot_b601_follower.py +++ b/tests/robots/test_rebot_b601_follower.py @@ -94,7 +94,8 @@ def test_send_action_clips_to_joint_limits(follower): # shoulder_pan limit is (-150, 150); request beyond the upper bound. returned = follower.send_action({"shoulder_pan.pos": 999.0}) assert returned["shoulder_pan.pos"] == 150.0 - follower.motors["shoulder_pan"].send_pos_vel.assert_called_once() + # Default control_mode is "mit", so arm joints are driven via send_mit. + follower.motors["shoulder_pan"].send_mit.assert_called_once() def test_send_action_routes_gripper_to_force_pos(follower): @@ -103,6 +104,22 @@ def test_send_action_routes_gripper_to_force_pos(follower): follower.motors["gripper"].send_pos_vel.assert_not_called() +def test_gripper_mit_mode_routes_to_send_mit(): + bus_mock = _make_bus_mock() + with ( + patch(f"{_MODULE}.require_package", lambda *a, **kw: None), + patch(f"{_MODULE}.MotorBridgeController") as controller_cls, + patch(f"{_MODULE}.MotorBridgeMode", MagicMock()), + ): + controller_cls.from_dm_serial.return_value = bus_mock + cfg = RebotB601FollowerRobotConfig(port="/dev/null", gripper_control_mode="mit") + robot = RebotB601Follower(cfg) + robot.connect(calibrate=False) + robot.send_action({"gripper.pos": -10.0}) + robot.motors["gripper"].send_mit.assert_called_once() + robot.motors["gripper"].send_force_pos.assert_not_called() + + def test_bimanual_prefixes_features(): with patch(f"{_MODULE}.require_package", lambda *a, **kw: None): cfg = BiRebotB601FollowerConfig(