pos teleop

src/lerobot/motors/damiao/damiao.py

@@ -16,6 +16,7 @@
 
 import logging
 import time
+from contextlib import contextmanager
 from copy import deepcopy
 from functools import cached_property
 from typing import Dict, List, Optional, Tuple, Union
@@ -74,6 +75,9 @@ class DamiaoMotorsBus(MotorsBusBase):
         motors: dict[str, Motor],
         calibration: dict[str, MotorCalibration] | None = None,
         can_interface: str = "auto",
+        use_can_fd: bool = True,
+        bitrate: int = 1000000,
+        data_bitrate: int | None = 5000000,
     ):
         """
         Initialize the Damiao motors bus.
@@ -83,10 +87,16 @@ class DamiaoMotorsBus(MotorsBusBase):
             motors: Dictionary mapping motor names to Motor objects
             calibration: Optional calibration data
             can_interface: CAN interface type - "auto" (default), "socketcan" (Linux), or "slcan" (macOS/serial)
+            use_can_fd: Whether to use CAN FD mode (default: True for OpenArms)
+            bitrate: Nominal bitrate in bps (default: 1000000 = 1 Mbps)
+            data_bitrate: Data bitrate for CAN FD in bps (default: 5000000 = 5 Mbps), ignored if use_can_fd is False
         """
         super().__init__(port, motors, calibration)
         self.port = port
         self.can_interface = can_interface
+        self.use_can_fd = use_can_fd
+        self.bitrate = bitrate
+        self.data_bitrate = data_bitrate
         self.canbus = None
         self._is_connected = False
         
@@ -138,26 +148,48 @@ class DamiaoMotorsBus(MotorsBusBase):
             
             # Connect to CAN bus
             if self.can_interface == "socketcan":
-                # Linux SocketCAN
-                self.canbus = can.interface.Bus(
-                    channel=self.port,
-                    interface="socketcan",
-                    bitrate=self.default_baudrate
-                )
+                # Linux SocketCAN with CAN FD support
+                if self.use_can_fd and self.data_bitrate is not None:
+                    self.canbus = can.interface.Bus(
+                        channel=self.port,
+                        interface="socketcan",
+                        bitrate=self.bitrate,
+                        data_bitrate=self.data_bitrate,
+                        fd=True
+                    )
+                    logger.info(f"Connected to {self.port} with CAN FD (bitrate={self.bitrate}, data_bitrate={self.data_bitrate})")
+                else:
+                    self.canbus = can.interface.Bus(
+                        channel=self.port,
+                        interface="socketcan",
+                        bitrate=self.bitrate
+                    )
+                    logger.info(f"Connected to {self.port} with CAN 2.0 (bitrate={self.bitrate})")
             elif self.can_interface == "slcan":
                 # Serial Line CAN (macOS, Windows, or USB adapters)
+                # Note: SLCAN typically doesn't support CAN FD
                 self.canbus = can.interface.Bus(
                     channel=self.port,
                     interface="slcan",
-                    bitrate=self.default_baudrate
+                    bitrate=self.bitrate
                 )
+                logger.info(f"Connected to {self.port} with SLCAN (bitrate={self.bitrate})")
             else:
                 # Generic interface (vector, pcan, etc.)
-                self.canbus = can.interface.Bus(
-                    channel=self.port,
-                    interface=self.can_interface,
-                    bitrate=self.default_baudrate
-                )
+                if self.use_can_fd and self.data_bitrate is not None:
+                    self.canbus = can.interface.Bus(
+                        channel=self.port,
+                        interface=self.can_interface,
+                        bitrate=self.bitrate,
+                        data_bitrate=self.data_bitrate,
+                        fd=True
+                    )
+                else:
+                    self.canbus = can.interface.Bus(
+                        channel=self.port,
+                        interface=self.can_interface,
+                        bitrate=self.bitrate
+                    )
             
             self._is_connected = True
             
@@ -251,6 +283,24 @@ class DamiaoMotorsBus(MotorsBusBase):
                         raise e
                     time.sleep(0.01)
 
+    @contextmanager
+    def torque_disabled(self, motors: str | list[str] | None = None):
+        """
+        Context manager that guarantees torque is re-enabled.
+        
+        This helper is useful to temporarily disable torque when configuring motors.
+        
+        Examples:
+            >>> with bus.torque_disabled():
+            ...     # Safe operations here with torque disabled
+            ...     pass
+        """
+        self.disable_torque(motors)
+        try:
+            yield
+        finally:
+            self.enable_torque(motors)
+
     def set_zero_position(self, motors: str | list[str] | None = None) -> None:
         """Set current position as zero for selected motors."""
         motors = self._get_motors_list(motors)

src/lerobot/robots/openarms/config_openarms_follower.py

@@ -15,7 +15,7 @@
 # limitations under the License.
 
 from dataclasses import dataclass, field
-from typing import Dict, List, Optional
+from typing import Dict, Optional
 
 from lerobot.cameras import CameraConfig
 from lerobot.motors.damiao.tables import MotorType
@@ -28,13 +28,21 @@ from ..config import RobotConfig
 class OpenArmsFollowerConfig(RobotConfig):
     """Configuration for the OpenArms follower robot with Damiao motors."""
     
-    # CAN interface to connect to
+    # CAN interfaces - one per arm
+    # Right arm CAN interface (e.g., "can0")
+    # Left arm CAN interface (e.g., "can1")
     # Linux: "can0", "can1", etc.
     # macOS: "/dev/cu.usbmodem*" (serial device)
