diff --git a/src/holosoma_inference/docker/Dockerfile b/src/holosoma_inference/docker/Dockerfile index 2c392689d..46436962b 100644 --- a/src/holosoma_inference/docker/Dockerfile +++ b/src/holosoma_inference/docker/Dockerfile @@ -7,7 +7,10 @@ WORKDIR /workspace/ # Install ohmybash + dev tooling RUN bash -c "$(curl -fsSL https://raw.githubusercontent.com/ohmybash/oh-my-bash/master/tools/install.sh)" -RUN apt-get update && apt-get install -y python3-pip vim +RUN apt-get update && apt-get install -y python3-pip vim ros-humble-rmw-cyclonedds-cpp + +# Upgrade pip for PEP 660 editable install support (ros:humble ships an old pip) +RUN pip3 install --upgrade pip setuptools # Install holosoma_inference (as the last step, since the cache of this layer will get invalidated frequently) # NOTE: This assumes the build context is holosoma/src/ (see build.sh) diff --git a/src/holosoma_inference/docker/run.sh b/src/holosoma_inference/docker/run.sh index 4312a2592..50baa181e 100755 --- a/src/holosoma_inference/docker/run.sh +++ b/src/holosoma_inference/docker/run.sh @@ -3,10 +3,12 @@ # Parse command line arguments NEW_CONTAINER=false EXT_DIR="" +EXTRA_MOUNTS=() while [[ $# -gt 0 ]]; do case $1 in --new) NEW_CONTAINER=true; shift ;; --ext-repo-path) EXT_DIR="$2"; shift 2 ;; + --extra-mount) EXTRA_MOUNTS+=("$2"); shift 2 ;; *) shift ;; esac done @@ -34,6 +36,9 @@ create_container() { -v "$HOLOSOMA_DEPS_DIR/bash_history":/root/.bash_history ) [[ -d "$EXT_DIR" ]] && mounts+=(-v "$EXT_DIR":/workspace/holosoma-extension) # optionally mount extension repo + for m in "${EXTRA_MOUNTS[@]}"; do + mounts+=(-v "$m") + done docker run -it \ --privileged \ diff --git a/src/holosoma_inference/docs/how_to_add_a_new_robot.md b/src/holosoma_inference/docs/how_to_add_a_new_robot.md new file mode 100644 index 000000000..f507c2456 --- /dev/null +++ b/src/holosoma_inference/docs/how_to_add_a_new_robot.md @@ -0,0 +1,89 @@ +# Adding a New Robot to Holosoma Inference + +This document is meant to serve as a high-level overview on how to add a new robot to work with `holosoma_inference`. + +--- + +## 1. Implement Interface. + +Create `MyRobotInterface(BaseInterface)` and implement all abstract methods. Most importantly, `get_low_state()` and `send_low_command()`: + +```python +from holosoma_inference.sdk.base.base_interface import BaseInterface + +class MyRobotInterface(BaseInterface): + + def __init__(self, robot_config, domain_id=0, interface_str=None, use_joystick=True): + super().__init__(robot_config, domain_id, interface_str, use_joystick) + # Initialize your SDK / communication layer here + + def get_low_state(self) -> np.ndarray: + """Return shape (1, 3+4+N+3+3+N) array: + [base_pos(3), quat(4), joint_pos(N), lin_vel(3), ang_vel(3), joint_vel(N)] + """ + ... + + def send_low_command(self, cmd_q, cmd_dq, cmd_tau, + dof_pos_latest=None, kp_override=None, kd_override=None): + """Map joint-space commands to your robot's motor API.""" + ... +``` + +Notes: +- `MyRobotInterface` can use robot-specific IPC (like ROS2) under the hood to communicate with the hardware. However, those libraries must not be required dependencies of `holosoma` or `holosoma_inference` itself. +- `get_low_state()` must return a `(1, 2*(3+N)+4+3)` numpy array in the exact field order above. The policy reads this array by offset, so the layout matters. +- `send_low_command()` receives arrays of length `num_joints` (joint-space). You are responsible for remapping to motor-space if they differ. +- Gain properties (`kp_level`/`kd_level`) are float multipliers (default 1.0) that scale `motor_kp`/`motor_kd` from the config. + +--- + +## 2. Implement `BasicSdk2Bridge` (simulation only) + +In order to test `sim2sim` workflow, create `bridge/myrobot/myrobot_sdk2py_bridge.py` and subclass `BasicSdk2Bridge`. Implement the four abstract methods: + +```python +from holosoma.bridge.base.basic_sdk2py_bridge import BasicSdk2Bridge + +class MyRobotSdk2Bridge(BasicSdk2Bridge): + + def _init_sdk_components(self): + """Initialize SDK-specific state (message types, publishers, etc.).""" + ... + + def low_cmd_handler(self, msg): + """Handle incoming low-level command messages from the policy.""" + ... + + def publish_low_state(self): + """Read simulator state and publish it in your SDK's format.""" + ... + + def compute_torques(self): + """Compute motor torques from the latest command. + Use the helper `_compute_pd_torques()` for standard PD control. + """ + ... +``` + +Notes: +- Helper methods `_get_dof_states()`, `_get_base_imu_data()`, and `_get_actuator_forces()` are provided by the base class for reading simulator state. +- `_compute_pd_torques(tau_ff, kp, kd, q_target, dq_target)` handles PD control + torque limiting — use it in `compute_torques()` unless you need custom logic. +- See `bridge/unitree/unitree_sdk2py_bridge.py` for a complete example. + +--- + +## 3. Register entrypoints + +It's possible for your package implementation to live in a separate package separate from `holosoma`. +To achieve that, implement `MyRobotSdk2Bridge` and `MyRobotInterface` in your python package, and register the SDK using [entry points](https://packaging.python.org/en/latest/specifications/entry-points/). Thanks to that, `create_interface()` and `create_sdk2py_bridge()` will discover your implementations at runtime. + +In your `pyproject.toml`: +```toml +[project.entry-points."holosoma.sdk"] +myrobot = "my_package.sdk.myrobot_interface:MyRobotInterface" + +[project.entry-points."holosoma.bridge"] +myrobot = "my_package.bridge.myrobot_sdk2py_bridge:MyRobotSdk2Bridge" +``` + +The key (e.g. `myrobot`) must match the `sdk_type` field in your robot config. Once your package is pip-installed, `create_interface()` and `create_sdk2py_bridge()` will resolve your implementation by name without any changes to the core codebase. diff --git a/src/holosoma_inference/holosoma_inference/sdk/__init__.py b/src/holosoma_inference/holosoma_inference/sdk/__init__.py index f979c79cc..1d77fb15b 100644 --- a/src/holosoma_inference/holosoma_inference/sdk/__init__.py +++ b/src/holosoma_inference/holosoma_inference/sdk/__init__.py @@ -4,13 +4,15 @@ from importlib.metadata import entry_points +from holosoma_inference.utils.network import detect_robot_interface + # Auto-discover SDK interfaces from installed packages using lazy loading. # Lazy loading is to avoid errors from SDK dependencies from extensions (e.g. ROS2) when working with other SDKs. _entry_points = {ep.name: ep for ep in entry_points(group="holosoma.sdk")} _registry = {} # Cache for loaded interfaces -def create_interface(robot_config, domain_id=0, interface_str=None, use_joystick=True): +def create_interface(robot_config, domain_id=0, interface_str="auto", use_joystick=True): """Create interface from registry. If *interface_str* is ``"auto"``, the network interface is resolved @@ -18,8 +20,6 @@ def create_interface(robot_config, domain_id=0, interface_str=None, use_joystick """ # Resolve "auto" interface before passing to the SDK backend if interface_str == "auto": - from holosoma_inference.utils.network import detect_robot_interface - interface_str = detect_robot_interface() sdk_type = robot_config.sdk_type @@ -31,3 +31,20 @@ def create_interface(robot_config, domain_id=0, interface_str=None, use_joystick _registry[sdk_type] = _entry_points[sdk_type].load() return _registry[sdk_type](robot_config, domain_id, interface_str, use_joystick) + + +def create_interface_with_default_config(robot_name, domain_id=0, interface_str="auto", use_joystick=True): + """Create interface using a built-in robot config by name. + + Example:: + + robot = create_interface_with_default_config("g1_29dof") + """ + from holosoma_inference.config.config_values.robot import get_defaults + + defaults = get_defaults() + key = robot_name.replace("_", "-") # Accept both g1-29dof and g1_29dof + if key not in defaults: + raise ValueError(f"Unknown robot config: {robot_name!r}. Available: {sorted(defaults.keys())}") + + return create_interface(defaults[key], domain_id, interface_str, use_joystick) diff --git a/src/holosoma_inference/holosoma_inference/sdk/unitree/unitree_interface.py b/src/holosoma_inference/holosoma_inference/sdk/unitree/unitree_interface.py index d0327d9bc..b9c571449 100644 --- a/src/holosoma_inference/holosoma_inference/sdk/unitree/unitree_interface.py +++ b/src/holosoma_inference/holosoma_inference/sdk/unitree/unitree_interface.py @@ -1,5 +1,7 @@ """Unitree robot interface using C++/pybind11 binding.""" +from __future__ import annotations + import numpy as np from holosoma_inference.config.config_types import RobotConfig @@ -9,7 +11,7 @@ class UnitreeInterface(BaseInterface): """Interface for Unitree robots using C++/pybind11 binding.""" - def __init__(self, robot_config: RobotConfig, domain_id=0, interface_str=None, use_joystick=True): + def __init__(self, robot_config: RobotConfig, domain_id=0, interface_str: str | None = None, use_joystick=True): super().__init__(robot_config, domain_id, interface_str, use_joystick) self._unitree_motor_order = None self._kp_level = 1.0