diff --git a/setup.sh b/setup.sh index b2cd2d91..d1ee6ce1 100755 --- a/setup.sh +++ b/setup.sh @@ -191,8 +191,11 @@ add_pkg() { } if [[ $OS_NAME == "Linux" ]]; then + # Core build dependencies required for native Python extensions (pybind11, etc.) + for pkg in build-essential python3-dev curl; do + APT_PACKAGES=$(add_pkg "$pkg" "$APT_PACKAGES") + done if $IS_RASPI; then - APT_PACKAGES=$(add_pkg curl "$APT_PACKAGES") if $want_vision; then for pkg in libgl1-mesa-glx libgl1-mesa-dri mesa-utils libcap-dev python3-libcamera python3-kms++; do APT_PACKAGES=$(add_pkg "$pkg" "$APT_PACKAGES") @@ -208,6 +211,12 @@ if [[ $OS_NAME == "Linux" ]]; then done fi fi + # Node.js for web UI + if $want_web_ui; then + for pkg in nodejs npm; do + APT_PACKAGES=$(add_pkg "$pkg" "$APT_PACKAGES") + done + fi elif [[ $OS_NAME == "Darwin" ]]; then if $want_vision; then BREW_PACKAGES=$(add_pkg libomp "$BREW_PACKAGES") @@ -215,6 +224,9 @@ elif [[ $OS_NAME == "Darwin" ]]; then if $want_ui; then BREW_PACKAGES=$(add_pkg qt "$BREW_PACKAGES") fi + if $want_web_ui; then + BREW_PACKAGES=$(add_pkg node "$BREW_PACKAGES") + fi else warn "No automated system dependency installation for $OS_NAME." fi @@ -281,6 +293,12 @@ fi # --- Python dependencies ---------------------------------------------------- headline "Installing Python packages" +# Increase timeout for slow network connections (e.g. Raspberry Pi) +export UV_HTTP_TIMEOUT=300 + +# Install pip in venv so fallback mechanism works +uv pip install pip + # Try isolated build first; if packaging/metadata error occurs, fall back set +e uv pip install -e ".[${EXTRA_SPEC}]" diff --git a/src/gradient_os/arm_controller/ARCHITECTURE.md b/src/gradient_os/arm_controller/ARCHITECTURE.md new file mode 100644 index 00000000..fc0300e7 --- /dev/null +++ b/src/gradient_os/arm_controller/ARCHITECTURE.md @@ -0,0 +1,636 @@ +# Arm Controller Architecture + +This document describes the abstraction architecture for multi-robot and multi-servo compatibility in GradientOS. + +## Overview + +The arm controller is designed to support: +- **Multiple robots** (e.g., `gradient0`, `gradient0_5`, future designs) +- **Multiple servo backends** (e.g., Feetech STS3215, Dynamixel, simulation) + +This is achieved through two parallel abstraction layers: + +``` +┌─────────────────────────────────────────────────────────────────────────────┐ +│ run_controller.py │ +│ │ +│ 1. Selects ROBOT config (--robot gradient0) │ +│ 2. Selects SERVO backend (--servo-backend feetech) │ +│ 3. Initializes both before any servo operations │ +└─────────────────────────────────────────────────────────────────────────────┘ + │ │ + ▼ ▼ +┌───────────────────────────────────┐ ┌───────────────────────────────────┐ +│ ROBOT CONFIGURATION │ │ SERVO BACKEND │ +│ │ │ │ +│ "What robot am I controlling?" │ │ "How do I talk to the servos?" │ +│ │ │ │ +│ • Number of joints (6 DOF) │ │ • Protocol (packet structure) │ +│ • Servo IDs (10,20,21,30,...) │ │ • Checksum algorithm │ +│ • Joint limits (radians) │ │ • Register addresses │ +│ • Twin motor mappings │ │ • Encoder resolution (4095) │ +│ • Inverted servos │ │ • Baud rate (1000000) │ +│ • Master calibration offsets │ │ • PID defaults │ +│ • Default servo backend │ │ • Telemetry parsing │ +└───────────────────────────────────┘ └───────────────────────────────────┘ + │ │ + ▼ ▼ +┌───────────────────────────────────┐ ┌───────────────────────────────────┐ +│ robots/base.py │ │ backends/base.py │ +│ RobotConfig (ABC) │ │ ActuatorBackend (ABC) │ +└───────────────────────────────────┘ └───────────────────────────────────┘ + │ │ + ▼ ▼ +┌───────────────────────────────────┐ ┌───────────────────────────────────┐ +│ robots/gradient0/config.py │ │ backends/feetech/backend.py │ +│ Gradient0Config(RobotConfig) │ │ FeetechBackend(ActuatorBackend) │ +│ │ │ │ +│ • 6 DOF + gripper │ │ • Feetech STS protocol │ +│ • Servo IDs: 10,20,21,30,31... │ │ • 0xFF 0xFF header │ +│ • Uses Feetech servos (default) │ │ • Registers: 0x2A, 0x38, etc. │ +└───────────────────────────────────┘ └───────────────────────────────────┘ + │ │ + ▼ ▼ +┌───────────────────────────────────┐ ┌───────────────────────────────────┐ +│ robots/gradient0_5/config.py │ │ backends/dynamixel/backend.py │ +│ Gradient05Config(RobotConfig) │ │ DynamixelBackend(ActuatorBackend)│ +│ │ │ │ +│ • Different joint config │ │ • Dynamixel Protocol 2.0 │ +│ • Different servo IDs │ │ • Different registers │ +│ • May use Dynamixel servos │ │ • Different packet structure │ +└───────────────────────────────────┘ └───────────────────────────────────┘ +``` + +## Initialization Flow + +```python +# In run_controller.py main(): + +# 1. Parse command-line arguments +# --robot gradient0 +# --servo-backend feetech (or uses robot's default) + +# 2. Load robot configuration +selected_robot = get_robot_config("gradient0") +robot_config.set_active_robot(selected_robot) + +# 3. Determine servo backend (CLI override or robot's default) +servo_backend = args.servo_backend or selected_robot.default_servo_backend + +# 4. Configure servo backend (MUST happen before any servo operations) +backend_registry.set_active_backend(servo_backend) + +# 5. Populate module-level constants from the active configuration +utils._populate_servo_constants() + +# 6. Now safe to use servo operations +servo_driver.initialize_servos() +``` + +## File Structure + +``` +arm_controller/ +├── ARCHITECTURE.md # This file +│ +├── robots/ # ROBOT CONFIGURATIONS +│ ├── __init__.py # Registry: get_robot_config(), list_available_robots() +│ ├── base.py # RobotConfig ABC +│ └── gradient0/ +│ ├── __init__.py +│ └── config.py # Gradient0Config implementation +│ +├── backends/ # SERVO BACKENDS +│ ├── __init__.py # Registers available backends +│ ├── registry.py # Active backend management +│ ├── feetech/ +│ │ ├── __init__.py +│ │ ├── config.py # Constants (registers, defaults, telemetry parsing) +│ │ ├── protocol.py # Low-level packet functions (INTERNAL) +│ │ └── backend.py # FeetechBackend(ActuatorBackend) +│ └── simulation/ +│ └── backend.py # SimulationBackend (in-memory, no hardware) +│ +├── actuator_interface.py # ActuatorBackend ABC + SimulationBackend +│ +├── servo_protocol.py # LEGACY: Feetech-specific protocol +│ # TODO: Move to backends/feetech/protocol.py +│ # Keep as thin dispatcher for backward compat +│ +├── servo_driver.py # High-level servo operations +│ # Uses robot_config for joint mapping +│ # Uses backend via servo_protocol (or registry) +│ +├── robot_config.py # LEGACY: Module-level constants +│ # Now populated dynamically by set_active_robot() +│ +├── utils.py # Shared utilities and runtime state +│ # Constants populated by _populate_servo_constants() +│ +├── trajectory_execution.py # Trajectory planning and execution +├── command_api.py # UDP command handlers +└── pid_tuner.py # PID tuning utilities +``` + +## ActuatorBackend Interface + +The complete interface that all servo backends must implement: + +```python +class ActuatorBackend(ABC): + """Abstract interface for servo/actuator communication.""" + + # --- Properties --- + @property + @abstractmethod + def encoder_resolution(self) -> int: + """Max encoder value (e.g., 4095 for 12-bit).""" + pass + + @property + @abstractmethod + def encoder_center(self) -> int: + """Center encoder value (e.g., 2048).""" + pass + + # --- Connection --- + @abstractmethod + def connect(self, port: str, baud_rate: int) -> bool: + """Open serial connection.""" + pass + + @abstractmethod + def disconnect(self) -> None: + """Close serial connection.""" + pass + + # --- Discovery --- + @abstractmethod + def ping(self, actuator_id: int) -> bool: + """Check if actuator is present.""" + pass + + @abstractmethod + def scan(self, id_range: range) -> list[int]: + """Scan for present actuators.""" + pass + + # --- Position Control --- + @abstractmethod + def sync_write_positions(self, commands: list[tuple[int, int, int, int]]) -> None: + """Write positions to multiple actuators. + Each tuple: (actuator_id, position, speed, acceleration) + """ + pass + + @abstractmethod + def sync_read_positions(self, actuator_ids: list[int]) -> dict[int, int]: + """Read positions from multiple actuators.""" + pass + + # --- Configuration --- + @abstractmethod + def set_pid_gains(self, actuator_id: int, kp: int, ki: int, kd: int) -> bool: + """Set PID gains for an actuator.""" + pass + + @abstractmethod + def set_angle_limits(self, actuator_id: int, min_raw: int, max_raw: int) -> bool: + """Set angle limits for an actuator.""" + pass + + # --- Calibration --- + @abstractmethod + def calibrate_middle_position(self, actuator_id: int) -> bool: + """Set current position as center (zero).""" + pass + + @abstractmethod + def factory_reset(self, actuator_id: int) -> bool: + """Reset actuator to factory defaults.""" + pass + + # --- Telemetry --- + @abstractmethod + def read_telemetry(self, actuator_ids: list[int]) -> dict[int, dict]: + """Read telemetry (voltage, temp, current, status) from actuators.""" + pass +``` + +## RobotConfig Interface + +The complete interface that all robot configurations must implement: + +```python +class RobotConfig(ABC): + """Abstract interface for robot-specific configuration.""" + + # --- Identity --- + @property + @abstractmethod + def name(self) -> str: ... + + @property + @abstractmethod + def version(self) -> str: ... + + # --- Kinematics --- + @property + @abstractmethod + def num_logical_joints(self) -> int: + """Number of logical joints (e.g., 6 for 6-DOF arm).""" + pass + + @property + @abstractmethod + def num_physical_actuators(self) -> int: + """Number of physical actuators (may differ due to twin motors).""" + pass + + # --- Actuator Mapping --- + @property + @abstractmethod + def actuator_ids(self) -> list[int]: + """List of physical actuator IDs.""" + pass + + @property + @abstractmethod + def logical_to_physical_map(self) -> list[list[int]]: + """Maps logical joint index to list of physical actuator indices.""" + pass + + @property + @abstractmethod + def twin_motor_actuator_ids(self) -> dict[int, int]: + """Maps logical joint index to secondary motor ID (for twin-motor joints).""" + pass + + @property + @abstractmethod + def logical_joint_to_actuator_ids(self) -> dict[int, list[int]]: + """Maps 1-based logical joint number to physical actuator IDs.""" + pass + + # --- Limits --- + @property + @abstractmethod + def logical_joint_limits_rad(self) -> list[tuple[float, float]]: + """Min/max angles for each logical joint in radians.""" + pass + + @property + @abstractmethod + def actuator_limits_rad(self) -> list[tuple[float, float]]: + """Min/max angles for each physical actuator in radians.""" + pass + + # --- Servo Backend --- + @property + @abstractmethod + def default_servo_backend(self) -> str: + """Default servo backend for this robot (e.g., "feetech").""" + pass + + @property + @abstractmethod + def actuator_pid_gains(self) -> dict[int, tuple[int, int, int]]: + """Per-actuator PID gains {actuator_id: (kp, ki, kd)}.""" + pass +``` + +## Migration Path + +### Current State +- `servo_protocol.py` contains all Feetech-specific code (1264 lines) +- Higher-level modules import directly from `servo_protocol` +- Some abstraction in place via `__getattr__` for constants + +### Target State +1. Move `servo_protocol.py` content to `backends/feetech/protocol.py` +2. Implement `FeetechBackend(ActuatorBackend)` in `backends/feetech/backend.py` +3. `servo_protocol.py` becomes thin dispatcher: + ```python + from .backends import registry + + def ping(servo_id): + return registry.get_active_backend().ping(servo_id) + + def sync_read_positions(servo_ids, **kwargs): + return registry.get_active_backend().sync_read_positions(servo_ids, **kwargs) + ``` +4. Gradually update higher-level modules to use backend directly + +### Backward Compatibility +- Existing imports from `servo_protocol` continue to work +- Existing imports from `robot_config` continue to work +- No breaking changes to external interfaces (UDP commands, etc.) + +## Adding a New Robot + +1. Create `robots/my_robot/config.py`: + ```python + from ..base import RobotConfig + + class MyRobotConfig(RobotConfig): + @property + def name(self) -> str: + return "My Robot" + + @property + def default_servo_backend(self) -> str: + return "dynamixel" # This robot uses Dynamixel servos + + # ... implement all abstract properties + ``` + +2. Register in `robots/__init__.py`: + ```python + from .my_robot.config import MyRobotConfig + register_robot("my_robot", MyRobotConfig) + ``` + +3. Use: `python -m gradient_os.run_controller --robot my_robot` + +## Adding a New Servo Backend + +1. Create `backends/dynamixel/`: + ``` + backends/dynamixel/ + ├── __init__.py + ├── config.py # Protocol constants, register addresses + ├── protocol.py # Low-level packet functions + └── backend.py # DynamixelBackend(ActuatorBackend) + ``` + +2. Implement `DynamixelBackend(ActuatorBackend)` with all required methods + +3. Register in `backends/__init__.py`: + ```python + from .dynamixel import DynamixelBackend + registry.register_backend("dynamixel", "...dynamixel.config") + ``` + +4. Use: `python -m gradient_os.run_controller --robot gradient0 --servo-backend dynamixel` + +## Design Principles + +1. **No hardcoded values in high-level code**: All robot/servo-specific values come from configs +2. **Fail loudly**: If config is not set, raise clear errors (no silent fallbacks) +3. **Configuration at startup**: Robot and backend are set once in `run_controller.py` +4. **Single source of truth**: Each value defined in exactly one place +5. **Backward compatibility**: Existing imports continue to work during migration + +--- + +## Detailed Migration TODO + +### Current State Assessment + +| File | Lines | Status | Notes | +|------|-------|--------|-------| +| `servo_protocol.py` | 1264 | ⚠️ LEGACY | Feetech-specific, used directly by all modules | +| `servo_driver.py` | 1021 | ⚠️ NEEDS UPDATE | Uses `servo_protocol` directly, mixes concerns | +| `trajectory_execution.py` | 1092 | ⚠️ NEEDS UPDATE | Uses `servo_protocol` directly | +| `command_api.py` | 1579 | ⚠️ NEEDS UPDATE | Uses `servo_protocol` directly | +| `run_controller.py` | 929 | ⚠️ PARTIAL | Sets robot/backend, but uses old modules | +| `backends/feetech/protocol.py` | 727 | ✅ NEW | Clean Feetech protocol implementation | +| `backends/feetech/driver.py` | 835 | ✅ NEW | FeetechBackend class (unused) | +| `backends/registry.py` | 183 | ⚠️ PARTIAL | Config-only, no backend instances | +| `actuator_interface.py` | 617 | ⚠️ PARTIAL | ABC defined, SimulationBackend partial | +| `robot_config.py` | 251 | ✅ DONE | Dynamic loading via set_active_robot() | +| `utils.py` | 305 | ✅ DONE | Constants from registry via `_populate_servo_constants()` and `_populate_robot_constants()` | +| `robots/base.py` | 589 | ✅ DONE | RobotConfig ABC complete | +| `robots/gradient0/config.py` | 403 | ✅ DONE | Gradient0Config complete | + +### Phase 1: Backend Instance Management ✅ COMPLETE + +**Goal**: Registry manages backend INSTANCES (not just configs) + +- [x] **1.1** Update `backends/registry.py`: + - Add `_active_backend_instance: Optional[ActuatorBackend] = None` + - Add `create_backend(name, robot_config) -> ActuatorBackend` + - Add `get_active_backend() -> ActuatorBackend` + - Add `set_active_backend_instance(backend: ActuatorBackend)` + +- [x] **1.2** Update `backends/__init__.py`: + - Register backend CLASSES (not just config paths) + - `BACKEND_CLASSES = {"feetech": FeetechBackend, "simulation": SimulationBackend}` + +- [x] **1.3** Update `run_controller.py` initialization: + ```python + # After robot config is set: + backend = backend_registry.create_backend( + servo_backend, + selected_robot.get_config_dict() + ) + backend.initialize() + backend_registry.set_active_backend_instance(backend) + ``` + +### Phase 2: High-Level Module Migration + +**Goal**: Modules use backend instance instead of `servo_protocol` directly + +#### 2.1 servo_driver.py (Priority: HIGH) ✅ COMPLETE + +- [x] **2.1.1** Add backend accessor at top: + ```python + def _get_backend(): + from .backends import registry + return registry.get_active_backend() + ``` + +- [x] **2.1.2** Migrate `initialize_servos()`: + - Current: Opens serial port, pings servos directly + - Target: Call `_get_backend().initialize()` + +- [x] **2.1.3** Migrate `set_servo_positions()`: + - Current: Builds commands, calls `servo_protocol.sync_write_goal_pos_speed_accel()` + - Target: Call `_get_backend().set_joint_positions()` or `sync_write()` + +- [x] **2.1.4** Migrate `get_current_arm_state_rad()`: + - Current: Calls `servo_protocol.sync_read_positions()` + - Target: Call `_get_backend().get_joint_positions()` + +- [x] **2.1.5** Migrate calibration functions: + - `set_current_position_as_hardware_zero()` → `backend.set_current_position_as_zero()` + - `set_servo_pid_gains()` → `backend.set_pid_gains()` + - `set_servo_angle_limits_from_urdf()` → `backend.apply_joint_limits()` + +- [x] **2.1.6** Keep angle conversion helpers (robot-config dependent): + - `angle_to_raw()`, `raw_to_angle_rad()` - these use robot config mapping + - Also added `_build_logical_to_servo_id_map()` helper + +#### 2.2 trajectory_execution.py (Priority: HIGH) ✅ COMPLETE + +- [x] **2.2.1** Add backend accessor functions: + - `_get_backend()`, `_use_backend()` + - `_build_primary_feedback_ids()` - dynamically builds primary IDs from robot config + - `_build_logical_to_physical_index_map()` - replaces hardcoded joint-to-servo mapping + - `_get_twin_motor_pairs()` - gets twin motor pairs from robot config + +- [x] **2.2.2** Migrate open-loop executor: + - `sync_write_goal_pos_speed_accel()` now uses backend if available + +- [x] **2.2.3** Migrate closed-loop execution: + - `sync_read_positions()` now uses backend if available + - `sync_write_goal_pos_speed_accel()` now uses backend if available + +- [x] **2.2.4** Remove hardcoded twin motor logic: + - Replaced hardcoded `20, 21, 30, 31` with dynamic `_get_twin_motor_pairs()` + +- [x] **2.2.5** Remove hardcoded primary ID mapping: + - Replaced `{0: 10, 1: 20, 2: 30, ...}` with `_build_primary_feedback_ids()` + +- [x] **2.2.6** Update sync_profiles for diagnostics: + - Uses `backend.get_sync_profiles()` if available + +#### 2.3 command_api.py (Priority: MEDIUM) ✅ COMPLETE + +- [x] **2.3.1** Audit all `servo_protocol` and `servo_driver` calls +- [x] **2.3.2** Remove direct `servo_protocol` import - all calls now go through `servo_driver` +- [x] **2.3.3** Added `read_single_servo_position()` helper in `servo_driver.py` +- [x] **2.3.4** Updated `set_single_servo_position_rads()` to use backend +- [x] **2.3.5** Fixed `SimulationBackend` to properly handle servo ID to joint index mapping +- [x] **2.3.6** Renamed utils functions for clarity: + - `_populate_backend_constants()` → `_populate_servo_constants()` + - `_reinitialize_state()` → `_populate_robot_constants()` + +#### 2.4 run_controller.py (Priority: MEDIUM) ✅ COMPLETE + +- [x] **2.4.1** Gripper initialization now uses `servo_driver.read_single_servo_position()` +- [x] **2.4.2** Telemetry loop updated to use backend if available: + - Uses `backend.present_servo_ids` for servo list + - Uses `backend.sync_read_block()` for telemetry data + - Falls back to `servo_protocol` if backend doesn't have method +- [x] **2.4.3** Calibration mode now uses `servo_driver.read_single_servo_position()` +- [x] **2.4.4** FACTORY_RESET uses `backend.factory_reset_actuator()` and `backend.restart_actuator()` +- [x] **2.4.5** GET_ALL_POSITIONS uses `backend.sync_read_positions()` +- [x] **2.4.6** Remaining `servo_protocol` calls are fallbacks when backend lacks method + +### Phase 3: Deprecate Old Modules ✅ COMPLETE + +**Goal**: `servo_protocol.py` becomes thin wrapper, then removed + +- [x] **3.1** Made `servo_protocol.py` a dispatcher: + - Added `_get_backend()`, `_use_backend()`, `_warn_deprecated()` helpers + - Updated module header with deprecation notice + - Key functions now dispatch to backend when available: + - `ping()` → `backend.ping_actuator()` + - `read_servo_position()` → `backend.read_single_actuator_position()` + - `sync_read_positions()` → `backend.sync_read_positions()` + - `sync_write_goal_pos_speed_accel()` → `backend.sync_write()` + - `factory_reset_servo()` → `backend.factory_reset_actuator()` + - `restart_servo()` → `backend.restart_actuator()` + - `sync_read_block()` → `backend.sync_read_block()` + - All functions fall back to direct serial communication if backend unavailable + +- [x] **3.2** Added deprecation notices to all key functions via docstrings + +- [x] **3.3** All imports still work - backward compatible + +- [ ] **3.4** (Future) Remove `servo_protocol.py` when all usages are migrated to backend + +### Phase 4: Clean Up ✅ COMPLETE + +- [x] **4.1** Created `backends/simulation/` with proper structure: + - `backends/simulation/__init__.py` - exports SimulationBackend + - `backends/simulation/backend.py` - full SimulationBackend implementation + +- [x] **4.2** Cleaned up `actuator_interface.py`: + - Moved `SimulationBackend` to `backends/simulation/backend.py` + - `actuator_interface.py` now only contains `ActuatorBackend` ABC + - Re-exports `SimulationBackend` for backward compatibility + +- [x] **4.3** Updated `backends/__init__.py`: + - Imports `SimulationBackend` from new location + - All backends now follow consistent structure + +- [x] **4.4** Added deprecation notice to `sim_backend.py`: + - Old monkey-patching approach is deprecated + - Points users to new backend-based approach + +**Backward compatibility maintained:** +- `from actuator_interface import SimulationBackend` still works +- `from backends import SimulationBackend` still works +- New preferred: `from backends.simulation import SimulationBackend` + +### Phase 5: Testing & Validation + +- [ ] **5.1** Test with real hardware: + - Verify `gradient0` robot works with new backend system + - Verify all motion commands work + - Verify calibration works + +- [ ] **5.2** Test simulation mode: + - Verify `--sim` flag works + - Verify telemetry in simulation + +- [ ] **5.3** Test error cases: + - Missing servos + - Communication errors + - Invalid robot/backend combinations + +--- + +## Function Migration Reference + +### servo_protocol.py → FeetechBackend + +| Old Function | New Method | Notes | +|--------------|------------|-------| +| `ping(servo_id)` | `backend.ping_actuator(servo_id)` | | +| `send_servo_command()` | `backend.set_single_actuator_position()` | | +| `sync_write_goal_pos_speed_accel()` | `backend.sync_write()` | Pre-compute with `prepare_sync_write_commands()` | +| `sync_read_positions()` | `backend.sync_read_positions()` | | +| `read_servo_position()` | `backend.read_single_actuator_position()` | | +| `read_servo_register_word()` | (internal to backend) | Not exposed in interface | +| `write_servo_register_word()` | (internal to backend) | Not exposed in interface | +| `calibrate_servo_middle_position()` | `backend.set_current_position_as_zero()` | | +| `factory_reset_servo()` | `backend.factory_reset_actuator()` | | +| `restart_servo()` | `backend.restart_actuator()` | | +| `write_servo_angle_limits()` | `backend.apply_joint_limits()` | Works on all servos | +| `set_servo_acceleration()` | (included in position commands) | | +| `get_present_servo_ids()` | `backend.get_present_actuator_ids()` | | + +### servo_driver.py → FeetechBackend + +| Old Function | New Method | Notes | +|--------------|------------|-------| +| `initialize_servos()` | `backend.initialize()` | | +| `set_servo_positions()` | `backend.set_joint_positions()` | | +| `get_current_arm_state_rad()` | `backend.get_joint_positions()` | | +| `set_single_servo_position_rads()` | `backend.set_single_actuator_position()` | | +| `set_servo_pid_gains()` | `backend.set_pid_gains()` | | +| `set_servo_angle_limits_from_urdf()` | `backend.apply_joint_limits()` | | +| `set_current_position_as_hardware_zero()` | `backend.set_current_position_as_zero()` | | +| `reinitialize_servo()` | (custom sequence) | Factory reset + init | +| `logical_q_to_syncwrite_tuple()` | `backend.prepare_sync_write_commands()` | | +| `angle_to_raw()` | (internal to backend) | | +| `raw_to_angle_rad()` | (used by `raw_to_joint_positions()`) | | + +--- + +## Files to Create + +``` +backends/ +├── simulation/ +│ ├── __init__.py # NEW +│ └── backend.py # MOVE from sim_backend.py +``` + +## Files to Modify + +- `backends/registry.py` - Add instance management +- `backends/__init__.py` - Register backend classes +- `servo_driver.py` - Use backend instance +- `trajectory_execution.py` - Use backend instance +- `command_api.py` - Use backend instance +- `run_controller.py` - Create backend instance at startup + +## Files to Deprecate/Remove + +- `servo_protocol.py` - Convert to dispatcher, then remove +- `sim_backend.py` - Move to `backends/simulation/` diff --git a/src/gradient_os/arm_controller/__init__.py b/src/gradient_os/arm_controller/__init__.py index c67316e5..9c1a209f 100644 --- a/src/gradient_os/arm_controller/__init__.py +++ b/src/gradient_os/arm_controller/__init__.py @@ -1 +1,71 @@ -# This file makes the 'arm_controller' directory a Python package. \ No newline at end of file +# arm_controller package +# +# This package provides robot arm control functionality for GradientOS. +# +# Architecture: +# ------------- +# - actuator_interface.py: Abstract base class for actuator backends +# - backends/: Servo/actuator backend implementations (Feetech, etc.) +# - robots/: Robot-specific configurations (gradient0, gradient0_5, etc.) +# - robot_config.py: Backward-compatible config re-exports +# - utils.py: Shared utilities and global state (backward-compatible) +# - servo_driver.py: High-level servo control (backward-compatible) +# - servo_protocol.py: Low-level protocol (backward-compatible, delegates to backends) +# - command_api.py: UDP command handlers +# - trajectory_execution.py: Trajectory planning and execution +# +# For new robot integrations: +# 1. Create a new robot config in robots/ (inherit from RobotConfig) +# 2. Use the appropriate actuator backend (e.g., FeetechBackend) +# 3. Initialize with your robot's configuration +# +# Example: +# ```python +# from gradient_os.arm_controller import FeetechBackend +# from gradient_os.arm_controller.robots import Gradient0Config +# +# robot = Gradient0Config() +# backend = FeetechBackend(robot) +# backend.initialize() +# backend.set_joint_positions([0, 0, 0, 0, 0, 0], speed=500, acceleration=100) +# ``` + +# Import actuator interface +from .actuator_interface import ActuatorBackend, SimulationBackend + +# Import backends +from .backends import FeetechBackend + +# Import robot configuration classes +from .robots import RobotConfig, Gradient0Config, get_robot_config, list_available_robots + +# Import backward-compatible robot config module +from . import robot_config + +# Import backward-compatible modules +from . import utils +from . import servo_driver +from . import servo_protocol +from . import command_api +from . import trajectory_execution + +__all__ = [ + # Interface + 'ActuatorBackend', + 'SimulationBackend', + # Backends + 'FeetechBackend', + # Robot Configuration + 'RobotConfig', + 'Gradient0Config', + 'get_robot_config', + 'list_available_robots', + # Legacy configuration module + 'robot_config', + # Legacy modules (backward compatible) + 'utils', + 'servo_driver', + 'servo_protocol', + 'command_api', + 'trajectory_execution', +] diff --git a/src/gradient_os/arm_controller/actuator_interface.py b/src/gradient_os/arm_controller/actuator_interface.py new file mode 100644 index 00000000..4fb8292f --- /dev/null +++ b/src/gradient_os/arm_controller/actuator_interface.py @@ -0,0 +1,453 @@ +# actuator_interface.py +# +# Abstract base class defining the interface that any actuator backend must implement. +# This allows GradientOS to work with different servo types (Feetech, Dynamixel, etc.) +# or even different actuator systems (steppers, hydraulics) by implementing this interface. +# +# The interface is designed around the concept of "logical joints" - the kinematic joints +# of the robot - rather than physical servos. This abstraction allows the backend to +# handle the mapping from logical joints to physical actuators (e.g., twin-motor joints, +# gear ratios, etc.) internally. + +from abc import ABC, abstractmethod +from typing import Optional, TYPE_CHECKING +import math +import numpy as np + +if TYPE_CHECKING: + from .robots.base import RobotConfig + + +class ActuatorBackend(ABC): + """ + Abstract base class defining the interface for actuator control systems. + + Any actuator backend (Feetech servos, Dynamixel servos, stepper motors, etc.) + must implement this interface to be compatible with GradientOS's motion planning + and trajectory execution systems. + + The interface operates on "logical joints" which represent the kinematic joints + of the robot. The backend is responsible for mapping these to physical actuators. + + Key Concepts: + ------------- + - Logical Joint: A joint in the kinematic model (e.g., shoulder, elbow) + - Physical Actuator: The actual hardware (servo, motor) that moves the joint + - A single logical joint may be driven by multiple physical actuators (e.g., twin motors) + + Thread Safety: + -------------- + Implementations should be thread-safe, as methods may be called from multiple threads + (e.g., the main control loop and a trajectory executor thread). + """ + + # ========================================================================= + # Initialization & Configuration + # ========================================================================= + + @abstractmethod + def initialize(self) -> bool: + """ + Initialize the actuator system (open serial ports, ping devices, etc.). + + This method should: + - Establish communication with the hardware + - Detect which actuators are present + - Apply default configuration (PID gains, limits, etc.) + - Populate internal state about the system + + Returns: + bool: True if initialization was successful, False otherwise. + """ + pass + + @abstractmethod + def shutdown(self) -> None: + """ + Cleanly shut down the actuator system. + + This method should: + - Stop any ongoing motion + - Close serial ports/connections + - Release any resources + """ + pass + + @property + @abstractmethod + def num_joints(self) -> int: + """ + Returns the number of logical joints this backend controls. + + Returns: + int: Number of controllable joints in the kinematic model. + """ + pass + + @property + @abstractmethod + def is_initialized(self) -> bool: + """ + Check if the backend has been successfully initialized. + + Returns: + bool: True if initialize() has been called successfully. + """ + pass + + @property + @abstractmethod + def encoder_resolution(self) -> int: + """ + Returns the encoder resolution (maximum raw value) for this actuator type. + + For example: + - Feetech STS3215 servos: 4095 (12-bit encoder) + - Dynamixel servos: may vary by model (typically 4095 or 65535) + - Stepper motors: depends on microstepping configuration + + This value is used for converting between angles and raw encoder values. + + Returns: + int: Maximum raw encoder value (e.g., 4095 for 12-bit encoders) + """ + pass + + @property + def encoder_center(self) -> int: + """ + Returns the center encoder value (representing 0 radians). + + Default implementation returns encoder_resolution // 2 (e.g., 2048 for 4095). + Override if your actuator uses a different center value. + + Returns: + int: Center encoder value + """ + return self.encoder_resolution // 2 + + # ========================================================================= + # Position Control - Standard Interface + # ========================================================================= + + @abstractmethod + def set_joint_positions( + self, + positions_rad: list[float], + speed: float, + acceleration: float, + ) -> None: + """ + Command all joints to move to specified positions. + + This is the primary method for commanding joint motion. The backend is + responsible for translating these logical joint angles into the appropriate + commands for the physical actuators. + + Args: + positions_rad: List of target positions in radians, one per logical joint. + Length must equal `num_joints`. + speed: Movement speed. Units are backend-specific but typically: + - For servos: 0-4095 (servo register value) or deg/s + - For steppers: steps/second + acceleration: Movement acceleration. Units are backend-specific but typically: + - For servos: deg/s² or register value + - For steppers: steps/s² + + Raises: + ValueError: If len(positions_rad) != num_joints + """ + pass + + @abstractmethod + def get_joint_positions(self, verbose: bool = False) -> list[float]: + """ + Read the current position of all logical joints. + + Args: + verbose: If True, print debug information about the read operation. + + Returns: + list[float]: Current joint positions in radians, one per logical joint. + Length equals `num_joints`. + """ + pass + + # ========================================================================= + # High-Speed Batch Operations (for real-time control loops) + # ========================================================================= + + @abstractmethod + def prepare_sync_write_commands( + self, + positions_rad: list[float], + speed: int = 4095, + accel: int = 0, + ) -> list[tuple]: + """ + Pre-compute the low-level commands for a sync write operation. + + This method converts logical joint positions to the format required by the + hardware, allowing the actual write to be performed with minimal latency + in a real-time control loop. + + Args: + positions_rad: Target joint positions in radians. + speed: Speed value (backend-specific, typically 0-4095 for servos). + accel: Acceleration value (backend-specific, typically 0-254 for servos). + + Returns: + list[tuple]: Backend-specific command data ready for sync_write(). + """ + pass + + @abstractmethod + def sync_write(self, commands: list[tuple]) -> None: + """ + Execute a batch write operation with pre-computed commands. + + This is the fastest way to command multiple actuators simultaneously. + The commands should be prepared using prepare_sync_write_commands(). + + Args: + commands: Pre-computed command data from prepare_sync_write_commands(). + """ + pass + + @abstractmethod + def sync_read_positions( + self, + timeout_s: Optional[float] = None, + ) -> dict[int, int]: + """ + Read positions from all actuators in a single batch operation. + + This is the fastest way to get feedback from multiple actuators for + closed-loop control. + + Args: + timeout_s: Optional timeout for the read operation in seconds. + + Returns: + dict[int, int]: Mapping of actuator ID to raw position value. + The backend is responsible for providing a method + to convert these to joint angles if needed. + """ + pass + + @abstractmethod + def raw_to_joint_positions(self, raw_positions: dict[int, int]) -> list[float]: + """ + Convert raw actuator position readings to logical joint angles. + + Args: + raw_positions: Mapping of actuator ID to raw position value + (as returned by sync_read_positions). + + Returns: + list[float]: Joint positions in radians, one per logical joint. + """ + pass + + # ========================================================================= + # Single Actuator Control (for gripper, calibration, etc.) + # ========================================================================= + + @abstractmethod + def set_single_actuator_position( + self, + actuator_id: int, + position_rad: float, + speed: int, + accel: int, + ) -> None: + """ + Command a single actuator to a specific position. + + Used for controlling auxiliary actuators (gripper) or for calibration. + + Args: + actuator_id: The hardware ID of the actuator. + position_rad: Target position in radians. + speed: Movement speed (backend-specific units). + accel: Movement acceleration (backend-specific units). + """ + pass + + @abstractmethod + def read_single_actuator_position(self, actuator_id: int) -> Optional[int]: + """ + Read the raw position of a single actuator. + + Args: + actuator_id: The hardware ID of the actuator. + + Returns: + Optional[int]: Raw position value, or None if read failed. + """ + pass + + # ========================================================================= + # Calibration & Configuration + # ========================================================================= + + @abstractmethod + def set_current_position_as_zero(self, actuator_id: int) -> bool: + """ + Set the current physical position of an actuator as its zero point. + + This is used for calibration - the actuator will treat its current + position as the origin (typically raw value 2048 for servos). + + Args: + actuator_id: The hardware ID of the actuator to calibrate. + + Returns: + bool: True if calibration was successful, False otherwise. + """ + pass + + @abstractmethod + def set_pid_gains( + self, + actuator_id: int, + kp: int, + ki: int, + kd: int, + ) -> bool: + """ + Set the PID gains for a specific actuator's internal controller. + + Args: + actuator_id: The hardware ID of the actuator. + kp: Proportional gain. + ki: Integral gain. + kd: Derivative gain. + + Returns: + bool: True if gains were set successfully, False otherwise. + """ + pass + + @abstractmethod + def apply_joint_limits(self) -> bool: + """ + Apply the configured joint limits to all actuators. + + This writes the limit values to the actuators' EEPROM so they + enforce the limits at the hardware level. + + Returns: + bool: True if all limits were set successfully, False otherwise. + """ + pass + + # ========================================================================= + # Gripper Support (Optional - default implementations provided) + # ========================================================================= + + @property + def has_gripper(self) -> bool: + """ + Check if this backend has a gripper attached. + + Returns: + bool: True if a gripper is present and controllable. + """ + return False + + @property + def gripper_actuator_id(self) -> Optional[int]: + """ + Get the hardware ID of the gripper actuator. + + Returns: + Optional[int]: Gripper actuator ID, or None if no gripper. + """ + return None + + def set_gripper_position(self, position_rad: float, speed: int = 50, accel: int = 0) -> None: + """ + Set the gripper to a specific position. + + Args: + position_rad: Target gripper position in radians. + speed: Movement speed. + accel: Movement acceleration. + """ + if self.has_gripper and self.gripper_actuator_id is not None: + self.set_single_actuator_position( + self.gripper_actuator_id, position_rad, speed, accel + ) + + def get_gripper_position(self) -> Optional[float]: + """ + Read the current gripper position. + + Returns: + Optional[float]: Gripper position in radians, or None if not available. + """ + return None + + # ========================================================================= + # Diagnostics & Utilities + # ========================================================================= + + @abstractmethod + def get_present_actuator_ids(self) -> set[int]: + """ + Get the set of actuator IDs that were detected during initialization. + + Returns: + set[int]: Set of hardware IDs for actuators that responded to ping. + """ + pass + + @abstractmethod + def ping_actuator(self, actuator_id: int) -> bool: + """ + Check if a specific actuator is responsive. + + Args: + actuator_id: The hardware ID to ping. + + Returns: + bool: True if the actuator responded, False otherwise. + """ + pass + + def factory_reset_actuator(self, actuator_id: int) -> bool: + """ + Reset an actuator to factory defaults. + + Args: + actuator_id: The hardware ID of the actuator to reset. + + Returns: + bool: True if reset was successful, False otherwise. + """ + return False # Default: not supported + + def restart_actuator(self, actuator_id: int) -> bool: + """ + Restart/reboot an actuator. + + Args: + actuator_id: The hardware ID of the actuator to restart. + + Returns: + bool: True if restart command was sent successfully. + """ + return False # Default: not supported + + +# ============================================================================= +# SimulationBackend - Now located in backends/simulation/backend.py +# ============================================================================= +# Re-export for backward compatibility - new code should import from: +# from gradient_os.arm_controller.backends.simulation import SimulationBackend + +from .backends.simulation import SimulationBackend + +__all__ = ['ActuatorBackend', 'SimulationBackend'] + diff --git a/src/gradient_os/arm_controller/backends/__init__.py b/src/gradient_os/arm_controller/backends/__init__.py new file mode 100644 index 00000000..1eded1c2 --- /dev/null +++ b/src/gradient_os/arm_controller/backends/__init__.py @@ -0,0 +1,99 @@ +# backends/__init__.py +# +# Actuator backend implementations for GradientOS. +# Each subdirectory contains a complete implementation of the ActuatorBackend interface +# for a specific servo/actuator type. +# +# Available backends: +# - feetech: Feetech STS/SCS series serial bus servos +# - simulation: In-memory simulation (no hardware required) +# +# Usage: +# ------ +# At startup (in run_controller.py): +# from gradient_os.arm_controller.backends import registry +# +# # 1. Set active backend config +# registry.set_active_backend("feetech") +# +# # 2. Create and initialize backend instance +# robot_config_dict = selected_robot.get_config_dict() +# backend = registry.create_backend("feetech", robot_config_dict) +# backend.initialize() +# registry.set_active_backend_instance(backend) +# +# In modules that need servo I/O: +# from gradient_os.arm_controller.backends import registry +# backend = registry.get_active_backend() +# backend.sync_write(commands) + +import math + +from . import registry + +# Import backend classes +from .feetech import FeetechBackend +from .simulation import SimulationBackend + +# ============================================================================= +# Backend Factory Functions +# ============================================================================= + +def _create_feetech_backend(robot_config: dict, **kwargs) -> FeetechBackend: + """ + Factory for FeetechBackend that maps RobotConfig dict to FeetechBackend format. + + The RobotConfig.get_config_dict() uses different key names than FeetechBackend + expects, so we need to map them here. + """ + # Map from RobotConfig keys to FeetechBackend keys + feetech_config = { + 'servo_ids': robot_config.get('actuator_ids', []), + 'logical_to_physical_map': robot_config.get('logical_to_physical_map', {}), + 'inverted_servo_ids': robot_config.get('inverted_actuator_ids', set()), + 'joint_limits_rad': robot_config.get('logical_joint_limits_rad', []), + 'master_offsets_rad': robot_config.get('logical_joint_master_offsets_rad', []), + 'gripper_servo_id': robot_config.get('gripper_actuator_id'), + 'gripper_limits_rad': robot_config.get('gripper_limits_rad', [0, math.pi]), + 'pid_gains': robot_config.get('actuator_pid_gains', {}), + } + + # Extract kwargs that FeetechBackend accepts + serial_port = kwargs.get('serial_port', robot_config.get('default_serial_port')) + baud_rate = kwargs.get('baud_rate') # None uses FeetechBackend default + + return FeetechBackend( + robot_config=feetech_config, + serial_port=serial_port, + baud_rate=baud_rate if baud_rate else 1000000, # Default to 1M + ) + + +def _create_simulation_backend(robot_config: dict, **kwargs) -> SimulationBackend: + """Factory for SimulationBackend that extracts relevant config.""" + return SimulationBackend( + num_joints=robot_config.get('num_logical_joints', 6), + has_gripper=robot_config.get('gripper_actuator_id') is not None, + robot_config=robot_config, # Pass full config for servo ID mapping + ) + + +# ============================================================================= +# Register available backends +# ============================================================================= + +# Feetech STS/SCS series servos +registry.register_backend_class( + name="feetech", + factory=_create_feetech_backend, + config_module_path="gradient_os.arm_controller.backends.feetech.config", +) + +# In-memory simulation (no hardware) +registry.register_backend_class( + name="simulation", + factory=_create_simulation_backend, + config_module_path=None, # Simulation uses feetech config as fallback +) + +__all__ = ['FeetechBackend', 'SimulationBackend', 'registry'] diff --git a/src/gradient_os/arm_controller/backends/feetech/__init__.py b/src/gradient_os/arm_controller/backends/feetech/__init__.py new file mode 100644 index 00000000..0452a6fe --- /dev/null +++ b/src/gradient_os/arm_controller/backends/feetech/__init__.py @@ -0,0 +1,11 @@ +# backends/feetech/__init__.py +# +# Feetech STS/SCS series servo backend for GradientOS. +# This module provides hardware control for Feetech serial bus servos. + +from .driver import FeetechBackend +from . import protocol +from . import config + +__all__ = ['FeetechBackend', 'protocol', 'config'] + diff --git a/src/gradient_os/arm_controller/backends/feetech/config.py b/src/gradient_os/arm_controller/backends/feetech/config.py new file mode 100644 index 00000000..09664dca --- /dev/null +++ b/src/gradient_os/arm_controller/backends/feetech/config.py @@ -0,0 +1,306 @@ +# backends/feetech/config.py +# +# Feetech STS/SCS series servo-specific configuration constants. +# These values are specific to the Feetech servo protocol and hardware. +# +# This module contains: +# - Protocol constants (headers, instruction codes, register addresses) +# - Communication parameters (baud rate, timeouts) +# - Default tuning values (PID gains, acceleration scaling) +# +# Robot-specific configuration (joint limits, servo IDs, etc.) should be +# defined elsewhere and passed to the FeetechBackend during initialization. + +import math + +# ============================================================================= +# Serial Communication +# ============================================================================= + +# Default baud rate for Feetech servo communication +DEFAULT_BAUD_RATE = 1000000 + +# Default serial read timeout in seconds +SERIAL_READ_TIMEOUT = 0.05 + +# ============================================================================= +# Feetech Protocol Constants +# ============================================================================= + +# Packet header bytes (all Feetech packets start with 0xFF 0xFF) +SERVO_HEADER = 0xFF + +# Broadcast ID for sync commands (affects all servos on the bus) +SERVO_BROADCAST_ID = 0xFE + +# ----------------------------------------------------------------------------- +# Instruction Codes +# ----------------------------------------------------------------------------- + +SERVO_INSTRUCTION_PING = 0x01 # Check if servo is present +SERVO_INSTRUCTION_READ = 0x02 # Read from register(s) +SERVO_INSTRUCTION_WRITE = 0x03 # Write to register(s) +SERVO_INSTRUCTION_RESET = 0x06 # Factory reset (preserves ID) +SERVO_INSTRUCTION_RESTART = 0x08 # Reboot servo +SERVO_INSTRUCTION_CALIBRATE_MIDDLE = 0x0B # Set current position as center +SERVO_INSTRUCTION_SYNC_READ = 0x82 # Read multiple servos at once +SERVO_INSTRUCTION_SYNC_WRITE = 0x83 # Write multiple servos at once + +# ----------------------------------------------------------------------------- +# Register Addresses - EEPROM Area (values persist after power cycle) +# ----------------------------------------------------------------------------- + +SERVO_ADDR_MIN_ANGLE_LIMIT = 0x09 # Minimum angle limit (2 bytes) +SERVO_ADDR_MAX_ANGLE_LIMIT = 0x0B # Maximum angle limit (2 bytes) +SERVO_ADDR_POS_KP = 0x15 # Position PID - Proportional gain +SERVO_ADDR_POS_KD = 0x16 # Position PID - Derivative gain +SERVO_ADDR_POS_KI = 0x17 # Position PID - Integral gain +SERVO_ADDR_POSITION_CORRECTION = 0x1F # Hardware zero offset (calibration) +SERVO_ADDR_WRITE_LOCK = 0x37 # EEPROM write lock (0=unlocked, 1=locked) + +# ----------------------------------------------------------------------------- +# Register Addresses - RAM Area (values reset on power cycle) +# ----------------------------------------------------------------------------- + +SERVO_ADDR_TARGET_ACCELERATION = 0x29 # Target acceleration (1 byte) +SERVO_ADDR_TARGET_POSITION = 0x2A # Target position (2 bytes) +# Note: 0x2C-0x2D is "Goal Time", 0x2E-0x2F is "Goal Speed" +SERVO_ADDR_PRESENT_POSITION = 0x38 # Current position feedback (2 bytes) + +# ----------------------------------------------------------------------------- +# Sync Write Configuration +# ----------------------------------------------------------------------------- + +# Start address for Sync Write block (Accel, Pos, Time, Speed) +SYNC_WRITE_START_ADDRESS = SERVO_ADDR_TARGET_ACCELERATION # 0x29 + +# Length of data block per servo in Sync Write: +# Accel (1) + Pos (2) + Time (2) + Speed (2) = 7 bytes +SYNC_WRITE_DATA_LEN_PER_SERVO = 7 + +# ============================================================================= +# Motion Control Defaults +# ============================================================================= + +# Default servo speed if not specified (0-4095 scale) +DEFAULT_SERVO_SPEED = 500 + +# Default acceleration in deg/s² (converted to register value when written) +DEFAULT_SERVO_ACCELERATION_DEG_S2 = 500 + +# Scale factor for converting deg/s² to register value +# Register value = deg/s² / ACCELERATION_SCALE_FACTOR +# The servo register accepts 0-254, where 0 = max acceleration +ACCELERATION_SCALE_FACTOR = 100 + +# ============================================================================= +# Default PID Gains +# ============================================================================= + +# These are reasonable starting values for Feetech STS3215 servos. +# Actual optimal values depend on mechanical load and desired response. + +DEFAULT_KP = 50 # Proportional gain (0-254) +DEFAULT_KI = 1 # Integral gain (0-254) +DEFAULT_KD = 30 # Derivative gain (0-254) + +# ============================================================================= +# Servo Value Mapping +# ============================================================================= + +# Feetech servos use a 12-bit position value (0-4095) +# Center position is typically 2048 +# Full range is typically ±π radians (±180°) around center + +SERVO_VALUE_MIN = 0 +SERVO_VALUE_MAX = 4095 +SERVO_VALUE_CENTER = 2048 + +# Default angle range in radians (assuming ±π from center) +DEFAULT_ANGLE_RANGE_RAD = (-math.pi, math.pi) + +# ============================================================================= +# Telemetry Register Addresses +# ============================================================================= + +# Block 1: Position, Speed, Load/Duty, Voltage, Temperature (8 bytes @ 0x38) +TELEMETRY_BLOCK1_ADDRESS = 0x38 +TELEMETRY_BLOCK1_LENGTH = 8 + +# Block 2: Status, Moving flag, reserved, reserved, Current (5 bytes @ 0x41) +TELEMETRY_BLOCK2_ADDRESS = 0x41 +TELEMETRY_BLOCK2_LENGTH = 5 + +# Block 3: Unloading condition, LED alarm condition (2 bytes @ 0x13) +TELEMETRY_BLOCK3_ADDRESS = 0x13 +TELEMETRY_BLOCK3_LENGTH = 2 + +# Current sensing scale factor: raw_value * CURRENT_SCALE_FACTOR = amps +CURRENT_SCALE_FACTOR = 0.0065 + +# Load bit mask: direction bit is bit 10 (0x400), magnitude is bits 0-9 (0x3FF) +LOAD_DIRECTION_BIT = 0x400 +LOAD_MAGNITUDE_MASK = 0x3FF + +# ============================================================================= +# Error/Status Bit Masks (for response packets) +# ============================================================================= + +# Status bits in the error byte of response packets +STATUS_BIT_INPUT_VOLTAGE = 0x01 # Input voltage error +STATUS_BIT_ANGLE_LIMIT = 0x02 # Angle limit error +STATUS_BIT_OVERHEATING = 0x04 # Overheating error +STATUS_BIT_RANGE = 0x08 # Range error +STATUS_BIT_CHECKSUM = 0x10 # Checksum error +STATUS_BIT_OVERLOAD = 0x20 # Overload error +STATUS_BIT_INSTRUCTION = 0x40 # Instruction error + +# Human-readable names for status bits +STATUS_BIT_NAMES = { + STATUS_BIT_INPUT_VOLTAGE: "Input Voltage", + STATUS_BIT_ANGLE_LIMIT: "Angle Limit", + STATUS_BIT_OVERHEATING: "Overheating", + STATUS_BIT_RANGE: "Range", + STATUS_BIT_CHECKSUM: "Checksum", + STATUS_BIT_OVERLOAD: "Overload", + STATUS_BIT_INSTRUCTION: "Instruction", +} + + +def names_for_status_bits(status_byte: int) -> list[str]: + """ + Convert a status byte to a list of human-readable error names. + + Args: + status_byte: The error/status byte from a servo response packet. + + Returns: + list[str]: List of error names that are set in the status byte. + """ + names = [] + for bit, name in STATUS_BIT_NAMES.items(): + if status_byte & bit: + names.append(name) + return names + + +# ============================================================================= +# Telemetry Parsing +# ============================================================================= + +# Alarm/condition bit names (for unloading and LED alarm registers) +ALARM_BIT_NAMES = { + 0: "Overload", + 1: "Overheat", + 2: "Overvoltage", + 3: "Undervoltage", + 4: "Stall", + 5: "Position Fault", + 6: "Comm/Error", + 7: "Unknown", +} + + +def names_for_alarm_bits(alarm_byte: int) -> list[str]: + """ + Convert an alarm byte to a list of human-readable alarm names. + + Args: + alarm_byte: The alarm/condition byte from a servo. + + Returns: + list[str]: List of alarm names that are set in the byte. + """ + return [ALARM_BIT_NAMES.get(i, f"b{i}") for i in range(8) if ((alarm_byte >> i) & 1) == 1] + + +def bits_to_string(byte_val: int) -> str: + """Convert a byte to a comma-separated string of set bit names.""" + bits = [f"b{i}" for i in range(8) if ((byte_val >> i) & 1) == 1] + return ",".join(bits) + + +def parse_telemetry_block1(data: bytes) -> dict: + """ + Parse telemetry block 1 (position, speed, load, voltage, temp). + + Args: + data: 8-byte response from block 1 read + + Returns: + dict: Parsed telemetry data with voltage_v, temp_c, drive_duty_per_mille + """ + if len(data) != 8: + return {} + + result = {} + + # Load/drive duty: direction in bit 10, magnitude in bits 0-9 (per-mille, 0-1023) + load_raw = int.from_bytes(data[4:6], "little", signed=False) + load_mag_pm = load_raw & LOAD_MAGNITUDE_MASK + result["drive_duty_per_mille"] = load_mag_pm + + # Voltage: value / 10.0 = volts + result["voltage_v"] = float(data[6]) / 10.0 + + # Temperature: raw value in Celsius + result["temp_c"] = int(data[7]) + + return result + + +def parse_telemetry_block2(data: bytes) -> dict: + """ + Parse telemetry block 2 (status, moving, current). + + Args: + data: 5-byte response from block 2 read + + Returns: + dict: Parsed telemetry data with status_byte, current_a + """ + if len(data) != 5: + return {} + + result = {} + + # Status byte + status_byte = int(data[0]) + result["status_byte"] = status_byte + result["status_bits"] = bits_to_string(status_byte) + result["status_names"] = names_for_alarm_bits(status_byte) + + # Current: signed 16-bit, scale to amps + current_raw = int.from_bytes(data[3:5], "little", signed=True) + result["current_a"] = current_raw * CURRENT_SCALE_FACTOR + + return result + + +def parse_telemetry_block3(data: bytes) -> dict: + """ + Parse telemetry block 3 (unloading condition, LED alarm). + + Args: + data: 2-byte response from block 3 read + + Returns: + dict: Parsed telemetry data with unloading/LED alarm info + """ + if len(data) != 2: + return {} + + result = {} + + unload = int(data[0]) + led = int(data[1]) + + result["unloading_condition"] = unload + result["led_alarm_condition"] = led + result["unloading_bits"] = bits_to_string(unload) + result["led_alarm_bits"] = bits_to_string(led) + result["unloading_names"] = names_for_alarm_bits(unload) + result["led_alarm_names"] = names_for_alarm_bits(led) + + return result + diff --git a/src/gradient_os/arm_controller/backends/feetech/driver.py b/src/gradient_os/arm_controller/backends/feetech/driver.py new file mode 100644 index 00000000..a9a00f70 --- /dev/null +++ b/src/gradient_os/arm_controller/backends/feetech/driver.py @@ -0,0 +1,834 @@ +# backends/feetech/driver.py +# +# Feetech servo backend implementation for GradientOS. +# This class implements the ActuatorBackend interface for Feetech STS/SCS servos. +# +# The driver handles: +# - Serial port management and auto-detection +# - Mapping between logical joints and physical servos +# - Angle-to-raw-value conversion (including servo orientation) +# - PID gain configuration +# - Calibration and limit setting + +import math +import os +import time +import json +import glob +from typing import Optional, Callable +import serial +import numpy as np + +from ...actuator_interface import ActuatorBackend +from . import config +from . import protocol + + +class FeetechBackend(ActuatorBackend): + """ + Feetech STS/SCS series servo backend for GradientOS. + + This class provides hardware control for Feetech serial bus servos. + It implements the ActuatorBackend interface, allowing GradientOS to + use Feetech servos for robot control. + + Configuration is provided at initialization time, making this class + reusable for different robot configurations. + + Example usage: + ```python + # Define robot configuration + robot_config = { + 'servo_ids': [10, 20, 21, 30, 31, 40, 50, 60], + 'logical_to_physical_map': { + 0: [0], # J1 -> servo index 0 (ID 10) + 1: [1, 2], # J2 -> servo indices 1, 2 (IDs 20, 21) + 2: [3, 4], # J3 -> servo indices 3, 4 (IDs 30, 31) + 3: [5], # J4 -> servo index 5 (ID 40) + 4: [6], # J5 -> servo index 6 (ID 50) + 5: [7], # J6 -> servo index 7 (ID 60) + }, + 'inverted_servo_ids': {10, 20, 30, 40, 50, 60}, + 'joint_limits_rad': [ + [-3.14, 3.14], # J1 + [-1.57, 1.57], # J2 + # ... etc + ], + 'gripper_servo_id': 100, # Optional + } + + backend = FeetechBackend(robot_config) + backend.initialize() + ``` + """ + + def __init__( + self, + robot_config: dict, + serial_port: Optional[str] = None, + baud_rate: int = config.DEFAULT_BAUD_RATE, + ): + """ + Initialize the Feetech backend with robot configuration. + + Args: + robot_config: Dictionary containing robot-specific configuration: + - servo_ids: List of physical servo IDs in order + - logical_to_physical_map: Dict mapping logical joint index to list of physical servo indices + - inverted_servo_ids: Set of servo IDs that use inverted mapping + - joint_limits_rad: List of [min, max] limits for each logical joint + - master_offsets_rad: Optional list of calibration offsets per logical joint + - gripper_servo_id: Optional ID for gripper servo + - gripper_limits_rad: Optional [min, max] for gripper + - pid_gains: Optional dict mapping servo_id to (kp, ki, kd) tuple + serial_port: Serial port path. If None, will attempt auto-detection. + baud_rate: Serial baud rate. + """ + # Store configuration + self._servo_ids: list[int] = robot_config['servo_ids'] + self._logical_to_physical_map: dict[int, list[int]] = robot_config['logical_to_physical_map'] + self._inverted_servo_ids: set[int] = set(robot_config.get('inverted_servo_ids', [])) + self._joint_limits_rad: list[list[float]] = robot_config['joint_limits_rad'] + self._master_offsets_rad: list[float] = robot_config.get( + 'master_offsets_rad', + [0.0] * len(robot_config['joint_limits_rad']) + ) + self._gripper_servo_id: Optional[int] = robot_config.get('gripper_servo_id') + self._gripper_limits_rad: list[float] = robot_config.get('gripper_limits_rad', [0, math.pi]) + self._default_pid_gains: dict[int, tuple[int, int, int]] = robot_config.get('pid_gains', {}) + + # Serial configuration + self._serial_port_path = serial_port + self._baud_rate = baud_rate + + # Runtime state + self._ser: Optional[serial.Serial] = None + self._initialized = False + self._present_servo_ids: set[int] = set() + self._gripper_present = False + self._current_positions_rad: list[float] = [0.0] * self.num_joints + self._current_gripper_rad: float = 0.0 + + # Build mapping ranges for each physical servo + # Default: -π to +π for all servos + self._mapping_ranges_rad: list[tuple[float, float]] = [ + (-math.pi, math.pi) for _ in self._servo_ids + ] + + # Alert callback (can be set by user for error reporting) + self._alert_callback: Optional[Callable] = None + + # ========================================================================= + # Properties + # ========================================================================= + + @property + def num_joints(self) -> int: + """Number of logical joints this backend controls.""" + return len(self._logical_to_physical_map) + + @property + def is_initialized(self) -> bool: + """Check if the backend has been initialized.""" + return self._initialized + + @property + def has_gripper(self) -> bool: + """Check if a gripper is present and detected.""" + return self._gripper_present + + @property + def gripper_actuator_id(self) -> Optional[int]: + """Get the gripper servo ID.""" + return self._gripper_servo_id if self._gripper_present else None + + @property + def serial_port(self) -> Optional[serial.Serial]: + """Get the serial port handle (for advanced usage).""" + return self._ser + + def set_alert_callback(self, callback: Callable) -> None: + """ + Set a callback function for error/warning alerts. + + Args: + callback: Function with signature (servo_id, error_code, error_names) + """ + self._alert_callback = callback + + # ========================================================================= + # Initialization & Shutdown + # ========================================================================= + + def initialize(self) -> bool: + """ + Initialize the Feetech servo system. + + This method: + 1. Opens the serial port (with auto-detection if needed) + 2. Pings all configured servos to detect presence + 3. Sets default PID gains for present servos + + Returns: + bool: True if initialization was successful. + """ + print("[Feetech] Initializing servo backend...") + + # Clear protocol-level cache + protocol.clear_present_servo_ids() + + # Resolve and open serial port + resolved_port = self._resolve_serial_port() + if not resolved_port: + print("[Feetech] ERROR: Could not find a valid serial port.") + return False + + try: + self._ser = serial.Serial(resolved_port, self._baud_rate, timeout=0.1) + print(f"[Feetech] Serial port {resolved_port} opened at {self._baud_rate} baud.") + except serial.SerialException as e: + print(f"[Feetech] ERROR: Could not open serial port {resolved_port}: {e}") + return False + + # Ping all configured servos + print("[Feetech] Pinging configured servos...") + all_servo_ids = list(self._servo_ids) + if self._gripper_servo_id and self._gripper_servo_id not in all_servo_ids: + all_servo_ids.append(self._gripper_servo_id) + + for servo_id in all_servo_ids: + if protocol.ping(self._ser, servo_id): + self._present_servo_ids.add(servo_id) + print(f"[Feetech] - Servo {servo_id}: PRESENT") + else: + print(f"[Feetech] - Servo {servo_id}: ABSENT") + + # Check gripper presence + if self._gripper_servo_id and self._gripper_servo_id in self._present_servo_ids: + self._gripper_present = True + print(f"[Feetech] Gripper (ID {self._gripper_servo_id}) is present.") + + # Set PID gains for present servos + print("[Feetech] Setting PID gains for present servos...") + for servo_id in self._present_servo_ids: + kp, ki, kd = self._default_pid_gains.get( + servo_id, + (config.DEFAULT_KP, config.DEFAULT_KI, config.DEFAULT_KD) + ) + self.set_pid_gains(servo_id, kp, ki, kd) + time.sleep(0.02) + + self._initialized = True + print("[Feetech] Backend initialization complete.") + return True + + def shutdown(self) -> None: + """Close the serial port and clean up.""" + if self._ser and self._ser.is_open: + self._ser.close() + print("[Feetech] Serial port closed.") + self._initialized = False + self._present_servo_ids = set() + + # ========================================================================= + # Position Control + # ========================================================================= + + def set_joint_positions( + self, + positions_rad: list[float], + speed: float, + acceleration: float, + ) -> None: + """ + Command all joints to specified positions. + + Args: + positions_rad: Target positions in radians for each logical joint. + speed: Speed value (0-4095 for servo register). + acceleration: Acceleration in deg/s² (converted to register value). + """ + if not self._initialized: + print("[Feetech] ERROR: Backend not initialized.") + return + + if len(positions_rad) != self.num_joints: + raise ValueError(f"Expected {self.num_joints} positions, got {len(positions_rad)}") + + # Store commanded positions + self._current_positions_rad = list(positions_rad) + + # Build sync write commands + commands = self.prepare_sync_write_commands( + positions_rad, + speed=int(speed), + accel=self._deg_s2_to_accel_reg(acceleration), + ) + + # Execute sync write + self.sync_write(commands) + + def get_joint_positions(self, verbose: bool = False) -> list[float]: + """ + Read current positions of all logical joints. + + Args: + verbose: If True, print debug information. + + Returns: + List of joint positions in radians. + """ + if not self._initialized: + return [0.0] * self.num_joints + + # Get IDs for arm servos (excluding gripper) + arm_servo_ids = [ + sid for sid in self._servo_ids + if sid in self._present_servo_ids and sid != self._gripper_servo_id + ] + + # Sync read positions + raw_positions = protocol.sync_read_positions( + self._ser, + arm_servo_ids, + alert_callback=self._alert_callback, + ) + + # Convert to logical joint angles + positions = self.raw_to_joint_positions(raw_positions) + + # Update internal state + self._current_positions_rad = positions + + if verbose: + angles_deg = np.rad2deg(positions) + print(f"[Feetech] Current positions (deg): {np.round(angles_deg, 2)}") + + return positions + + # ========================================================================= + # High-Speed Batch Operations + # ========================================================================= + + def prepare_sync_write_commands( + self, + positions_rad: list[float], + speed: int = 4095, + accel: int = 0, + ) -> list[tuple]: + """ + Pre-compute sync write commands for given joint positions. + + Args: + positions_rad: Target positions in radians. + speed: Speed value (0-4095). + accel: Acceleration register value (0-254). + + Returns: + List of (servo_id, position, speed, accel) tuples. + """ + commands = [] + + for logical_idx, physical_indices in self._logical_to_physical_map.items(): + # Apply master offset + angle_with_offset = positions_rad[logical_idx] + self._master_offsets_rad[logical_idx] + + for physical_idx in physical_indices: + servo_id = self._servo_ids[physical_idx] + + # Skip absent servos + if servo_id not in self._present_servo_ids: + continue + + # Convert angle to raw value + raw_pos = self._angle_to_raw(angle_with_offset, physical_idx) + commands.append((servo_id, raw_pos, speed, accel)) + + return commands + + def sync_write(self, commands: list[tuple]) -> None: + """Execute a batch write with pre-computed commands.""" + if not self._initialized or not commands: + return + protocol.sync_write_goal_pos_speed_accel(self._ser, commands) + + def sync_read_positions(self, timeout_s: Optional[float] = None) -> dict[int, int]: + """ + Batch read positions from all arm servos. + + Returns: + Dict mapping servo_id to raw position value. + """ + if not self._initialized: + return {} + + arm_servo_ids = [ + sid for sid in self._servo_ids + if sid in self._present_servo_ids and sid != self._gripper_servo_id + ] + + return protocol.sync_read_positions( + self._ser, + arm_servo_ids, + timeout_s=timeout_s, + alert_callback=self._alert_callback, + ) + + def raw_to_joint_positions(self, raw_positions: dict[int, int]) -> list[float]: + """ + Convert raw servo values to logical joint angles. + + Args: + raw_positions: Dict mapping servo_id to raw value. + + Returns: + List of joint angles in radians. + """ + positions = [0.0] * self.num_joints + + for logical_idx, physical_indices in self._logical_to_physical_map.items(): + angles = [] + for physical_idx in physical_indices: + servo_id = self._servo_ids[physical_idx] + if servo_id in raw_positions: + raw = raw_positions[servo_id] + angle = self._raw_to_angle(raw, physical_idx) + angles.append(angle) + + if angles: + # Average angles for multi-servo joints + avg_angle = np.mean(angles) + # Remove master offset + positions[logical_idx] = avg_angle - self._master_offsets_rad[logical_idx] + + return positions + + # ========================================================================= + # Single Actuator Control + # ========================================================================= + + def set_single_actuator_position( + self, + actuator_id: int, + position_rad: float, + speed: int, + accel: int, + ) -> None: + """Command a single actuator to a position.""" + if not self._initialized: + return + + if actuator_id not in self._present_servo_ids: + print(f"[Feetech] Cannot command absent servo {actuator_id}") + return + + # Find config index for this servo + try: + config_idx = self._servo_ids.index(actuator_id) + except ValueError: + # Might be gripper + if actuator_id == self._gripper_servo_id: + config_idx = len(self._servo_ids) # Use a placeholder index + else: + print(f"[Feetech] Unknown servo ID {actuator_id}") + return + + # Convert angle to raw + if actuator_id == self._gripper_servo_id: + raw_pos = self._gripper_angle_to_raw(position_rad) + else: + raw_pos = self._angle_to_raw(position_rad, config_idx) + + accel_reg = self._deg_s2_to_accel_reg(accel) if accel > 0 else 0 + + # Send via sync write for consistency + protocol.sync_write_goal_pos_speed_accel( + self._ser, + [(actuator_id, raw_pos, speed, accel_reg)] + ) + + def read_single_actuator_position(self, actuator_id: int) -> Optional[int]: + """Read the raw position of a single actuator.""" + if not self._initialized: + return None + return protocol.read_position(self._ser, actuator_id, self._alert_callback) + + # ========================================================================= + # Calibration & Configuration + # ========================================================================= + + def set_current_position_as_zero(self, actuator_id: int) -> bool: + """ + Set the current position of an actuator as its zero point. + + Args: + actuator_id: The servo ID to calibrate. + + Returns: + bool: True on success. + """ + if not self._initialized: + return False + + if actuator_id not in self._present_servo_ids: + print(f"[Feetech] Cannot calibrate absent servo {actuator_id}") + return False + + print(f"[Feetech] Calibrating servo {actuator_id} - setting current position as zero...") + result = protocol.calibrate_middle_position(self._ser, actuator_id) + + if result: + time.sleep(0.1) # Wait for EEPROM write + print(f"[Feetech] Servo {actuator_id} calibrated successfully.") + else: + print(f"[Feetech] Failed to calibrate servo {actuator_id}.") + + return result + + def set_pid_gains(self, actuator_id: int, kp: int, ki: int, kd: int) -> bool: + """ + Set PID gains for a servo. + + Args: + actuator_id: The servo ID. + kp: Proportional gain (0-254). + ki: Integral gain (0-254). + kd: Derivative gain (0-254). + + Returns: + bool: True on success. + """ + if not self._initialized: + return False + + # Clamp values + kp = max(0, min(254, int(kp))) + ki = max(0, min(254, int(ki))) + kd = max(0, min(254, int(kd))) + + success = True + if not protocol.write_register_byte(self._ser, actuator_id, config.SERVO_ADDR_POS_KP, kp): + success = False + time.sleep(0.01) + + if not protocol.write_register_byte(self._ser, actuator_id, config.SERVO_ADDR_POS_KI, ki): + success = False + time.sleep(0.01) + + if not protocol.write_register_byte(self._ser, actuator_id, config.SERVO_ADDR_POS_KD, kd): + success = False + time.sleep(0.01) + + if success: + print(f"[Feetech] Set PID for servo {actuator_id}: Kp={kp}, Ki={ki}, Kd={kd}") + + return success + + def apply_joint_limits(self) -> bool: + """ + Apply joint limits to all servos. + + Returns: + bool: True if all limits were set successfully. + """ + if not self._initialized: + return False + + print("[Feetech] Applying joint limits to servos...") + all_ok = True + + for logical_idx, physical_indices in self._logical_to_physical_map.items(): + limits = self._joint_limits_rad[logical_idx] + + for physical_idx in physical_indices: + servo_id = self._servo_ids[physical_idx] + + if servo_id not in self._present_servo_ids: + continue + + # Convert limits to raw values + min_raw = self._angle_to_raw(limits[0], physical_idx) + max_raw = self._angle_to_raw(limits[1], physical_idx) + + # Ensure min < max for the register + final_min = min(min_raw, max_raw) + final_max = max(min_raw, max_raw) + + if not protocol.write_angle_limits(self._ser, servo_id, final_min, final_max): + all_ok = False + print(f"[Feetech] Failed to set limits for servo {servo_id}") + + time.sleep(0.02) + + # Handle gripper limits + if self._gripper_present and self._gripper_servo_id: + min_raw = self._gripper_angle_to_raw(self._gripper_limits_rad[0]) + max_raw = self._gripper_angle_to_raw(self._gripper_limits_rad[1]) + final_min = min(min_raw, max_raw) + final_max = max(min_raw, max_raw) + + if not protocol.write_angle_limits(self._ser, self._gripper_servo_id, final_min, final_max): + all_ok = False + + return all_ok + + # ========================================================================= + # Gripper Support + # ========================================================================= + + def set_gripper_position(self, position_rad: float, speed: int = 50, accel: int = 0) -> None: + """Set the gripper position.""" + if not self._gripper_present or not self._gripper_servo_id: + print("[Feetech] No gripper present.") + return + + # Clamp to limits + min_rad, max_rad = self._gripper_limits_rad + position_rad = max(min_rad, min(max_rad, position_rad)) + + self.set_single_actuator_position( + self._gripper_servo_id, + position_rad, + speed, + accel + ) + self._current_gripper_rad = position_rad + + def get_gripper_position(self) -> Optional[float]: + """Get current gripper position.""" + if not self._gripper_present or not self._gripper_servo_id: + return None + + raw = protocol.read_position(self._ser, self._gripper_servo_id, self._alert_callback) + if raw is not None: + angle = self._gripper_raw_to_angle(raw) + self._current_gripper_rad = angle + return angle + return None + + # ========================================================================= + # Diagnostics + # ========================================================================= + + def get_present_actuator_ids(self) -> set[int]: + """Get the set of detected servo IDs.""" + return self._present_servo_ids.copy() + + def ping_actuator(self, actuator_id: int) -> bool: + """Check if a servo responds to ping.""" + if not self._ser or not self._ser.is_open: + return False + return protocol.ping(self._ser, actuator_id) + + def factory_reset_actuator(self, actuator_id: int) -> bool: + """Factory reset a servo.""" + if not self._initialized: + return False + return protocol.factory_reset(self._ser, actuator_id) + + def restart_actuator(self, actuator_id: int) -> bool: + """Restart a servo.""" + if not self._initialized: + return False + return protocol.restart(self._ser, actuator_id) + + # ========================================================================= + # Internal Helper Methods + # ========================================================================= + + def _is_servo_inverted(self, physical_idx: int) -> bool: + """Check if a servo uses inverted mapping.""" + servo_id = self._servo_ids[physical_idx] + return servo_id in self._inverted_servo_ids + + def _angle_to_raw(self, angle_rad: float, physical_idx: int) -> int: + """ + Convert an angle in radians to a raw servo value (0-4095). + + Args: + angle_rad: Angle in radians. + physical_idx: Index in the servo_ids list. + + Returns: + int: Raw servo value (0-4095). + """ + min_rad, max_rad = self._mapping_ranges_rad[physical_idx] + + # Clamp angle to range + angle_clamped = max(min_rad, min(max_rad, angle_rad)) + + # Normalize to 0-1 + normalized = (angle_clamped - min_rad) / (max_rad - min_rad) + + # Apply inversion if needed + if self._is_servo_inverted(physical_idx): + raw = (1.0 - normalized) * 4095.0 + else: + raw = normalized * 4095.0 + + return int(round(max(0, min(4095, raw)))) + + def _raw_to_angle(self, raw_value: int, physical_idx: int) -> float: + """ + Convert a raw servo value to an angle in radians. + + Args: + raw_value: Raw servo value (0-4095). + physical_idx: Index in the servo_ids list. + + Returns: + float: Angle in radians. + """ + raw_clamped = max(0, min(4095, raw_value)) + min_rad, max_rad = self._mapping_ranges_rad[physical_idx] + + # Normalize raw to 0-1 + normalized = raw_clamped / 4095.0 + + # Apply inversion if needed + if self._is_servo_inverted(physical_idx): + normalized = 1.0 - normalized + + # Convert to angle + angle = normalized * (max_rad - min_rad) + min_rad + return angle + + def _gripper_angle_to_raw(self, angle_rad: float) -> int: + """Convert gripper angle to raw value.""" + # Assume gripper uses same mapping as arm servos + min_rad, max_rad = -math.pi, math.pi + angle_clamped = max(min_rad, min(max_rad, angle_rad)) + normalized = (angle_clamped - min_rad) / (max_rad - min_rad) + + # Check if gripper is inverted + if self._gripper_servo_id in self._inverted_servo_ids: + raw = (1.0 - normalized) * 4095.0 + else: + raw = normalized * 4095.0 + + return int(round(max(0, min(4095, raw)))) + + def _gripper_raw_to_angle(self, raw_value: int) -> float: + """Convert raw value to gripper angle.""" + raw_clamped = max(0, min(4095, raw_value)) + min_rad, max_rad = -math.pi, math.pi + normalized = raw_clamped / 4095.0 + + if self._gripper_servo_id in self._inverted_servo_ids: + normalized = 1.0 - normalized + + return normalized * (max_rad - min_rad) + min_rad + + def _deg_s2_to_accel_reg(self, accel_deg_s2: float) -> int: + """Convert deg/s² to acceleration register value.""" + if accel_deg_s2 <= 0: + return 0 # Max acceleration + reg_val = int(round(accel_deg_s2 / config.ACCELERATION_SCALE_FACTOR)) + return max(1, min(254, reg_val)) + + # ========================================================================= + # Serial Port Auto-Detection + # ========================================================================= + + def _resolve_serial_port(self) -> Optional[str]: + """ + Determine the serial port to use. + + Priority: + 1. Explicitly provided port + 2. SERIAL_PORT environment variable + 3. Auto-detection + + Returns: + Optional[str]: Path to serial port, or None if not found. + """ + # 1. Explicit port + if self._serial_port_path: + if os.path.exists(self._serial_port_path): + return self._serial_port_path + print(f"[Feetech] WARNING: Specified port {self._serial_port_path} does not exist.") + + # 2. Environment variable + env_port = os.environ.get("SERIAL_PORT") + if env_port and os.path.exists(env_port): + print(f"[Feetech] Using serial port from SERIAL_PORT env var: {env_port}") + return env_port + + # 3. Auto-detect + return self._auto_detect_serial_port() + + def _auto_detect_serial_port(self) -> Optional[str]: + """ + Auto-detect the serial port with connected Feetech servos. + + Returns: + Optional[str]: Detected port path, or None if not found. + """ + # USB serial patterns to try + patterns = [ + "/dev/serial/by-id/usb-*", + "/dev/ttyUSB*", + "/dev/ttyACM*", + ] + + candidates = [] + seen = set() + + for pattern in patterns: + for path in sorted(glob.glob(pattern)): + try: + realpath = os.path.realpath(path) + if realpath in seen: + continue + seen.add(realpath) + candidates.append(path) + except OSError: + continue + + if not candidates: + print("[Feetech] No candidate serial devices found.") + return None + + # Probe each candidate + responsive = [] + for path in candidates: + if self._probe_serial_port(path): + responsive.append(path) + + if len(responsive) == 1: + print(f"[Feetech] Auto-detected servo serial port: {responsive[0]}") + return responsive[0] + elif len(responsive) > 1: + print(f"[Feetech] WARNING: Multiple responsive ports found: {responsive}") + return responsive[0] + + print("[Feetech] No responsive serial port found.") + return None + + def _probe_serial_port(self, path: str) -> bool: + """ + Probe a serial port for connected servos. + + Args: + path: Serial port path to probe. + + Returns: + bool: True if servos respond on this port. + """ + # Try a few servo IDs + probe_ids = self._servo_ids[:4] # First 4 servos + if self._gripper_servo_id: + probe_ids.append(self._gripper_servo_id) + + try: + with serial.Serial(path, self._baud_rate, timeout=0.1) as test_ser: + time.sleep(0.05) + for _ in range(2): + for servo_id in probe_ids: + if protocol.ping(test_ser, servo_id): + return True + return False + except Exception as e: + print(f"[Feetech] Probe skipped {path}: {e}") + return False + diff --git a/src/gradient_os/arm_controller/backends/feetech/protocol.py b/src/gradient_os/arm_controller/backends/feetech/protocol.py new file mode 100644 index 00000000..f2d8de9f --- /dev/null +++ b/src/gradient_os/arm_controller/backends/feetech/protocol.py @@ -0,0 +1,726 @@ +# backends/feetech/protocol.py +# +# Low-level Feetech servo protocol implementation. +# Handles packet construction, checksum calculation, and serial communication +# for Feetech STS/SCS series servos. +# +# This module is designed to be independent of robot-specific configuration. +# It receives a serial port handle and servo IDs as parameters rather than +# using global state. + +import time +import threading +from typing import Optional, Callable +import serial + +from . import config + +# ============================================================================= +# Module State +# ============================================================================= + +# Global re-entrant lock to serialize all serial I/O across threads +_SERIAL_LOCK = threading.RLock() + +# Telemetry buffer for Sync Read profiling (write, read, parse durations in seconds) +_sync_profiles: list[tuple[float, float, float]] = [] + +# Cache of detected servo IDs (populated by ping operations) +_present_servo_ids: set[int] = set() + + +def get_present_servo_ids() -> set[int]: + """ + Returns the set of servo IDs that responded to ping commands. + + Returns: + set[int]: Set of detected servo IDs. + """ + return _present_servo_ids.copy() + + +def clear_present_servo_ids() -> None: + """Clear the cache of detected servo IDs.""" + global _present_servo_ids + _present_servo_ids = set() + + +def add_present_servo_id(servo_id: int) -> None: + """Add a servo ID to the detected set.""" + _present_servo_ids.add(servo_id) + + +def get_sync_profiles() -> list[tuple[float, float, float]]: + """ + Return and clear the collected Sync-Read timing tuples. + + Returns: + list[tuple[float, float, float]]: List of (write_dur, read_dur, parse_dur) in seconds. + """ + global _sync_profiles + out = _sync_profiles[:] + _sync_profiles.clear() + return out + + +# ============================================================================= +# Checksum Calculation +# ============================================================================= + +def calculate_checksum(packet_data: bytearray) -> int: + """ + Calculates the Feetech checksum for a packet. + + The checksum is the bitwise inverse of the sum of all bytes in the packet + (excluding the initial 0xFF headers). + + Args: + packet_data: The packet data (from Servo ID to last parameter) to sum. + + Returns: + int: The calculated checksum byte (0-255). + """ + current_sum = sum(packet_data) + return (~current_sum) & 0xFF + + +# ============================================================================= +# Basic Communication Functions +# ============================================================================= + +def ping(ser: serial.Serial, servo_id: int) -> bool: + """ + Send a PING instruction to check if a servo is present. + + Args: + ser: Open serial port handle. + servo_id: The hardware ID of the servo to ping. + + Returns: + bool: True if a valid status packet is received, False otherwise. + """ + if ser is None or not ser.is_open: + return False + + # PING Packet: [0xFF, 0xFF, ID, Length=2, Instr=0x01, Checksum] + ping_command = bytearray(6) + ping_command[0] = config.SERVO_HEADER + ping_command[1] = config.SERVO_HEADER + ping_command[2] = servo_id + ping_command[3] = 2 # Length = NumParams(0) + 2 + ping_command[4] = config.SERVO_INSTRUCTION_PING + ping_command[5] = calculate_checksum(ping_command[2:5]) + + try: + with _SERIAL_LOCK: + ser.reset_input_buffer() + ser.write(ping_command) + # Expected response: [0xFF, 0xFF, ID, Length=2, Error=0, Checksum] + response = ser.read(6) + + if len(response) == 6 and response[0] == 0xFF and response[1] == 0xFF and response[2] == servo_id: + _present_servo_ids.add(servo_id) + return True + return False + + except Exception as e: + print(f"[Feetech PING] Error during ping for servo {servo_id}: {e}") + return False + + +def read_register_byte(ser: serial.Serial, servo_id: int, register_address: int) -> Optional[int]: + """ + Read a single byte from a servo register. + + Args: + ser: Open serial port handle. + servo_id: The hardware ID of the target servo. + register_address: The address of the register to read. + + Returns: + Optional[int]: The byte value (0-255), or None on failure. + """ + if ser is None or not ser.is_open: + return None + + # Command: [0xFF, 0xFF, ID, Length=4, Instr=0x02, Addr, BytesToRead=1, Checksum] + read_command = bytearray(8) + read_command[0] = config.SERVO_HEADER + read_command[1] = config.SERVO_HEADER + read_command[2] = servo_id + read_command[3] = 4 # Length + read_command[4] = config.SERVO_INSTRUCTION_READ + read_command[5] = register_address + read_command[6] = 1 # Number of bytes to read + read_command[7] = calculate_checksum(read_command[2:7]) + + try: + with _SERIAL_LOCK: + ser.reset_input_buffer() + ser.write(read_command) + # Expected response: [0xFF, 0xFF, ID, Length=3, Error, Value, Checksum] + response = ser.read(7) + + if len(response) < 7: + return None + if not (response[0] == 0xFF and response[1] == 0xFF and response[2] == servo_id): + return None + if response[6] != calculate_checksum(response[2:6]): + return None + if response[4] != 0: # Error byte + return None + return response[5] + + except Exception: + return None + + +def read_register_word(ser: serial.Serial, servo_id: int, register_address: int) -> Optional[int]: + """ + Read a 16-bit word (2 bytes) from a servo register. + + Args: + ser: Open serial port handle. + servo_id: The hardware ID of the target servo. + register_address: The starting address of the register to read. + + Returns: + Optional[int]: The 16-bit value, or None on failure. + """ + if ser is None or not ser.is_open: + return None + + # Command: [0xFF, 0xFF, ID, Length=4, Instr=0x02, Addr, BytesToRead=2, Checksum] + read_command = bytearray(8) + read_command[0] = config.SERVO_HEADER + read_command[1] = config.SERVO_HEADER + read_command[2] = servo_id + read_command[3] = 4 # Length + read_command[4] = config.SERVO_INSTRUCTION_READ + read_command[5] = register_address + read_command[6] = 2 # Number of bytes to read + read_command[7] = calculate_checksum(read_command[2:7]) + + try: + with _SERIAL_LOCK: + ser.reset_input_buffer() + ser.write(read_command) + # Expected response: [0xFF, 0xFF, ID, Length=4, Error, Val_L, Val_H, Checksum] + response = ser.read(8) + + if len(response) < 8: + return None + if not (response[0] == 0xFF and response[1] == 0xFF and response[2] == servo_id): + return None + if response[7] != calculate_checksum(response[2:7]): + return None + if response[4] != 0: # Error byte + return None + + # Little-endian 16-bit value + value = response[5] | (response[6] << 8) + return value + + except Exception: + return None + + +def write_register_byte(ser: serial.Serial, servo_id: int, register_address: int, value: int) -> bool: + """ + Write a single byte to a servo register. + + Args: + ser: Open serial port handle. + servo_id: The hardware ID of the target servo. + register_address: The address of the register to write. + value: The byte value to write (0-255). + + Returns: + bool: True on success, False on failure. + """ + if ser is None or not ser.is_open: + return False + + val_clamped = int(max(0, min(255, value))) + + # Packet: [0xFF, 0xFF, ID, Length=4, Instr=0x03, Addr, Value, Checksum] + packet = bytearray(8) + packet[0] = config.SERVO_HEADER + packet[1] = config.SERVO_HEADER + packet[2] = servo_id + packet[3] = 4 # Length = Instr(1) + Addr(1) + Value(1) + 1 + packet[4] = config.SERVO_INSTRUCTION_WRITE + packet[5] = register_address + packet[6] = val_clamped + packet[7] = calculate_checksum(packet[2:7]) + + try: + with _SERIAL_LOCK: + ser.write(packet) + return True + except Exception as e: + print(f"[Feetech] Error writing byte to servo {servo_id} register {hex(register_address)}: {e}") + return False + + +def write_register_word(ser: serial.Serial, servo_id: int, register_address: int, value: int) -> bool: + """ + Write a 16-bit word to a servo register. + + Args: + ser: Open serial port handle. + servo_id: The hardware ID of the target servo. + register_address: The address of the register to write. + value: The 16-bit value to write. + + Returns: + bool: True on success, False on failure. + """ + if ser is None or not ser.is_open: + return False + + val_clamped = int(value) + + # Packet: [0xFF, 0xFF, ID, Length=5, Instr=0x03, Addr, Val_L, Val_H, Checksum] + packet = bytearray(9) + packet[0] = config.SERVO_HEADER + packet[1] = config.SERVO_HEADER + packet[2] = servo_id + packet[3] = 5 # Length + packet[4] = config.SERVO_INSTRUCTION_WRITE + packet[5] = register_address + packet[6] = val_clamped & 0xFF # Low byte + packet[7] = (val_clamped >> 8) & 0xFF # High byte + packet[8] = calculate_checksum(packet[2:8]) + + try: + with _SERIAL_LOCK: + ser.write(packet) + return True + except Exception as e: + print(f"[Feetech] Error writing word to servo {servo_id} register {hex(register_address)}: {e}") + return False + + +# ============================================================================= +# Position Reading +# ============================================================================= + +def read_position( + ser: serial.Serial, + servo_id: int, + alert_callback: Optional[Callable] = None, +) -> Optional[int]: + """ + Read the current position of a single servo. + + Args: + ser: Open serial port handle. + servo_id: The hardware ID of the target servo. + alert_callback: Optional callback for reporting errors. + + Returns: + Optional[int]: The servo's current raw position (0-4095), or None on failure. + """ + if ser is None or not ser.is_open: + return None + + # Command: [0xFF, 0xFF, ID, Length=4, Instr=0x02, Addr=0x38, BytesToRead=2, Checksum] + read_command = bytearray(8) + read_command[0] = config.SERVO_HEADER + read_command[1] = config.SERVO_HEADER + read_command[2] = servo_id + read_command[3] = 4 + read_command[4] = config.SERVO_INSTRUCTION_READ + read_command[5] = config.SERVO_ADDR_PRESENT_POSITION + read_command[6] = 2 + read_command[7] = calculate_checksum(read_command[2:7]) + + try: + with _SERIAL_LOCK: + ser.reset_input_buffer() + ser.write(read_command) + response = ser.read(8) + + if not response or len(response) < 8: + return None + if response[0] != config.SERVO_HEADER or response[1] != config.SERVO_HEADER: + return None + if response[2] != servo_id: + return None + if response[3] != 4: # Length field should be 4 + return None + + expected_checksum = calculate_checksum(response[2:7]) + if expected_checksum != response[7]: + return None + + error_byte = response[4] + if error_byte != 0: + if alert_callback: + names = config.names_for_status_bits(error_byte) + alert_callback(servo_id, error_byte, names) + return None + + # Position is a signed 16-bit value (for multi-turn mode compatibility) + position = int.from_bytes(response[5:7], byteorder='little', signed=True) + return position + + except Exception as e: + print(f"[Feetech ReadPos] Servo {servo_id}: Error: {e}") + return None + + +# ============================================================================= +# Sync Write (Batch Position/Speed/Accel) +# ============================================================================= + +def sync_write_goal_pos_speed_accel(ser: serial.Serial, servo_data_list: list[tuple[int, int, int, int]]) -> None: + """ + Send a SYNC WRITE packet to command multiple servos simultaneously. + + This is the most efficient way to command multiple servos as it bundles + all commands into a single serial transmission. + + Args: + ser: Open serial port handle. + servo_data_list: List of tuples, each containing: + (servo_id, position_value, speed_value, accel_register_value) + - servo_id: Hardware ID of the servo + - position_value: Target position (0-4095) + - speed_value: Target speed (0-4095) + - accel_register_value: Acceleration (0-254, 0=max) + """ + if ser is None or not ser.is_open: + return + + num_servos = len(servo_data_list) + if num_servos == 0: + return + + # Calculate packet length + # PacketLen = num_servos * (ID + DataLen) + 4 + packet_len_field = num_servos * (1 + config.SYNC_WRITE_DATA_LEN_PER_SERVO) + 4 + + # Total packet: Header(2) + BroadcastID(1) + Len(1) + Content + Checksum(1) + total_packet_bytes = 4 + packet_len_field + 1 + packet = bytearray(total_packet_bytes) + + packet[0] = config.SERVO_HEADER + packet[1] = config.SERVO_HEADER + packet[2] = config.SERVO_BROADCAST_ID + packet[3] = packet_len_field + packet[4] = config.SERVO_INSTRUCTION_SYNC_WRITE + packet[5] = config.SYNC_WRITE_START_ADDRESS + packet[6] = config.SYNC_WRITE_DATA_LEN_PER_SERVO + + idx = 7 + for servo_id, pos_val, speed_val, accel_reg_val in servo_data_list: + packet[idx] = servo_id + idx += 1 + + # Data order: Accel(1), Pos_L(1), Pos_H(1), Time_L(1), Time_H(1), Spd_L(1), Spd_H(1) + packet[idx] = accel_reg_val + idx += 1 + packet[idx] = pos_val & 0xFF + idx += 1 + packet[idx] = (pos_val >> 8) & 0xFF + idx += 1 + packet[idx] = 0 # Time_L (always 0) + idx += 1 + packet[idx] = 0 # Time_H (always 0) + idx += 1 + packet[idx] = speed_val & 0xFF + idx += 1 + packet[idx] = (speed_val >> 8) & 0xFF + idx += 1 + + # Calculate checksum (from Broadcast_ID to last data byte) + packet[idx] = calculate_checksum(packet[2:idx]) + + try: + with _SERIAL_LOCK: + ser.write(packet[:idx + 1]) + except Exception as e: + print(f"[Feetech SyncWrite] Error: {e}") + + +# ============================================================================= +# Sync Read (Batch Position Reading) +# ============================================================================= + +def sync_read_positions( + ser: serial.Serial, + servo_ids: list[int], + timeout_s: Optional[float] = None, + poll_delay_s: float = 0.0, + diagnostics: bool = True, + alert_callback: Optional[Callable] = None, +) -> dict[int, int]: + """ + Read positions from multiple servos using a single SYNC READ command. + + This is significantly faster than reading one by one, enabling high-frequency + feedback for closed-loop control. + + Args: + ser: Open serial port handle. + servo_ids: List of servo IDs to read from. + timeout_s: Optional per-call serial read timeout override. + poll_delay_s: Optional delay after command before reading response. + diagnostics: Whether to collect timing data. + alert_callback: Optional callback for reporting errors. + + Returns: + dict[int, int]: Mapping of servo_id to raw position. May be partial + if some servos did not respond. + """ + if ser is None or not ser.is_open: + return {} + + num_servos = len(servo_ids) + if num_servos == 0: + return {} + + # Construct Sync Read packet + # [0xFF, 0xFF, Broadcast_ID, Len, Instr, StartAddr, DataLen, ID1, ID2, ..., Checksum] + packet_len_field = num_servos + 4 + packet = bytearray(7 + num_servos + 1) + + packet[0] = config.SERVO_HEADER + packet[1] = config.SERVO_HEADER + packet[2] = config.SERVO_BROADCAST_ID + packet[3] = packet_len_field + packet[4] = config.SERVO_INSTRUCTION_SYNC_READ + packet[5] = config.SERVO_ADDR_PRESENT_POSITION + packet[6] = 2 # Read 2 bytes (position) + + for i, servo_id in enumerate(servo_ids): + packet[7 + i] = servo_id + + packet[-1] = calculate_checksum(packet[2:-1]) + + try: + # Optionally override timeout + original_timeout = None + if timeout_s is not None: + original_timeout = ser.timeout + ser.timeout = timeout_s + + # WRITE + write_start = time.perf_counter() + with _SERIAL_LOCK: + ser.reset_input_buffer() + ser.write(packet) + write_dur = time.perf_counter() - write_start + + if poll_delay_s > 0.0: + time.sleep(poll_delay_s) + + # READ + bytes_to_read = num_servos * 8 # Each servo sends 8-byte status packet + read_start = time.perf_counter() + with _SERIAL_LOCK: + response_data = ser.read(bytes_to_read) + read_dur = time.perf_counter() - read_start + + if len(response_data) < bytes_to_read: + print(f"[Feetech SyncRead] WARNING: Expected {bytes_to_read} bytes, got {len(response_data)}.") + + # PARSE + positions = {} + expected_ids = set(servo_ids) + parse_start = time.perf_counter() + + for i in range(0, len(response_data) - 7): + if response_data[i] == config.SERVO_HEADER and response_data[i+1] == config.SERVO_HEADER: + pkt = response_data[i : i+8] + response_id = pkt[2] + + if response_id not in expected_ids: + continue + + # Check error byte + if pkt[4] != 0: + if alert_callback: + names = config.names_for_status_bits(pkt[4]) + alert_callback(response_id, pkt[4], names) + continue + + # Validate checksum + if calculate_checksum(pkt[2:7]) != pkt[7]: + continue + + # Extract position (signed 16-bit, little-endian) + position = int.from_bytes(pkt[5:7], byteorder='little', signed=True) + positions[response_id] = position + expected_ids.discard(response_id) + + parse_dur = time.perf_counter() - parse_start + + if diagnostics: + _sync_profiles.append((write_dur, read_dur, parse_dur)) + + if expected_ids: + missing = list(expected_ids) + print(f"[Feetech SyncRead] No response from IDs: {missing}") + if alert_callback: + for sid in missing: + alert_callback(sid, -1, ["Timeout"]) + + return positions + + except Exception as e: + print(f"[Feetech SyncRead] Error: {e}") + return {} + finally: + if timeout_s is not None and original_timeout is not None: + ser.timeout = original_timeout + + +# ============================================================================= +# Special Commands +# ============================================================================= + +def calibrate_middle_position(ser: serial.Serial, servo_id: int) -> bool: + """ + Send the Calibrate Middle Position command (0x0B). + + This instructs the servo to treat its current physical position as the + center point (raw value 2048). + + Args: + ser: Open serial port handle. + servo_id: The hardware ID of the target servo. + + Returns: + bool: True on success, False on failure. + """ + if ser is None or not ser.is_open: + return False + + # Packet: [0xFF, 0xFF, ID, Length=2, Instr=0x0B, Checksum] + packet = bytearray(6) + packet[0] = config.SERVO_HEADER + packet[1] = config.SERVO_HEADER + packet[2] = servo_id + packet[3] = 2 + packet[4] = config.SERVO_INSTRUCTION_CALIBRATE_MIDDLE + packet[5] = calculate_checksum(packet[2:5]) + + try: + with _SERIAL_LOCK: + ser.write(packet) + return True + except Exception as e: + print(f"[Feetech] Error sending calibrate command to servo {servo_id}: {e}") + return False + + +def factory_reset(ser: serial.Serial, servo_id: int) -> bool: + """ + Send the Factory Reset command (0x06). + + This resets all EEPROM values to factory defaults EXCEPT the servo ID. + + Args: + ser: Open serial port handle. + servo_id: The hardware ID of the target servo. + + Returns: + bool: True on success, False on failure. + """ + if ser is None or not ser.is_open: + return False + + packet = bytearray(6) + packet[0] = config.SERVO_HEADER + packet[1] = config.SERVO_HEADER + packet[2] = servo_id + packet[3] = 2 + packet[4] = config.SERVO_INSTRUCTION_RESET + packet[5] = calculate_checksum(packet[2:5]) + + try: + with _SERIAL_LOCK: + ser.write(packet) + return True + except Exception as e: + print(f"[Feetech] Error sending factory reset to servo {servo_id}: {e}") + return False + + +def restart(ser: serial.Serial, servo_id: int) -> bool: + """ + Send the Restart command (0x08). + + This is equivalent to a power cycle of the servo. + + Args: + ser: Open serial port handle. + servo_id: The hardware ID of the target servo. + + Returns: + bool: True on success, False on failure. + """ + if ser is None or not ser.is_open: + return False + + packet = bytearray(6) + packet[0] = config.SERVO_HEADER + packet[1] = config.SERVO_HEADER + packet[2] = servo_id + packet[3] = 2 + packet[4] = config.SERVO_INSTRUCTION_RESTART + packet[5] = calculate_checksum(packet[2:5]) + + try: + with _SERIAL_LOCK: + ser.write(packet) + return True + except Exception as e: + print(f"[Feetech] Error sending restart to servo {servo_id}: {e}") + return False + + +def write_angle_limits( + ser: serial.Serial, + servo_id: int, + min_limit_raw: int, + max_limit_raw: int, +) -> bool: + """ + Write minimum and maximum angle limits to a servo's EEPROM. + + This requires unlocking the EEPROM, writing values, and re-locking. + + Args: + ser: Open serial port handle. + servo_id: The hardware ID of the target servo. + min_limit_raw: Raw minimum angle limit (0-4095). + max_limit_raw: Raw maximum angle limit (0-4095). + + Returns: + bool: True if all steps were successful, False otherwise. + """ + # 1. Unlock EEPROM + if not write_register_byte(ser, servo_id, config.SERVO_ADDR_WRITE_LOCK, 0): + print(f"[Feetech] Failed to unlock EEPROM for servo {servo_id}") + return False + time.sleep(0.01) + + # 2. Write limits + min_ok = write_register_word(ser, servo_id, config.SERVO_ADDR_MIN_ANGLE_LIMIT, min_limit_raw) + time.sleep(0.01) + max_ok = write_register_word(ser, servo_id, config.SERVO_ADDR_MAX_ANGLE_LIMIT, max_limit_raw) + time.sleep(0.01) + + if not (min_ok and max_ok): + # Try to re-lock even on failure + write_register_byte(ser, servo_id, config.SERVO_ADDR_WRITE_LOCK, 1) + return False + + # 3. Re-lock EEPROM + write_register_byte(ser, servo_id, config.SERVO_ADDR_WRITE_LOCK, 1) + return True + diff --git a/src/gradient_os/arm_controller/backends/registry.py b/src/gradient_os/arm_controller/backends/registry.py new file mode 100644 index 00000000..dbaa9a3b --- /dev/null +++ b/src/gradient_os/arm_controller/backends/registry.py @@ -0,0 +1,356 @@ +# backends/registry.py +# +# Global registry for the active servo backend. +# This module manages both the backend CONFIGURATION (constants, register addresses) +# and the backend INSTANCE (the actual ActuatorBackend object that does I/O). +# +# Usage: +# ------ +# At startup (in run_controller.py): +# from gradient_os.arm_controller.backends import registry +# +# # 1. Set active backend (loads config module) +# registry.set_active_backend("feetech") +# +# # 2. Create and initialize backend instance +# backend = registry.create_backend("feetech", robot_config_dict) +# backend.initialize() +# registry.set_active_backend_instance(backend) +# +# In any module that needs to perform servo I/O: +# from gradient_os.arm_controller.backends import registry +# backend = registry.get_active_backend() +# backend.sync_write(commands) +# +# In any module that needs config values only: +# from gradient_os.arm_controller.backends import registry +# encoder_resolution = registry.get_encoder_resolution() + +from typing import Optional, TYPE_CHECKING, Type, Callable +import importlib + +if TYPE_CHECKING: + from types import ModuleType + from ..actuator_interface import ActuatorBackend + +# ============================================================================= +# Module State +# ============================================================================= + +# The active backend name and config module +_active_backend_name: Optional[str] = None +_active_backend_config: Optional['ModuleType'] = None + +# The active backend INSTANCE (for I/O operations) +_active_backend_instance: Optional['ActuatorBackend'] = None + +# Mapping of backend names to their config module paths +BACKEND_CONFIG_MODULES: dict[str, str] = { + "feetech": "gradient_os.arm_controller.backends.feetech.config", + # Add more backends here as they're implemented: + # "dynamixel": "gradient_os.arm_controller.backends.dynamixel.config", + # "simulation": "gradient_os.arm_controller.backends.simulation.config", +} + +# Mapping of backend names to their backend class factory functions +# Each factory takes a robot_config dict and returns an ActuatorBackend instance +BACKEND_CLASSES: dict[str, Callable[..., 'ActuatorBackend']] = {} + + +# ============================================================================= +# Exceptions +# ============================================================================= + +class BackendNotConfiguredError(Exception): + """Raised when trying to access backend config before it's been set.""" + pass + + +class BackendInstanceNotSetError(Exception): + """Raised when trying to access backend instance before it's been created.""" + pass + + +def register_backend_class( + name: str, + factory: Callable[..., 'ActuatorBackend'], + config_module_path: Optional[str] = None, +) -> None: + """ + Register a backend class with the registry. + + This is called from backends/__init__.py to register available backends. + + Args: + name: Backend name (e.g., "feetech", "simulation") + factory: Callable that creates an ActuatorBackend instance. + Signature: factory(robot_config: dict, **kwargs) -> ActuatorBackend + config_module_path: Optional path to config module (for backends with config) + """ + BACKEND_CLASSES[name] = factory + if config_module_path: + BACKEND_CONFIG_MODULES[name] = config_module_path + print(f"[Backend Registry] Registered backend: {name}") + + +def set_active_backend(backend_name: str) -> None: + """ + Set the active servo backend (loads config module). + + This should be called once at startup (e.g., in run_controller.py) before + any modules try to access backend configuration. + + Note: This only sets the CONFIG. To create a backend INSTANCE, call + create_backend() and set_active_backend_instance(). + + Args: + backend_name: Name of the backend (e.g., "feetech", "dynamixel") + + Raises: + ValueError: If the backend name is not recognized + """ + global _active_backend_name, _active_backend_config + + if backend_name not in BACKEND_CONFIG_MODULES: + available = ", ".join(BACKEND_CONFIG_MODULES.keys()) + raise ValueError(f"Unknown servo backend '{backend_name}'. Available: {available}") + + module_path = BACKEND_CONFIG_MODULES[backend_name] + _active_backend_config = importlib.import_module(module_path) + _active_backend_name = backend_name + + print(f"[Backend Registry] Active servo backend set to: {backend_name}") + + +def get_active_backend_name() -> str: + """ + Get the name of the active servo backend. + + Returns: + str: Backend name (e.g., "feetech") + + Raises: + BackendNotConfiguredError: If no backend has been set + """ + if _active_backend_name is None: + raise BackendNotConfiguredError( + "No servo backend configured. Call set_active_backend() at startup." + ) + return _active_backend_name + + +def get_config() -> 'ModuleType': + """ + Get the active backend's config module. + + Returns: + ModuleType: The backend config module (e.g., backends.feetech.config) + + Raises: + BackendNotConfiguredError: If no backend has been set + """ + if _active_backend_config is None: + raise BackendNotConfiguredError( + "No servo backend configured. Call set_active_backend() at startup." + ) + return _active_backend_config + + +# ============================================================================= +# Convenience functions for commonly accessed config values +# ============================================================================= + +def get_encoder_resolution() -> int: + """Get the encoder resolution (max value) for the active backend.""" + return get_config().SERVO_VALUE_MAX + + +def get_encoder_center() -> int: + """Get the encoder center value for the active backend.""" + return get_config().SERVO_VALUE_CENTER + + +def get_default_baud_rate() -> int: + """Get the default baud rate for the active backend.""" + return get_config().DEFAULT_BAUD_RATE + + +def get_default_pid_gains() -> tuple[int, int, int]: + """Get the default PID gains (Kp, Ki, Kd) for the active backend.""" + cfg = get_config() + return (cfg.DEFAULT_KP, cfg.DEFAULT_KI, cfg.DEFAULT_KD) + + +def get_serial_read_timeout() -> float: + """Get the serial read timeout for the active backend.""" + return get_config().SERIAL_READ_TIMEOUT + + +def is_configured() -> bool: + """Check if a backend CONFIG has been set.""" + return _active_backend_config is not None + + +def is_instance_set() -> bool: + """Check if a backend INSTANCE has been created and set.""" + return _active_backend_instance is not None + + +def list_available_backends() -> list[str]: + """List all available backend names.""" + # Return union of config modules and registered classes + return list(set(BACKEND_CONFIG_MODULES.keys()) | set(BACKEND_CLASSES.keys())) + + +# ============================================================================= +# Backend Instance Management +# ============================================================================= + +def create_backend( + backend_name: str, + robot_config: dict, + serial_port: Optional[str] = None, + baud_rate: Optional[int] = None, +) -> 'ActuatorBackend': + """ + Create a new backend instance for the specified backend type. + + This creates but does NOT set the active instance. Call set_active_backend_instance() + to make it the active backend for all modules. + + Args: + backend_name: Name of the backend (e.g., "feetech", "simulation") + robot_config: Dictionary containing robot configuration: + - servo_ids: List of physical servo IDs + - logical_to_physical_map: Dict mapping logical joint to physical servo indices + - inverted_servo_ids: Set of servo IDs using inverted mapping + - joint_limits_rad: List of [min, max] limits per logical joint + - gripper_servo_id: Optional gripper servo ID + - pid_gains: Optional dict mapping servo_id to (kp, ki, kd) + serial_port: Optional serial port path (for hardware backends) + baud_rate: Optional baud rate (for hardware backends) + + Returns: + ActuatorBackend: The created backend instance (not yet initialized) + + Raises: + ValueError: If the backend name is not registered + """ + if backend_name not in BACKEND_CLASSES: + available = ", ".join(BACKEND_CLASSES.keys()) + raise ValueError( + f"Unknown backend class '{backend_name}'. " + f"Available: {available}. " + f"Make sure the backend is registered in backends/__init__.py" + ) + + factory = BACKEND_CLASSES[backend_name] + + # Build kwargs based on backend type + kwargs = {"robot_config": robot_config} + if serial_port is not None: + kwargs["serial_port"] = serial_port + if baud_rate is not None: + kwargs["baud_rate"] = baud_rate + + backend = factory(**kwargs) + print(f"[Backend Registry] Created {backend_name} backend instance") + return backend + + +def set_active_backend_instance(backend: 'ActuatorBackend') -> None: + """ + Set the active backend instance. + + This makes the given backend available to all modules via get_active_backend(). + The backend should already be initialized before calling this. + + Args: + backend: The ActuatorBackend instance to make active + """ + global _active_backend_instance + _active_backend_instance = backend + print(f"[Backend Registry] Active backend instance set: {type(backend).__name__}") + + +def get_active_backend() -> 'ActuatorBackend': + """ + Get the active backend instance. + + Use this to perform servo I/O operations like sync_write, sync_read, etc. + + Returns: + ActuatorBackend: The active backend instance + + Raises: + BackendInstanceNotSetError: If no backend instance has been set + + Example: + backend = registry.get_active_backend() + backend.sync_write([(10, 2048, 100, 0), (20, 2048, 100, 0)]) + """ + if _active_backend_instance is None: + raise BackendInstanceNotSetError( + "No backend instance set. Call create_backend() and " + "set_active_backend_instance() at startup in run_controller.py" + ) + return _active_backend_instance + + +def shutdown_backend() -> None: + """ + Shutdown and clear the active backend instance. + + This should be called during cleanup (e.g., when run_controller exits). + """ + global _active_backend_instance + if _active_backend_instance is not None: + try: + _active_backend_instance.shutdown() + except Exception as e: + print(f"[Backend Registry] Error during backend shutdown: {e}") + _active_backend_instance = None + print("[Backend Registry] Backend instance cleared") + + +# ============================================================================= +# Telemetry functions +# ============================================================================= + +def get_telemetry_blocks() -> list[tuple[int, int]]: + """ + Get the telemetry register blocks for the active backend. + + Returns: + list[tuple[int, int]]: List of (start_address, length) tuples + """ + cfg = get_config() + return [ + (cfg.TELEMETRY_BLOCK1_ADDRESS, cfg.TELEMETRY_BLOCK1_LENGTH), + (cfg.TELEMETRY_BLOCK2_ADDRESS, cfg.TELEMETRY_BLOCK2_LENGTH), + (cfg.TELEMETRY_BLOCK3_ADDRESS, cfg.TELEMETRY_BLOCK3_LENGTH), + ] + + +def parse_telemetry_block(block_index: int, data: bytes) -> dict: + """ + Parse telemetry data for a specific block. + + Args: + block_index: Which block (0, 1, or 2) to parse + data: Raw bytes from the block read + + Returns: + dict: Parsed telemetry data + """ + cfg = get_config() + + if block_index == 0: + return cfg.parse_telemetry_block1(data) + elif block_index == 1: + return cfg.parse_telemetry_block2(data) + elif block_index == 2: + return cfg.parse_telemetry_block3(data) + + return {} + diff --git a/src/gradient_os/arm_controller/backends/simulation/__init__.py b/src/gradient_os/arm_controller/backends/simulation/__init__.py new file mode 100644 index 00000000..645a9d81 --- /dev/null +++ b/src/gradient_os/arm_controller/backends/simulation/__init__.py @@ -0,0 +1,9 @@ +# backends/simulation/__init__.py +# +# Simulation backend for GradientOS - no hardware required. +# Useful for development, testing, and debugging. + +from .backend import SimulationBackend + +__all__ = ['SimulationBackend'] + diff --git a/src/gradient_os/arm_controller/backends/simulation/backend.py b/src/gradient_os/arm_controller/backends/simulation/backend.py new file mode 100644 index 00000000..8e399977 --- /dev/null +++ b/src/gradient_os/arm_controller/backends/simulation/backend.py @@ -0,0 +1,276 @@ +# backends/simulation/backend.py +# +# Simulation backend for GradientOS. +# Provides an in-memory simulation of the actuator system for development and testing. + +from typing import Optional, TYPE_CHECKING +import math +import numpy as np + +from ..registry import get_encoder_resolution +from ...actuator_interface import ActuatorBackend + +if TYPE_CHECKING: + from ...robots.base import RobotConfig + + +class SimulationBackend(ActuatorBackend): + """ + In-memory simulation backend for development and testing. + + This backend simulates the behavior of physical actuators without requiring + actual hardware. It's useful for: + - Development and debugging of motion planning algorithms + - Testing trajectory execution logic + - Running the controller UI without a physical robot + + The simulation maintains joint positions in radians internally and converts + to/from raw encoder values as needed to match the interface expected by + other parts of the system. + """ + + def __init__( + self, + robot_config: Optional[dict] = None, + num_joints: Optional[int] = None, + has_gripper: Optional[bool] = None, + encoder_resolution: Optional[int] = None, + ): + """ + Initialize the simulation backend. + + Can be initialized either with a robot_config dict (preferred) or with + explicit parameters. If robot_config is provided, its values are used + as defaults, but explicit parameters will override them. + + Args: + robot_config: Optional dict from RobotConfig.get_config_dict() to get parameters from. + num_joints: Number of logical joints to simulate (default: 6 or from robot_config). + has_gripper: Whether to simulate a gripper (default: False or from robot_config). + encoder_resolution: Encoder resolution to simulate (default: 4095 for 12-bit). + """ + # Get defaults from robot_config if provided + if robot_config is not None: + if isinstance(robot_config, dict): + default_num_joints = robot_config.get('num_logical_joints', 6) + default_has_gripper = robot_config.get('has_gripper', False) + else: + # Assume it's a RobotConfig object with attributes + default_num_joints = robot_config.num_logical_joints + default_has_gripper = robot_config.has_gripper + else: + # No robot config provided - require explicit parameters + if num_joints is None: + raise ValueError("SimulationBackend requires either robot_config or explicit num_joints parameter") + default_num_joints = num_joints + default_has_gripper = False + + # For encoder resolution, get from the active servo backend via registry + try: + default_encoder_resolution = get_encoder_resolution() + except Exception: + default_encoder_resolution = 4095 # Fallback to common 12-bit encoder + + # Apply overrides + self._num_joints = num_joints if num_joints is not None else default_num_joints + self._has_gripper = has_gripper if has_gripper is not None else default_has_gripper + self._encoder_resolution = encoder_resolution if encoder_resolution is not None else default_encoder_resolution + + self._robot_config = robot_config + self._positions = [0.0] * self._num_joints + self._gripper_position = 0.0 + self._initialized = False + + def initialize(self) -> bool: + self._initialized = True + print(f"[Sim] Simulation backend initialized with {self._num_joints} joints") + return True + + def shutdown(self) -> None: + self._initialized = False + print("[Sim] Simulation backend shut down") + + @property + def num_joints(self) -> int: + return self._num_joints + + @property + def is_initialized(self) -> bool: + return self._initialized + + @property + def encoder_resolution(self) -> int: + """Returns the configured encoder resolution.""" + return self._encoder_resolution + + def set_joint_positions( + self, + positions_rad: list[float], + speed: float, + acceleration: float, + ) -> None: + if len(positions_rad) != self._num_joints: + raise ValueError(f"Expected {self._num_joints} positions, got {len(positions_rad)}") + self._positions = list(positions_rad) + + def get_joint_positions(self, verbose: bool = False) -> list[float]: + if verbose: + print(f"[Sim] Current positions: {np.round(self._positions, 3)}") + return list(self._positions) + + def prepare_sync_write_commands( + self, + positions_rad: list[float], + speed: int = 4095, + accel: int = 0, + ) -> list[tuple]: + # In simulation, just return the positions as-is + return [(i, pos, speed, accel) for i, pos in enumerate(positions_rad)] + + def sync_write(self, commands: list[tuple]) -> None: + # Update positions from the commands + # Commands are in format [(servo_id, raw_pos, speed, accel), ...] + # We need to map servo_id to joint index + actuator_ids = self._robot_config.get('actuator_ids', []) if self._robot_config else [] + logical_to_physical = self._robot_config.get('logical_to_physical_map', {}) if self._robot_config else {} + gripper_id = self._robot_config.get('gripper_actuator_id') if self._robot_config else None + + # Build reverse mapping: servo_id -> logical_joint_index + servo_to_joint = {} + for logical_idx, physical_indices in logical_to_physical.items(): + for phys_idx in physical_indices: + if phys_idx < len(actuator_ids): + servo_id = actuator_ids[phys_idx] + servo_to_joint[servo_id] = logical_idx + + for servo_id_or_idx, raw_pos, _, _ in commands: + # Check if it's a servo ID or already an index + if servo_id_or_idx in servo_to_joint: + joint_idx = servo_to_joint[servo_id_or_idx] + if 0 <= joint_idx < self._num_joints: + # Convert raw position to radians for storage + center = self._encoder_resolution // 2 + self._positions[joint_idx] = (raw_pos - center) * math.pi / center + elif servo_id_or_idx == gripper_id: + # Handle gripper + center = self._encoder_resolution // 2 + self._gripper_position = (raw_pos - center) * math.pi / center + elif 0 <= servo_id_or_idx < self._num_joints: + # Fallback: treat as index for backward compatibility + center = self._encoder_resolution // 2 + self._positions[servo_id_or_idx] = (raw_pos - center) * math.pi / center + + def sync_read_positions(self, servo_ids: list[int] = None, timeout_s: Optional[float] = None) -> dict[int, int]: + # Return simulated raw values using configured encoder resolution + # Keys are servo IDs (matching the real hardware behavior) + center = self._encoder_resolution // 2 + scale = center / math.pi + + actuator_ids = self._robot_config.get('actuator_ids', []) if self._robot_config else [] + logical_to_physical = self._robot_config.get('logical_to_physical_map', {}) if self._robot_config else {} + + # Build mapping: servo_id -> logical_joint_index (just the primary servo per joint) + servo_to_joint = {} + for logical_idx, physical_indices in logical_to_physical.items(): + if physical_indices and physical_indices[0] < len(actuator_ids): + servo_id = actuator_ids[physical_indices[0]] + servo_to_joint[servo_id] = logical_idx + + result = {} + target_ids = servo_ids if servo_ids else list(servo_to_joint.keys()) + + for servo_id in target_ids: + if servo_id in servo_to_joint: + joint_idx = servo_to_joint[servo_id] + if 0 <= joint_idx < len(self._positions): + result[servo_id] = int(self._positions[joint_idx] * scale + center) + + return result + + def raw_to_joint_positions(self, raw_positions: dict[int, int]) -> list[float]: + result = [0.0] * self._num_joints + center = self._encoder_resolution // 2 + scale = math.pi / center + for idx, raw in raw_positions.items(): + if 0 <= idx < self._num_joints: + result[idx] = (raw - center) * scale + return result + + def set_single_actuator_position( + self, + actuator_id: int, + position_rad: float, + speed: int, + accel: int, + ) -> None: + gripper_id = self._robot_config.get('gripper_actuator_id') if self._robot_config else 100 + if actuator_id == gripper_id and self._has_gripper: + self._gripper_position = position_rad + return + + # Map servo ID to joint index + joint_idx = self._servo_id_to_joint_index(actuator_id) + if joint_idx is not None and 0 <= joint_idx < self._num_joints: + self._positions[joint_idx] = position_rad + + def read_single_actuator_position(self, actuator_id: int) -> Optional[int]: + center = self._encoder_resolution // 2 + scale = center / math.pi + gripper_id = self._robot_config.get('gripper_actuator_id') if self._robot_config else 100 + + if actuator_id == gripper_id and self._has_gripper: + return int(self._gripper_position * scale + center) + + # Map servo ID to joint index + joint_idx = self._servo_id_to_joint_index(actuator_id) + if joint_idx is not None and 0 <= joint_idx < self._num_joints: + return int(self._positions[joint_idx] * scale + center) + + return None + + def _servo_id_to_joint_index(self, servo_id: int) -> Optional[int]: + """Map a servo ID to its logical joint index.""" + if not self._robot_config: + return servo_id if 0 <= servo_id < self._num_joints else None + + actuator_ids = self._robot_config.get('actuator_ids', []) + logical_to_physical = self._robot_config.get('logical_to_physical_map', {}) + + for logical_idx, physical_indices in logical_to_physical.items(): + for phys_idx in physical_indices: + if phys_idx < len(actuator_ids) and actuator_ids[phys_idx] == servo_id: + return logical_idx + return None + + def set_current_position_as_zero(self, actuator_id: int) -> bool: + print(f"[Sim] Set zero for actuator {actuator_id}") + return True + + def set_pid_gains(self, actuator_id: int, kp: int, ki: int, kd: int) -> bool: + print(f"[Sim] Set PID for actuator {actuator_id}: Kp={kp}, Ki={ki}, Kd={kd}") + return True + + def apply_joint_limits(self) -> bool: + print("[Sim] Applied joint limits (simulated)") + return True + + @property + def has_gripper(self) -> bool: + return self._has_gripper + + @property + def gripper_actuator_id(self) -> Optional[int]: + return 100 if self._has_gripper else None + + def get_gripper_position(self) -> Optional[float]: + return self._gripper_position if self._has_gripper else None + + def get_present_actuator_ids(self) -> set[int]: + ids = set(range(self._num_joints)) + if self._has_gripper: + ids.add(100) + return ids + + def ping_actuator(self, actuator_id: int) -> bool: + return actuator_id in self.get_present_actuator_ids() + diff --git a/src/gradient_os/arm_controller/command_api.py b/src/gradient_os/arm_controller/command_api.py index 3b0a4c03..c9edf79d 100644 --- a/src/gradient_os/arm_controller/command_api.py +++ b/src/gradient_os/arm_controller/command_api.py @@ -20,7 +20,6 @@ from . import utils from . import servo_driver -from . import servo_protocol from . import trajectory_execution from . import pid_tuner @@ -1030,8 +1029,8 @@ def handle_get_gripper_state(sock: 'socket.socket', addr: tuple): print(f"[Controller] Received GET_GRIPPER_STATE from {addr}.") - # Read the raw position from the servo - raw_pos = servo_protocol.read_servo_position(utils.SERVO_ID_GRIPPER) + # Read the raw position from the servo (uses backend if available) + raw_pos = servo_driver.read_single_servo_position(utils.SERVO_ID_GRIPPER) if raw_pos is not None: # Convert raw position to angle in degrees diff --git a/src/gradient_os/arm_controller/robot_config.py b/src/gradient_os/arm_controller/robot_config.py new file mode 100644 index 00000000..9cfd212b --- /dev/null +++ b/src/gradient_os/arm_controller/robot_config.py @@ -0,0 +1,256 @@ +# robot_config.py +# +# Robot configuration module for GradientOS. +# +# This module provides backward-compatible access to robot configuration values. +# It imports from the new modular robot configuration system (robots/) and +# re-exports values as module-level constants for existing code. +# +# IMPORTANT: set_active_robot() MUST be called before accessing any values. +# This is done in run_controller.py at startup. +# +# For new code, prefer using the RobotConfig class directly: +# from gradient_os.arm_controller.robots import get_robot_config +# robot = get_robot_config("gradient0") + +import math +import numpy as np +from typing import Optional + +# Import the robot configuration class +from .robots import RobotConfig, get_robot_config + +# The active robot instance - MUST be set by set_active_robot() before use +_robot: Optional[RobotConfig] = None + + +def _ensure_robot_configured(): + """Raise an error if no robot has been configured.""" + if _robot is None: + raise RuntimeError( + "No robot configured. Call robot_config.set_active_robot() first. " + "This is normally done in run_controller.py at startup." + ) + +# ============================================================================= +# Module-level constants for backward compatibility +# These are initialized to None and populated by set_active_robot() +# ============================================================================= + +# --- Robot Identity --- +ROBOT_NAME = None +ROBOT_VERSION = None + +# --- Kinematic Configuration --- +NUM_LOGICAL_JOINTS = None +NUM_PHYSICAL_SERVOS = None + +# --- Servo Configuration --- +SERVO_IDS = None +SERVO_ID_GRIPPER = None +SERVO_ID_JOINT_2_SECOND = None +SERVO_ID_JOINT_3_SECOND = None + +# --- Joint Mapping --- +LOGICAL_TO_PHYSICAL_MAP = None + +# --- Joint Limits --- +LOGICAL_JOINT_LIMITS_RAD = None +GRIPPER_LIMITS_RAD = None +URDF_JOINT_LIMITS = None + +# --- Angle Mapping --- +EFFECTIVE_MAPPING_RANGES = None +INVERTED_SERVO_IDS = None + +# --- Calibration --- +LOGICAL_JOINT_MASTER_OFFSETS_RAD = None + +# --- Motion Parameters --- +DEFAULT_SERVO_SPEED = None +DEFAULT_SERVO_ACCELERATION_DEG_S2 = None +ACCELERATION_SCALE_FACTOR = None + +# --- Trapezoidal Profile --- +DEFAULT_PROFILE_VELOCITY = None +DEFAULT_PROFILE_ACCELERATION = None + +# --- Closed-Loop Control --- +CORRECTION_KP_GAIN = None +CORRECTION_KI_GAIN = None +CORRECTION_INTEGRAL_CLAMP_RAD = None + +# --- Communication --- +SERIAL_PORT = None +PI_IP = None +UDP_PORT = None +BUFFER_SIZE = None + +# --- PID Gains --- +DEFAULT_KP = None +DEFAULT_KI = None +DEFAULT_KD = None +DEFAULT_PID_GAINS = None +J1_PID_GAINS = None +J2_PID_GAINS = None +J3_PID_GAINS = None +J4_PID_GAINS = None +J5_PID_GAINS = None +J6_PID_GAINS = None +GRIPPER_PID_GAINS = None + +# --- Servo-specific (from backend) --- +BAUD_RATE = None + +# ============================================================================= +# Helper Functions (Backward Compatibility) +# ============================================================================= + +def _is_servo_direct_mapping(physical_servo_config_index: int) -> bool: + """ + Check if a servo uses direct mapping (raw increases with angle). + + Args: + physical_servo_config_index: Index in SERVO_IDS list. + + Returns: + bool: True if direct, False if inverted. + """ + _ensure_robot_configured() + return _robot.is_actuator_direct_mapping(physical_servo_config_index) + + +def get_robot_config_dict() -> dict: + """ + Get the robot configuration as a dictionary. + + This is useful for passing configuration to ActuatorBackend or serialization. + + Returns: + dict: Configuration dictionary. + """ + _ensure_robot_configured() + return _robot.get_config_dict() + + +def get_active_robot() -> RobotConfig: + """ + Get the currently active robot configuration instance. + + Returns: + RobotConfig: The active robot configuration. + + Raises: + RuntimeError: If no robot has been configured. + """ + _ensure_robot_configured() + return _robot + + +def set_active_robot(robot: RobotConfig) -> None: + """ + Set the active robot configuration. + + This updates ALL module-level constants to match the new robot. + MUST be called before any other code accesses robot configuration. + + Args: + robot: The new robot configuration to use. + """ + global _robot + global ROBOT_NAME, ROBOT_VERSION + global NUM_LOGICAL_JOINTS, NUM_PHYSICAL_SERVOS, SERVO_IDS + global SERVO_ID_GRIPPER, SERVO_ID_JOINT_2_SECOND, SERVO_ID_JOINT_3_SECOND + global LOGICAL_TO_PHYSICAL_MAP, LOGICAL_JOINT_LIMITS_RAD, GRIPPER_LIMITS_RAD + global URDF_JOINT_LIMITS, EFFECTIVE_MAPPING_RANGES, INVERTED_SERVO_IDS + global LOGICAL_JOINT_MASTER_OFFSETS_RAD, SERIAL_PORT, PI_IP, UDP_PORT, BUFFER_SIZE + global DEFAULT_SERVO_SPEED, DEFAULT_SERVO_ACCELERATION_DEG_S2, ACCELERATION_SCALE_FACTOR + global DEFAULT_PROFILE_VELOCITY, DEFAULT_PROFILE_ACCELERATION + global CORRECTION_KP_GAIN, CORRECTION_KI_GAIN, CORRECTION_INTEGRAL_CLAMP_RAD + global DEFAULT_KP, DEFAULT_KI, DEFAULT_KD, DEFAULT_PID_GAINS + global J1_PID_GAINS, J2_PID_GAINS, J3_PID_GAINS, J4_PID_GAINS, J5_PID_GAINS, J6_PID_GAINS + global GRIPPER_PID_GAINS, BAUD_RATE + + _robot = robot + + # --- Robot Identity --- + ROBOT_NAME = robot.name + ROBOT_VERSION = robot.version + + # --- Kinematic Configuration --- + NUM_LOGICAL_JOINTS = robot.num_logical_joints + NUM_PHYSICAL_SERVOS = robot.num_physical_actuators + + # --- Servo Configuration --- + SERVO_IDS = robot.actuator_ids + SERVO_ID_GRIPPER = robot.gripper_actuator_id + _twin = robot.twin_motor_actuator_ids + SERVO_ID_JOINT_2_SECOND = _twin.get(1) + SERVO_ID_JOINT_3_SECOND = _twin.get(2) + + # --- Joint Mapping --- + LOGICAL_TO_PHYSICAL_MAP = robot.logical_to_physical_map + + # --- Joint Limits --- + LOGICAL_JOINT_LIMITS_RAD = robot.logical_joint_limits_rad + GRIPPER_LIMITS_RAD = list(robot.gripper_limits_rad) if robot.gripper_limits_rad else [0, math.pi] + URDF_JOINT_LIMITS = robot.actuator_limits_rad + + # --- Angle Mapping --- + EFFECTIVE_MAPPING_RANGES = robot.actuator_mapping_ranges_rad + INVERTED_SERVO_IDS = robot.inverted_actuator_ids + + # --- Calibration --- + LOGICAL_JOINT_MASTER_OFFSETS_RAD = robot.logical_joint_master_offsets_rad + + # --- Motion Parameters --- + DEFAULT_SERVO_SPEED = robot.default_speed + DEFAULT_SERVO_ACCELERATION_DEG_S2 = robot.default_acceleration_deg_s2 + ACCELERATION_SCALE_FACTOR = robot.acceleration_scale_factor + + # --- Trapezoidal Profile --- + DEFAULT_PROFILE_VELOCITY = robot.default_profile_velocity + DEFAULT_PROFILE_ACCELERATION = robot.default_profile_acceleration + + # --- Closed-Loop Control --- + CORRECTION_KP_GAIN = robot.correction_kp_gain + CORRECTION_KI_GAIN = robot.correction_ki_gain + CORRECTION_INTEGRAL_CLAMP_RAD = robot.correction_integral_clamp_rad + + # --- Communication --- + SERIAL_PORT = robot.default_serial_port + PI_IP = robot.udp_listen_ip + UDP_PORT = robot.udp_port + BUFFER_SIZE = robot.udp_buffer_size + + # --- PID Gains --- + _dpid = robot.default_pid_gains + DEFAULT_KP = _dpid[0] + DEFAULT_KI = _dpid[1] + DEFAULT_KD = _dpid[2] + DEFAULT_PID_GAINS = robot.actuator_pid_gains + + # Per-joint gains (using actuator IDs from this robot's config) + _pid = robot.actuator_pid_gains + _actuator_ids = robot.actuator_ids + if len(_actuator_ids) >= 9: # Standard 6DOF + gripper robot + J1_PID_GAINS = _pid.get(_actuator_ids[0], _dpid) + J2_PID_GAINS = _pid.get(_actuator_ids[1], _dpid) + J3_PID_GAINS = _pid.get(_actuator_ids[3], _dpid) + J4_PID_GAINS = _pid.get(_actuator_ids[5], _dpid) + J5_PID_GAINS = _pid.get(_actuator_ids[6], _dpid) + J6_PID_GAINS = _pid.get(_actuator_ids[7], _dpid) + GRIPPER_PID_GAINS = _pid.get(_actuator_ids[8], _dpid) if robot.gripper_actuator_id else _dpid + + # --- Servo-specific (from backend registry) --- + from .backends import registry as backend_registry + if backend_registry.is_configured(): + BAUD_RATE = backend_registry.get_default_baud_rate() + + print(f"[RobotConfig] Active robot set to: {robot.name} v{robot.version}") + + # Reinitialize utils module state (e.g., current_logical_joint_angles_rad) + # Populate utils with robot-specific constants from the active robot config. + # This must be called AFTER all the globals are set above. + from . import utils + utils._populate_robot_constants() diff --git a/src/gradient_os/arm_controller/robots/__init__.py b/src/gradient_os/arm_controller/robots/__init__.py new file mode 100644 index 00000000..90255925 --- /dev/null +++ b/src/gradient_os/arm_controller/robots/__init__.py @@ -0,0 +1,85 @@ +# robots/__init__.py +# +# Robot configuration package for GradientOS. +# This package contains configuration classes for different robot models. +# +# Each robot has its own subfolder with a config.py defining a RobotConfig subclass. +# The base RobotConfig class provides the interface that all robots must implement. +# +# Supported Robots: +# ----------------- +# - Gradient0: Original 6-DOF arm with 9 Feetech servos (8 arm + 1 gripper) +# - [Future] Gradient0.5: Next iteration with improvements +# +# Usage: +# ------ +# ```python +# from gradient_os.arm_controller.robots import Gradient0Config, RobotConfig +# +# # Get the config for Gradient0 +# robot = Gradient0Config() +# +# # Or get by name +# robot = get_robot_config("gradient0") +# ``` + +from .base import RobotConfig +from .gradient0 import Gradient0Config + +# Registry of available robot configurations +_ROBOT_REGISTRY: dict[str, type[RobotConfig]] = { + 'gradient0': Gradient0Config, +} + + +def get_robot_config(robot_name: str) -> RobotConfig: + """ + Get a robot configuration instance by name. + + Args: + robot_name: Name of the robot (case-insensitive) + + Returns: + RobotConfig: An instance of the robot's configuration class + + Raises: + ValueError: If the robot name is not recognized + """ + name_lower = robot_name.lower() + if name_lower not in _ROBOT_REGISTRY: + available = ', '.join(_ROBOT_REGISTRY.keys()) + raise ValueError(f"Unknown robot '{robot_name}'. Available: {available}") + return _ROBOT_REGISTRY[name_lower]() + + +def list_available_robots() -> list[str]: + """ + List all available robot configurations. + + Returns: + list[str]: Names of available robots + """ + return list(_ROBOT_REGISTRY.keys()) + + +def register_robot(name: str, config_class: type[RobotConfig]) -> None: + """ + Register a new robot configuration. + + This allows external packages to add robot configurations at runtime. + + Args: + name: Name to register the robot under + config_class: The RobotConfig subclass to register + """ + _ROBOT_REGISTRY[name.lower()] = config_class + + +__all__ = [ + 'RobotConfig', + 'Gradient0Config', + 'get_robot_config', + 'list_available_robots', + 'register_robot', +] + diff --git a/src/gradient_os/arm_controller/robots/base.py b/src/gradient_os/arm_controller/robots/base.py new file mode 100644 index 00000000..f5cdd2d9 --- /dev/null +++ b/src/gradient_os/arm_controller/robots/base.py @@ -0,0 +1,590 @@ +# robots/base.py +# +# Abstract base class defining the interface for robot configurations. +# Each robot (gradient0, gradient0_5, etc.) must implement this interface +# to provide its kinematic structure, servo mappings, and motion parameters. +# +# This abstraction allows GradientOS to support multiple robot designs +# by simply swapping the robot configuration module. + +from abc import ABC, abstractmethod +from typing import Optional +import math + + +class RobotConfig(ABC): + """ + Abstract base class defining the interface for robot-specific configuration. + + A robot configuration defines the physical and kinematic properties of a robot: + - Number of joints and their limits + - Mapping from logical joints to physical actuators + - Calibration offsets + - Motion parameters + - Gripper configuration (if applicable) + + The configuration is independent of the actuator type (Feetech, Dynamixel, etc.). + Actuator-specific settings (encoder resolution, protocol constants, etc.) are + handled by the ActuatorBackend implementation. + + To add a new robot: + 1. Create a new subfolder under robots/ (e.g., robots/my_new_robot/) + 2. Create a config.py that defines a class inheriting from RobotConfig + 3. Implement all abstract methods/properties + 4. Update robots/__init__.py to expose the new config + + Example Usage: + -------------- + ```python + from gradient_os.arm_controller.robots import Gradient0Config + + robot = Gradient0Config() + print(f"Robot has {robot.num_logical_joints} joints") + print(f"Joint 1 limits: {robot.logical_joint_limits_rad[0]}") + ``` + """ + + # ========================================================================= + # Robot Identity + # ========================================================================= + + @property + @abstractmethod + def name(self) -> str: + """ + Human-readable name of the robot. + + Returns: + str: Robot name (e.g., "Gradient0", "Gradient0.5") + """ + pass + + @property + @abstractmethod + def version(self) -> str: + """ + Version string for this robot configuration. + + Returns: + str: Version (e.g., "1.0.0") + """ + pass + + # ========================================================================= + # Kinematic Structure + # ========================================================================= + + @property + @abstractmethod + def num_logical_joints(self) -> int: + """ + Number of controllable joints in the kinematic model. + + This is the number of degrees of freedom for motion planning. + Twin-motor joints still count as a single logical joint. + + Returns: + int: Number of logical joints (typically 6 for a 6-DOF arm) + """ + pass + + @property + @abstractmethod + def num_physical_actuators(self) -> int: + """ + Total number of physical actuators (servos/motors) in the robot. + + This includes all arm actuators plus gripper if present. + For robots with twin-motor joints, this will be > num_logical_joints. + + Returns: + int: Total number of physical actuators + """ + pass + + @property + @abstractmethod + def actuator_ids(self) -> list[int]: + """ + Hardware IDs of all actuators in the robot. + + The order matters - it defines the "config index" used for looking up + per-actuator settings like mapping ranges and limits. + + Returns: + list[int]: List of actuator hardware IDs + """ + pass + + @property + @abstractmethod + def logical_to_physical_map(self) -> dict[int, list[int]]: + """ + Mapping from logical joint index to physical actuator config indices. + + This defines how each logical joint (used by motion planning) maps to + one or more physical actuators. For twin-motor joints, a single logical + joint maps to two physical actuator indices. + + The values are indices into the actuator_ids list, not the IDs themselves. + + Example: + { + 0: [0], # Logical J1 -> actuator_ids[0] only + 1: [1, 2], # Logical J2 -> actuator_ids[1] and actuator_ids[2] + ... + } + + Returns: + dict[int, list[int]]: Mapping from logical joint to actuator indices + """ + pass + + @property + def twin_motor_actuator_ids(self) -> dict[int, int]: + """ + Map of logical joint index to secondary actuator ID for twin-motor joints. + + For robots with twin-motor joints (two motors driving one joint for + increased torque), this identifies which joints have a secondary motor + and what its ID is. + + Returns: + dict[int, int]: Map from logical joint index to secondary actuator ID. + Empty dict if no twin-motor joints. + """ + return {} # Default: no twin-motor joints + + @property + def logical_joint_to_actuator_ids(self) -> dict[int, list[int]]: + """ + Mapping from 1-based logical joint numbers to actual actuator IDs. + + This is a convenience property that combines logical_to_physical_map + and actuator_ids to produce a ready-to-use mapping for commands like + SET_ZERO that work with joint numbers. + + Example for Gradient0: + { + 1: [10], # Joint 1 (base) -> Servo ID 10 + 2: [20, 21], # Joint 2 (shoulder) -> Servo IDs 20, 21 + 3: [30, 31], # Joint 3 (elbow) -> Servo IDs 30, 31 + 4: [40], # Joint 4 (wrist roll) -> Servo ID 40 + 5: [50], # Joint 5 (wrist pitch) -> Servo ID 50 + 6: [60], # Joint 6 (wrist yaw) -> Servo ID 60 + 7: [100], # Joint 7 (gripper) -> Servo ID 100 (if present) + } + + Returns: + dict[int, list[int]]: Map from 1-based joint number to actuator IDs + """ + result = {} + actuator_ids = self.actuator_ids + + # Map arm joints (1-based) + for logical_idx, config_indices in self.logical_to_physical_map.items(): + joint_num = logical_idx + 1 # Convert 0-based to 1-based + result[joint_num] = [actuator_ids[i] for i in config_indices] + + # Add gripper as joint 7 if present + if self.gripper_actuator_id is not None: + gripper_joint = self.num_logical_joints + 1 # Typically 7 + result[gripper_joint] = [self.gripper_actuator_id] + + return result + + @property + @abstractmethod + def default_servo_backend(self) -> str: + """ + The default servo backend for this robot. + + This specifies which actuator backend to use if not explicitly + overridden via command-line arguments. + + Each robot MUST declare its servo backend explicitly. + + Returns: + str: Backend name (e.g., "feetech", "dynamixel") + """ + pass + + # ========================================================================= + # Joint Limits + # ========================================================================= + + @property + @abstractmethod + def logical_joint_limits_rad(self) -> list[tuple[float, float]]: + """ + Safe motion limits for each logical joint in radians. + + These define the software limits used by motion planning. + Each tuple is (min_limit, max_limit) in radians. + + Returns: + list[tuple[float, float]]: List of (min, max) limits per joint + """ + pass + + @property + @abstractmethod + def actuator_limits_rad(self) -> list[tuple[float, float]]: + """ + Motion limits for each physical actuator in radians. + + These are written to the actuator's EEPROM for hardware-level limit enforcement. + For twin-motor joints, both actuators get the same limits as their logical joint. + + Returns: + list[tuple[float, float]]: List of (min, max) limits per actuator + """ + pass + + # ========================================================================= + # Angle Mapping + # ========================================================================= + + @property + @abstractmethod + def actuator_mapping_ranges_rad(self) -> list[tuple[float, float]]: + """ + The angular range that maps to the full encoder range for each actuator. + + Most servos map their full encoder range (e.g., 0-4095) to a specific + angular range. This is typically (-π, +π) for 360° rotation, but may + differ for limited-rotation servos. + + Each tuple is (min_angle, max_angle) in radians, where: + - min_angle maps to encoder value 0 (or 4095 for inverted servos) + - max_angle maps to encoder value 4095 (or 0 for inverted servos) + + Returns: + list[tuple[float, float]]: Angular range per actuator + """ + pass + + @property + @abstractmethod + def inverted_actuator_ids(self) -> set[int]: + """ + Set of actuator IDs that use inverted mapping. + + For inverted actuators, the encoder value DECREASES when the angle + INCREASES. This is determined by how the actuator is physically mounted. + + Returns: + set[int]: Set of actuator IDs with inverted mapping + """ + pass + + def is_actuator_direct_mapping(self, actuator_config_index: int) -> bool: + """ + Check if an actuator uses direct mapping (encoder increases with angle). + + Args: + actuator_config_index: Index of the actuator in actuator_ids + + Returns: + bool: True if direct mapping, False if inverted + """ + actuator_id = self.actuator_ids[actuator_config_index] + return actuator_id not in self.inverted_actuator_ids + + # ========================================================================= + # Calibration + # ========================================================================= + + @property + @abstractmethod + def logical_joint_master_offsets_rad(self) -> list[float]: + """ + Master calibration offsets per logical joint in radians. + + These offsets are added to commanded angles before conversion to + actuator values. They compensate for mechanical assembly variations. + + Returns: + list[float]: Offset in radians for each logical joint + """ + pass + + # ========================================================================= + # Gripper Configuration + # ========================================================================= + + @property + def has_gripper(self) -> bool: + """ + Check if this robot has a gripper. + + Returns: + bool: True if gripper is configured + """ + return self.gripper_actuator_id is not None + + @property + def gripper_actuator_id(self) -> Optional[int]: + """ + Hardware ID of the gripper actuator. + + Returns: + Optional[int]: Gripper actuator ID, or None if no gripper + """ + return None + + @property + def gripper_limits_rad(self) -> Optional[tuple[float, float]]: + """ + Motion limits for the gripper in radians. + + Returns: + Optional[tuple[float, float]]: (min, max) limits, or None if no gripper + """ + return None + + # ========================================================================= + # Motion Parameters (Default values, can be overridden) + # ========================================================================= + + @property + def default_speed(self) -> int: + """ + Default movement speed for actuators. + + The units are actuator-specific (typically 0-4095 for servos). + + Returns: + int: Default speed value + """ + return 500 + + @property + def default_acceleration_deg_s2(self) -> float: + """ + Default acceleration in degrees per second squared. + + Returns: + float: Default acceleration value + """ + return 500.0 + + @property + def acceleration_scale_factor(self) -> float: + """ + Factor for converting deg/s² to actuator register value. + + register_value = acceleration_deg_s2 / acceleration_scale_factor + + Returns: + float: Scale factor for acceleration conversion + """ + return 100.0 + + # ========================================================================= + # Communication Configuration + # ========================================================================= + + @property + def default_serial_port(self) -> str: + """ + Default serial port path for actuator communication. + + Returns: + str: Serial port path (e.g., "/dev/ttyUSB0") + """ + return "/dev/ttyUSB0" + + # ========================================================================= + # UDP Configuration (for remote control) + # ========================================================================= + + @property + def udp_listen_ip(self) -> str: + """ + IP address to listen on for UDP commands. + + Returns: + str: IP address (e.g., "0.0.0.0" for all interfaces) + """ + return "0.0.0.0" + + @property + def udp_port(self) -> int: + """ + UDP port for receiving commands. + + Returns: + int: Port number + """ + return 3000 + + @property + def udp_buffer_size(self) -> int: + """ + Buffer size for UDP receive operations. + + Returns: + int: Buffer size in bytes + """ + return 1024 + + # ========================================================================= + # Closed-Loop Control Parameters + # ========================================================================= + + @property + def correction_kp_gain(self) -> float: + """ + Proportional gain for closed-loop trajectory correction. + + Set to 0.0 to disable (rely on actuator's internal PID only). + + Returns: + float: Proportional gain + """ + return 0.0 + + @property + def correction_ki_gain(self) -> float: + """ + Integral gain for closed-loop trajectory correction. + + Set to 0.0 to disable. + + Returns: + float: Integral gain + """ + return 0.0 + + @property + def correction_integral_clamp_rad(self) -> float: + """ + Anti-windup clamp for integral term in radians. + + Returns: + float: Maximum integral accumulation + """ + return 0.6 + + # ========================================================================= + # Trapezoidal Profile Defaults + # ========================================================================= + + @property + def default_profile_velocity(self) -> float: + """ + Default velocity for trapezoidal motion profiles in m/s. + + Returns: + float: Velocity in meters per second + """ + return 0.1 + + @property + def default_profile_acceleration(self) -> float: + """ + Default acceleration for trapezoidal motion profiles in m/s². + + Returns: + float: Acceleration in meters per second squared + """ + return 0.05 + + # ========================================================================= + # PID Configuration + # ========================================================================= + + @property + def default_pid_gains(self) -> tuple[int, int, int]: + """ + Default PID gains (Kp, Ki, Kd) for actuators not specified individually. + + These defaults come from the active servo backend configuration. Override in + robot subclasses if different defaults are needed for your robot. + + Returns: + tuple[int, int, int]: (Kp, Ki, Kd) gains + """ + # Get defaults from the active servo backend via registry + from ..backends import registry as backend_registry + return backend_registry.get_default_pid_gains() + + @property + def actuator_pid_gains(self) -> dict[int, tuple[int, int, int]]: + """ + Per-actuator PID gains (Kp, Ki, Kd). + + Override this in robot subclasses to provide tuned PID gains + for each actuator. These gains depend on both the robot's mechanical + characteristics and the specific actuator type being used. + + Returns: + dict[int, tuple[int, int, int]]: Map from actuator ID to (Kp, Ki, Kd) + """ + # Default: use default_pid_gains for all actuators + default = self.default_pid_gains + return {aid: default for aid in self.actuator_ids} + + # ========================================================================= + # Utility Methods + # ========================================================================= + + def get_config_dict(self) -> dict: + """ + Export the robot configuration as a dictionary. + + This is useful for passing configuration to the ActuatorBackend + or for serialization. + + Returns: + dict: Configuration dictionary + """ + return { + 'name': self.name, + 'version': self.version, + 'num_logical_joints': self.num_logical_joints, + 'num_physical_actuators': self.num_physical_actuators, + 'actuator_ids': self.actuator_ids, + 'logical_to_physical_map': self.logical_to_physical_map, + 'logical_joint_limits_rad': self.logical_joint_limits_rad, + 'actuator_limits_rad': self.actuator_limits_rad, + 'actuator_mapping_ranges_rad': self.actuator_mapping_ranges_rad, + 'inverted_actuator_ids': self.inverted_actuator_ids, + 'logical_joint_master_offsets_rad': self.logical_joint_master_offsets_rad, + 'has_gripper': self.has_gripper, + 'gripper_actuator_id': self.gripper_actuator_id, + 'gripper_limits_rad': self.gripper_limits_rad, + 'default_speed': self.default_speed, + 'default_acceleration_deg_s2': self.default_acceleration_deg_s2, + 'default_serial_port': self.default_serial_port, + 'actuator_pid_gains': self.actuator_pid_gains, + 'default_servo_backend': self.default_servo_backend, + } + + def get_actuator_config_index(self, actuator_id: int) -> Optional[int]: + """ + Get the config index for an actuator ID. + + Args: + actuator_id: Hardware ID of the actuator + + Returns: + Optional[int]: Index in actuator_ids, or None if not found + """ + try: + return self.actuator_ids.index(actuator_id) + except ValueError: + return None + + def get_arm_actuator_ids(self) -> list[int]: + """ + Get actuator IDs for the arm only (excluding gripper). + + Returns: + list[int]: List of arm actuator IDs + """ + if self.gripper_actuator_id is not None: + return [aid for aid in self.actuator_ids if aid != self.gripper_actuator_id] + return list(self.actuator_ids) + diff --git a/src/gradient_os/arm_controller/robots/gradient0/__init__.py b/src/gradient_os/arm_controller/robots/gradient0/__init__.py new file mode 100644 index 00000000..26b0c698 --- /dev/null +++ b/src/gradient_os/arm_controller/robots/gradient0/__init__.py @@ -0,0 +1,9 @@ +# robots/gradient0/__init__.py +# +# Gradient0 robot configuration package. +# The Gradient0 is the original 6-DOF robotic arm with Feetech STS3215 servos. + +from .config import Gradient0Config + +__all__ = ['Gradient0Config'] + diff --git a/src/gradient_os/arm_controller/robots/gradient0/config.py b/src/gradient_os/arm_controller/robots/gradient0/config.py new file mode 100644 index 00000000..e1a73f39 --- /dev/null +++ b/src/gradient_os/arm_controller/robots/gradient0/config.py @@ -0,0 +1,402 @@ +# robots/gradient0/config.py +# +# Configuration for the Gradient0 robotic arm. +# +# The Gradient0 is a 6-DOF robotic arm with the following specifications: +# - 6 logical joints (base, shoulder, elbow, wrist roll, wrist pitch, wrist yaw) +# - 9 physical actuators (Feetech STS3215 servos): +# - Joint 1 (Base): 1 servo (ID 10) +# - Joint 2 (Shoulder): 2 twin servos (IDs 20, 21) +# - Joint 3 (Elbow): 2 twin servos (IDs 30, 31) +# - Joints 4-6 (Wrist): 3 servos (IDs 40, 50, 60) +# - Gripper: 1 servo (ID 100) +# +# Note: This configuration is independent of the servo type. Servo-specific +# settings (encoder resolution, protocol constants, PID gains) are defined +# in the servo backend (e.g., backends/feetech/config.py). + +import math +from typing import Optional + +from ..base import RobotConfig + + +class Gradient0Config(RobotConfig): + """ + Configuration for the Gradient0 6-DOF robotic arm. + + This class defines all robot-specific parameters including: + - Kinematic structure (6 logical joints, 9 physical servos) + - Joint limits and mapping ranges + - Servo orientation (direct vs inverted mounting) + - Calibration offsets + - Motion parameters + + The Gradient0 uses Feetech STS3215 servos, but this configuration + is actuator-agnostic - it could theoretically work with any actuator + backend that implements the ActuatorBackend interface. + """ + + # ========================================================================= + # Robot Identity + # ========================================================================= + + @property + def name(self) -> str: + return "Gradient0" + + @property + def version(self) -> str: + return "1.0.0" + + @property + def default_servo_backend(self) -> str: + """ + The Gradient0 uses Feetech STS3215 servos. + """ + return "feetech" + + # ========================================================================= + # Kinematic Structure + # ========================================================================= + + @property + def num_logical_joints(self) -> int: + """Gradient0 has 6 degrees of freedom (6 controllable joints).""" + return 6 + + @property + def num_physical_actuators(self) -> int: + """ + Gradient0 has 9 physical servos: + - 1 for J1 (base) + - 2 for J2 (shoulder, twin motors) + - 2 for J3 (elbow, twin motors) + - 1 for J4 (wrist roll) + - 1 for J5 (wrist pitch) + - 1 for J6 (wrist yaw) + - 1 for gripper + """ + return 9 + + @property + def actuator_ids(self) -> list[int]: + """ + Hardware IDs of all servos in order. + + The order defines the "config index" for each servo: + - Index 0: ID 10 (J1) + - Index 1: ID 20 (J2 primary) + - Index 2: ID 21 (J2 secondary) + - Index 3: ID 30 (J3 primary) + - Index 4: ID 31 (J3 secondary) + - Index 5: ID 40 (J4) + - Index 6: ID 50 (J5) + - Index 7: ID 60 (J6) + - Index 8: ID 100 (Gripper) + """ + return [10, 20, 21, 30, 31, 40, 50, 60, 100] + + @property + def logical_to_physical_map(self) -> dict[int, list[int]]: + """ + Maps each logical joint index to its physical servo config indices. + + Joint 2 (shoulder) and Joint 3 (elbow) use twin motors for increased torque. + Both motors in a pair receive the same position command. + """ + return { + 0: [0], # J1 (Base) -> Servo index 0 (ID 10) + 1: [1, 2], # J2 (Shoulder) -> Servo indices 1, 2 (IDs 20, 21) + 2: [3, 4], # J3 (Elbow) -> Servo indices 3, 4 (IDs 30, 31) + 3: [5], # J4 (Wrist Roll) -> Servo index 5 (ID 40) + 4: [6], # J5 (Wrist Pitch) -> Servo index 6 (ID 50) + 5: [7], # J6 (Wrist Yaw) -> Servo index 7 (ID 60) + } + + @property + def twin_motor_actuator_ids(self) -> dict[int, int]: + """ + Map of logical joint index to secondary actuator ID for twin-motor joints. + + On the Gradient0, joints 2 and 3 use twin motors for increased torque. + This property identifies the secondary motor for each twin-motor joint. + + Returns: + dict[int, int]: Map from logical joint index to secondary actuator ID + """ + return { + 1: 21, # J2 secondary motor + 2: 31, # J3 secondary motor + } + + # ========================================================================= + # Joint Limits + # ========================================================================= + + @property + def logical_joint_limits_rad(self) -> list[tuple[float, float]]: + """ + Motion limits for each logical joint in radians. + + These are the software limits used by motion planning. + Hardware limits (written to servo EEPROM) may be slightly different. + """ + return [ + (-math.pi, math.pi), # J1 (Base): ±180° + (-math.pi/2, math.pi/2), # J2 (Shoulder): ±90° + (-math.pi/2, math.pi/2), # J3 (Elbow): ±90° + (-math.pi, math.pi), # J4 (Wrist Roll): ±180° + (-1.8326, 2.0944), # J5 (Wrist Pitch): ~-105° to +120° + (-math.pi, math.pi), # J6 (Wrist Yaw): ±180° + ] + + @property + def actuator_limits_rad(self) -> list[tuple[float, float]]: + """ + Motion limits for each physical servo in radians. + + For twin-motor joints, both servos get the same limits as their logical joint. + These values are written to the servos' EEPROM for hardware-level enforcement. + """ + logical = self.logical_joint_limits_rad + return [ + logical[0], # Servo 10 (J1) + logical[1], # Servo 20 (J2) + logical[1], # Servo 21 (J2) - same as primary + logical[2], # Servo 30 (J3) + logical[2], # Servo 31 (J3) - same as primary + logical[3], # Servo 40 (J4) + logical[4], # Servo 50 (J5) + logical[5], # Servo 60 (J6) + (0, math.pi), # Servo 100 (Gripper): 0° to 180° + ] + + @property + def gripper_limits_rad(self) -> Optional[tuple[float, float]]: + """Gripper motion limits: 0 (closed) to π (fully open).""" + return (0, math.pi) + + # ========================================================================= + # Angle Mapping + # ========================================================================= + + @property + def actuator_mapping_ranges_rad(self) -> list[tuple[float, float]]: + """ + Angular range that maps to the full encoder range for each servo. + + The Feetech STS3215 servos have a 360° rotation range that maps + to encoder values 0-4095. This corresponds to ±π radians from center. + """ + return [ + (-math.pi, math.pi), # Servo 10 (J1) + (-math.pi, math.pi), # Servo 20 (J2) + (-math.pi, math.pi), # Servo 21 (J2) + (-math.pi, math.pi), # Servo 30 (J3) + (-math.pi, math.pi), # Servo 31 (J3) + (-math.pi, math.pi), # Servo 40 (J4) + (-math.pi, math.pi), # Servo 50 (J5) + (-math.pi, math.pi), # Servo 60 (J6) + (-math.pi, math.pi), # Servo 100 (Gripper) + ] + + @property + def inverted_actuator_ids(self) -> set[int]: + """ + Set of servo IDs with inverted mounting. + + For inverted servos, the encoder value DECREASES when the joint + angle INCREASES. This is determined by how the servo is physically + mounted on the robot. + + On the Gradient0, most servos are inverted due to their mounting orientation. + """ + return {10, 20, 30, 40, 50, 60, 100} + + # ========================================================================= + # Calibration + # ========================================================================= + + @property + def logical_joint_master_offsets_rad(self) -> list[float]: + """ + Master calibration offsets for each logical joint in radians. + + These are applied to commanded angles before conversion to servo values. + They compensate for minor assembly variations without requiring + individual servo recalibration. + + These offsets are typically adjusted during initial robot setup + to ensure the arm's home position matches the CAD model. + """ + return [ + 0.0, # J1 (Base) + 0.0, # J2 (Shoulder) + 0.0, # J3 (Elbow) + 0.0, # J4 (Wrist Roll) + 0.0, # J5 (Wrist Pitch) + 0.0, # J6 (Wrist Yaw) + ] + + # ========================================================================= + # Gripper Configuration + # ========================================================================= + + @property + def gripper_actuator_id(self) -> Optional[int]: + """Hardware ID of the gripper servo.""" + return 100 + + # ========================================================================= + # Motion Parameters + # ========================================================================= + + @property + def default_speed(self) -> int: + """ + Default servo speed (0-4095 scale). + + 500 provides a moderate speed suitable for most operations. + Higher values = faster movement. + """ + return 500 + + @property + def default_acceleration_deg_s2(self) -> float: + """ + Default acceleration in degrees per second squared. + + 500 deg/s² provides smooth acceleration/deceleration. + """ + return 500.0 + + @property + def acceleration_scale_factor(self) -> float: + """ + Factor for converting deg/s² to servo register value. + + For Feetech STS3215 servos: + - Register accepts 0-254 (0 = maximum acceleration) + - register_value = deg/s² / 100 + + Example: 500 deg/s² -> register value 5 + """ + return 100.0 + + # ========================================================================= + # Communication + # ========================================================================= + + @property + def default_serial_port(self) -> str: + """Default serial port for USB-TTL adapter.""" + return "/dev/ttyUSB0" + + @property + def udp_listen_ip(self) -> str: + """Listen on all interfaces for remote control.""" + return "0.0.0.0" + + @property + def udp_port(self) -> int: + """UDP port for receiving motion commands.""" + return 3000 + + @property + def udp_buffer_size(self) -> int: + """UDP receive buffer size in bytes.""" + return 1024 + + # ========================================================================= + # Closed-Loop Control + # ========================================================================= + + @property + def correction_kp_gain(self) -> float: + """ + Proportional gain for trajectory correction. + + Set to 0.0 to rely solely on the servo's internal PID controller. + This is often preferable during servo PID tuning. + """ + return 0.0 + + @property + def correction_ki_gain(self) -> float: + """ + Integral gain for trajectory correction. + + Set to 0.0 to disable integral correction. + """ + return 0.0 + + @property + def correction_integral_clamp_rad(self) -> float: + """ + Anti-windup clamp for integral term in radians. + + 0.6 rad ≈ ±34° maximum integral accumulation. + """ + return 0.6 + + # ========================================================================= + # Trapezoidal Profile Defaults + # ========================================================================= + + @property + def default_profile_velocity(self) -> float: + """Default velocity for trapezoidal profiles: 0.1 m/s.""" + return 0.1 + + @property + def default_profile_acceleration(self) -> float: + """Default acceleration for trapezoidal profiles: 0.05 m/s².""" + return 0.05 + + # ========================================================================= + # PID Gains (Robot-specific tuning for Feetech STS3215 servos) + # ========================================================================= + + @property + def default_pid_gains(self) -> tuple[int, int, int]: + """ + Default PID gains (Kp, Ki, Kd) if not specified per-actuator. + + These values are tuned for Feetech STS3215 servos on the Gradient0 arm. + """ + return (50, 1, 30) + + @property + def actuator_pid_gains(self) -> dict[int, tuple[int, int, int]]: + """ + Per-actuator PID gains (Kp, Ki, Kd) tuned for this robot. + + These gains are optimized for the Gradient0's specific mechanical + characteristics (weight distribution, gear ratios, mounting, etc.) + with Feetech STS3215 servos. + + Returns: + dict[int, tuple[int, int, int]]: Map from actuator ID to (Kp, Ki, Kd) + """ + # Per-joint tuned gains + j1_gains = (50, 1, 20) # Base joint + j2_gains = (65, 1, 30) # Shoulder (higher P for heavier load) + j3_gains = (50, 1, 25) # Elbow + j4_gains = (50, 0, 10) # Wrist roll (lighter, less I needed) + j5_gains = (50, 0, 10) # Wrist pitch + j6_gains = (50, 0, 10) # Wrist yaw + gripper_gains = (40, 0, 10) # Gripper (lower P for softer grip) + + return { + 10: j1_gains, + 20: j2_gains, + 21: j2_gains, # Twin motor, same gains + 30: j3_gains, + 31: j3_gains, # Twin motor, same gains + 40: j4_gains, + 50: j5_gains, + 60: j6_gains, + 100: gripper_gains, + } + diff --git a/src/gradient_os/arm_controller/servo_driver.py b/src/gradient_os/arm_controller/servo_driver.py index 8d2a2532..7abf856b 100644 --- a/src/gradient_os/arm_controller/servo_driver.py +++ b/src/gradient_os/arm_controller/servo_driver.py @@ -1,15 +1,69 @@ # Contains high-level functions for controlling the servos, acting as a -# hardware abstraction layer. Imports from servo_protocol.py. +# hardware abstraction layer. Imports from servo_protocol.py. +# +# This module provides backward-compatible functions that work with the existing +# codebase while also supporting the new ActuatorBackend abstraction layer. +# +# Migration Status (Phase 2): +# --------------------------- +# Functions in this module are being migrated to use the ActuatorBackend interface. +# Each function checks if a backend is available and uses it when possible, +# falling back to legacy servo_protocol calls otherwise. +# +# For new code, prefer using the backend directly: +# from gradient_os.arm_controller.backends import registry +# backend = registry.get_active_backend() +# backend.set_joint_positions(angles, speed, accel) + import time import serial import numpy as np import os import json import glob -from typing import Iterable, Optional +from typing import Iterable, Optional, TYPE_CHECKING from . import utils from . import servo_protocol +from . import robot_config + +if TYPE_CHECKING: + from .actuator_interface import ActuatorBackend + + +# ============================================================================= +# Backend Accessor +# ============================================================================= + +def _get_backend() -> Optional['ActuatorBackend']: + """ + Get the active ActuatorBackend instance from the registry. + + Returns None if no backend is configured (allows fallback to legacy code). + This enables gradual migration - functions can check for a backend and + use it if available, otherwise fall back to servo_protocol. + + Returns: + Optional[ActuatorBackend]: The active backend, or None. + """ + try: + from .backends import registry + if registry.is_instance_set(): + return registry.get_active_backend() + except Exception: + pass + return None + + +def _use_backend() -> bool: + """ + Check if we should use the new backend system. + + Returns True if a backend instance is available and initialized. + Functions can use this for quick checks before deciding which path to take. + """ + backend = _get_backend() + return backend is not None and backend.is_initialized def _env_truthy(var_name: str, default: bool = False) -> bool: """Return True if the given env var is set to a truthy value.""" @@ -304,8 +358,48 @@ def initialize_servos(): """ Initializes the serial connection to the servos, checks for their presence, and sets their default PID gains. This function must be called once at application startup. + + Migration Note: + --------------- + If an ActuatorBackend is available and initialized, this function will use it + to populate the present servo IDs and gripper status. The backend handles + serial port management, pinging, and PID configuration internally. + + For legacy operation (no backend), this function opens the serial port directly + and performs all initialization via servo_protocol. """ print("[Pi] Initializing servos...") + + # Check if we have a backend available + backend = _get_backend() + if backend is not None and backend.is_initialized: + # --- Backend-based initialization --- + print("[Pi] Using ActuatorBackend for initialization...") + + # Get present servo IDs from the backend + present_servo_ids = list(backend.get_present_actuator_ids()) + + # Update the servo_protocol cache for compatibility with legacy code + for s_id in present_servo_ids: + servo_protocol._present_servo_ids.add(s_id) + + # Check for gripper + gripper_id = backend.gripper_actuator_id + if gripper_id and gripper_id in present_servo_ids: + utils.gripper_present = True + print(f"[Pi] Gripper (ID {gripper_id}) is present.") + else: + utils.gripper_present = False + print(f"[Pi] Gripper is ABSENT.") + + # The backend's serial port should be accessible for legacy code + if hasattr(backend, 'serial_port') and backend.serial_port: + utils.ser = backend.serial_port + + print("[Pi] Servos initialized via backend.") + return + + # --- Legacy initialization (no backend) --- resolved_port = _resolve_serial_port() if resolved_port: utils.SERIAL_PORT = resolved_port @@ -384,7 +478,38 @@ def set_single_servo_position_rads(servo_id: int, position_rad: float, speed: in """ Commands a single servo to a specified position in radians using the efficient SYNC_WRITE protocol, consistent with how the main arm is controlled. + + Migration Note: + --------------- + If an ActuatorBackend is available, uses backend.sync_write() which handles + the conversion internally. Otherwise falls back to legacy servo_protocol. """ + backend = _get_backend() + if backend and _use_backend(): + # Use backend - it expects raw values, so we need to convert + try: + config_index = utils.SERVO_IDS.index(servo_id) + except ValueError: + print(f"[Pi] ERROR: Servo ID {servo_id} not found in configuration.") + return + + raw_pos_value = angle_to_raw(position_rad, config_index) + clamped_speed = int(max(0, min(utils.ENCODER_RESOLUTION, speed))) + + # Convert acceleration to register value + accel_reg_val = 0 + if accel > 0: + accel_reg_val = int(round(accel / utils.ACCELERATION_SCALE_FACTOR)) + accel_reg_val = max(1, min(254, accel_reg_val)) + + # sync_write expects a list of tuples: [(servo_id, pos, speed, accel), ...] + backend.sync_write([(servo_id, raw_pos_value, clamped_speed, accel_reg_val)]) + + print(f"[Pi] Commanded single servo {servo_id} to {position_rad:.2f} rad ({raw_pos_value}) " + f"with Speed={clamped_speed}, AccelReg={accel_reg_val} (via backend)") + return + + # --- Fallback to legacy servo_protocol --- if utils.ser is None: print("[Pi] Serial port not initialized, cannot set single servo position.") return @@ -406,7 +531,7 @@ def set_single_servo_position_rads(servo_id: int, position_rad: float, speed: in raw_pos_value = angle_to_raw(position_rad, config_index) # Clamp the speed value. - clamped_speed = int(max(0, min(4095, speed))) + clamped_speed = int(max(0, min(utils.ENCODER_RESOLUTION, speed))) # Build the command tuple in the format the sync write function expects. command_tuple = (servo_id, raw_pos_value, clamped_speed, accel_reg_val) @@ -418,6 +543,29 @@ def set_single_servo_position_rads(servo_id: int, position_rad: float, speed: in f"with Speed={clamped_speed}, AccelReg={accel_reg_val}") +def read_single_servo_position(servo_id: int) -> Optional[int]: + """ + Read the raw position of a single servo. + + Args: + servo_id: The hardware ID of the servo. + + Returns: + The raw encoder value (0-4095 typically), or None if read failed. + + Migration Note: + --------------- + If an ActuatorBackend is available, uses backend.read_single_actuator_position(). + Otherwise falls back to legacy servo_protocol. + """ + backend = _get_backend() + if backend and _use_backend(): + return backend.read_single_actuator_position(servo_id) + + # Fallback to legacy servo_protocol + return servo_protocol.read_servo_position(servo_id) + + def set_servo_positions(logical_joint_angles_rad: list[float], speed_value: int, acceleration_value_deg_s2: float): """ Translates a list of 6 logical joint angles into the corresponding commands @@ -433,7 +581,33 @@ def set_servo_positions(logical_joint_angles_rad: list[float], speed_value: int, logical_joint_angles_rad (list[float]): A list of 6 joint angles in radians. speed_value (int): The speed for the move (0-4095). acceleration_value_deg_s2 (float): The acceleration for the move in deg/s^2. + + Migration Note: + --------------- + If an ActuatorBackend is available, uses backend.set_joint_positions() which + handles all the mapping and conversion internally. Otherwise falls back to + legacy servo_protocol-based writing. """ + # Check if we can use the backend + backend = _get_backend() + if backend is not None and backend.is_initialized: + # --- Backend-based position setting --- + if len(logical_joint_angles_rad) != backend.num_joints: + print(f"[Pi] Error: Expected {backend.num_joints} logical joint angles, got {len(logical_joint_angles_rad)}") + return + + # Update the global state + utils.current_logical_joint_angles_rad = list(logical_joint_angles_rad) + + # Use the backend + backend.set_joint_positions( + positions_rad=logical_joint_angles_rad, + speed=float(speed_value), + acceleration=acceleration_value_deg_s2, + ) + return + + # --- Legacy servo_protocol-based writing --- if utils.ser is None: print("[Pi] Serial port not initialized, cannot set positions.") return @@ -461,16 +635,10 @@ def set_servo_positions(logical_joint_angles_rad: list[float], speed_value: int, accel_reg_val_for_cycle = 0 # Explicitly set to 0 for max physical acceleration # Clamp the global speed value once - clamped_speed_value_for_cycle = int(max(0, min(4095, speed_value))) + clamped_speed_value_for_cycle = int(max(0, min(utils.ENCODER_RESOLUTION, speed_value))) - logical_to_physical_map = { - 0: [0], # Logical J1 -> Physical Servo ID 10 - 1: [1, 2], # Logical J2 -> Physical Servo IDs 20, 21 - 2: [3, 4], # Logical J3 -> Physical Servo IDs 30, 31 - 3: [5], # Logical J4 -> Physical Servo ID 40 - 4: [6], # Logical J5 -> Physical Servo ID 50 - 5: [7], # Logical J6 -> Physical Servo ID 60 - } + # Use robot configuration for logical-to-physical mapping + logical_to_physical_map = robot_config.LOGICAL_TO_PHYSICAL_MAP for logical_joint_index in range(utils.NUM_LOGICAL_JOINTS): # Master offset is still useful for high-level adjustments @@ -502,13 +670,13 @@ def set_servo_positions(logical_joint_angles_rad: list[float], speed_value: int, # Apply mapping direction if utils._is_servo_direct_mapping(physical_servo_config_index): - raw_servo_value = normalized_value * 4095.0 + raw_servo_value = normalized_value * utils.ENCODER_RESOLUTION else: - raw_servo_value = (1.0 - normalized_value) * 4095.0 + raw_servo_value = (1.0 - normalized_value) * utils.ENCODER_RESOLUTION # The calculation is now simpler as we don't add a software offset. final_servo_pos_value = int(round(raw_servo_value)) - final_servo_pos_value = max(0, min(4095, final_servo_pos_value)) + final_servo_pos_value = max(0, min(utils.ENCODER_RESOLUTION, final_servo_pos_value)) # --- IMPORTANT: Only command servos that are present --- if current_physical_servo_id not in servo_protocol.get_present_servo_ids(): @@ -555,18 +723,18 @@ def servo_value_to_radians(servo_value: int, physical_servo_config_index: int) - return 0.0 # Or some indicator of failure # Ensure servo_value is within expected bounds for safety in calculation, though it should be. - servo_value = max(0, min(4095, servo_value)) + servo_value = max(0, min(utils.ENCODER_RESOLUTION, servo_value)) min_map_rad, max_map_rad = utils.EFFECTIVE_MAPPING_RANGES[physical_servo_config_index] - # 1. Normalize the servo value from [0, 4095] to [0, 1] + # 1. Normalize the servo value from [0, ENCODER_RESOLUTION] to [0, 1] # Undo inversion based on the physical_servo_config_index is_direct_mapping = utils._is_servo_direct_mapping(physical_servo_config_index) if is_direct_mapping: - normalized_direct = servo_value / 4095.0 + normalized_direct = servo_value / utils.ENCODER_RESOLUTION else: # Standard inverted mapping - normalized_inverted = servo_value / 4095.0 + normalized_inverted = servo_value / utils.ENCODER_RESOLUTION normalized_direct = 1.0 - normalized_inverted # 3. De-normalize this direct value back to the radian range for that joint. @@ -577,7 +745,7 @@ def servo_value_to_radians(servo_value: int, physical_servo_config_index: int) - def angle_to_raw(angle_rad: float, physical_servo_config_index: int) -> int: """ - Converts a physical angle in radians to a raw servo value (0-4095). + Converts a physical angle in radians to a raw servo value (0-ENCODER_RESOLUTION). This is the core conversion logic used by both single and sync servo writes. """ min_map_rad, max_map_rad = utils.EFFECTIVE_MAPPING_RANGES[physical_servo_config_index] @@ -589,31 +757,31 @@ def angle_to_raw(angle_rad: float, physical_servo_config_index: int) -> int: # Apply mapping direction if utils._is_servo_direct_mapping(physical_servo_config_index): - raw_servo_value = normalized_value * 4095.0 + raw_servo_value = normalized_value * utils.ENCODER_RESOLUTION else: - raw_servo_value = (1.0 - normalized_value) * 4095.0 + raw_servo_value = (1.0 - normalized_value) * utils.ENCODER_RESOLUTION final_servo_pos_value = int(round(raw_servo_value)) - return max(0, min(4095, final_servo_pos_value)) + return max(0, min(utils.ENCODER_RESOLUTION, final_servo_pos_value)) def raw_to_angle_rad(raw_value: int, physical_servo_config_index: int) -> float: """ - Converts a raw servo value (0-4095) to a physical angle in radians. + Converts a raw servo value (0-ENCODER_RESOLUTION) to a physical angle in radians. This is the inverse of angle_to_raw. """ if raw_value is None: return 0.0 - servo_value = max(0, min(4095, raw_value)) + servo_value = max(0, min(utils.ENCODER_RESOLUTION, raw_value)) min_map_rad, max_map_rad = utils.EFFECTIVE_MAPPING_RANGES[physical_servo_config_index] is_direct = utils._is_servo_direct_mapping(physical_servo_config_index) if is_direct: - normalized = servo_value / 4095.0 + normalized = servo_value / utils.ENCODER_RESOLUTION else: - normalized = 1.0 - (servo_value / 4095.0) + normalized = 1.0 - (servo_value / utils.ENCODER_RESOLUTION) angle = normalized * (max_map_rad - min_map_rad) + min_map_rad return angle @@ -632,6 +800,11 @@ def set_servo_pid_gains(servo_id: int, kp: int, ki: int, kd: int) -> bool: Returns: bool: True on success, False on failure. + + Migration Note: + --------------- + If an ActuatorBackend is available, uses backend.set_pid_gains() which + handles the register writes internally. """ def _clamp_byte(value: int) -> int: try: @@ -652,6 +825,19 @@ def _clamp_byte(value: int) -> int: print(f"[Pi] PID inputs clamped to 0-254 for Servo {servo_id}: Kp={kp}, Ki={ki}, Kd={kd}") else: print(f"[Pi] Setting PID for Servo {servo_id}: Kp={kp}, Ki={ki}, Kd={kd}") + + # Check if we can use the backend + backend = _get_backend() + if backend is not None and backend.is_initialized: + # --- Backend-based PID setting --- + success = backend.set_pid_gains(servo_id, kp, ki, kd) + if success: + print(f"[Pi] Successfully set PID for Servo {servo_id} via backend") + else: + print(f"[Pi] PID setting FAILED for Servo {servo_id} via backend") + return success + + # --- Legacy servo_protocol-based PID setting --- # Note: PID registers are in EEPROM. Need to ensure EEPROM is unlocked if necessary. # The STSServoDriver.cpp unlocks EEPROM (reg 0x37) before writing to ID (0x05) or PosCorrection (0x1F). # For standard PID registers, direct write might be okay if servo isn't locked by default for these. @@ -739,10 +925,33 @@ def get_current_arm_state_rad(verbose: bool = True) -> list[float]: Returns: list[float]: A list of 6 joint angles in radians. + + Migration Note: + --------------- + If an ActuatorBackend is available, uses backend.get_joint_positions() which + handles all the servo reading and mapping internally. Otherwise falls back to + legacy servo_protocol-based reading. """ if verbose: print("[Pi] Getting current arm state...") + # Check if we can use the backend + backend = _get_backend() + if backend is not None and backend.is_initialized: + # --- Backend-based reading --- + current_logical_angles_rad = backend.get_joint_positions(verbose=verbose) + + # Update the global state with the newly read values + utils.current_logical_joint_angles_rad = current_logical_angles_rad + + if verbose: + angles_deg = np.rad2deg(current_logical_angles_rad) + print(f"[Pi] Current logical angles (deg): {np.round(angles_deg, 2)}") + + return current_logical_angles_rad + + # --- Legacy servo_protocol-based reading --- + # Use a single SYNC READ for efficiency, targeting only the arm servos (not gripper) arm_servo_ids = [sid for sid in utils.SERVO_IDS if sid != utils.SERVO_ID_GRIPPER] @@ -763,17 +972,11 @@ def get_current_arm_state_rad(verbose: bool = True) -> list[float]: # Convert raw servo values to logical joint angles in radians current_logical_angles_rad = [0.0] * utils.NUM_LOGICAL_JOINTS - # Define the mapping from logical joints to their corresponding physical servo IDs - logical_to_physical_map = { - 0: [10], - 1: [20, 21], - 2: [30, 31], - 3: [40], - 4: [50], - 5: [60], - } - - for logical_joint_index, physical_ids in logical_to_physical_map.items(): + # Get logical-to-physical mapping from robot config (not hardcoded) + # Build a map from logical joint index to servo IDs + logical_to_physical_id_map = _build_logical_to_servo_id_map() + + for logical_joint_index, physical_ids in logical_to_physical_id_map.items(): angles_for_this_joint = [] for servo_id in physical_ids: if servo_id in raw_positions: @@ -811,6 +1014,34 @@ def get_current_arm_state_rad(verbose: bool = True) -> list[float]: return current_logical_angles_rad +def _build_logical_to_servo_id_map() -> dict[int, list[int]]: + """ + Build a map from logical joint index (0-based) to physical servo IDs. + + This uses the robot configuration instead of hardcoded values. + + Returns: + dict[int, list[int]]: Maps logical joint index to list of servo IDs. + """ + # Get the logical_to_physical_map from robot config + # This maps logical joint index -> list of config indices + # NOTE: Must access robot_config directly (not utils) to get latest values + logical_to_physical = robot_config.LOGICAL_TO_PHYSICAL_MAP + servo_ids = robot_config.SERVO_IDS + + if logical_to_physical is None or servo_ids is None: + # Fallback if robot config not yet set + print("[Pi] Warning: Robot config not set, cannot build logical-to-servo map") + return {} + + result = {} + for logical_idx, physical_indices in logical_to_physical.items(): + # Convert config indices to servo IDs + result[logical_idx] = [servo_ids[idx] for idx in physical_indices] + + return result + + def set_current_position_as_hardware_zero(servo_id: int): """ Commands a servo to set its current physical position as its new zero point. @@ -818,7 +1049,33 @@ def set_current_position_as_hardware_zero(servo_id: int): Args: servo_id (int): The ID of the servo to calibrate. + + Migration Note: + --------------- + If an ActuatorBackend is available, uses backend.set_current_position_as_zero() + which handles the calibration command internally. """ + # Check if we can use the backend + backend = _get_backend() + if backend is not None and backend.is_initialized: + # --- Backend-based calibration --- + present_ids = backend.get_present_actuator_ids() + if servo_id not in present_ids: + print(f"[Pi] Cannot set zero for absent servo {servo_id}.") + return + + print(f"[Pi] Setting zero for servo {servo_id} via backend...") + if backend.set_current_position_as_zero(servo_id): + print(f"[Pi] Servo {servo_id} has set its current position as the new zero point.") + # Refresh limits for gripper + if servo_id == utils.SERVO_ID_GRIPPER: + print(f"[Pi] Refreshing gripper limits after zeroing...") + set_servo_angle_limits_from_urdf() + else: + print(f"[Pi] Failed to set zero for servo {servo_id}.") + return + + # --- Legacy servo_protocol-based calibration --- if utils.ser is None or not utils.ser.is_open: print(f"[Pi] Serial port not open. Cannot set zero for servo {servo_id}.") return @@ -991,10 +1248,8 @@ def logical_q_to_syncwrite_tuple(logical_joint_angles_rad: list[float], Returns: A list of tuples, where each tuple is (servo_id, position, speed, accel). """ - # This map needs to be kept in sync with the physical robot structure. - logical_to_physical_map = { - 0: [0], 1: [1, 2], 2: [3, 4], 3: [5], 4: [6], 5: [7], - } + # Use robot configuration for logical-to-physical mapping + logical_to_physical_map = robot_config.LOGICAL_TO_PHYSICAL_MAP commands = [] for logical_idx, physical_indices in logical_to_physical_map.items(): diff --git a/src/gradient_os/arm_controller/servo_protocol.py b/src/gradient_os/arm_controller/servo_protocol.py index 7beb387b..656f250c 100644 --- a/src/gradient_os/arm_controller/servo_protocol.py +++ b/src/gradient_os/arm_controller/servo_protocol.py @@ -1,48 +1,103 @@ -# Contains low-level functions for communicating with Feetech servos, -# such as packet creation and checksum calculation. +# ============================================================================= +# servo_protocol.py - DEPRECATED +# ============================================================================= +# +# WARNING: This module is deprecated and will be removed in a future release. +# For new code, use the ActuatorBackend interface directly: +# +# from gradient_os.arm_controller.backends import registry +# backend = registry.get_active_backend() +# backend.sync_read_positions(servo_ids) +# backend.sync_write(commands) +# +# This module now acts as a thin dispatcher that routes calls to the active +# backend when available, falling back to direct serial communication for +# legacy compatibility. +# +# ============================================================================= + from . import utils from ..telemetry import alerts import time import threading +import warnings +from typing import Optional +# Import backend registry for servo-specific constants and backend access +from .backends import registry as backend_registry -# Servo Protocol Constants -SERVO_HEADER = 0xFF -SERVO_INSTRUCTION_WRITE = 0x03 -SERVO_ADDR_TARGET_POSITION = 0x2A -SERVO_INSTRUCTION_READ = 0x02 -SERVO_ADDR_PRESENT_POSITION = 0x38 -SERIAL_READ_TIMEOUT = 0.05 -SERVO_ADDR_TARGET_ACCELERATION = 0x29 -DEFAULT_SERVO_ACCELERATION_DEG_S2 = 500 -ACCELERATION_SCALE_FACTOR = 100 -SERVO_ADDR_POS_KP = 0x15 -SERVO_ADDR_POS_KI = 0x17 -SERVO_ADDR_POS_KD = 0x16 -DEFAULT_KP = 32 -DEFAULT_KI = 0 -DEFAULT_KD = 0 -SERVO_INSTRUCTION_CALIBRATE_MIDDLE = 0x0B -SERVO_INSTRUCTION_RESET = 0x06 -SERVO_INSTRUCTION_RESTART = 0x08 -SERVO_INSTRUCTION_PING = 0x01 -SERVO_ADDR_WRITE_LOCK = 0x37 -SERVO_ADDR_MIN_ANGLE_LIMIT = 0x09 -SERVO_ADDR_MAX_ANGLE_LIMIT = 0x0B - - -# Servo Sync Write Constants -SERVO_INSTRUCTION_SYNC_WRITE = 0x83 -SERVO_BROADCAST_ID = 0xFE # Typically 0xFE for Feetech Sync Write -# Start address for Sync Write block (Accel, Pos, Time, Speed) -SYNC_WRITE_START_ADDRESS = SERVO_ADDR_TARGET_ACCELERATION # 0x29 -# Length of data block per servo in Sync Write: Accel (1) + Pos (2) + Time (2) + Speed (2) = 7 bytes -SYNC_WRITE_DATA_LEN_PER_SERVO = 7 -# Servo Sync Read Constants -SERVO_INSTRUCTION_SYNC_READ = 0x82 + +def _get_backend(): + """Get the active backend instance, or None if not set.""" + try: + return backend_registry.get_active_backend() + except backend_registry.BackendInstanceNotSetError: + return None + + +def _use_backend() -> bool: + """Returns True if an ActuatorBackend is active and initialized.""" + backend = _get_backend() + return backend is not None and backend.is_initialized + + +def _warn_deprecated(func_name: str): + """Emit a deprecation warning for servo_protocol usage.""" + warnings.warn( + f"servo_protocol.{func_name}() is deprecated. " + f"Use backends.registry.get_active_backend() methods instead.", + DeprecationWarning, + stacklevel=3 + ) + + +def _get_backend_config(): + """Get the active backend config. Raises if not configured.""" + return backend_registry.get_config() + + +# These are accessed as module-level constants but delegate to the backend. +# They're populated when first accessed after backend is configured. +def __getattr__(name): + """Lazy attribute access for backend constants.""" + cfg = _get_backend_config() + + _BACKEND_ATTRS = { + 'SERVO_HEADER': 'SERVO_HEADER', + 'SERVO_INSTRUCTION_WRITE': 'SERVO_INSTRUCTION_WRITE', + 'SERVO_ADDR_TARGET_POSITION': 'SERVO_ADDR_TARGET_POSITION', + 'SERVO_INSTRUCTION_READ': 'SERVO_INSTRUCTION_READ', + 'SERVO_ADDR_PRESENT_POSITION': 'SERVO_ADDR_PRESENT_POSITION', + 'SERIAL_READ_TIMEOUT': 'SERIAL_READ_TIMEOUT', + 'SERVO_ADDR_TARGET_ACCELERATION': 'SERVO_ADDR_TARGET_ACCELERATION', + 'DEFAULT_SERVO_ACCELERATION_DEG_S2': 'DEFAULT_SERVO_ACCELERATION_DEG_S2', + 'ACCELERATION_SCALE_FACTOR': 'ACCELERATION_SCALE_FACTOR', + 'SERVO_ADDR_POS_KP': 'SERVO_ADDR_POS_KP', + 'SERVO_ADDR_POS_KI': 'SERVO_ADDR_POS_KI', + 'SERVO_ADDR_POS_KD': 'SERVO_ADDR_POS_KD', + 'DEFAULT_KP': 'DEFAULT_KP', + 'DEFAULT_KI': 'DEFAULT_KI', + 'DEFAULT_KD': 'DEFAULT_KD', + 'SERVO_INSTRUCTION_CALIBRATE_MIDDLE': 'SERVO_INSTRUCTION_CALIBRATE_MIDDLE', + 'SERVO_INSTRUCTION_RESET': 'SERVO_INSTRUCTION_RESET', + 'SERVO_INSTRUCTION_RESTART': 'SERVO_INSTRUCTION_RESTART', + 'SERVO_INSTRUCTION_PING': 'SERVO_INSTRUCTION_PING', + 'SERVO_ADDR_WRITE_LOCK': 'SERVO_ADDR_WRITE_LOCK', + 'SERVO_ADDR_MIN_ANGLE_LIMIT': 'SERVO_ADDR_MIN_ANGLE_LIMIT', + 'SERVO_ADDR_MAX_ANGLE_LIMIT': 'SERVO_ADDR_MAX_ANGLE_LIMIT', + 'SERVO_INSTRUCTION_SYNC_WRITE': 'SERVO_INSTRUCTION_SYNC_WRITE', + 'SERVO_BROADCAST_ID': 'SERVO_BROADCAST_ID', + 'SYNC_WRITE_START_ADDRESS': 'SYNC_WRITE_START_ADDRESS', + 'SYNC_WRITE_DATA_LEN_PER_SERVO': 'SYNC_WRITE_DATA_LEN_PER_SERVO', + 'SERVO_INSTRUCTION_SYNC_READ': 'SERVO_INSTRUCTION_SYNC_READ', + } + + if name in _BACKEND_ATTRS: + return getattr(cfg, _BACKEND_ATTRS[name]) + + raise AttributeError(f"module '{__name__}' has no attribute '{name}'") -# A simple in-memory cache for which servos were detected at startup # A simple in-memory cache for which servos were detected at startup _present_servo_ids: set[int] = set() @@ -94,7 +149,19 @@ def ping(servo_id: int) -> bool: Returns: bool: True if a valid status packet is received, False otherwise. + + .. deprecated:: + Use ``backend.ping_actuator(servo_id)`` instead. """ + # Dispatch to backend if available + backend = _get_backend() + if backend and hasattr(backend, 'ping_actuator'): + result = backend.ping_actuator(servo_id) + if result: + _present_servo_ids.add(servo_id) + return result + + # Fallback to direct serial communication if utils.ser is None or not utils.ser.is_open: return False @@ -199,9 +266,10 @@ def send_servo_command(servo_id: int, position_value: int, speed_value: int = No print("[Pi] Serial port not initialized.") return - # Clamp position and speed to their valid ranges - pos_val_clamped = int(max(0, min(4095, position_value))) - spd_val_clamped = int(max(0, min(4095, speed_value))) + # Clamp position and speed to their valid ranges (encoder resolution from backend) + encoder_max = utils.ENCODER_RESOLUTION + pos_val_clamped = int(max(0, min(encoder_max, position_value))) + spd_val_clamped = int(max(0, min(encoder_max, speed_value))) # Goal Position (Address 0x2A), 2 bytes for position, 2 bytes for "time" (set to 0), 2 bytes for speed # This means we are writing 6 bytes starting at address 0x2A. @@ -392,7 +460,16 @@ def read_servo_position(servo_id: int) -> int | None: Returns: int | None: The servo's current raw position (0-4095), or None on failure. + + .. deprecated:: + Use ``backend.read_single_actuator_position(servo_id)`` instead. """ + # Dispatch to backend if available + backend = _get_backend() + if backend and hasattr(backend, 'read_single_actuator_position'): + return backend.read_single_actuator_position(servo_id) + + # Fallback to direct serial communication if utils.ser is None or not utils.ser.is_open: print(f"[Pi ReadPos] Servo {servo_id}: Serial port not open.") return None @@ -641,7 +718,17 @@ def sync_write_goal_pos_speed_accel(servo_data_list: list[tuple[int, int, int, i Args: servo_data_list: A list of tuples, where each tuple contains: (servo_id, position_value, speed_value, acceleration_register_value) + + .. deprecated:: + Use ``backend.sync_write(commands)`` instead. """ + # Dispatch to backend if available + backend = _get_backend() + if backend and hasattr(backend, 'sync_write'): + backend.sync_write(servo_data_list) + return + + # Fallback to direct serial communication if utils.ser is None or not utils.ser.is_open: print("[Pi SyncWrite] Serial port not initialized.") return @@ -751,7 +838,16 @@ def factory_reset_servo(servo_id: int) -> bool: Returns: bool: True on success, False on failure. + + .. deprecated:: + Use ``backend.factory_reset_actuator(servo_id)`` instead. """ + # Dispatch to backend if available + backend = _get_backend() + if backend and hasattr(backend, 'factory_reset_actuator'): + return backend.factory_reset_actuator(servo_id) + + # Fallback to direct serial communication if utils.ser is None: print(f"[Pi] Serial port not initialized for factory reset command.") return False @@ -787,7 +883,16 @@ def restart_servo(servo_id: int) -> bool: Returns: bool: True on success, False on failure. + + .. deprecated:: + Use ``backend.restart_actuator(servo_id)`` instead. """ + # Dispatch to backend if available + backend = _get_backend() + if backend and hasattr(backend, 'restart_actuator'): + return backend.restart_actuator(servo_id) + + # Fallback to direct serial communication if utils.ser is None: print(f"[Pi] Serial port not initialized for restart command.") return False @@ -836,7 +941,16 @@ def sync_read_positions( Returns: dict[int, int]: A dictionary mapping servo_id to its raw position. This may be a partial result if some servos did not respond. + + .. deprecated:: + Use ``backend.sync_read_positions(servo_ids)`` instead. """ + # Dispatch to backend if available + backend = _get_backend() + if backend and hasattr(backend, 'sync_read_positions'): + return backend.sync_read_positions(servo_ids, timeout_s=timeout_s) + + # Fallback to direct serial communication if utils.ser is None or not utils.ser.is_open: print("[Pi SyncRead] Serial port not initialized.") return {} @@ -1140,7 +1254,16 @@ def sync_read_block( Returns a mapping of servo_id -> raw bytes (length == data_len) for all servos that responded with a valid status packet. Missing IDs indicate no valid response was parsed. + + .. deprecated:: + Use ``backend.sync_read_block(servo_ids, start_address, data_len)`` instead. """ + # Dispatch to backend if available + backend = _get_backend() + if backend and hasattr(backend, 'sync_read_block'): + return backend.sync_read_block(servo_ids, start_address=start_address, data_len=data_len) + + # Fallback to direct serial communication if utils.ser is None or not utils.ser.is_open: print("[Pi SyncReadBlk] Serial port not initialized.") return {} diff --git a/src/gradient_os/arm_controller/sim_backend.py b/src/gradient_os/arm_controller/sim_backend.py index 31f4716f..0ff4ec9d 100644 --- a/src/gradient_os/arm_controller/sim_backend.py +++ b/src/gradient_os/arm_controller/sim_backend.py @@ -1,11 +1,19 @@ """ -Lightweight in-process simulator for the Gradient controller. +DEPRECATED: Lightweight in-process simulator for the Gradient controller. -When activated, this module monkey patches the low-level `servo_driver` and -`servo_protocol` helpers with in-memory implementations so the existing -controller loop can run without physical hardware. The goal is to provide the -minimal surface area needed for development and testing, while keeping the -overrides modular so a richer simulator can replace them later. +This module is DEPRECATED. Use the new backend-based simulation instead: + + from gradient_os.arm_controller.backends import registry + + # Create simulation backend + backend = registry.create_backend("simulation", robot_config_dict) + registry.set_active_backend_instance(backend) + backend.initialize() + +The new approach provides a cleaner architecture that integrates with the +ActuatorBackend interface rather than monkey-patching servo_driver/servo_protocol. + +This module is kept for backward compatibility but will be removed in a future release. """ from __future__ import annotations @@ -15,6 +23,7 @@ import numpy as np from . import servo_driver, servo_protocol, utils +from .backends import registry as backend_registry class _DummySerial: @@ -65,18 +74,21 @@ def reset(self) -> None: def set_raw(self, servo_id: int, raw_value: int) -> None: with self._lock: - clamped = max(0, min(4095, int(raw_value))) + encoder_max = backend_registry.get_encoder_resolution() + clamped = max(0, min(encoder_max, int(raw_value))) self.raw_positions[servo_id] = clamped self._refresh_globals() def get_raw(self, servo_id: int) -> int: with self._lock: - return self.raw_positions.get(servo_id, 2048) + encoder_center = backend_registry.get_encoder_center() + return self.raw_positions.get(servo_id, encoder_center) def set_angle_limits(self, servo_id: int, min_raw: int, max_raw: int) -> None: with self._lock: - lo = max(0, min(4095, int(min_raw))) - hi = max(0, min(4095, int(max_raw))) + encoder_max = backend_registry.get_encoder_resolution() + lo = max(0, min(encoder_max, int(min_raw))) + hi = max(0, min(encoder_max, int(max_raw))) self.angle_limits_raw[servo_id] = (min(lo, hi), max(lo, hi)) def set_register_word(self, servo_id: int, register: int, value: int) -> None: @@ -114,7 +126,7 @@ def _refresh_globals(self) -> None: servo_idx = utils.SERVO_IDS.index(servo_id) except ValueError: continue - raw = self.raw_positions.get(servo_id, 2048) + raw = self.raw_positions.get(servo_id, backend_registry.get_encoder_center()) angles.append(servo_driver.raw_to_angle_rad(raw, servo_idx)) if angles: mean_angle = float(np.mean(angles)) diff --git a/src/gradient_os/arm_controller/trajectory_execution.py b/src/gradient_os/arm_controller/trajectory_execution.py index e26ab99a..0c8a4046 100644 --- a/src/gradient_os/arm_controller/trajectory_execution.py +++ b/src/gradient_os/arm_controller/trajectory_execution.py @@ -12,6 +12,8 @@ import csv import threading +from typing import Optional + try: from .. import ik_solver from .. import trajectory_planner @@ -23,6 +25,84 @@ from . import utils from . import servo_driver from . import servo_protocol +from . import robot_config +from .backends import registry as backend_registry +from .actuator_interface import ActuatorBackend + + +# ============================================================================= +# Backend Accessor Functions +# ============================================================================= + +def _get_backend() -> Optional[ActuatorBackend]: + """Returns the active ActuatorBackend instance, or None if not set.""" + try: + return backend_registry.get_active_backend() + except backend_registry.BackendInstanceNotSetError: + return None + + +def _use_backend() -> bool: + """Returns True if an ActuatorBackend instance is active and initialized.""" + backend = _get_backend() + return backend is not None and backend.is_initialized + + +def _build_primary_feedback_ids() -> list[int]: + """ + Build a list of primary servo IDs for feedback (one per logical joint). + Uses the first servo ID from each logical joint's actuator list. + """ + primary_ids = [] + active_robot = robot_config.get_active_robot() + logical_map = active_robot.logical_joint_to_actuator_ids + num_logical_joints = active_robot.num_logical_joints + # Sort by logical joint number (1-based in the map) + for joint_num in sorted(logical_map.keys()): + actuator_ids = logical_map[joint_num] + if actuator_ids and joint_num <= num_logical_joints: # Skip gripper (joint 7) + primary_ids.append(actuator_ids[0]) + return primary_ids + + +def _build_logical_to_physical_index_map() -> dict[int, list[int]]: + """ + Build a mapping from logical joint index (0-based) to physical servo indices in SERVO_IDS. + """ + active_robot = robot_config.get_active_robot() + logical_map = active_robot.logical_joint_to_actuator_ids + num_logical_joints = active_robot.num_logical_joints + servo_ids = robot_config.SERVO_IDS + result = {} + for joint_num in sorted(logical_map.keys()): + if joint_num > num_logical_joints: # Skip gripper + continue + actuator_ids = logical_map[joint_num] + indices = [] + for aid in actuator_ids: + if aid in servo_ids: + indices.append(servo_ids.index(aid)) + result[joint_num - 1] = indices # Convert to 0-based + return result + + +def _get_twin_motor_pairs() -> list[tuple[int, int]]: + """ + Get the twin motor pairs (primary_id, secondary_id) from robot config. + Returns a list of tuples like [(20, 21), (30, 31)]. + """ + twin_map = robot_config.get_active_robot().twin_motor_actuator_ids + pairs = [] + # twin_map is like {1: 21, 2: 31} mapping logical joint to secondary servo ID + logical_map = robot_config.get_active_robot().logical_joint_to_actuator_ids + for logical_joint, secondary_id in twin_map.items(): + # The logical_joint here is 1-based index in the physical config + # We need to find the primary ID for this joint + actuator_ids = logical_map.get(logical_joint + 1, []) # +1 because logical_joint in twin_map is 0-based offset + if len(actuator_ids) >= 2: + primary_id = actuator_ids[0] + pairs.append((primary_id, secondary_id)) + return pairs JointTraj = Union[Sequence[Sequence[float]], np.ndarray] @@ -656,7 +736,7 @@ def _open_loop_executor_thread( # Pre-allocate Sync-Write command buffers # ---------------------------------------------- precomputed_cmds: list[list[tuple[int,int,int,int]]] = [ - servo_driver.logical_q_to_syncwrite_tuple(q, 4095, 0) for q in joint_path + servo_driver.logical_q_to_syncwrite_tuple(q, utils.ENCODER_RESOLUTION, 0) for q in joint_path ] # ---------------------------------------------- @@ -680,7 +760,14 @@ def _open_loop_executor_thread( # --- Actuation (WRITE) --- w_t0 = time.perf_counter() - servo_protocol.sync_write_goal_pos_speed_accel(cmd) + backend = _get_backend() + if backend and _use_backend(): + # Backend expects a dict of {servo_id: (position, speed, accel)} + backend.sync_write( + {entry[0]: (entry[1], entry[2], entry[3]) for entry in cmd} + ) + else: + servo_protocol.sync_write_goal_pos_speed_accel(cmd) _write_durations.append(time.perf_counter() - w_t0) # --- Timing / sleep --- @@ -809,18 +896,11 @@ def _closed_loop_executor_thread( _actual_angles_per_joint: list[list[float]] = [[] for _ in range(utils.NUM_LOGICAL_JOINTS)] # We need a mapping from logical joint index back to the physical servos to command. - # This is the reverse of the logic in set_servo_positions. - # This could be pre-calculated, but is clear here for now. - logical_to_physical_map = { - 0: [0], # Logical J1 -> Physical Servo ID 10 - 1: [1, 2], # Logical J2 -> Physical Servo IDs 20, 21 - 2: [3, 4], # Logical J3 -> Physical Servo IDs 30, 31 - 3: [5], # Logical J4 -> Physical Servo ID 40 - 4: [6], # Logical J5 -> Physical Servo ID 50 - 5: [7] # Logical J6 -> Physical Servo ID 60 - } + # This is dynamically built from robot config. + logical_to_physical_map = _build_logical_to_physical_index_map() all_physical_servo_ids = [utils.SERVO_IDS[i] for i in range(utils.NUM_PHYSICAL_SERVOS)] - PRIMARY_FB_IDS = [10, 20, 30, 40, 50, 60] + PRIMARY_FB_IDS = _build_primary_feedback_ids() + twin_motor_pairs = _get_twin_motor_pairs() # Integral accumulators for steady-state error compensation (gravity, bias) integral_error_rad = [0.0 for _ in range(utils.NUM_LOGICAL_JOINTS)] @@ -842,11 +922,15 @@ def _closed_loop_executor_thread( # Use a fixed but small timeout to ensure full packets arrive; setting # too low leads to intermittent Sync Read failures. per_cycle_timeout = max(0.01, time_step * 0.8) - raw_positions = servo_protocol.sync_read_positions( - PRIMARY_FB_IDS, - timeout_s=per_cycle_timeout, - poll_delay_s=0.0, - ) + backend = _get_backend() + if backend and _use_backend(): + raw_positions = backend.sync_read_positions(PRIMARY_FB_IDS) + else: + raw_positions = servo_protocol.sync_read_positions( + PRIMARY_FB_IDS, + timeout_s=per_cycle_timeout, + poll_delay_s=0.0, + ) _read_durations.append(time.perf_counter() - read_t0) # ------------------------------------------------------------ @@ -862,37 +946,37 @@ def _closed_loop_executor_thread( # This keeps the downstream control law unchanged. def _angle_to_raw(angle_rad: float, physical_idx: int) -> int: - """Convert a physical angle into a raw servo value (0-4095) using the + """Convert a physical angle into a raw servo value using the mapping rules held in utils. Clamp to valid range.""" min_map_rad, max_map_rad = utils.EFFECTIVE_MAPPING_RANGES[physical_idx] angle_clamped = max(min_map_rad, min(max_map_rad, angle_rad)) norm_val = (angle_clamped - min_map_rad) / (max_map_rad - min_map_rad) + encoder_max = utils.ENCODER_RESOLUTION if utils._is_servo_direct_mapping(physical_idx): - raw = norm_val * 4095.0 + raw = norm_val * encoder_max else: - raw = (1.0 - norm_val) * 4095.0 + raw = (1.0 - norm_val) * encoder_max - return int(round(max(0, min(4095, raw)))) + return int(round(max(0, min(encoder_max, raw)))) - # --- Joint 2 mirror (IDs 20 & 21) --- - if 20 in raw_positions and 21 not in raw_positions: - angle_rad_20 = servo_driver.servo_value_to_radians(raw_positions[20], 1) # index of 20 - raw_positions[21] = _angle_to_raw(angle_rad_20, 2) # index of 21 - - # --- Joint 3 mirror (IDs 30 & 31) --- - if 30 in raw_positions and 31 not in raw_positions: - angle_rad_30 = servo_driver.servo_value_to_radians(raw_positions[30], 3) # index of 30 - raw_positions[31] = _angle_to_raw(angle_rad_30, 4) # index of 31 + # --- Twin motor mirroring (dynamically from robot config) --- + for primary_id, secondary_id in twin_motor_pairs: + if primary_id in raw_positions and secondary_id not in raw_positions: + primary_idx = utils.SERVO_IDS.index(primary_id) + secondary_idx = utils.SERVO_IDS.index(secondary_id) + angle_rad = servo_driver.servo_value_to_radians(raw_positions[primary_id], primary_idx) + raw_positions[secondary_id] = _angle_to_raw(angle_rad, secondary_idx) # Record target vs actual angles per logical joint using primary IDs try: - primary_ids = {0: 10, 1: 20, 2: 30, 3: 40, 4: 50, 5: 60} + # Build primary_ids dict from the PRIMARY_FB_IDS list + primary_ids = {i: PRIMARY_FB_IDS[i] for i in range(len(PRIMARY_FB_IDS))} for logical_joint_index in range(utils.NUM_LOGICAL_JOINTS): target_angle_rad = target_q_step[logical_joint_index] _target_angles_per_joint[logical_joint_index].append(target_angle_rad) - primary_id = primary_ids[logical_joint_index] - if primary_id in raw_positions: + primary_id = primary_ids.get(logical_joint_index) + if primary_id and primary_id in raw_positions: config_index = utils.SERVO_IDS.index(primary_id) actual_angle = servo_driver.servo_value_to_radians(raw_positions[primary_id], config_index) # Undo master offset to get logical angle @@ -953,7 +1037,7 @@ def _angle_to_raw(angle_rad: float, physical_idx: int) -> int: # to let inner servo PID be the only stabilizing loop during tuning. commanded_physical_angle_rad = target_physical_angle_rad - # 4. Convert the final commanded angle to a raw servo value (0-4095) + # 4. Convert the final commanded angle to a raw servo value # This block is the same as in set_servo_positions min_urdf_rad, max_urdf_rad = utils.URDF_JOINT_LIMITS[physical_servo_config_index] angle_clamped_to_urdf = max(min_urdf_rad, min(max_urdf_rad, commanded_physical_angle_rad)) @@ -961,21 +1045,29 @@ def _angle_to_raw(angle_rad: float, physical_idx: int) -> int: min_map_rad, max_map_rad = utils.EFFECTIVE_MAPPING_RANGES[physical_servo_config_index] angle_for_norm = max(min_map_rad, min(max_map_rad, angle_clamped_to_urdf)) normalized_value = (angle_for_norm - min_map_rad) / (max_map_rad - min_map_rad) + encoder_max = utils.ENCODER_RESOLUTION - raw_servo_value = (normalized_value * 4095.0) if utils._is_servo_direct_mapping(physical_servo_config_index) else ((1.0 - normalized_value) * 4095.0) + raw_servo_value = (normalized_value * encoder_max) if utils._is_servo_direct_mapping(physical_servo_config_index) else ((1.0 - normalized_value) * encoder_max) final_servo_pos_value = int(round(raw_servo_value)) - # Add to the command list. Speed is max (4095) and Accel is none (0) + # Add to the command list. Speed is max and Accel is none (0) # because the path is timed by the closed-loop executor itself. - commands_for_sync_write.append((servo_id, final_servo_pos_value, 4095, 0)) + commands_for_sync_write.append((servo_id, final_servo_pos_value, encoder_max, 0)) _compute_durations.append(time.perf_counter() - compute_t0) # --- Actuation (Write) --- if commands_for_sync_write: write_t0 = time.perf_counter() - servo_protocol.sync_write_goal_pos_speed_accel(commands_for_sync_write) + backend = _get_backend() + if backend and _use_backend(): + # Backend expects a dict of {servo_id: (position, speed, accel)} + backend.sync_write( + {entry[0]: (entry[1], entry[2], entry[3]) for entry in commands_for_sync_write} + ) + else: + servo_protocol.sync_write_goal_pos_speed_accel(commands_for_sync_write) _write_durations.append(time.perf_counter() - write_t0) else: _write_durations.append(0.0) @@ -1033,6 +1125,12 @@ def _angle_to_raw(angle_rad: float, physical_idx: int) -> int: # ------------------ if diagnostics_enabled: session_id = utils.trajectory_state.get('diagnostics_session_id') + # Get sync profiles from backend if available, else from servo_protocol + backend = _get_backend() + if backend and _use_backend() and hasattr(backend, 'get_sync_profiles'): + sync_profiles = backend.get_sync_profiles() + else: + sync_profiles = servo_protocol.get_sync_profiles() _save_executor_diagnostics_charts( mode="closed_loop", session_id=session_id, @@ -1044,7 +1142,7 @@ def _angle_to_raw(angle_rad: float, physical_idx: int) -> int: actual_angles_per_joint=_actual_angles_per_joint, read_durations=_read_durations, compute_durations=_compute_durations, - sync_profiles=servo_protocol.get_sync_profiles(), + sync_profiles=sync_profiles, ) # If this executor thread is the one registered in trajectory_state, clear it diff --git a/src/gradient_os/arm_controller/utils.py b/src/gradient_os/arm_controller/utils.py index 5694847b..44f20b9e 100644 --- a/src/gradient_os/arm_controller/utils.py +++ b/src/gradient_os/arm_controller/utils.py @@ -1,159 +1,173 @@ # Contains utility functions and constants shared across the arm_controller package. +# +# NOTE: This file imports configuration from: +# - robot_config.py for robot-specific settings +# - backends/registry.py for servo-backend-specific constants +# +# The backend registry MUST be configured before accessing servo-specific values. +# This is done in run_controller.py at startup via backend_registry.set_active_backend(). +# +# For new code, prefer importing directly from: +# - robot_config.py for robot-specific settings +# - backends/registry.py for servo-backend constants + import math import os import numpy as np +# Import robot configuration for backward compatibility +from . import robot_config + +# Import backend registry for servo-specific constants (dynamically configured at runtime) +from .backends import registry as backend_registry + +# ============================================================================= +# Re-export robot configuration for backward compatibility +# ============================================================================= + # --- UDP Configuration --- -PI_IP = "0.0.0.0" # Listen on all available interfaces -UDP_PORT = 3000 # Same port as in follow_target_rmp_UDPsend.py -BUFFER_SIZE = 1024 # Size of the UDP receive buffer in bytes +PI_IP = robot_config.PI_IP +UDP_PORT = robot_config.UDP_PORT +BUFFER_SIZE = robot_config.BUFFER_SIZE # --- Kinematics & Planning --- -NUM_LOGICAL_JOINTS = 6 # Number of controllable joints in the kinematic model +NUM_LOGICAL_JOINTS = robot_config.NUM_LOGICAL_JOINTS # --- Servo Configuration --- -NUM_PHYSICAL_SERVOS = 9 # Total number of physical servos (J1 is no longer doubled up) -SERVO_IDS = [10, 20, 21, 30, 31, 40, 50, 60, 100] # Hardware ID of each servo, 100 is gripper +NUM_PHYSICAL_SERVOS = robot_config.NUM_PHYSICAL_SERVOS +SERVO_IDS = robot_config.SERVO_IDS # IDs for the second motor on multi-servo joints -SERVO_ID_JOINT_2_SECOND = 21 -SERVO_ID_JOINT_3_SECOND = 31 +SERVO_ID_JOINT_2_SECOND = robot_config.SERVO_ID_JOINT_2_SECOND +SERVO_ID_JOINT_3_SECOND = robot_config.SERVO_ID_JOINT_3_SECOND # ID for the gripper servo -SERVO_ID_GRIPPER = 100 +SERVO_ID_GRIPPER = robot_config.SERVO_ID_GRIPPER # --- Default Motion Parameters --- -DEFAULT_SERVO_SPEED = 500 # Default speed for servos if not specified (0-4095) -DEFAULT_PROFILE_VELOCITY = 0.1 # m/s, for trapezoidal profiles -DEFAULT_PROFILE_ACCELERATION = 0.05 # m/s^2, for trapezoidal profiles -CORRECTION_KP_GAIN = 0.0 # Proportional gain for the closed-loop executor (disabled for inner-PID tuning) -CORRECTION_KI_GAIN = 0.0 # Integral gain (rad/s) for the closed-loop executor (disabled for inner-PID tuning) -CORRECTION_INTEGRAL_CLAMP_RAD = 0.6 # Anti-windup clamp for integral term (±20°) +DEFAULT_SERVO_SPEED = robot_config.DEFAULT_SERVO_SPEED +DEFAULT_PROFILE_VELOCITY = robot_config.DEFAULT_PROFILE_VELOCITY +DEFAULT_PROFILE_ACCELERATION = robot_config.DEFAULT_PROFILE_ACCELERATION +CORRECTION_KP_GAIN = robot_config.CORRECTION_KP_GAIN +CORRECTION_KI_GAIN = robot_config.CORRECTION_KI_GAIN +CORRECTION_INTEGRAL_CLAMP_RAD = robot_config.CORRECTION_INTEGRAL_CLAMP_RAD # --- Serial Port Configuration --- -# Raspberry Pi GPIO 14 (TXD) and GPIO 15 (RXD) often map to /dev/ttyS0 or /dev/serial0. -# You may need to configure your Pi to enable the hardware serial port and disable the -# serial console. On a Raspberry Pi, run 'sudo raspi-config' and go to: -# 1. Interface Options -> Serial Port -# 2. Answer 'No' to 'Would you like a login shell to be accessible over serial?' -# 3. Answer 'Yes' to 'Would you like the serial port hardware to be enabled?' -# 4. Reboot your Pi. -SERIAL_PORT = "/dev/ttyUSB0" # this uses the usb port # "/dev/ttyAMA0" # use ttyAMA0 if using GPIO setup with Raspberry Pi GPIO -BAUD_RATE = 1000000 # Communication speed for the Feetech servos +SERIAL_PORT = robot_config.SERIAL_PORT +BAUD_RATE = robot_config.BAUD_RATE # --- Master Calibration Offsets --- -LOGICAL_JOINT_MASTER_OFFSETS_RAD = [ - 0.0, # Logical Joint 1 (IDs 10, 11) - 0.0, # Logical Joint 2 (IDs 20, 21) - 0.0, # Logical Joint 3 (IDs 30, 31) - 0.0, # Logical Joint 4 (ID 40) - 0.0, # Logical Joint 5 (ID 50) - 0.0 # Logical Joint 6 (ID 60) -] - -# The `SERVO_ENCODER_ZERO_POSITIONS` list is now obsolete and has been removed. -# Calibration is now handled by the servo's internal `POSITION_CORRECTION` register, -# which is set using the `SET_ZERO,ID` UDP command. - -# --- END CALIBRATION INPUTS --- - -# Logical Joint Limits (radians) [min_limit, max_limit] -# These are the effective limits for the 6 logical joints of the arm. -LOGICAL_JOINT_LIMITS_RAD = [ - [-3.1416, 3.1416], # Logical J1 (Base) - [-1.5708, 1.5708], # Logical J2 (Shoulder) - [-1.5708, 1.5708], # Logical J3 (Elbow) - [-3.1416, 3.1416], # Logical J4 (Wrist Roll) - [-1.8326, 2.0944], # Logical J5 (Wrist Pitch) - [-3.1416, 3.1416] # Logical J6 (Wrist Yaw) -] - -# Gripper servo limits (radians) -GRIPPER_LIMITS_RAD = [0, 3.1416] # 0° (closed) to 90° (open) - -# URDF Joint Limits (radians) [min_limit, max_limit] - These are for the LOGICAL joints -# but need to be consistent with the physical servo capabilities after all offsets. -# The config for physical servos (config indices 0-8) must match up. -# E.g. URDF_JOINT_LIMITS[0] and [1] are for Servos ID 10 and 11 (J1). -URDF_JOINT_LIMITS = [ # Per PHYSICAL servo config index - LOGICAL_JOINT_LIMITS_RAD[0], # Servo ID 10 (J1) - LOGICAL_JOINT_LIMITS_RAD[1], # Servo ID 20 (Logical J2, Servo 1) - LOGICAL_JOINT_LIMITS_RAD[1], # Servo ID 21 (Logical J2, Servo 2) - Must match Servo ID 20 - LOGICAL_JOINT_LIMITS_RAD[2], # Servo ID 30 (Logical J3, Servo 1) - LOGICAL_JOINT_LIMITS_RAD[2], # Servo ID 31 (Logical J3, Servo 2) - Must match Servo ID 30 - LOGICAL_JOINT_LIMITS_RAD[3], # Servo ID 40 (Logical J4) - LOGICAL_JOINT_LIMITS_RAD[4], # Servo ID 50 (Logical J5) - LOGICAL_JOINT_LIMITS_RAD[5], # Servo ID 60 (Logical J6) - GRIPPER_LIMITS_RAD # Servo ID 100 (Gripper) -] - -# Effective Mapping Ranges: Assuming 0-4095 on servo maps to +/- PI radians from servo zero. -EFFECTIVE_MAPPING_RANGES = [] # Per PHYSICAL servo config index -for _ in range(NUM_PHYSICAL_SERVOS): - EFFECTIVE_MAPPING_RANGES.append([-math.pi, math.pi]) - -# --- Servo Protocol Constants --- -"""Constants that define the Feetech servo protocol, such as instruction bytes and register addresses.""" -SERVO_HEADER = 0xFF -SERVO_INSTRUCTION_WRITE = 0x03 -SERVO_INSTRUCTION_READ = 0x02 -SERIAL_READ_TIMEOUT = 0.05 - -SERVO_ADDR_TARGET_POSITION = 0x2A -SERVO_ADDR_PRESENT_POSITION = 0x38 -SERVO_ADDR_TARGET_ACCELERATION = 0x29 -SERVO_ADDR_POSITION_CORRECTION = 31 # Register 0x1F, for storing hardware zero offset - -# Default values for the servo's internal controller and motion profiles -DEFAULT_SERVO_ACCELERATION_DEG_S2 = 500 -ACCELERATION_SCALE_FACTOR = 100 - -SERVO_ADDR_POS_KP = 0x15 -SERVO_ADDR_POS_KI = 0x17 -SERVO_ADDR_POS_KD = 0x16 - -# PID values range from 0 to 254 - -# If it overshoots a lot and oscillates, either the integral gain (I) needs to be increased or all gains (P,I,D) should be reduced -# Too much overshoot? Increase D, decrease P. -# Response too damped? Increase P. -# Ramps up quickly to a value below target value and then slows down as it approaches target value? Try increasing the I constant. - -DEFAULT_KP = 50 -DEFAULT_KI = 1 -DEFAULT_KD = 30 - -# Default PID gains for the servos (per-joint tuned baseline) -J1_PID_GAINS = (50, 1, 20) # J1 pattern is (KP, KI, KD) -J2_PID_GAINS = (65, 1, 30) # J2 pattern is (KP, KI, KD) -J3_PID_GAINS = (50, 1, 25) # J3 pattern is (KP, KI, KD) -J4_PID_GAINS = (50, 0, 10) # J4 pattern is (KP, KI, KD) -J5_PID_GAINS = (50, 0, 10) # J5 pattern is (KP, KI, KD) -J6_PID_GAINS = (50, 0, 10) # J6 pattern is (KP, KI, KD) -Gripper_PID_GAINS = (40, 0, 10) # Gripper pattern is (KP, KI, KD) - -# Per-servo default PID gains (Kp, Ki, Kd), keyed by hardware Servo ID. -# If a servo ID is missing from this map, the controller falls back to -# DEFAULT_KP/DEFAULT_KI/DEFAULT_KD above for that servo. -DEFAULT_PID_GAINS: dict[int, tuple[int, int, int]] = { - 10: J1_PID_GAINS, # J1 - 20: J2_PID_GAINS, # J2 master - 21: J2_PID_GAINS, # J2 slave - 30: J3_PID_GAINS, # J3 master - 31: J3_PID_GAINS, # J3 slave - 40: J4_PID_GAINS, # J4 - 50: J5_PID_GAINS, # J5 - 60: J6_PID_GAINS, # J6 - 100: Gripper_PID_GAINS, # Gripper -} - -# Trajectory Planning Optimization -IK_PLANNING_FREQUENCY = 100 # Hz. We solve IK at this rate, then interpolate for smoother, faster execution. +LOGICAL_JOINT_MASTER_OFFSETS_RAD = robot_config.LOGICAL_JOINT_MASTER_OFFSETS_RAD + +# --- Joint Limits --- +LOGICAL_JOINT_LIMITS_RAD = robot_config.LOGICAL_JOINT_LIMITS_RAD +GRIPPER_LIMITS_RAD = robot_config.GRIPPER_LIMITS_RAD +URDF_JOINT_LIMITS = robot_config.URDF_JOINT_LIMITS +EFFECTIVE_MAPPING_RANGES = robot_config.EFFECTIVE_MAPPING_RANGES + +# --- PID Configuration --- +DEFAULT_KP = robot_config.DEFAULT_KP +DEFAULT_KI = robot_config.DEFAULT_KI +DEFAULT_KD = robot_config.DEFAULT_KD +J1_PID_GAINS = robot_config.J1_PID_GAINS +J2_PID_GAINS = robot_config.J2_PID_GAINS +J3_PID_GAINS = robot_config.J3_PID_GAINS +J4_PID_GAINS = robot_config.J4_PID_GAINS +J5_PID_GAINS = robot_config.J5_PID_GAINS +J6_PID_GAINS = robot_config.J6_PID_GAINS +Gripper_PID_GAINS = robot_config.GRIPPER_PID_GAINS +DEFAULT_PID_GAINS = robot_config.DEFAULT_PID_GAINS + +# ============================================================================= +# Servo Protocol Constants (from active backend via registry) +# These are initialized to None and populated when the backend is configured. +# ============================================================================= + +# Encoder resolution (actuator-specific) - populated from backend registry +ENCODER_RESOLUTION = None +ENCODER_CENTER = None + +# Protocol constants - populated from backend registry +SERVO_HEADER = None +SERVO_INSTRUCTION_WRITE = None +SERVO_INSTRUCTION_READ = None +SERIAL_READ_TIMEOUT = None +SERVO_ADDR_TARGET_POSITION = None +SERVO_ADDR_PRESENT_POSITION = None +SERVO_ADDR_TARGET_ACCELERATION = None +SERVO_ADDR_POSITION_CORRECTION = None +DEFAULT_SERVO_ACCELERATION_DEG_S2 = None +ACCELERATION_SCALE_FACTOR = None +SERVO_ADDR_POS_KP = None +SERVO_ADDR_POS_KI = None +SERVO_ADDR_POS_KD = None + +# Sync Write/Read Constants +SERVO_INSTRUCTION_SYNC_WRITE = None +SERVO_BROADCAST_ID = None +SYNC_WRITE_START_ADDRESS = None +SYNC_WRITE_DATA_LEN_PER_SERVO = None +SERVO_INSTRUCTION_SYNC_READ = None + + +def _populate_servo_constants(): + """ + Populate servo-specific constants from the active servo backend. + Called after backend_registry.set_active_backend() in run_controller.py. + + These are protocol/hardware constants for the specific servo type (e.g., Feetech), + distinct from robot-specific constants which are populated by _populate_robot_constants(). + """ + global ENCODER_RESOLUTION, ENCODER_CENTER + global SERVO_HEADER, SERVO_INSTRUCTION_WRITE, SERVO_INSTRUCTION_READ + global SERIAL_READ_TIMEOUT, SERVO_ADDR_TARGET_POSITION, SERVO_ADDR_PRESENT_POSITION + global SERVO_ADDR_TARGET_ACCELERATION, SERVO_ADDR_POSITION_CORRECTION + global DEFAULT_SERVO_ACCELERATION_DEG_S2, ACCELERATION_SCALE_FACTOR + global SERVO_ADDR_POS_KP, SERVO_ADDR_POS_KI, SERVO_ADDR_POS_KD + global SERVO_INSTRUCTION_SYNC_WRITE, SERVO_BROADCAST_ID + global SYNC_WRITE_START_ADDRESS, SYNC_WRITE_DATA_LEN_PER_SERVO, SERVO_INSTRUCTION_SYNC_READ + + servo_config = backend_registry.get_config() + + ENCODER_RESOLUTION = servo_config.SERVO_VALUE_MAX + ENCODER_CENTER = servo_config.SERVO_VALUE_CENTER + SERVO_HEADER = servo_config.SERVO_HEADER + SERVO_INSTRUCTION_WRITE = servo_config.SERVO_INSTRUCTION_WRITE + SERVO_INSTRUCTION_READ = servo_config.SERVO_INSTRUCTION_READ + SERIAL_READ_TIMEOUT = servo_config.SERIAL_READ_TIMEOUT + SERVO_ADDR_TARGET_POSITION = servo_config.SERVO_ADDR_TARGET_POSITION + SERVO_ADDR_PRESENT_POSITION = servo_config.SERVO_ADDR_PRESENT_POSITION + SERVO_ADDR_TARGET_ACCELERATION = servo_config.SERVO_ADDR_TARGET_ACCELERATION + SERVO_ADDR_POSITION_CORRECTION = servo_config.SERVO_ADDR_POSITION_CORRECTION + DEFAULT_SERVO_ACCELERATION_DEG_S2 = servo_config.DEFAULT_SERVO_ACCELERATION_DEG_S2 + ACCELERATION_SCALE_FACTOR = servo_config.ACCELERATION_SCALE_FACTOR + SERVO_ADDR_POS_KP = servo_config.SERVO_ADDR_POS_KP + SERVO_ADDR_POS_KI = servo_config.SERVO_ADDR_POS_KI + SERVO_ADDR_POS_KD = servo_config.SERVO_ADDR_POS_KD + SERVO_INSTRUCTION_SYNC_WRITE = servo_config.SERVO_INSTRUCTION_SYNC_WRITE + SERVO_BROADCAST_ID = servo_config.SERVO_BROADCAST_ID + SYNC_WRITE_START_ADDRESS = servo_config.SYNC_WRITE_START_ADDRESS + SYNC_WRITE_DATA_LEN_PER_SERVO = servo_config.SYNC_WRITE_DATA_LEN_PER_SERVO + SERVO_INSTRUCTION_SYNC_READ = servo_config.SERVO_INSTRUCTION_SYNC_READ + + +# Keep old name as alias for backward compatibility during migration +_populate_backend_constants = _populate_servo_constants + +# ============================================================================= +# Trajectory Planning Configuration +# ============================================================================= + +# IK planning frequency (Hz) - solve IK at this rate, interpolate for execution +IK_PLANNING_FREQUENCY = 100 # Trajectory Caching -# Note: The path is adjusted to navigate from `src/arm_controller` up to the project root. TRAJECTORY_CACHE_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), "..", "..", "trajectory_cache")) +# ============================================================================= +# Global Runtime State +# ============================================================================= + # Global state for trajectory execution trajectory_state = { "is_running": False, @@ -161,9 +175,9 @@ "thread": None, # Diagnostics toggle (runtime), used to enable IK/executor logging and charts "diagnostics_enabled": False, - # --- New state for real-time jogging --- + # --- Real-time jogging state --- "is_jogging": False, - # 6D vector: [vx, vy, vz, v_roll, v_pitch, v_yaw] + # 6D velocity vector: [vx, vy, vz, v_roll, v_pitch, v_yaw] "jog_velocities": np.zeros(6, dtype=float), "last_jog_command_time": 0.0, # Deadman gate for jog (must be True to apply non-zero velocities) @@ -172,48 +186,70 @@ "jog_debug": False, } +# ============================================================================= +# Global Serial & State Objects +# ============================================================================= -# Servo Sync Write Constants -SERVO_INSTRUCTION_SYNC_WRITE = 0x83 -SERVO_BROADCAST_ID = 0xFE # Typically 0xFE for Feetech Sync Write -# Start address for Sync Write block (Accel, Pos, Time, Speed) -SYNC_WRITE_START_ADDRESS = SERVO_ADDR_TARGET_ACCELERATION # 0x29 -# Length of data block per servo in Sync Write: Accel (1) + Pos (2) + Time (2) + Speed (2) = 7 bytes -SYNC_WRITE_DATA_LEN_PER_SERVO = 7 -# Servo Sync Read Constants -SERVO_INSTRUCTION_SYNC_READ = 0x82 - -# Global serial object +# Global serial object (managed by servo_driver.initialize_servos()) ser: 'serial.Serial | None' = None # Global state for the arm's last known logical joint angles (in radians) -current_logical_joint_angles_rad = [0.0] * NUM_LOGICAL_JOINTS -# NEW: Global state for the gripper's last known angle (in radians) +# NOTE: This is initialized as empty and populated by run_controller.py after +# the robot configuration is set. Code should check len() before accessing. +current_logical_joint_angles_rad: list[float] = [] + +# Global state for the gripper's last known angle (in radians) current_gripper_angle_rad = 0.0 -# NEW: Flag to indicate if the gripper servo (ID 100) was detected on startup + +# Flag to indicate if the gripper servo was detected on startup gripper_present = False -# ----------------------------------------------------------------------------- -# Servo Orientation Mapping (RAW ↔︎ Angle) -# ----------------------------------------------------------------------------- -# For each physical servo we have to know whether a positive physical rotation of -# the joint corresponds to an increase ("direct") or a decrease ("inverted") of -# the raw encoder/command value. This depends on how the servo is mounted. -# -# Mapping for the current robot (verified by physical testing): -# Joint 1 : ID 10 → inverted -# Joint 2 : ID 20 → inverted| ID 21 → direct -# Joint 3 : ID 30 → inverted| ID 31 → direct -# Joint 4 : ID 40 → inverted -# Joint 5 : ID 50 → inverted -# Joint 6 : ID 60 → direct -# -# If you commission a new robot and the mounting differs, simply edit the -# `INVERTED_SERVO_IDS` set below so the control-system math stays correct. -# IDs whose raw value DECREASES when the joint rotates in the positive logical -# direction. -INVERTED_SERVO_IDS: set[int] = {10, 20, 30, 40, 50, 60, 100} # Assuming gripper (100) is inverse mapping +def _populate_robot_constants() -> None: + """ + Populate robot-specific constants from the active robot configuration. + + Called by robot_config.set_active_robot() to ensure utils module globals + are refreshed when the active robot changes. + + These are robot-specific constants (joint IDs, limits, mappings), + distinct from servo-specific constants which are populated by _populate_servo_constants(). + """ + global current_logical_joint_angles_rad + global NUM_LOGICAL_JOINTS, NUM_PHYSICAL_SERVOS, SERVO_IDS + global SERVO_ID_GRIPPER, SERVO_ID_JOINT_2_SECOND, SERVO_ID_JOINT_3_SECOND + global URDF_JOINT_LIMITS, EFFECTIVE_MAPPING_RANGES, INVERTED_SERVO_IDS + global LOGICAL_JOINT_MASTER_OFFSETS_RAD, LOGICAL_JOINT_LIMITS_RAD, GRIPPER_LIMITS_RAD + + # Re-read from robot_config (which was just updated by set_active_robot) + NUM_LOGICAL_JOINTS = robot_config.NUM_LOGICAL_JOINTS + NUM_PHYSICAL_SERVOS = robot_config.NUM_PHYSICAL_SERVOS + SERVO_IDS = robot_config.SERVO_IDS + SERVO_ID_GRIPPER = robot_config.SERVO_ID_GRIPPER + SERVO_ID_JOINT_2_SECOND = robot_config.SERVO_ID_JOINT_2_SECOND + SERVO_ID_JOINT_3_SECOND = robot_config.SERVO_ID_JOINT_3_SECOND + URDF_JOINT_LIMITS = robot_config.URDF_JOINT_LIMITS + EFFECTIVE_MAPPING_RANGES = robot_config.EFFECTIVE_MAPPING_RANGES + INVERTED_SERVO_IDS = robot_config.INVERTED_SERVO_IDS + LOGICAL_JOINT_MASTER_OFFSETS_RAD = robot_config.LOGICAL_JOINT_MASTER_OFFSETS_RAD + LOGICAL_JOINT_LIMITS_RAD = robot_config.LOGICAL_JOINT_LIMITS_RAD + GRIPPER_LIMITS_RAD = robot_config.GRIPPER_LIMITS_RAD + + n_joints = NUM_LOGICAL_JOINTS + if n_joints is not None and len(current_logical_joint_angles_rad) != n_joints: + current_logical_joint_angles_rad = [0.0] * n_joints + + +# Keep old name as alias for backward compatibility during migration +_reinitialize_state = _populate_robot_constants + +# ============================================================================= +# Servo Orientation Mapping +# ============================================================================= + +# Import from robot_config for backward compatibility +INVERTED_SERVO_IDS = robot_config.INVERTED_SERVO_IDS + def _is_servo_direct_mapping(physical_servo_config_index: int) -> bool: """ @@ -221,16 +257,17 @@ def _is_servo_direct_mapping(physical_servo_config_index: int) -> bool: or decreases (inverted). This is necessary for correct angle-to-raw-value conversion. Args: - physical_servo_config_index (int): The 0-based index of the servo in the SERVO_IDS list. + physical_servo_config_index: The 0-based index of the servo in the SERVO_IDS list. Returns: bool: True if the mapping is direct, False if inverted. """ - current_physical_servo_id = SERVO_IDS[physical_servo_config_index] + return robot_config._is_servo_direct_mapping(physical_servo_config_index) - # Return False (inverted) if the ID is in the inverted set; True otherwise. - return current_physical_servo_id not in INVERTED_SERVO_IDS +# ============================================================================= +# Utility Functions +# ============================================================================= def _convert_numpy_to_list(obj): """ @@ -251,3 +288,17 @@ def _convert_numpy_to_list(obj): return [_convert_numpy_to_list(elem) for elem in obj] return obj + +def get_actuator_backend(): + """ + Get the currently active actuator backend instance. + + This function provides access to the actuator backend for advanced usage. + For most cases, use the functions in servo_driver.py instead. + + Returns: + ActuatorBackend: The active backend, or None if not initialized. + """ + # Import here to avoid circular dependency + from . import servo_driver + return getattr(servo_driver, '_actuator_backend', None) diff --git a/src/gradient_os/run_controller.py b/src/gradient_os/run_controller.py index 00db241f..5d2f1d63 100644 --- a/src/gradient_os/run_controller.py +++ b/src/gradient_os/run_controller.py @@ -1,22 +1,41 @@ # This script will be the main entry point for running the robot controller. # It will import logic from the 'src/arm_controller' package and start the UDP server. +# +# Configuration: +# -------------- +# The robot and servo backend are configured via command-line arguments: +# --robot: Which robot configuration to use (e.g., "gradient0") +# --servo-backend: Which servo backend to use (e.g., "feetech") +# +# This allows the frontend to expose robot/servo selection and makes the +# configuration explicit at startup rather than buried in code. + import socket import time import traceback import sys import os -import numpy as np # Added for gripper angle conversion +import numpy as np import argparse import threading import subprocess import json try: + # Import the backend registry FIRST - it must be configured before other modules + from .arm_controller.backends import registry as backend_registry + from .arm_controller import ( command_api, servo_driver, servo_protocol, - utils + utils, + robot_config, + ) + from .arm_controller.robots import ( + get_robot_config, + list_available_robots, + RobotConfig, ) from .telemetry import alerts as _alerts except ImportError as e: @@ -24,44 +43,175 @@ print("Please ensure the script is run from the project root directory and 'src' is in the Python path.") sys.exit(1) +# Get available servo backends from the registry +AVAILABLE_SERVO_BACKENDS = backend_registry.list_available_backends() + def main(): """ Main entry point for the robot controller. This function performs the following steps: - 1. Initializes the hardware (serial port, servos, PID gains, angle limits). - 2. Performs an initial read of servo positions to synchronize the internal state. - 3. Enters an infinite loop to listen for UDP commands. - 4. Parses incoming commands and dispatches them to the appropriate handler + 1. Parses command-line arguments for robot and servo configuration. + 2. Initializes the robot configuration and servo backend. + 3. Initializes the hardware (serial port, servos, PID gains, angle limits). + 4. Performs an initial read of servo positions to synchronize the internal state. + 5. Enters an infinite loop to listen for UDP commands. + 6. Parses incoming commands and dispatches them to the appropriate handler in the `command_api` module. - 5. Manages a simple calibration mode for streaming servo data. - 6. Ensures a graceful shutdown of the serial port on exit. + 7. Manages a simple calibration mode for streaming servo data. + 8. Ensures a graceful shutdown of the serial port on exit. """ - parser = argparse.ArgumentParser(description="Robot Arm Controller") + # Get list of available robots for help text + available_robots = list_available_robots() + + parser = argparse.ArgumentParser( + description="Robot Arm Controller", + formatter_class=argparse.RawDescriptionHelpFormatter, + epilog=f""" +Available robots: {', '.join(available_robots)} +Available servo backends: {', '.join(AVAILABLE_SERVO_BACKENDS)} + +Examples: + python -m gradient_os.run_controller --robot gradient0 --servo-backend feetech + python -m gradient_os.run_controller --robot gradient0 --sim + """ + ) + parser.add_argument( + "--robot", + type=str, + default="gradient0", + choices=available_robots, + help=f"Robot configuration to use. Available: {', '.join(available_robots)} (default: gradient0)", + ) + parser.add_argument( + "--servo-backend", + type=str, + default=None, # Will use robot's default if not specified + choices=AVAILABLE_SERVO_BACKENDS, + help=f"Servo backend to use. Available: {', '.join(AVAILABLE_SERVO_BACKENDS)} (default: from robot config)", + ) parser.add_argument( "--serial-port", type=str, - default="/dev/ttyUSB0", - help="The serial port to connect to the robot arm.", + default=None, + help="Override the serial port (default: from robot config).", ) parser.add_argument( "--sim", action="store_true", help="Run the controller against an in-memory servo simulator instead of hardware.", ) + parser.add_argument( + "--list-robots", + action="store_true", + help="List available robot configurations and exit.", + ) args = parser.parse_args() - # Update the serial port from the command line argument + # Handle --list-robots + if args.list_robots: + print("Available robot configurations:") + for robot_name in available_robots: + try: + cfg = get_robot_config(robot_name) + print(f" - {robot_name}: {cfg.name} v{cfg.version} ({cfg.num_logical_joints} joints, {cfg.num_physical_actuators} actuators)") + except Exception as e: + print(f" - {robot_name}: (error loading: {e})") + sys.exit(0) + + # ========================================================================== + # Initialize Robot Configuration + # ========================================================================== + print(f"[Controller] Loading robot configuration: {args.robot}") + try: + selected_robot = get_robot_config(args.robot) + print(f"[Controller] Robot: {selected_robot.name} v{selected_robot.version}") + print(f"[Controller] - {selected_robot.num_logical_joints} logical joints") + print(f"[Controller] - {selected_robot.num_physical_actuators} physical actuators") + print(f"[Controller] - Gripper: {'Yes' if selected_robot.has_gripper else 'No'}") + + # Update the global robot configuration + robot_config.set_active_robot(selected_robot) + except Exception as e: + print(f"[Controller] Error loading robot configuration '{args.robot}': {e}") + sys.exit(1) + + # ========================================================================== + # Configure Serial Port + # ========================================================================== + # Priority: command-line arg > robot config default if args.serial_port: utils.SERIAL_PORT = args.serial_port - + print(f"[Controller] Serial port (from command line): {utils.SERIAL_PORT}") + else: + utils.SERIAL_PORT = selected_robot.default_serial_port + print(f"[Controller] Serial port (from robot config): {utils.SERIAL_PORT}") + + # ========================================================================== + # Configure Servo Backend (MUST be done before using any servo-dependent modules) + # ========================================================================== + # Priority: command-line arg > robot's default + # If --sim is specified, override to use simulation backend + if args.sim: + servo_backend = "simulation" + print("[Controller] Running in SIMULATION mode (no hardware)") + else: + servo_backend = args.servo_backend if args.servo_backend else selected_robot.default_servo_backend + + print(f"[Controller] Servo backend: {servo_backend}") + + # Set active backend CONFIG (loads constants like encoder resolution, register addresses) + # Note: For simulation, we still use feetech config since it doesn't have its own + config_backend = servo_backend if servo_backend != "simulation" else "feetech" + backend_registry.set_active_backend(config_backend) + + # Populate utils with servo-specific constants from the active backend + utils._populate_servo_constants() + + # Now that backend is configured, update robot_config's BAUD_RATE + robot_config.BAUD_RATE = backend_registry.get_default_baud_rate() + + # ========================================================================== + # Create and Initialize Backend Instance + # ========================================================================== + # This creates the actual ActuatorBackend object that performs I/O operations. + # The backend is created from the robot config and handles all hardware communication. + print(f"[Controller] Creating {servo_backend} backend instance...") + robot_config_dict = selected_robot.get_config_dict() + + try: + active_backend = backend_registry.create_backend( + backend_name=servo_backend, + robot_config=robot_config_dict, + serial_port=utils.SERIAL_PORT, + ) + + # Set as active backend BEFORE initialization (in case other modules query it) + backend_registry.set_active_backend_instance(active_backend) + + # Initialize the backend (opens serial port, pings servos, sets PID gains) + # Note: We still call the legacy servo_driver.initialize_servos() for now + # since higher-level modules depend on it. In Phase 2, we'll migrate to + # using the backend directly. + except Exception as e: + print(f"[Controller] Error creating backend: {e}") + print("[Controller] Falling back to legacy initialization...") + # For backward compatibility during migration, if backend creation fails, + # we continue with legacy initialization + + # ========================================================================== + # Legacy Initialization (to be migrated in Phase 2) + # ========================================================================== + # For now, we still use servo_driver which uses servo_protocol directly. + # Once Phase 2 is complete, this will be replaced with: + # active_backend.initialize() + # active_backend.apply_joint_limits() if args.sim: from .arm_controller import sim_backend - sim_backend.activate() - # Initialize the hardware + # Initialize the hardware using legacy servo_driver servo_driver.initialize_servos() servo_driver.set_servo_angle_limits_from_urdf() @@ -70,11 +220,8 @@ def main(): utils.current_logical_joint_angles_rad = servo_driver.get_current_arm_state_rad() # If gripper is present, also get its initial state if utils.gripper_present: - # Use the generic and robust word-reading function - raw_pos = servo_protocol.read_servo_register_word( - utils.SERVO_ID_GRIPPER, - utils.SERVO_ADDR_PRESENT_POSITION - ) + # Read gripper position via servo_driver (uses backend if available) + raw_pos = servo_driver.read_single_servo_position(utils.SERVO_ID_GRIPPER) if raw_pos is not None: try: gripper_config_index = utils.SERVO_IDS.index(utils.SERVO_ID_GRIPPER) @@ -113,6 +260,10 @@ def _telemetry_loop(): period = 1.0 / max(1, int(telemetry_hz)) udp = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) last_extra_ts = 0.0 # throttle extended servo telemetry to ~2 Hz + + # Get telemetry block configuration from the active backend + telemetry_blocks = backend_registry.get_telemetry_blocks() + while not telemetry_stop_event.is_set(): try: q = servo_driver.get_current_arm_state_rad(verbose=False) @@ -120,101 +271,53 @@ def _telemetry_loop(): msg: dict[str, object] = {"t": time.time(), "joints": [float(x) for x in q]} if g is not None: msg["gripper"] = float(g) + # --- Servo telemetry (voltage/temp/current/torque + alarms) --- now = time.time() if now - last_extra_ts >= 0.5: last_extra_ts = now try: - present_ids = list(servo_protocol.get_present_servo_ids()) + # Get present servo IDs from backend or use configured IDs + backend = backend_registry.get_active_backend() + if backend and hasattr(backend, 'present_servo_ids'): + present_ids = list(backend.present_servo_ids) + else: + present_ids = list(utils.SERVO_IDS) + if present_ids: - blk1 = servo_protocol.sync_read_block(present_ids, start_address=0x38, data_len=8, timeout_s=0.05, diagnostics=False) # pos,speed,duty,voltage,temp - blk2 = servo_protocol.sync_read_block(present_ids, start_address=0x41, data_len=5, timeout_s=0.05, diagnostics=False) # status,moving,*,*,current - blk3 = servo_protocol.sync_read_block(present_ids, start_address=0x13, data_len=2, timeout_s=0.05, diagnostics=False) # unloading,led_alarm + # Read telemetry blocks using backend-defined addresses + block_data = [] + for addr, length in telemetry_blocks: + if backend and hasattr(backend, 'sync_read_block'): + block_data.append(backend.sync_read_block( + present_ids, start_address=addr, data_len=length + )) + else: + block_data.append(servo_protocol.sync_read_block( + present_ids, start_address=addr, data_len=length, + timeout_s=0.05, diagnostics=False + )) + servos: dict[str, dict[str, object]] = {} for sid in present_ids: - d1 = blk1.get(sid) - d2 = blk2.get(sid) - d3 = blk3.get(sid) - if not d1 and not d2 and not d3: - continue - voltage_v = None - temp_c = None - current_a = None - drive_pm = None - if d1 and len(d1) == 8: - # d1 layout (@0x38, len 8): pos(2), speed(2), load/drive duty (2), voltage(1), temp(1) - load_raw = int.from_bytes(d1[4:6], "little", signed=False) - # Feetech STS "Present Load" encodes direction in bit 10 (0x400), - # magnitude in bits 0..9 (per‑mille, 0..1023). Keep full magnitude (allows >100%). - load_mag_pm = (load_raw & 0x3FF) - # Direction bit is not needed for the UI; use absolute magnitude only - drive_pm = load_mag_pm - voltage_v = float(d1[6]) / 10.0 - temp_c = int(d1[7]) - if d2 and len(d2) == 5: - # d2: [status(1), moving(1), rsvd(1), rsvd(1), current(2)] - status_byte = int(d2[0]) - current_raw = int.from_bytes(d2[4:6], "little", signed=True) - current_a = current_raw * 0.0065 sample: dict[str, object] = {} - if voltage_v is not None: - sample["voltage_v"] = voltage_v - if temp_c is not None: - sample["temp_c"] = temp_c - if current_a is not None: - sample["current_a"] = current_a - if drive_pm is not None: - sample["drive_duty_per_mille"] = drive_pm - if d3 and len(d3) == 2: - unload = int(d3[0]); led = int(d3[1]) - sample["unloading_condition"] = unload - sample["led_alarm_condition"] = led - def _names_for(val: int) -> list[str]: - labels = { - 0: "Overload", - 1: "Overheat", - 2: "Overvoltage", - 3: "Undervoltage", - 4: "Stall", - 5: "Position Fault", - 6: "Comm/Error", - 7: "Unknown", - } - return [labels.get(i, f"b{i}") for i in range(8) if ((val >> i) & 1) == 1] - def _bits(b: int) -> str: - bits = [f"b{i}" for i in range(8) if ((b >> i) & 1) == 1] - return ",".join(bits) - sample["unloading_bits"] = _bits(unload) - sample["led_alarm_bits"] = _bits(led) - sample["unloading_names"] = _names_for(unload) - sample["led_alarm_names"] = _names_for(led) - # Active status (live) - try: - sbits = _bits(status_byte) - sample["status_bits"] = sbits - # Map status bits to readable names (best-effort) - def _status_names(val: int) -> list[str]: - labels = { - 0: "Overload", - 1: "Overheat", - 2: "Overvoltage", - 3: "Undervoltage", - 4: "Stall", - 5: "Position Fault", - 6: "Comm/Error", - 7: "Unknown", - } - return [labels.get(i, f"b{i}") for i in range(8) if ((val >> i) & 1) == 1] - sample["status_names"] = _status_names(status_byte) - except Exception: - pass + + # Parse each block using backend-specific parsing + for block_idx, raw_block in enumerate(block_data): + data = raw_block.get(sid) + if data: + parsed = backend_registry.parse_telemetry_block(block_idx, data) + sample.update(parsed) + if sample: servos[str(sid)] = sample + if servos: msg["servos"] = servos except Exception: # Do not let telemetry extras break the main joints stream pass + # Drain any alerts collected by lower layers and attach them try: drained = _alerts.drain_alerts(max_items=25) @@ -226,6 +329,7 @@ def _status_names(val: int) -> list[str]: msg["alerts"] = drained except Exception: pass + if telemetry_target is not None: udp.sendto(json.dumps(msg).encode("utf-8"), telemetry_target) except Exception: @@ -264,7 +368,7 @@ def _status_names(val: int) -> list[str]: calibrating_servo_id = None calibration_client_addr = None else: # Continue streaming calibration data - raw_pos = servo_protocol.read_servo_position(calibrating_servo_id) + raw_pos = servo_driver.read_single_servo_position(calibrating_servo_id) if raw_pos is not None: reply = f"CALIB_DATA,{calibrating_servo_id},{raw_pos}" sock.sendto(reply.encode("utf-8"), calibration_client_addr) @@ -286,23 +390,16 @@ def _status_names(val: int) -> list[str]: elif command == "SET_ZERO": try: - joint_num = int(parts[1]) # Expect 1-6 now - - if not (1 <= joint_num <= utils.NUM_LOGICAL_JOINTS or joint_num == 7): # 7 is for gripper - print("[Controller] Error: Joint number must be 1-6 for arm, or 7 for gripper.") + joint_num = int(parts[1]) # Expect 1-6 for arm, 7 for gripper + + # Get joint-to-servo mapping from robot config + joint_to_servo_ids = selected_robot.logical_joint_to_actuator_ids + max_joint = max(joint_to_servo_ids.keys()) + + if joint_num not in joint_to_servo_ids: + print(f"[Controller] Error: Joint number must be 1-{selected_robot.num_logical_joints} for arm, or {max_joint} for gripper.") continue - # Map logical joint numbers to their physical servo IDs - joint_to_servo_ids = { - 1: [10], # Base - 2: [20, 21], # Shoulder - 3: [30, 31], # Elbow - 4: [40], # Wrist roll - 5: [50], # Wrist pitch - 6: [60], # Wrist yaw - 7: [utils.SERVO_ID_GRIPPER], # Gripper - } - servos_to_zero = joint_to_servo_ids[joint_num] print(f"[Controller] SET_ZERO (Joint {joint_num}) will calibrate servos: {servos_to_zero}") @@ -318,14 +415,28 @@ def _status_names(val: int) -> list[str]: print(f"[Controller] WARNING: Received FACTORY_RESET for Servo ID: {servo_id_to_reset}.") print("[Controller] This will reset all EEPROM values (PID, offsets, limits) to factory defaults, except for the ID.") - if servo_protocol.factory_reset_servo(servo_id_to_reset): + # Use backend if available, otherwise fall back to servo_protocol + backend = backend_registry.get_active_backend() + reset_success = False + if backend and hasattr(backend, 'factory_reset_actuator'): + reset_success = backend.factory_reset_actuator(servo_id_to_reset) + else: + reset_success = servo_protocol.factory_reset_servo(servo_id_to_reset) + + if reset_success: print(f"[Controller] Factory reset command sent to servo {servo_id_to_reset}.") # Add a longer delay for the servo to process the EEPROM write before restarting. print("[Controller] Waiting 1 second for servo to process reset...") time.sleep(1.0) print(f"[Controller] Now sending RESTART command to servo ID {servo_id_to_reset}.") - if servo_protocol.restart_servo(servo_id_to_reset): + restart_success = False + if backend and hasattr(backend, 'restart_actuator'): + restart_success = backend.restart_actuator(servo_id_to_reset) + else: + restart_success = servo_protocol.restart_servo(servo_id_to_reset) + + if restart_success: print(f"[Controller] Servo {servo_id_to_reset} has been reset and restarted.") # CRITICAL: Re-initialize the servo with our application's settings time.sleep(1.0) # Wait for servo to be fully online after restart @@ -339,13 +450,17 @@ def _status_names(val: int) -> list[str]: elif command == "GET_ALL_POSITIONS": # Use a single SYNC READ command for faster bulk feedback - positions_dict = servo_protocol.sync_read_positions(utils.SERVO_IDS) + backend = backend_registry.get_active_backend() + if backend and hasattr(backend, 'sync_read_positions'): + positions_dict = backend.sync_read_positions(utils.SERVO_IDS) + else: + positions_dict = servo_protocol.sync_read_positions(utils.SERVO_IDS) # If the sync read failed, fall back to the slower per-servo read to maintain functionality if positions_dict is None: positions_dict = {} for s_id in utils.SERVO_IDS: - raw_pos = servo_protocol.read_servo_position(s_id) + raw_pos = servo_driver.read_single_servo_position(s_id) positions_dict[s_id] = raw_pos time.sleep(0.01) # brief spacing to avoid overwhelming the bus @@ -857,7 +972,7 @@ def _status_names(val: int) -> list[str]: except socket.timeout: if in_calibration_mode and calibrating_servo_id is not None: # Keep streaming calibration data if no new command arrives - raw_pos = servo_protocol.read_servo_position(calibrating_servo_id) + raw_pos = servo_driver.read_single_servo_position(calibrating_servo_id) if raw_pos is not None: reply = f"CALIB_DATA,{calibrating_servo_id},{raw_pos}" sock.sendto(reply.encode("utf-8"), calibration_client_addr) @@ -875,6 +990,14 @@ def _status_names(val: int) -> list[str]: finally: print("[Controller] Shutting down.") sock.close() + + # Shutdown the backend instance (closes serial port, releases resources) + try: + backend_registry.shutdown_backend() + except Exception as e: + print(f"[Controller] Backend shutdown error: {e}") + + # Legacy: Also close the global serial port if open if utils.ser and utils.ser.is_open: utils.ser.close() print("[Controller] Serial port closed.") diff --git a/src/gradient_os/telemetry/__init__.py b/src/gradient_os/telemetry/__init__.py index 8d8df0ed..3d5213ff 100644 --- a/src/gradient_os/telemetry/__init__.py +++ b/src/gradient_os/telemetry/__init__.py @@ -1,4 +1,7 @@ from .publisher import UdpTelemetryPublisher # noqa: F401 -from .mjpeg import MjpegStream # noqa: F401 - +# MjpegStream requires OpenCV which is optional - only import if available +try: + from .mjpeg import MjpegStream # noqa: F401 +except ImportError: + MjpegStream = None # type: ignore diff --git a/todo.text b/todo.text new file mode 100644 index 00000000..e69de29b