-    port: str = "can0"
+    port_right: str = "can0"    # CAN interface for right arm
+    port_left: str = "can1"     # CAN interface for left arm
     
     # CAN interface type: "socketcan" (Linux), "slcan" (macOS/serial), or "auto" (auto-detect)
-    can_interface: str = "auto"
+    can_interface: str = "socketcan"
+    
+    # CAN FD settings (OpenArms uses CAN FD by default)
+    use_can_fd: bool = True
+    can_bitrate: int = 1000000      # Nominal bitrate (1 Mbps)
+    can_data_bitrate: int = 5000000  # Data bitrate for CAN FD (5 Mbps)
     
     # Whether to disable torque when disconnecting
     disable_torque_on_disconnect: bool = True
@@ -64,9 +72,10 @@ class OpenArmsFollowerConfig(RobotConfig):
         "gripper": (0x08, 0x18, "dm4310"),   # J8 - Gripper (DM4310)
     })
     
-    # MIT control parameters for position control
-    position_kp: float = 10.0  # Position gain
-    position_kd: float = 0.5   # Velocity damping
+    # MIT control parameters for position control (per motor)
+    # Values: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6, joint_7, gripper]
+    position_kp: list[float] = field(default_factory=lambda: [240.0, 240.0, 240.0, 240.0, 24.0, 31.0, 25.0, 16.0])
+    position_kd: list[float] = field(default_factory=lambda: [3.0, 3.0, 3.0, 3.0, 0.2, 0.2, 0.2, 0.2])
     
     # Calibration parameters
     calibration_mode: str = "manual"  # "manual" or "auto"

src/lerobot/robots/openarms/openarms_follower.py

@@ -48,31 +48,44 @@ class OpenArmsFollower(Robot):
         super().__init__(config)
         self.config = config
         
-        norm_mode_body = MotorNormMode.DEGREES # Always use degrees for Damiao motors
-        motors = {}
+        norm_mode_body = MotorNormMode.DEGREES  # Always use degrees for Damiao motors
         
-        # Right arm
+        # Right arm motors (on port_right)
+        # Each arm uses the same CAN IDs since they're on separate buses
+        motors_right = {}
         for motor_name, (send_id, recv_id, motor_type_str) in config.motor_config.items():
-            prefixed_name = f"right_{motor_name}"
             motor = Motor(send_id, motor_type_str, norm_mode_body)
             motor.recv_id = recv_id
             motor.motor_type = getattr(MotorType, motor_type_str.upper().replace("-", "_"))
-            motors[prefixed_name] = motor
+            motors_right[motor_name] = motor
         
-        # Left arm (offset IDs by 8)
+        # Left arm motors (on port_left, same IDs as right since separate bus)
+        motors_left = {}
         for motor_name, (send_id, recv_id, motor_type_str) in config.motor_config.items():
-            prefixed_name = f"left_{motor_name}"
-            motor = Motor(send_id + 0x08, motor_type_str, norm_mode_body)
-            motor.recv_id = recv_id + 0x08
+            motor = Motor(send_id, motor_type_str, norm_mode_body)
+            motor.recv_id = recv_id
             motor.motor_type = getattr(MotorType, motor_type_str.upper().replace("-", "_"))
-            motors[prefixed_name] = motor
+            motors_left[motor_name] = motor
         
