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..947de9f6b 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 @@ -51,7 +51,10 @@ class BiRebotB601Follower(Robot): max_relative_target=config.left_arm_config.max_relative_target, cameras=config.left_arm_config.cameras, motor_can_ids=config.left_arm_config.motor_can_ids, + control_mode=config.left_arm_config.control_mode, pos_vel_velocity=config.left_arm_config.pos_vel_velocity, + mit_kp=config.left_arm_config.mit_kp, + mit_kd=config.left_arm_config.mit_kd, gripper_torque_ratio=config.left_arm_config.gripper_torque_ratio, joint_limits=config.left_arm_config.joint_limits, ) @@ -66,7 +69,10 @@ class BiRebotB601Follower(Robot): max_relative_target=config.right_arm_config.max_relative_target, cameras=config.right_arm_config.cameras, motor_can_ids=config.right_arm_config.motor_can_ids, + control_mode=config.right_arm_config.control_mode, pos_vel_velocity=config.right_arm_config.pos_vel_velocity, + mit_kp=config.right_arm_config.mit_kp, + mit_kd=config.right_arm_config.mit_kd, gripper_torque_ratio=config.right_arm_config.gripper_torque_ratio, 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 096548afb..7c9dddf8e 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,10 +65,24 @@ 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). + # Control mode for the arm joints (the gripper always runs in FORCE_POS): + # "pos_vel" - position control with velocity limit (firmware PID gains) + # "mit" - full impedance control with caller-supplied kp/kd + control_mode: str = "pos_vel" + + # Target velocity for joints in POS_VEL mode, or velocity feedforward for joints + # in MIT mode, in degrees/s. Scalar applies to every joint; a list gives one + # value per joint (in motor order). pos_vel_velocity: float | list[float] = field(default_factory=lambda: [150.0] * 7) + # MIT-mode position stiffness (Nm/rad). Scalar applies to every arm joint; a + # list gives one value per joint (in motor order). Ignored when control_mode + # is "pos_vel". The gripper entry is unused (gripper stays in FORCE_POS). + mit_kp: float | list[float] = 100.0 + + # MIT-mode velocity damping (Nm·s/rad). Same shape conventions as ``mit_kp``. + mit_kd: float | list[float] = 3.0 + # Torque/current ratio for the gripper's FORCE_POS mode, in range [0, 1]. gripper_torque_ratio: float = 0.1 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..e3e155eb2 100644 --- a/src/lerobot/robots/rebot_b601_follower/rebot_b601_follower.py +++ b/src/lerobot/robots/rebot_b601_follower/rebot_b601_follower.py @@ -38,7 +38,8 @@ else: logger = logging.getLogger(__name__) -# Joint controlled in FORCE_POS mode; every other joint runs in POS_VEL mode. +# Joint always controlled in FORCE_POS mode; every other joint runs in the mode +# selected by ``RebotB601FollowerConfig.control_mode`` (POS_VEL or MIT). GRIPPER_MOTOR = "gripper" # Per-joint Damiao motor models for the B601-DM (passed to motorbridge). MOTOR_MODELS = { @@ -168,12 +169,22 @@ class RebotB601Follower(Robot): self._save_calibration() print(f"Calibration saved to {self.calibration_fpath}") + def _arm_mode(self): + """MotorBridge mode used for the arm joints (gripper always uses FORCE_POS).""" + mode = self.config.control_mode + if mode == "pos_vel": + return MotorBridgeMode.POS_VEL + if mode == "mit": + return MotorBridgeMode.MIT + raise ValueError( + f"Unsupported control_mode '{mode}'. Use 'pos_vel' or 'mit'." + ) + def configure(self) -> None: self.bus.enable_all() + arm_mode = self._arm_mode() for motor_name, motor in self.motors.items(): - target_mode = ( - MotorBridgeMode.FORCE_POS if motor_name == GRIPPER_MOTOR else MotorBridgeMode.POS_VEL - ) + target_mode = MotorBridgeMode.FORCE_POS if motor_name == GRIPPER_MOTOR else arm_mode for attempt in range(_ENSURE_MODE_RETRIES + 1): try: motor.ensure_mode(target_mode) @@ -252,6 +263,7 @@ class RebotB601Follower(Robot): 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: @@ -266,6 +278,10 @@ class RebotB601Follower(Robot): 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) + 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, vel_rad, kp, kd, 0.0) else: motor.send_pos_vel(pos_rad, vel_rad)