-        # Initialize the Damiao motors bus
-        self.bus = DamiaoMotorsBus(
-            port=self.config.port,
-            motors=motors,
-            calibration=self.calibration,
+        # Initialize separate Damiao motors buses (one per arm) with CAN FD support
+        self.bus_right = DamiaoMotorsBus(
+            port=self.config.port_right,
+            motors=motors_right,
+            calibration={k.replace("right_", ""): v for k, v in (self.calibration or {}).items() if k.startswith("right_")},
             can_interface=self.config.can_interface,
+            use_can_fd=self.config.use_can_fd,
+            bitrate=self.config.can_bitrate,
+            data_bitrate=self.config.can_data_bitrate if self.config.use_can_fd else None,
+        )
+        
+        self.bus_left = DamiaoMotorsBus(
+            port=self.config.port_left,
+            motors=motors_left,
+            calibration={k.replace("left_", ""): v for k, v in (self.calibration or {}).items() if k.startswith("left_")},
+            can_interface=self.config.can_interface,
+            use_can_fd=self.config.use_can_fd,
+            bitrate=self.config.can_bitrate,
+            data_bitrate=self.config.can_data_bitrate if self.config.use_can_fd else None,
         )
         
         # Initialize cameras
@@ -92,10 +105,16 @@ class OpenArmsFollower(Robot):
     def _motors_ft(self) -> Dict[str, type]:
         """Motor features for observation and action spaces."""
         features = {}
-        for motor in self.bus.motors:
-            features[f"{motor}.pos"] = float
-            features[f"{motor}.vel"] = float
-            features[f"{motor}.torque"] = float
+        # Right arm motors
+        for motor in self.bus_right.motors:
+            features[f"right_{motor}.pos"] = float
+            features[f"right_{motor}.vel"] = float
+            features[f"right_{motor}.torque"] = float
+        # Left arm motors
+        for motor in self.bus_left.motors:
+            features[f"left_{motor}.pos"] = float
+            features[f"left_{motor}.vel"] = float
+            features[f"left_{motor}.torque"] = float
         return features
 
     @property
@@ -119,20 +138,25 @@ class OpenArmsFollower(Robot):
     @property
     def is_connected(self) -> bool:
         """Check if robot is connected."""
-        return self.bus.is_connected and all(cam.is_connected for cam in self.cameras.values())
+        return (self.bus_right.is_connected and 
+                self.bus_left.is_connected and 
+                all(cam.is_connected for cam in self.cameras.values()))
 
     def connect(self, calibrate: bool = True) -> None:
         """
         Connect to the robot and optionally calibrate.
         
-        We assume that at connection time, the arm is in a safe rest position,
+        We assume that at connection time, the arms are in a safe rest position,
         and torque can be safely disabled to run calibration if needed.
         """
         if self.is_connected:
             raise DeviceAlreadyConnectedError(f"{self} already connected")
 
-        # Connect to CAN bus
-        self.bus.connect()
+        # Connect to both CAN buses
+        logger.info(f"Connecting right arm on {self.config.port_right}...")
+        self.bus_right.connect()
+        logger.info(f"Connecting left arm on {self.config.port_left}...")
+        self.bus_left.connect()
         
         # Run calibration if needed
         if not self.is_calibrated and calibrate:
@@ -151,14 +175,15 @@ class OpenArmsFollower(Robot):
         # Optionally set zero position
         if self.config.zero_position_on_connect:
             logger.info("Setting current position as zero...")
-            self.bus.set_zero_position()
+            self.bus_right.set_zero_position()
+            self.bus_left.set_zero_position()
         
         logger.info(f"{self} connected.")
 
     @property
     def is_calibrated(self) -> bool:
         """Check if robot is calibrated."""
-        return self.bus.is_calibrated
+        return self.bus_right.is_calibrated and self.bus_left.is_calibrated
 
     def calibrate(self) -> None:
         """
@@ -166,7 +191,7 @@ class OpenArmsFollower(Robot):
         
         The calibration procedure:
         1. Disable torque
-        2. Ask user to position arm in hanging position with gripper closed
+        2. Ask user to position arms in hanging position with grippers closed
         3. Set this as zero position
         4. Record range of motion for each joint
         5. Save calibration
@@ -179,18 +204,32 @@ class OpenArmsFollower(Robot):
             )
             if user_input.strip().lower() != "c":
                 logger.info(f"Using existing calibration for {self.id}")
-                self.bus.write_calibration(self.calibration)
+                # Split calibration for each bus
+                cal_right = {k.replace("right_", ""): v for k, v in self.calibration.items() if k.startswith("right_")}
+                cal_left = {k.replace("left_", ""): v for k, v in self.calibration.items() if k.startswith("left_")}
+                self.bus_right.write_calibration(cal_right)
+                self.bus_left.write_calibration(cal_left)
                 return
         
         logger.info(f"\nRunning calibration for {self}")
         
+        # Calibrate each arm separately
+        self._calibrate_arm("right", self.bus_right)
+        self._calibrate_arm("left", self.bus_left)
+        
+        print(f"\nCalibration complete and saved to {self.calibration_fpath}")
+    
+    def _calibrate_arm(self, arm_name: str, bus: DamiaoMotorsBus) -> None:
+        """Calibrate a single arm."""
+        logger.info(f"\n=== Calibrating {arm_name.upper()} arm ===")
+        
         # Disable torque for manual positioning
-        self.bus.disable_torque()
+        bus.disable_torque()
         time.sleep(0.1)
         
         # Step 1: Set zero position
         input(
-            "\nCalibration Step 1: Zero Position\n"
+            f"\nCalibration: Zero Position ({arm_name.upper()} arm)\n"
             "Position the arm in the following configuration:\n"
             "  - Arm hanging straight down\n"
             "  - Gripper closed\n"
@@ -198,60 +237,51 @@ class OpenArmsFollower(Robot):
         )
         
         # Set current position as zero for all motors
-        self.bus.set_zero_position()
-        logger.info("Zero position set.")
+        bus.set_zero_position()
+        logger.info(f"{arm_name.capitalize()} arm zero position set.")
         
-        # Step 2: Record range of motion
+        # Automatically set range to -90° to +90° for all joints
         print(
-            "\nCalibration Step 2: Range of Motion\n"
-            "Move each joint through its full range of motion.\n"
-            "The system will record min/max positions.\n"
-            "Press ENTER when done..."
+            f"\nAutomatically setting range: -90° to +90° for all joints"
         )
         
-        # Record ranges
-        range_mins, range_maxes = self.bus.record_ranges_of_motion()
-        
-        # Create calibration data (ranges are already in degrees)
-        self.calibration = {}
-        for motor_name, motor in self.bus.motors.items():
-            self.calibration[motor_name] = MotorCalibration(
+        # Create calibration data with fixed ranges
+        if self.calibration is None:
+            self.calibration = {}
+            
+        for motor_name, motor in bus.motors.items():
+            # Prefix motor name with arm name for storage
+            prefixed_name = f"{arm_name}_{motor_name}"
+            
+            # Use -90 to +90 for all joints and gripper (integers required)
+            self.calibration[prefixed_name] = MotorCalibration(
                 id=motor.id,
                 drive_mode=0,  # Normal direction
                 homing_offset=0,  # Already set via set_zero_position
-                range_min=range_mins.get(motor_name, -180.0),  # Default -180 degrees
-                range_max=range_maxes.get(motor_name, 180.0),   # Default +180 degrees
+                range_min=-90,  # -90 degrees (integer)
+                range_max=90,   # +90 degrees (integer)
             )
+            logger.info(f"  {prefixed_name}: range set to [-90°, +90°]")
         
-        # Special handling for gripper range
-        if "gripper" in self.calibration:
-            gripper_cal = self.calibration["gripper"]
-            gripper_range = abs(gripper_cal.range_max - gripper_cal.range_min)
-            if gripper_range < 5.0:  # If gripper wasn't moved much (less than 5 degrees)
-                # Set default gripper range in degrees
-                gripper_cal.range_min = 0.0
-                gripper_cal.range_max = 90.0  # 90 degrees for full gripper motion
-        
-        # Write calibration to motors and save to file
-        self.bus.write_calibration(self.calibration)
-        self._save_calibration()
-        
-        print(f"\nCalibration complete and saved to {self.calibration_fpath}")
+        # Write calibration to this arm's motors
+        cal_for_bus = {k.replace(f"{arm_name}_", ""): v for k, v in self.calibration.items() if k.startswith(f"{arm_name}_")}
+        bus.write_calibration(cal_for_bus)
         
         # Re-enable torque
-        self.bus.enable_torque()
+        bus.enable_torque()
+        
+        # Save calibration after each arm
+        self._save_calibration()
 
     def configure(self) -> None:
         """Configure motors with appropriate settings."""
-        with self.bus.torque_disabled():
-            # Configure all motors
-            self.bus.configure_motors()
-            
-            # Set specific parameters for gripper if present
-            if "gripper" in self.bus.motors:
-                # Gripper uses lower gains to avoid damage
-                # These will be applied during MIT control commands
-                pass  # Parameters are set during control commands
+        # Configure right arm
+        with self.bus_right.torque_disabled():
+            self.bus_right.configure_motors()
+        
+        # Configure left arm
+        with self.bus_left.torque_disabled():
+            self.bus_left.configure_motors()
 
     def setup_motors(self) -> None:
         raise NotImplementedError("Motor ID configuration is typically done via manufacturer tools for CAN motors.")
@@ -263,16 +293,26 @@ class OpenArmsFollower(Robot):
         
         obs_dict = {}
         
-        # Read motor positions, velocities, and torques
+        # Read motor positions, velocities, and torques from right arm
         start = time.perf_counter()
-        positions = self.bus.sync_read("Present_Position")
-        velocities = self.bus.sync_read("Present_Velocity")
-        torques = self.bus.sync_read("Present_Torque")
+        positions_right = self.bus_right.sync_read("Present_Position")
+        velocities_right = self.bus_right.sync_read("Present_Velocity")
+        torques_right = self.bus_right.sync_read("Present_Torque")
         
-        for motor in self.bus.motors:
-            obs_dict[f"{motor}.pos"] = positions.get(motor, 0.0)
-            obs_dict[f"{motor}.vel"] = velocities.get(motor, 0.0)
-            obs_dict[f"{motor}.torque"] = torques.get(motor, 0.0)
+        for motor in self.bus_right.motors:
+            obs_dict[f"right_{motor}.pos"] = positions_right.get(motor, 0.0)
+            obs_dict[f"right_{motor}.vel"] = velocities_right.get(motor, 0.0)
+            obs_dict[f"right_{motor}.torque"] = torques_right.get(motor, 0.0)
+        
+        # Read motor positions, velocities, and torques from left arm
+        positions_left = self.bus_left.sync_read("Present_Position")
+        velocities_left = self.bus_left.sync_read("Present_Velocity")
+        torques_left = self.bus_left.sync_read("Present_Torque")
+        
+        for motor in self.bus_left.motors:
+            obs_dict[f"left_{motor}.pos"] = positions_left.get(motor, 0.0)
+            obs_dict[f"left_{motor}.vel"] = velocities_left.get(motor, 0.0)
+            obs_dict[f"left_{motor}.torque"] = torques_left.get(motor, 0.0)
         
         dt_ms = (time.perf_counter() - start) * 1e3
         logger.debug(f"{self} read state: {dt_ms:.1f}ms")
@@ -293,7 +333,7 @@ class OpenArmsFollower(Robot):
         The action magnitude may be clipped based on safety limits.
         
         Args:
-            action: Dictionary with motor positions
+            action: Dictionary with motor positions (e.g., "right_joint_1.pos", "left_joint_2.pos")
             
         Returns:
             The action actually sent (potentially clipped)
@@ -301,37 +341,69 @@ class OpenArmsFollower(Robot):
         if not self.is_connected:
             raise DeviceNotConnectedError(f"{self} is not connected.")
         
-        # Extract motor positions from action
-        goal_pos = {
-            key.removesuffix(".pos"): val 
-            for key, val in action.items() 
-            if key.endswith(".pos")
-        }
+        # Extract motor positions from action and split by arm
+        goal_pos_right = {}
+        goal_pos_left = {}
+        
+        for key, val in action.items():
+            if key.endswith(".pos"):
+                motor_name = key.removesuffix(".pos")
+                if motor_name.startswith("right_"):
+                    # Remove "right_" prefix for bus access
+                    goal_pos_right[motor_name.removeprefix("right_")] = val
+                elif motor_name.startswith("left_"):
+                    # Remove "left_" prefix for bus access
+                    goal_pos_left[motor_name.removeprefix("left_")] = val
         
         # Apply safety limits if configured
         if self.config.max_relative_target is not None:
-            present_pos = self.bus.sync_read("Present_Position")
-            goal_present_pos = {
-                key: (g_pos, present_pos[key]) 
-                for key, g_pos in goal_pos.items()
-            }
-            goal_pos = ensure_safe_goal_position(
-                goal_present_pos, 
-                self.config.max_relative_target
-            )
+            # Get current positions
+            present_pos_right = self.bus_right.sync_read("Present_Position")
+            present_pos_left = self.bus_left.sync_read("Present_Position")
+            
+            # Apply safety limits to right arm
+            if goal_pos_right:
+                goal_present_pos_right = {
+                    key: (g_pos, present_pos_right.get(key, 0.0)) 
+                    for key, g_pos in goal_pos_right.items()
+                }
+                goal_pos_right = ensure_safe_goal_position(
+                    goal_present_pos_right, 
+                    self.config.max_relative_target
+                )
+            
+            # Apply safety limits to left arm
+            if goal_pos_left:
+                goal_present_pos_left = {
+                    key: (g_pos, present_pos_left.get(key, 0.0)) 
+                    for key, g_pos in goal_pos_left.items()
+                }
+                goal_pos_left = ensure_safe_goal_position(
+                    goal_present_pos_left, 
+                    self.config.max_relative_target
+                )
         
-        # Prepare MIT control commands for each motor
-        for motor_name, position_degrees in goal_pos.items():
-            # Use different gains for gripper
-            if motor_name == "gripper":
-                kp = self.config.position_kp * 0.5  # Lower gain for gripper
-                kd = self.config.position_kd * 0.5
-            else:
-                kp = self.config.position_kp
-                kd = self.config.position_kd
+        # Motor name to index mapping for gains
+        motor_index = {
+            "joint_1": 0,
+            "joint_2": 1,
+            "joint_3": 2,
+            "joint_4": 3,
+            "joint_5": 4,
+            "joint_6": 5,
+            "joint_7": 6,
+            "gripper": 7,
+        }
+        
+        # Send MIT control commands to right arm
+        for motor_name, position_degrees in goal_pos_right.items():
+            # Get per-motor gains from config
+            idx = motor_index.get(motor_name, 0)
+            kp = self.config.position_kp[idx]
+            kd = self.config.position_kd[idx]
             
             # Send MIT control command (position is in degrees)
-            self.bus._mit_control(
+            self.bus_right._mit_control(
                 motor_name,
                 kp=kp,
                 kd=kd,
@@ -340,15 +412,39 @@ class OpenArmsFollower(Robot):
                 torque=0.0
             )
         
-        return {f"{motor}.pos": val for motor, val in goal_pos.items()}
+        # Send MIT control commands to left arm
+        for motor_name, position_degrees in goal_pos_left.items():
+            # Get per-motor gains from config
+            idx = motor_index.get(motor_name, 0)
+            kp = self.config.position_kp[idx]
+            kd = self.config.position_kd[idx]
+            
+            # Send MIT control command (position is in degrees)
+            self.bus_left._mit_control(
+                motor_name,
+                kp=kp,
+                kd=kd,
+                position_degrees=position_degrees,
+                velocity_deg_per_sec=0.0,
+                torque=0.0
+            )
+        
+        # Return the actions that were actually sent
+        result = {}
+        for motor, val in goal_pos_right.items():
+            result[f"right_{motor}.pos"] = val
+        for motor, val in goal_pos_left.items():
+            result[f"left_{motor}.pos"] = val
+        return result
 
     def disconnect(self):
         """Disconnect from robot."""
         if not self.is_connected:
             raise DeviceNotConnectedError(f"{self} is not connected.")
         
-        # Disconnect from CAN bus
-        self.bus.disconnect(self.config.disable_torque_on_disconnect)
+        # Disconnect from CAN buses
+        self.bus_right.disconnect(self.config.disable_torque_on_disconnect)
+        self.bus_left.disconnect(self.config.disable_torque_on_disconnect)
         
         # Disconnect cameras
         for cam in self.cameras.values():
@@ -363,10 +459,10 @@ class OpenArmsFollower(Robot):
     def _gravity_from_q(self, q_rad: Dict[str, float]) -> Dict[str, float]:
         """
         Compute g(q) [N·m] for all joints in the robot.
-        The order of joints in the URDF matches self.bus.motors.
+        The order of joints in the URDF matches the concatenated motor lists (right then left).
         
         Args:
-            q_rad: Dictionary mapping motor names to positions in radians
+            q_rad: Dictionary mapping motor names (with arm prefix) to positions in radians
             
         Returns:
             Dictionary mapping motor names to gravity torques in N·m
@@ -380,14 +476,34 @@ class OpenArmsFollower(Robot):
                 "Ensure urdf/openarms.urdf exists and is valid."
             )
         
-        # Build position vector in the order of motors
+        # Build position vector in the order of motors (right arm, then left arm)
         q = np.zeros(self.pin_robot.model.nq)
-        for i, motor_name in enumerate(self.bus.motors):
-            q[i] = q_rad[motor_name]
+        idx = 0
+        
+        # Right arm motors
+        for motor_name in self.bus_right.motors:
+            full_name = f"right_{motor_name}"
+            q[idx] = q_rad.get(full_name, 0.0)
+            idx += 1
+        
+        # Left arm motors
+        for motor_name in self.bus_left.motors:
+            full_name = f"left_{motor_name}"
+            q[idx] = q_rad.get(full_name, 0.0)
+            idx += 1
         
         # Compute generalized gravity vector
         g = pin.computeGeneralizedGravity(self.pin_robot.model, self.pin_robot.data, q)
         
         # Map back to motor names
-        return {motor_name: float(g[i]) for i, motor_name in enumerate(self.bus.motors)}
+        result = {}
+        idx = 0
+        for motor_name in self.bus_right.motors:
+            result[f"right_{motor_name}"] = float(g[idx])
+            idx += 1
+        for motor_name in self.bus_left.motors:
+            result[f"left_{motor_name}"] = float(g[idx])
+            idx += 1
+        
+        return result
     

src/lerobot/teleoperators/openarms/config_openarms_leader.py

@@ -25,13 +25,21 @@ from ..config import TeleoperatorConfig
 class OpenArmsLeaderConfig(TeleoperatorConfig):
     """Configuration for the OpenArms leader/teleoperator with Damiao motors."""
     
-    # CAN interface to connect to
+    # CAN interfaces - one per arm
+    # Right arm CAN interface (e.g., "can2")
+    # Left arm CAN interface (e.g., "can3")
     # Linux: "can0", "can1", etc.
     # macOS: "/dev/cu.usbmodem*" (serial device)
-    port: str = "can0"
+    port_right: str = "can2"    # CAN interface for right arm
+    port_left: str = "can3"     # CAN interface for left arm
     
     # CAN interface type: "socketcan" (Linux), "slcan" (macOS/serial), or "auto" (auto-detect)
-    can_interface: str = "auto"
+    can_interface: str = "socketcan"
+    
+    # CAN FD settings (OpenArms uses CAN FD by default)
+    use_can_fd: bool = True
+    can_bitrate: int = 1000000      # Nominal bitrate (1 Mbps)
+    can_data_bitrate: int = 5000000  # Data bitrate for CAN FD (5 Mbps)
     
     # Motor configuration for OpenArms (7 DOF per arm)
     # Maps motor names to (send_can_id, recv_can_id, motor_type)

src/lerobot/teleoperators/openarms/openarms_leader.py

@@ -16,7 +16,7 @@
 
 import logging
 import time
-from typing import Dict
+from typing import Any, Dict
 
 from lerobot.motors import Motor, MotorCalibration, MotorNormMode
 from lerobot.motors.damiao import DamiaoMotorsBus
@@ -44,42 +44,60 @@ class OpenArmsLeader(Teleoperator):
         super().__init__(config)
         self.config = config
         
+        norm_mode_body = MotorNormMode.DEGREES  # Always use degrees for Damiao motors
         
-        norm_mode_body = MotorNormMode.DEGREES # Always use degrees for Damiao motors
-        motors = {}
-        
-        # Right arm (original IDs)
+        # Right arm motors (on port_right)
+        # Each arm uses the same CAN IDs since they're on separate buses
+        motors_right = {}
         for motor_name, (send_id, recv_id, motor_type_str) in config.motor_config.items():
-            prefixed_name = f"right_{motor_name}"
             motor = Motor(send_id, motor_type_str, norm_mode_body)
             motor.recv_id = recv_id
             motor.motor_type = getattr(MotorType, motor_type_str.upper().replace("-", "_"))
-            motors[prefixed_name] = motor
+            motors_right[motor_name] = motor
         
-        # Left arm (offset IDs by 8)
+        # Left arm motors (on port_left, same IDs as right since separate bus)
+        motors_left = {}
         for motor_name, (send_id, recv_id, motor_type_str) in config.motor_config.items():
-            prefixed_name = f"left_{motor_name}"
-            motor = Motor(send_id + 0x08, motor_type_str, norm_mode_body)
-            motor.recv_id = recv_id + 0x08
+            motor = Motor(send_id, motor_type_str, norm_mode_body)
+            motor.recv_id = recv_id
             motor.motor_type = getattr(MotorType, motor_type_str.upper().replace("-", "_"))
-            motors[prefixed_name] = motor
-    
-        # Initialize the Damiao motors bus
-        self.bus = DamiaoMotorsBus(
-            port=self.config.port,
-            motors=motors,
-            calibration=self.calibration,
+            motors_left[motor_name] = motor
+        
+        # Initialize separate Damiao motors buses (one per arm) with CAN FD support
+        self.bus_right = DamiaoMotorsBus(
+            port=self.config.port_right,
+            motors=motors_right,
+            calibration={k.replace("right_", ""): v for k, v in (self.calibration or {}).items() if k.startswith("right_")},
             can_interface=self.config.can_interface,
+            use_can_fd=self.config.use_can_fd,
+            bitrate=self.config.can_bitrate,
+            data_bitrate=self.config.can_data_bitrate if self.config.use_can_fd else None,
+        )
+        
+        self.bus_left = DamiaoMotorsBus(
+            port=self.config.port_left,
+            motors=motors_left,
+            calibration={k.replace("left_", ""): v for k, v in (self.calibration or {}).items() if k.startswith("left_")},
+            can_interface=self.config.can_interface,
+            use_can_fd=self.config.use_can_fd,
+            bitrate=self.config.can_bitrate,
+            data_bitrate=self.config.can_data_bitrate if self.config.use_can_fd else None,
         )
 
     @property
     def action_features(self) -> Dict[str, type]:
         """Features produced by this teleoperator."""
         features = {}
-        for motor in self.bus.motors:
-            features[f"{motor}.pos"] = float
-            features[f"{motor}.vel"] = float
-            features[f"{motor}.torque"] = float
+        # Right arm motors
+        for motor in self.bus_right.motors:
+            features[f"right_{motor}.pos"] = float
+            features[f"right_{motor}.vel"] = float
+            features[f"right_{motor}.torque"] = float
+        # Left arm motors
+        for motor in self.bus_left.motors:
+            features[f"left_{motor}.pos"] = float
+            features[f"left_{motor}.vel"] = float
+            features[f"left_{motor}.torque"] = float
         return features
 
     @property
@@ -90,7 +108,7 @@ class OpenArmsLeader(Teleoperator):
     @property
     def is_connected(self) -> bool:
         """Check if teleoperator is connected."""
-        return self.bus.is_connected
+        return self.bus_right.is_connected and self.bus_left.is_connected
 
     def connect(self, calibrate: bool = True) -> None:
         """
@@ -102,8 +120,11 @@ class OpenArmsLeader(Teleoperator):
         if self.is_connected:
             raise DeviceAlreadyConnectedError(f"{self} already connected")
 
-        # Connect to CAN bus
-        self.bus.connect()
+        # Connect to CAN buses
+        logger.info(f"Connecting right arm on {self.config.port_right}...")
+        self.bus_right.connect()
+        logger.info(f"Connecting left arm on {self.config.port_left}...")
+        self.bus_left.connect()
         
         # Run calibration if needed
         if not self.is_calibrated and calibrate:
@@ -120,7 +141,7 @@ class OpenArmsLeader(Teleoperator):
     @property
     def is_calibrated(self) -> bool:
         """Check if teleoperator is calibrated."""
-        return self.bus.is_calibrated
+        return self.bus_right.is_calibrated and self.bus_left.is_calibrated
 
     def calibrate(self) -> None:
         """
@@ -141,18 +162,32 @@ class OpenArmsLeader(Teleoperator):
             )
             if user_input.strip().lower() != "c":
                 logger.info(f"Using existing calibration for {self.id}")
-                self.bus.write_calibration(self.calibration)
+                # Split calibration for each bus
+                cal_right = {k.replace("right_", ""): v for k, v in self.calibration.items() if k.startswith("right_")}
+                cal_left = {k.replace("left_", ""): v for k, v in self.calibration.items() if k.startswith("left_")}
+                self.bus_right.write_calibration(cal_right)
+                self.bus_left.write_calibration(cal_left)
                 return
         
         logger.info(f"\nRunning calibration for {self}")
         
+        # Calibrate each arm separately
+        self._calibrate_arm("right", self.bus_right)
+        self._calibrate_arm("left", self.bus_left)
+        
+        print(f"\nCalibration complete and saved to {self.calibration_fpath}")
+    
+    def _calibrate_arm(self, arm_name: str, bus: DamiaoMotorsBus) -> None:
+        """Calibrate a single arm."""
+        logger.info(f"\n=== Calibrating {arm_name.upper()} arm ===")
+        
         # Ensure torque is disabled for manual positioning
-        self.bus.disable_torque()
+        bus.disable_torque()
         time.sleep(0.1)
         
         # Step 1: Set zero position
         input(
-            "\nCalibration Step 1: Zero Position\n"
+            f"\nCalibration: Zero Position ({arm_name.upper()} arm)\n"
             "Position the arm in the following configuration:\n"
             "  - Arm hanging straight down\n"
             "  - Gripper closed\n"
@@ -160,45 +195,38 @@ class OpenArmsLeader(Teleoperator):
         )
         
         # Set current position as zero for all motors
-        self.bus.set_zero_position()
-        logger.info("Zero position set.")
+        bus.set_zero_position()
+        logger.info(f"{arm_name.capitalize()} arm zero position set.")
         
-        # Step 2: Record range of motion
+        # Automatically set range to -90° to +90° for all joints
         print(
-            "\nCalibration Step 2: Range of Motion\n"
-            "Move each joint through its full range of motion.\n"
-            "The system will record min/max positions.\n"
-            "Press ENTER when done..."
+            f"\nAutomatically setting range: -90° to +90° for all joints"
         )
         
-        # Record ranges
-        range_mins, range_maxes = self.bus.record_ranges_of_motion()
-        
-        # Create calibration data (ranges are already in degrees)
-        self.calibration = {}
-        for motor_name, motor in self.bus.motors.items():
-            self.calibration[motor_name] = MotorCalibration(
+        # Create calibration data with fixed ranges
+        if self.calibration is None:
+            self.calibration = {}
+            
+        for motor_name, motor in bus.motors.items():
+            # Prefix motor name with arm name for storage
+            prefixed_name = f"{arm_name}_{motor_name}"
+            
+            # Use -90 to +90 for all joints and gripper (integers required)
+            self.calibration[prefixed_name] = MotorCalibration(
                 id=motor.id,
                 drive_mode=0,  # Normal direction
                 homing_offset=0,  # Already set via set_zero_position
-                range_min=range_mins.get(motor_name, -180.0),  # Default -180 degrees
-                range_max=range_maxes.get(motor_name, 180.0),   # Default +180 degrees
+                range_min=-90,  # -90 degrees (integer)
+                range_max=90,   # +90 degrees (integer)
             )
+            logger.info(f"  {prefixed_name}: range set to [-90°, +90°]")
         
-        # Special handling for gripper range
-        if "gripper" in self.calibration:
-            gripper_cal = self.calibration["gripper"]
-            gripper_range = abs(gripper_cal.range_max - gripper_cal.range_min)
-            if gripper_range < 5.0:  # If gripper wasn't moved much (less than 5 degrees)
-                # Set default gripper range in degrees
-                gripper_cal.range_min = 0.0
-                gripper_cal.range_max = 90.0  # 90 degrees for full gripper motion
+        # Write calibration to this arm's motors
+        cal_for_bus = {k.replace(f"{arm_name}_", ""): v for k, v in self.calibration.items() if k.startswith(f"{arm_name}_")}
+        bus.write_calibration(cal_for_bus)
         
-        # Write calibration and save to file
-        self.bus.write_calibration(self.calibration)
+        # Save calibration after each arm
         self._save_calibration()
-        
-        print(f"\nCalibration complete and saved to {self.calibration_fpath}")
 
     def configure(self) -> None:
         """
@@ -209,44 +237,54 @@ class OpenArmsLeader(Teleoperator):
         if self.config.manual_control:
             # Disable torque for manual control
             logger.info("Disabling torque for manual control...")
-            self.bus.disable_torque()
+            self.bus_right.disable_torque()
+            self.bus_left.disable_torque()
         else:
             # Configure motors normally
-            self.bus.configure_motors()
+            self.bus_right.configure_motors()
+            self.bus_left.configure_motors()
 
     def setup_motors(self) -> None:
         raise NotImplementedError("Motor ID configuration is typically done via manufacturer tools for CAN motors.")
 
 
-    def get_observation(self) -> Dict[str, Any]:
-        """Get current observation from robot including position, velocity, and torque."""
+    def get_action(self) -> Dict[str, Any]:
+        """
+        Get current action from the leader arm.
+        
+        This is the main method for teleoperators - it reads the current state
+        of the leader arm and returns it as an action that can be sent to a follower.
+        """
         if not self.is_connected:
             raise DeviceNotConnectedError(f"{self} is not connected.")
         
-        obs_dict = {}
+        action_dict = {}
         
-        # Read motor positions, velocities, and torques
+        # Read motor positions, velocities, and torques from right arm
         start = time.perf_counter()
-        positions = self.bus.sync_read("Present_Position")
-        velocities = self.bus.sync_read("Present_Velocity")
-        torques = self.bus.sync_read("Present_Torque")
+        positions_right = self.bus_right.sync_read("Present_Position")
+        velocities_right = self.bus_right.sync_read("Present_Velocity")
+        torques_right = self.bus_right.sync_read("Present_Torque")
         
-        for motor in self.bus.motors:
-            obs_dict[f"{motor}.pos"] = positions.get(motor, 0.0)
-            obs_dict[f"{motor}.vel"] = velocities.get(motor, 0.0)
-            obs_dict[f"{motor}.torque"] = torques.get(motor, 0.0)
+        for motor in self.bus_right.motors:
+            action_dict[f"right_{motor}.pos"] = positions_right.get(motor, 0.0)
+            action_dict[f"right_{motor}.vel"] = velocities_right.get(motor, 0.0)
+            action_dict[f"right_{motor}.torque"] = torques_right.get(motor, 0.0)
+        
+        # Read motor positions, velocities, and torques from left arm
+        positions_left = self.bus_left.sync_read("Present_Position")
+        velocities_left = self.bus_left.sync_read("Present_Velocity")
+        torques_left = self.bus_left.sync_read("Present_Torque")
+        
+        for motor in self.bus_left.motors:
+            action_dict[f"left_{motor}.pos"] = positions_left.get(motor, 0.0)
+            action_dict[f"left_{motor}.vel"] = velocities_left.get(motor, 0.0)
+            action_dict[f"left_{motor}.torque"] = torques_left.get(motor, 0.0)
         
         dt_ms = (time.perf_counter() - start) * 1e3
         logger.debug(f"{self} read state: {dt_ms:.1f}ms")
         
-        # Capture images from cameras
-        for cam_key, cam in self.cameras.items():
-            start = time.perf_counter()
-            obs_dict[cam_key] = cam.async_read()
-            dt_ms = (time.perf_counter() - start) * 1e3
-            logger.debug(f"{self} read {cam_key}: {dt_ms:.1f}ms")
-        
-        return obs_dict
+        return action_dict
 
     def send_feedback(self, feedback: Dict[str, float]) -> None:
         raise NotImplementedError("Feedback is not yet implemented for OpenArms leader.")
@@ -259,12 +297,14 @@ class OpenArmsLeader(Teleoperator):
         # For manual control, ensure torque is disabled before disconnecting
         if self.config.manual_control:
             try:
-                self.bus.disable_torque()
+                self.bus_right.disable_torque()
+                self.bus_left.disable_torque()
             except Exception as e:
                 logger.warning(f"Failed to disable torque during disconnect: {e}")
         
-        # Disconnect from CAN bus
-        self.bus.disconnect(disable_torque=False)  # Already disabled above if needed
+        # Disconnect from CAN buses
+        self.bus_right.disconnect(disable_torque=False)  # Already disabled above if needed
+        self.bus_left.disconnect(disable_torque=False)
         
         logger.info(f"{self} disconnected.")