diff --git a/GR00T-WholeBodyControl/.github/workflows/docs.yml b/GR00T-WholeBodyControl/.github/workflows/docs.yml new file mode 100644 index 0000000000000000000000000000000000000000..4ecd80bcf8e6cc467aeea52136bfd4087f124e08 --- /dev/null +++ b/GR00T-WholeBodyControl/.github/workflows/docs.yml @@ -0,0 +1,74 @@ +name: Build and Deploy Documentation + +on: + push: + branches: + - main + - gear-sonic + paths: + - "docs/**" + - ".github/workflows/docs.yml" + - ".gitattributes" + workflow_dispatch: + +# Allow only one concurrent deployment; cancel in-flight runs +concurrency: + group: "pages" + cancel-in-progress: true + +jobs: + build: + name: Build Sphinx Docs + runs-on: ubuntu-latest + steps: + - name: Checkout repository + uses: actions/checkout@v4 + with: + lfs: false + + - name: Restore docs static assets (bypass git-lfs smudge) + run: | + # git-lfs on the runner rewrites files tracked by *.png/*.gif + # even when our .gitattributes override removes filter=lfs. + # Use git cat-file to write real binary content directly from + # the object store, bypassing all smudge filters. + git ls-tree -r HEAD -- docs/source/_static \ + | awk '{print $3, $4}' \ + | while IFS=" " read -r hash path; do + git cat-file blob "$hash" > "$path" + done + + - name: Set up Python + uses: actions/setup-python@v5 + with: + python-version: "3.10" + cache: "pip" + cache-dependency-path: "docs/requirements.txt" + + - name: Install documentation dependencies + run: pip install -r docs/requirements.txt + + - name: Build HTML documentation + run: sphinx-build -b html docs/source docs/build/html + + - name: Upload Pages artifact + if: github.ref == 'refs/heads/main' || github.ref == 'refs/heads/gear-sonic' + uses: actions/upload-pages-artifact@v3 + with: + path: docs/build/html + + deploy: + name: Deploy to GitHub Pages + needs: build + if: github.ref == 'refs/heads/main' || github.ref == 'refs/heads/gear-sonic' + runs-on: ubuntu-latest + permissions: + pages: write + id-token: write + environment: + name: github-pages + url: ${{ steps.deployment.outputs.page_url }} + steps: + - name: Deploy to GitHub Pages + id: deployment + uses: actions/deploy-pages@v4 diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/__init__.py b/GR00T-WholeBodyControl/decoupled_wbc/control/__init__.py new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/base/__init__.py b/GR00T-WholeBodyControl/decoupled_wbc/control/base/__init__.py new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/base/env.py b/GR00T-WholeBodyControl/decoupled_wbc/control/base/env.py new file mode 100644 index 0000000000000000000000000000000000000000..dcca21e547d3b163e9d6f25b1ae114296c8f2f1c --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/base/env.py @@ -0,0 +1,45 @@ +import gymnasium as gym + + +class Env: + """Base interface for all environments in the Gr00t framework""" + + def observe(self) -> dict[str, any]: + """Read the current state of this environment + + Returns: + dict: A dictionary of observations + """ + pass + + def queue_action(self, action: dict[str, any]): + """Queue an action to be executed + + Args: + action: A dictionary of action parameters + """ + pass + + def reset(self, **kwargs): + """Reset this environment to initial state""" + pass + + def observation_space(self) -> gym.Space: + """Get the observation space of this environment + + Returns: + gym.Space: The observation space + """ + pass + + def action_space(self) -> gym.Space: + """Get the action space of this environment + + Returns: + gym.Space: The action space + """ + pass + + def close(self): + """Close and clean up this environment""" + pass diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/base/humanoid_env.py b/GR00T-WholeBodyControl/decoupled_wbc/control/base/humanoid_env.py new file mode 100644 index 0000000000000000000000000000000000000000..59515970bb39e03615a02413af1b247413369459 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/base/humanoid_env.py @@ -0,0 +1,60 @@ +from abc import abstractmethod + +from decoupled_wbc.control.base.env import Env +from decoupled_wbc.control.base.sensor import Sensor +from decoupled_wbc.control.robot_model.robot_model import RobotModel + + +class Hands: + """Container class for left and right hand environments. + + Attributes: + left: Environment for the left hand + right: Environment for the right hand + """ + + left: Env + right: Env + + +class HumanoidEnv(Env): + """Base class for humanoid robot environments. + + This class provides the interface for accessing the robot's body, hands, and sensors. + """ + + def body(self) -> Env: + """Get the robot's body environment. + + Returns: + Env: The body environment + """ + pass + + def hands(self) -> Hands: + """Get the robot's hands. + + Returns: + Hands: Container with left and right hand environments + """ + pass + + def sensors(self) -> dict[str, Sensor]: + """Get the sensors of this environment + + Returns: + dict: A dictionary of sensors + """ + pass + + @abstractmethod + def robot_model(self) -> RobotModel: + """Get the robot model of this environment + This robot model is used to dispatch whole body actions to body + and hand actuators and to reconstruct proprioceptive + observations from body and hands. + + Returns: + RobotModel: The robot model + """ + pass diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/base/policy.py b/GR00T-WholeBodyControl/decoupled_wbc/control/base/policy.py new file mode 100644 index 0000000000000000000000000000000000000000..a6ebb7edff75e63e0fa30523b5110a3e10c4a468 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/base/policy.py @@ -0,0 +1,47 @@ +from abc import ABC, abstractmethod +from typing import Optional + + +class Policy(ABC): + """Base class for implementing control policies in the Gr00t framework. + + A Policy defines how an agent should behave in an environment by mapping observations + to actions. This abstract base class provides the interface that all concrete policy + implementations must follow. + """ + + def set_goal(self, goal: dict[str, any]): + """Set the command from the planner that the policy should follow. + + Args: + goal: Dictionary containing high-level commands or goals from the planner + """ + pass + + def set_observation(self, observation: dict[str, any]): + """Update the policy's current observation of the environment. + + Args: + observation: Dictionary containing the current state/observation of the environment + """ + self.observation = observation + + @abstractmethod + def get_action(self, time: Optional[float] = None) -> dict[str, any]: + """Compute and return the next action at the specified time, based on current observation + and planner command. + + Args: + time: Optional "monotonic time" for time-dependent policies + + Returns: + Dictionary containing the action to be executed + """ + + def close(self): + """Clean up any resources used by the policy.""" + pass + + def reset(self): + """Reset the policy to its initial state.""" + pass diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/base/sensor.py b/GR00T-WholeBodyControl/decoupled_wbc/control/base/sensor.py new file mode 100644 index 0000000000000000000000000000000000000000..c9e5ba81d1075354303c87bea39aaa55635f991b --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/base/sensor.py @@ -0,0 +1,35 @@ +import gymnasium as gym + + +class Sensor: + """Base class for implementing sensors in the Gr00t framework. + + A Sensor provides information about a specific sensor on the robot (e.g. camera, IMU, + force sensor). This abstract base class defines the interface that all concrete sensor + implementations must follow. + """ + + def read(self, **kwargs) -> any: + """Read the current sensor value. + + Args: + **kwargs: Additional parameters specific to the sensor implementation + (e.g. camera resolution, sampling rate) + + Returns: + The sensor reading value (e.g. image data, acceleration measurements) + """ + pass + + def observation_space(self) -> gym.Space: + """Get the observation space of this sensor. + + Returns: + gym.Space: The observation space defining the shape and bounds of sensor readings + (e.g. image dimensions for camera, measurement ranges for IMU) + """ + pass + + def close(self): + """Clean up any resources used by the sensor.""" + pass diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/envs/__init__.py b/GR00T-WholeBodyControl/decoupled_wbc/control/envs/__init__.py new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/main/__init__.py b/GR00T-WholeBodyControl/decoupled_wbc/control/main/__init__.py new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/main/config_template.py b/GR00T-WholeBodyControl/decoupled_wbc/control/main/config_template.py new file mode 100644 index 0000000000000000000000000000000000000000..8da84d56b969f33a7d0604f026ef53a04571ffeb --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/main/config_template.py @@ -0,0 +1,45 @@ +from dataclasses import asdict, dataclass +from typing import Any + + +@dataclass +class ArgsConfig: + """Args Config for running the data collection loop.""" + + def update( + self, + config_dict: dict, + strict: bool = False, + skip_keys: list[str] = [], + allowed_keys: list[str] | None = None, + ): + for k, v in config_dict.items(): + if k in skip_keys: + continue + if allowed_keys is not None and k not in allowed_keys: + continue + if strict and not hasattr(self, k): + raise ValueError(f"Config {k} not found in {self.__class__.__name__}") + if not strict and not hasattr(self, k): + continue + setattr(self, k, v) + + @classmethod + def from_dict( + cls, + config_dict: dict, + strict: bool = False, + skip_keys: list[str] = [], + allowed_keys: list[str] | None = None, + ): + instance = cls() + instance.update( + config_dict=config_dict, strict=strict, skip_keys=skip_keys, allowed_keys=allowed_keys + ) + return instance + + def to_dict(self): + return asdict(self) + + def get(self, key: str, default: Any = None): + return getattr(self, key) if hasattr(self, key) else default diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/main/constants.py b/GR00T-WholeBodyControl/decoupled_wbc/control/main/constants.py new file mode 100644 index 0000000000000000000000000000000000000000..8d54f25a9c46802be4d64c21e4133a88de489dc4 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/main/constants.py @@ -0,0 +1,16 @@ +IMAGE_TOPIC_NAME = "realsense/color/image_raw" +STATE_TOPIC_NAME = "G1Env/env_state_act" +CONTROL_GOAL_TOPIC = "ControlPolicy/upper_body_pose" +ROBOT_CONFIG_TOPIC = "WBCPolicy/robot_config" +KEYBOARD_INPUT_TOPIC = "/keyboard_input" +LOCO_MANIP_TASK_STATUS_TOPIC = "LocoManipPolicy/task_status" +LOCO_NAV_TASK_STATUS_TOPIC = "NavigationPolicy/task_status" +LOWER_BODY_POLICY_STATUS_TOPIC = "ControlPolicy/lower_body_policy_status" +JOINT_SAFETY_STATUS_TOPIC = "ControlPolicy/joint_safety_status" + + +DEFAULT_NAV_CMD = [0.0, 0.0, 0.0] +DEFAULT_BASE_HEIGHT = 0.74 +DEFAULT_WRIST_POSE = [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0] * 2 # x, y, z + w, x, y, z + +DEFAULT_MODEL_SERVER_PORT = 5555 # port used to host the model server diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/policy/__init__.py b/GR00T-WholeBodyControl/decoupled_wbc/control/policy/__init__.py new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/policy/g1_decoupled_whole_body_policy.py b/GR00T-WholeBodyControl/decoupled_wbc/control/policy/g1_decoupled_whole_body_policy.py new file mode 100644 index 0000000000000000000000000000000000000000..d3f457f8b1057e84ece093a98c72c066a45ffb5b --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/policy/g1_decoupled_whole_body_policy.py @@ -0,0 +1,157 @@ +import time as time_module +from typing import Optional + +import numpy as np +from pinocchio import rpy + +from decoupled_wbc.control.base.policy import Policy +from decoupled_wbc.control.main.constants import DEFAULT_NAV_CMD + + +class G1DecoupledWholeBodyPolicy(Policy): + """ + This class implements a whole-body policy for the G1 robot by combining an upper-body + policy and a lower-body RL-based policy. + It is designed to work with the G1 robot's specific configuration and control requirements. + """ + + def __init__( + self, + robot_model, + lower_body_policy: Policy, + upper_body_policy: Policy, + ): + self.robot_model = robot_model + self.lower_body_policy = lower_body_policy + self.upper_body_policy = upper_body_policy + self.last_goal_time = time_module.monotonic() + self.is_in_teleop_mode = False # Track if lower body is in teleop mode + + def set_observation(self, observation): + # Upper body policy is open loop (just interpolation), so we don't need to set the observation + self.lower_body_policy.set_observation(observation) + + def set_goal(self, goal): + """ + Set the goal for both upper and lower body policies. + + Args: + goal: Command from the planners + goal["target_upper_body_pose"]: Target pose for the upper body policy + goal["target_time"]: Target goal time + goal["interpolation_garbage_collection_time"]: Waypoints earlier than this time are removed + goal["navigate_cmd"]: Target navigation velocities for the lower body policy + goal["base_height_command"]: Target base height for both upper and lower body policies + """ + # Update goal timestamp for timeout safety + self.last_goal_time = time_module.monotonic() + + upper_body_goal = {} + lower_body_goal = {} + + # Upper body goal keys + upper_body_keys = [ + "target_upper_body_pose", + "base_height_command", + "target_time", + "interpolation_garbage_collection_time", + "navigate_cmd", + ] + for key in upper_body_keys: + if key in goal: + upper_body_goal[key] = goal[key] + + # Always ensure navigate_cmd is present to prevent interpolation from old dangerous values + if "navigate_cmd" not in goal: + # Safety: Inject safe default navigate_cmd to ensure interpolation goes to stop + if "target_time" in goal and isinstance(goal["target_time"], list): + upper_body_goal["navigate_cmd"] = [np.array(DEFAULT_NAV_CMD)] * len( + goal["target_time"] + ) + else: + upper_body_goal["navigate_cmd"] = np.array(DEFAULT_NAV_CMD) + + # Set teleop policy command flag + has_teleop_commands = ("navigate_cmd" in goal) or ("base_height_command" in goal) + self.is_in_teleop_mode = has_teleop_commands # Track teleop state for timeout safety + self.lower_body_policy.set_use_teleop_policy_cmd(has_teleop_commands) + + # Lower body goal keys + lower_body_keys = [ + "toggle_stand_command", + "toggle_policy_action", + ] + for key in lower_body_keys: + if key in goal: + lower_body_goal[key] = goal[key] + + self.upper_body_policy.set_goal(upper_body_goal) + self.lower_body_policy.set_goal(lower_body_goal) + + def get_action(self, time: Optional[float] = None): + current_time = time if time is not None else time_module.monotonic() + + # Safety timeout: Only apply when in teleop mode (communication loss dangerous) + # When in keyboard mode, no timeout needed (user controls directly) + if self.is_in_teleop_mode: + time_since_goal = current_time - self.last_goal_time + if time_since_goal > 1.0: # 1 second timeout + print( + f"SAFETY: Teleop mode timeout after {time_since_goal:.1f}s, injecting safe goal" + ) + # Inject safe goal to trigger all safety mechanisms (gear_wbc reset + interpolation reset) + safe_goal = { + "target_time": current_time + 0.1, + "interpolation_garbage_collection_time": current_time - 1.0, + } + self.set_goal( + safe_goal + ) # This will reset is_in_teleop_mode to False and trigger all safety + + # Get indices for groups + lower_body_indices = self.robot_model.get_joint_group_indices("lower_body") + upper_body_indices = self.robot_model.get_joint_group_indices("upper_body") + + # Initialize full configuration with zeros + q = np.zeros(self.robot_model.num_dofs) + + upper_body_action = self.upper_body_policy.get_action(time) + q[upper_body_indices] = upper_body_action["target_upper_body_pose"] + q_arms = q[self.robot_model.get_joint_group_indices("arms")] + base_height_command = upper_body_action.get("base_height_command", None) + interpolated_navigate_cmd = upper_body_action.get("navigate_cmd", None) + + # Compute torso orientation relative to waist, to pass to lower body policy + self.robot_model.cache_forward_kinematics(q, auto_clip=False) + torso_orientation = self.robot_model.frame_placement("torso_link").rotation + waist_orientation = self.robot_model.frame_placement("pelvis").rotation + # Extract yaw from rotation matrix and create a rotation with only yaw + # The rotation property is a 3x3 numpy array + waist_yaw = np.arctan2(waist_orientation[1, 0], waist_orientation[0, 0]) + # Create a rotation matrix with only yaw using Pinocchio's rpy functions + waist_yaw_only_rotation = rpy.rpyToMatrix(0, 0, waist_yaw) + yaw_only_waist_from_torso = waist_yaw_only_rotation.T @ torso_orientation + torso_orientation_rpy = rpy.matrixToRpy(yaw_only_waist_from_torso) + + lower_body_action = self.lower_body_policy.get_action( + time, q_arms, base_height_command, torso_orientation_rpy, interpolated_navigate_cmd + ) + + # If pelvis is both in upper and lower body, lower body policy takes preference + q[lower_body_indices] = lower_body_action["body_action"][0][ + : len(lower_body_indices) + ] # lower body (legs + waist) + + self.last_action = {"q": q} + + return {"q": q} + + def handle_keyboard_button(self, key): + try: + self.lower_body_policy.locomotion_policy.handle_keyboard_button(key) + except AttributeError: + # Only catch AttributeError, let other exceptions propagate + self.lower_body_policy.handle_keyboard_button(key) + + def activate_policy(self): + self.handle_keyboard_button("]") diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/policy/g1_gear_wbc_policy.py b/GR00T-WholeBodyControl/decoupled_wbc/control/policy/g1_gear_wbc_policy.py new file mode 100644 index 0000000000000000000000000000000000000000..02021d63a1e256001159b7b1dec813d2215080c3 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/policy/g1_gear_wbc_policy.py @@ -0,0 +1,295 @@ +import collections +from pathlib import Path +from typing import Any, Dict, Optional + +import numpy as np +import onnxruntime as ort +import torch + +from decoupled_wbc.control.base.policy import Policy +from decoupled_wbc.control.utils.gear_wbc_utils import get_gravity_orientation, load_config + + +class G1GearWbcPolicy(Policy): + """Simple G1 robot policy using OpenGearWbc trained neural network.""" + + def __init__(self, robot_model, config: str, model_path: str): + """Initialize G1GearWbcPolicy. + + Args: + config_path: Path to gear_wbc YAML configuration file + """ + self.config, self.LEGGED_GYM_ROOT_DIR = load_config(config) + self.robot_model = robot_model + self.use_teleop_policy_cmd = False + + package_root = Path(__file__).resolve().parents[2] + self.sim2mujoco_root_dir = str(package_root / "sim2mujoco") + model_path_1, model_path_2 = model_path.split(",") + + self.policy_1 = self.load_onnx_policy( + self.sim2mujoco_root_dir + "/resources/robots/g1/" + model_path_1 + ) + self.policy_2 = self.load_onnx_policy( + self.sim2mujoco_root_dir + "/resources/robots/g1/" + model_path_2 + ) + + # Initialize observation history buffer + self.observation = None + self.obs_history = collections.deque(maxlen=self.config["obs_history_len"]) + self.obs_buffer = np.zeros(self.config["num_obs"], dtype=np.float32) + self.counter = 0 + + # Initialize state variables + self.use_policy_action = False + self.action = np.zeros(self.config["num_actions"], dtype=np.float32) + self.target_dof_pos = self.config["default_angles"].copy() + self.cmd = self.config["cmd_init"].copy() + self.height_cmd = self.config["height_cmd"] + self.freq_cmd = self.config["freq_cmd"] + self.roll_cmd = self.config["rpy_cmd"][0] + self.pitch_cmd = self.config["rpy_cmd"][1] + self.yaw_cmd = self.config["rpy_cmd"][2] + self.gait_indices = torch.zeros((1), dtype=torch.float32) + + def load_onnx_policy(self, model_path: str): + print(f"Loading ONNX policy from {model_path}") + model = ort.InferenceSession(model_path) + + def run_inference(input_tensor): + ort_inputs = {model.get_inputs()[0].name: input_tensor.cpu().numpy()} + ort_outs = model.run(None, ort_inputs) + return torch.tensor(ort_outs[0], device="cpu") + + print(f"Successfully loaded ONNX policy from {model_path}") + + return run_inference + + def compute_observation(self, observation: Dict[str, Any]) -> tuple[np.ndarray, int]: + """Compute the observation vector from current state""" + # Get body joint indices (excluding waist roll and pitch) + self.gait_indices = torch.remainder(self.gait_indices + 0.02 * self.freq_cmd, 1.0) + durations = torch.full_like(self.gait_indices, 0.5) + phases = 0.5 + foot_indices = [ + self.gait_indices + phases, # FL + self.gait_indices, # FR + ] + self.foot_indices = torch.remainder( + torch.cat([foot_indices[i].unsqueeze(1) for i in range(2)], dim=1), 1.0 + ) + for fi in foot_indices: + stance = fi < durations + swing = fi >= durations + fi[stance] = fi[stance] * (0.5 / durations[stance]) + fi[swing] = 0.5 + (fi[swing] - durations[swing]) * (0.5 / (1 - durations[swing])) + + self.clock_inputs = torch.stack([torch.sin(2 * np.pi * fi) for fi in foot_indices], dim=1) + + body_indices = self.robot_model.get_joint_group_indices("body") + body_indices = [idx for idx in body_indices] + + n_joints = len(body_indices) + + # Extract joint data + qj = observation["q"][body_indices].copy() + dqj = observation["dq"][body_indices].copy() + + # Extract floating base data + quat = observation["floating_base_pose"][3:7].copy() # quaternion + omega = observation["floating_base_vel"][3:6].copy() # angular velocity + + # Handle default angles padding + if len(self.config["default_angles"]) < n_joints: + padded_defaults = np.zeros(n_joints, dtype=np.float32) + padded_defaults[: len(self.config["default_angles"])] = self.config["default_angles"] + else: + padded_defaults = self.config["default_angles"][:n_joints] + + # Scale the values + qj_scaled = (qj - padded_defaults) * self.config["dof_pos_scale"] + dqj_scaled = dqj * self.config["dof_vel_scale"] + gravity_orientation = get_gravity_orientation(quat) + omega_scaled = omega * self.config["ang_vel_scale"] + + # Calculate single observation dimension + single_obs_dim = 86 # 3 + 1 + 3 + 3 + 3 + n_joints + n_joints + 15, n_joints = 29 + + # Create single observation + single_obs = np.zeros(single_obs_dim, dtype=np.float32) + single_obs[0:3] = self.cmd[:3] * self.config["cmd_scale"] + single_obs[3:4] = np.array([self.height_cmd]) + single_obs[4:7] = np.array([self.roll_cmd, self.pitch_cmd, self.yaw_cmd]) + single_obs[7:10] = omega_scaled + single_obs[10:13] = gravity_orientation + # single_obs[14:17] = omega_scaled_torso + # single_obs[17:20] = gravity_torso + single_obs[13 : 13 + n_joints] = qj_scaled + single_obs[13 + n_joints : 13 + 2 * n_joints] = dqj_scaled + single_obs[13 + 2 * n_joints : 13 + 2 * n_joints + 15] = self.action + # single_obs[13 + 2 * n_joints + 15 : 13 + 2 * n_joints + 15 + 2] = ( + # processed_clock_inputs.detach().cpu().numpy() + # ) + return single_obs, single_obs_dim + + def set_observation(self, observation: Dict[str, Any]): + """Update the policy's current observation of the environment. + + Args: + observation: Dictionary containing single observation from current state + Should include 'obs' key with current single observation + """ + + # Extract the single observation + self.observation = observation + single_obs, single_obs_dim = self.compute_observation(observation) + + # Update observation history every control_decimation steps + # if self.counter % self.config['control_decimation'] == 0: + # Add current observation to history + self.obs_history.append(single_obs) + + # Fill history with zeros if not enough observations yet + while len(self.obs_history) < self.config["obs_history_len"]: + self.obs_history.appendleft(np.zeros_like(single_obs)) + + # Construct full observation with history + single_obs_dim = len(single_obs) + for i, hist_obs in enumerate(self.obs_history): + start_idx = i * single_obs_dim + end_idx = start_idx + single_obs_dim + self.obs_buffer[start_idx:end_idx] = hist_obs + + # Convert to tensor for policy + self.obs_tensor = torch.from_numpy(self.obs_buffer).unsqueeze(0) + # self.counter += 1 + + assert self.obs_tensor.shape[1] == self.config["num_obs"] + + def set_use_teleop_policy_cmd(self, use_teleop_policy_cmd: bool): + self.use_teleop_policy_cmd = use_teleop_policy_cmd + # Safety: When teleop is disabled, reset navigation to stop + if not use_teleop_policy_cmd: + self.nav_cmd = self.config["cmd_init"].copy() # Reset to safe default + + def set_goal(self, goal: Dict[str, Any]): + """Set the goal for the policy. + + Args: + goal: Dictionary containing the goal for the policy + """ + + if "toggle_policy_action" in goal: + if goal["toggle_policy_action"]: + self.use_policy_action = not self.use_policy_action + + def get_action( + self, + time: Optional[float] = None, + arms_target_pose: Optional[np.ndarray] = None, + base_height_command: Optional[np.ndarray] = None, + torso_orientation_rpy: Optional[np.ndarray] = None, + interpolated_navigate_cmd: Optional[np.ndarray] = None, + ) -> Dict[str, Any]: + """Compute and return the next action based on current observation. + + Args: + time: Optional "monotonic time" for time-dependent policies (unused) + + Returns: + Dictionary containing the action to be executed + """ + if self.obs_tensor is None: + raise ValueError("No observation set. Call set_observation() first.") + + if base_height_command is not None and self.use_teleop_policy_cmd: + self.height_cmd = ( + base_height_command[0] + if isinstance(base_height_command, list) + else base_height_command + ) + + if interpolated_navigate_cmd is not None and self.use_teleop_policy_cmd: + self.cmd = interpolated_navigate_cmd + + if torso_orientation_rpy is not None and self.use_teleop_policy_cmd: + self.roll_cmd = torso_orientation_rpy[0] + self.pitch_cmd = torso_orientation_rpy[1] + self.yaw_cmd = torso_orientation_rpy[2] + + # Run policy inference + with torch.no_grad(): + # Select appropriate policy based on command magnitude + if np.linalg.norm(self.cmd) < 0.05: + # Use standing policy for small commands + policy = self.policy_1 + else: + # Use walking policy for movement commands + policy = self.policy_2 + + self.action = policy(self.obs_tensor).detach().numpy().squeeze() + + # Transform action to target_dof_pos + if self.use_policy_action: + cmd_q = self.action * self.config["action_scale"] + self.config["default_angles"] + else: + cmd_q = self.observation["q"][self.robot_model.get_joint_group_indices("lower_body")] + + cmd_dq = np.zeros(self.config["num_actions"]) + cmd_tau = np.zeros(self.config["num_actions"]) + + return {"body_action": (cmd_q, cmd_dq, cmd_tau)} + + def handle_keyboard_button(self, key): + if key == "]": + self.use_policy_action = True + elif key == "o": + self.use_policy_action = False + elif key == "w": + self.cmd[0] += 0.2 + elif key == "s": + self.cmd[0] -= 0.2 + elif key == "a": + self.cmd[1] += 0.2 + elif key == "d": + self.cmd[1] -= 0.2 + elif key == "q": + self.cmd[2] += 0.2 + elif key == "e": + self.cmd[2] -= 0.2 + elif key == "z": + self.cmd[0] = 0.0 + self.cmd[1] = 0.0 + self.cmd[2] = 0.0 + elif key == "1": + self.height_cmd += 0.1 + elif key == "2": + self.height_cmd -= 0.1 + elif key == "n": + self.freq_cmd -= 0.1 + self.freq_cmd = max(1.0, self.freq_cmd) + elif key == "m": + self.freq_cmd += 0.1 + self.freq_cmd = min(2.0, self.freq_cmd) + elif key == "3": + self.roll_cmd -= np.deg2rad(10) + elif key == "4": + self.roll_cmd += np.deg2rad(10) + elif key == "5": + self.pitch_cmd -= np.deg2rad(10) + elif key == "6": + self.pitch_cmd += np.deg2rad(10) + elif key == "7": + self.yaw_cmd -= np.deg2rad(10) + elif key == "8": + self.yaw_cmd += np.deg2rad(10) + + if key: + print("--------------------------------") + print(f"Linear velocity command: {self.cmd}") + print(f"Base height command: {self.height_cmd}") + print(f"Use policy action: {self.use_policy_action}") + print(f"roll deg angle: {np.rad2deg(self.roll_cmd)}") + print(f"pitch deg angle: {np.rad2deg(self.pitch_cmd)}") + print(f"yaw deg angle: {np.rad2deg(self.yaw_cmd)}") + print(f"Gait frequency: {self.freq_cmd}") diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/policy/identity_policy.py b/GR00T-WholeBodyControl/decoupled_wbc/control/policy/identity_policy.py new file mode 100644 index 0000000000000000000000000000000000000000..53c955dfb2bfd67d90de38ef4d9811112fe6dd96 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/policy/identity_policy.py @@ -0,0 +1,25 @@ +from copy import deepcopy +from typing import Optional + +import gymnasium as gym + +from decoupled_wbc.control.base.policy import Policy + + +class IdentityPolicy(Policy): + def __init__(self): + self.reset() + + def get_action(self, time: Optional[float] = None) -> dict[str, any]: + return self.goal + + def set_goal(self, goal: dict[str, any]) -> None: + self.goal = deepcopy(goal) + self.goal.pop("interpolation_garbage_collection_time", None) + self.goal.pop("target_time", None) + + def observation_space(self) -> gym.spaces.Dict: + return gym.spaces.Dict() + + def action_space(self) -> gym.spaces.Dict: + return gym.spaces.Dict() diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/policy/interpolation_policy.py b/GR00T-WholeBodyControl/decoupled_wbc/control/policy/interpolation_policy.py new file mode 100644 index 0000000000000000000000000000000000000000..03b88e725b8c3f891069879d91f4abf851fd8040 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/policy/interpolation_policy.py @@ -0,0 +1,297 @@ +import numbers +import time as time_module +from typing import Any, Dict, Optional, Union + +import gymnasium as gym +import numpy as np +import scipy.interpolate as si + +from decoupled_wbc.control.base.policy import Policy + + +class InterpolationPolicy(Policy): + def __init__( + self, + init_time: float, + init_values: dict[str, np.ndarray], + max_change_rate: float, + ): + """ + Args: + init_time: The time of recording the initial values. + init_values: The initial values of the features. + The keys are the names of the features, and the values + are the initial values of the features (1D array). + max_change_rate: The maximum change rate. + """ + super().__init__() + self.last_action = init_values # Vecs are 1D arrays + self.concat_order = sorted(init_values.keys()) + self.concat_dims = [] + for key in self.concat_order: + vec = np.array(init_values[key]) + if vec.ndim == 2 and vec.shape[0] == 1: + vec = vec[0] + init_values[key] = vec + assert vec.ndim == 1, f"The shape of {key} should be (D,). Got {vec.shape}." + self.concat_dims.append(vec.shape[0]) + + self.init_values_concat = self._concat_vecs(init_values, 1) + self.max_change_rate = max_change_rate + self.reset(init_time) + + def reset(self, init_time: float = time_module.monotonic()): + self.interp = PoseTrajectoryInterpolator(np.array([init_time]), self.init_values_concat) + self.last_waypoint_time = init_time + self.max_change_rate = self.max_change_rate + + def _concat_vecs(self, values: dict[str, np.ndarray], length: int) -> np.ndarray: + """ + Concatenate the vectors into a 2D array to be used for interpolation. + Args: + values: The values to concatenate. + length: The length of the concatenated vectors (time dimension). + Returns: + The concatenated vectors (T, D) arrays. + """ + concat_vecs = [] + for key in self.concat_order: + if key in values: + vec = np.array(values[key]) + if vec.ndim == 1: + # If the vector is 1D, tile it to the length of the time dimension + vec = np.tile(vec, (length, 1)) + assert vec.ndim == 2, f"The shape of {key} should be (T, D). Got {vec.shape}." + concat_vecs.append(vec) + else: + # If the vector is not in the values, use the last action + # Since the last action is 1D, we need to tile it to the length of the time dimension + concat_vecs.append(np.tile(self.last_action[key], (length, 1))) + return np.concatenate(concat_vecs, axis=1) # Vecs are 2D (T, D) arrays + + def _unconcat_vecs(self, concat_vec: np.ndarray) -> dict[str, np.ndarray]: + curr_idx = 0 + action = {} + assert ( + concat_vec.ndim == 1 + ), f"The shape of the concatenated vectors should be (T, D). Got {concat_vec.shape}." + for key, dim in zip(self.concat_order, self.concat_dims): + action[key] = concat_vec[curr_idx : curr_idx + dim] + curr_idx += dim + return action # Vecs are 1D arrays + + def __call__( + self, observation: Dict[str, Any], goal: Dict[str, Any], time: float + ) -> Dict[str, np.ndarray]: + raise NotImplementedError( + "`InterpolationPolicy` accepts goal and provide action in two separate methods." + ) + + def set_goal(self, goal: Dict[str, Any]) -> None: + if "target_time" not in goal: + return + assert ( + "interpolation_garbage_collection_time" in goal + ), "`interpolation_garbage_collection_time` is required." + target_time = goal.pop("target_time") + interpolation_garbage_collection_time = goal.pop("interpolation_garbage_collection_time") + + if isinstance(target_time, list): + for key, vec in goal.items(): + assert isinstance(vec, list) + assert len(vec) == len(target_time), ( + f"The length of {key} and `target_time` should be the same. " + f"Got {len(vec)} and {len(target_time)}." + ) + else: + target_time = [target_time] + for key in goal: + goal[key] = [goal[key]] + + # Concatenate all vectors in goal + concat_vecs = self._concat_vecs(goal, len(target_time)) + assert concat_vecs.shape[0] == len(target_time), ( + f"The length of the concatenated goal and `target_time` should be the same. " + f"Got {concat_vecs.shape[0]} and {len(target_time)}." + ) + + for tt, vec in zip(target_time, concat_vecs): + if tt < interpolation_garbage_collection_time: + continue + self.interp = self.interp.schedule_waypoint( + pose=vec, + time=tt, + max_change_rate=self.max_change_rate, + interpolation_garbage_collection_time=interpolation_garbage_collection_time, + last_waypoint_time=self.last_waypoint_time, + ) + self.last_waypoint_time = tt + + def get_action(self, time: Optional[float] = None) -> dict[str, Any]: + """Get the next action based on the (current) monotonic time.""" + if time is None: + time = time_module.monotonic() + concat_vec = self.interp(time) + self.last_action.update(self._unconcat_vecs(concat_vec)) + return self.last_action + + def observation_space(self) -> gym.spaces.Dict: + """Return the observation space.""" + pass + + def action_space(self) -> gym.spaces.Dict: + """Return the action space.""" + pass + + def close(self) -> None: + """Clean up resources.""" + pass + + +class PoseTrajectoryInterpolator: + def __init__(self, times: np.ndarray, poses: np.ndarray): + assert len(times) >= 1 + assert len(poses) == len(times) + + times = np.asarray(times) + poses = np.asarray(poses) + + self.num_joint = len(poses[0]) + + if len(times) == 1: + # special treatment for single step interpolation + self.single_step = True + self._times = times + self._poses = poses + else: + self.single_step = False + assert np.all(times[1:] >= times[:-1]) + self.pose_interp = si.interp1d(times, poses, axis=0, assume_sorted=True) + + @property + def times(self) -> np.ndarray: + if self.single_step: + return self._times + else: + return self.pose_interp.x + + @property + def poses(self) -> np.ndarray: + if self.single_step: + return self._poses + else: + return self.pose_interp.y + + def trim(self, start_t: float, end_t: float) -> "PoseTrajectoryInterpolator": + assert start_t <= end_t + times = self.times + should_keep = (start_t < times) & (times < end_t) + keep_times = times[should_keep] + all_times = np.concatenate([[start_t], keep_times, [end_t]]) + # remove duplicates, Slerp requires strictly increasing x + all_times = np.unique(all_times) + # interpolate + all_poses = self(all_times) + return PoseTrajectoryInterpolator(times=all_times, poses=all_poses) + + def schedule_waypoint( + self, + pose, + time, + max_change_rate=np.inf, + interpolation_garbage_collection_time=None, + last_waypoint_time=None, + ) -> "PoseTrajectoryInterpolator": + if not isinstance(max_change_rate, np.ndarray): + max_change_rate = np.array([max_change_rate] * self.num_joint) + + assert len(max_change_rate) == self.num_joint + assert np.max(max_change_rate) > 0 + + if last_waypoint_time is not None: + assert interpolation_garbage_collection_time is not None + + # trim current interpolator to between interpolation_garbage_collection_time and last_waypoint_time + start_time = self.times[0] + end_time = self.times[-1] + assert start_time <= end_time + if interpolation_garbage_collection_time is not None: + if time <= interpolation_garbage_collection_time: + # if insert time is earlier than current time + # no effect should be done to the interpolator + return self + # now, interpolation_garbage_collection_time < time + start_time = max(interpolation_garbage_collection_time, start_time) + + if last_waypoint_time is not None: + # if last_waypoint_time is earlier than start_time + # use start_time + if time <= last_waypoint_time: + end_time = interpolation_garbage_collection_time + else: + end_time = max(last_waypoint_time, interpolation_garbage_collection_time) + else: + end_time = interpolation_garbage_collection_time + + end_time = min(end_time, time) + start_time = min(start_time, end_time) + # end time should be the latest of all times except time + # after this we can assume order (proven by zhenjia, due to the 2 min operations) + + # Constraints: + # start_time <= end_time <= time (proven by zhenjia) + # interpolation_garbage_collection_time <= start_time (proven by zhenjia) + # interpolation_garbage_collection_time <= time (proven by zhenjia) + + # time can't change + # last_waypoint_time can't change + # interpolation_garbage_collection_time can't change + assert start_time <= end_time + assert end_time <= time + if last_waypoint_time is not None: + if time <= last_waypoint_time: + assert end_time == interpolation_garbage_collection_time + else: + assert end_time == max(last_waypoint_time, interpolation_garbage_collection_time) + + if interpolation_garbage_collection_time is not None: + assert interpolation_garbage_collection_time <= start_time + assert interpolation_garbage_collection_time <= time + trimmed_interp = self.trim(start_time, end_time) + # after this, all waypoints in trimmed_interp is within start_time and end_time + # and is earlier than time + + # determine speed + duration = time - end_time + end_pose = trimmed_interp(end_time) + pose_min_duration = np.max(np.abs(end_pose - pose) / max_change_rate) + duration = max(duration, pose_min_duration) + assert duration >= 0 + last_waypoint_time = end_time + duration + + # insert new pose + times = np.append(trimmed_interp.times, [last_waypoint_time], axis=0) + poses = np.append(trimmed_interp.poses, [pose], axis=0) + + # create new interpolator + final_interp = PoseTrajectoryInterpolator(times, poses) + return final_interp + + def __call__(self, t: Union[numbers.Number, np.ndarray]) -> np.ndarray: + is_single = False + if isinstance(t, numbers.Number): + is_single = True + t = np.array([t]) + + pose = np.zeros((len(t), self.num_joint)) + if self.single_step: + pose[:] = self._poses[0] + else: + start_time = self.times[0] + end_time = self.times[-1] + t = np.clip(t, start_time, end_time) + pose = self.pose_interp(t) + + if is_single: + pose = pose[0] + return pose diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/policy/keyboard_navigation_policy.py b/GR00T-WholeBodyControl/decoupled_wbc/control/policy/keyboard_navigation_policy.py new file mode 100644 index 0000000000000000000000000000000000000000..e93e08042cf46950d0aba42e9835493c6dbeb0d5 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/policy/keyboard_navigation_policy.py @@ -0,0 +1,87 @@ +from typing import Any, Dict, Optional + +import numpy as np + +from decoupled_wbc.control.base.policy import Policy + + +class KeyboardNavigationPolicy(Policy): + def __init__( + self, + max_linear_velocity: float = 0.5, + max_angular_velocity: float = 0.5, + verbose: bool = True, + **kwargs, + ): + """ + Initialize the navigation policy. + + Args: + max_linear_velocity: Maximum linear velocity in m/s (for x and y components) + max_angular_velocity: Maximum angular velocity in rad/s (for yaw component) + **kwargs: Additional arguments passed to the base Policy class + """ + super().__init__(**kwargs) + self.max_linear_velocity = max_linear_velocity + self.max_angular_velocity = max_angular_velocity + self.verbose = verbose + + # Initialize velocity commands + self.lin_vel_command = np.zeros(2, dtype=np.float32) # [vx, vy] + self.ang_vel_command = np.zeros(1, dtype=np.float32) # [wz] + + def get_action(self, time: Optional[float] = None) -> Dict[str, Any]: + """ + Get the action to execute based on current state. + + Args: + time: Current time (optional) + + Returns: + Dict containing the action to execute with: + - navigate_cmd: np.array([vx, vy, wz]) where: + - vx: linear velocity in x direction (m/s) + - vy: linear velocity in y direction (m/s) + - wz: angular velocity around z axis (rad/s) + """ + # Combine linear and angular velocities into a single command + # Ensure velocities are within limits + vx = np.clip(self.lin_vel_command[0], -self.max_linear_velocity, self.max_linear_velocity) + vy = np.clip(self.lin_vel_command[1], -self.max_linear_velocity, self.max_linear_velocity) + wz = np.clip(self.ang_vel_command[0], -self.max_angular_velocity, self.max_angular_velocity) + + navigate_cmd = np.array([vx, vy, wz], dtype=np.float32) + + action = {"navigate_cmd": navigate_cmd} + return action + + def handle_keyboard_button(self, keycode: str): + """ + Handle keyboard inputs for navigation control. + + Args: + keycode: The key that was pressed + """ + if keycode == "w": + self.lin_vel_command[0] += 0.1 # Increase forward velocity + elif keycode == "s": + self.lin_vel_command[0] -= 0.1 # Increase backward velocity + elif keycode == "a": + self.lin_vel_command[1] += 0.1 # Increase left velocity + elif keycode == "d": + self.lin_vel_command[1] -= 0.1 # Increase right velocity + elif keycode == "q": + self.ang_vel_command[0] += 0.1 # Increase counter-clockwise rotation + elif keycode == "e": + self.ang_vel_command[0] -= 0.1 # Increase clockwise rotation + elif keycode == "z": + # Reset all velocities + self.lin_vel_command[:] = 0.0 + self.ang_vel_command[:] = 0.0 + if self.verbose: + print("Navigation policy: Reset all velocity commands to zero") + + # Print current velocities after any keyboard input + if self.verbose: + print(f"Nav lin vel: ({self.lin_vel_command[0]:.2f}, {self.lin_vel_command[1]:.2f})") + print(f"Nav ang vel: {self.ang_vel_command[0]:.2f}") diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/policy/lerobot_replay_policy.py b/GR00T-WholeBodyControl/decoupled_wbc/control/policy/lerobot_replay_policy.py new file mode 100644 index 0000000000000000000000000000000000000000..b650bc930cf22d3b68e50b31855d900e64eacc78 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/policy/lerobot_replay_policy.py @@ -0,0 +1,111 @@ +import time + +import pandas as pd + +from decoupled_wbc.control.base.policy import Policy +from decoupled_wbc.control.main.constants import ( + DEFAULT_BASE_HEIGHT, + DEFAULT_NAV_CMD, + DEFAULT_WRIST_POSE, +) +from decoupled_wbc.control.robot_model.robot_model import RobotModel +from decoupled_wbc.data.viz.rerun_viz import RerunViz + + +class LerobotReplayPolicy(Policy): + """Replay policy for Lerobot dataset, so we can replay the dataset + and just use the action from the dataset. + + Args: + parquet_path: Path to the parquet file containing the dataset. + """ + + is_active = True # by default, the replay policy is active + + def __init__(self, robot_model: RobotModel, parquet_path: str, use_viz: bool = False): + # self.dataset = LerobotDataset(dataset_path) + self.parquet_path = parquet_path + self._ctr = 0 + # read the parquet file + self.df = pd.read_parquet(self.parquet_path) + self._max_ctr = len(self.df) + # get the action from the dataframe + self.action = self.df.iloc[self._ctr]["action"] + self.use_viz = use_viz + if self.use_viz: + self.viz = RerunViz( + image_keys=["egoview_image"], + tensor_keys=[ + "left_arm_qpos", + "left_hand_qpos", + "right_arm_qpos", + "right_hand_qpos", + ], + window_size=5.0, + ) + self.robot_model = robot_model + self.upper_body_joint_indices = self.robot_model.get_joint_group_indices("upper_body") + + def get_action(self) -> dict[str, any]: + # get the action from the dataframe + action = self.df.iloc[self._ctr]["action"] + wrist_pose = self.df.iloc[self._ctr]["action.eef"] + navigate_cmd = self.df.iloc[self._ctr].get("teleop.navigate_command", DEFAULT_NAV_CMD) + base_height_cmd = self.df.iloc[self._ctr].get( + "teleop.base_height_command", DEFAULT_BASE_HEIGHT + ) + + self._ctr += 1 + if self._ctr >= self._max_ctr: + self._ctr = 0 + # print(f"Replay {self._ctr} / {self._max_ctr}") + if self.use_viz: + self.viz.plot_tensors( + { + "left_arm_qpos": action[self.robot_model.get_joint_group_indices("left_arm")] + + 15, + "left_hand_qpos": action[self.robot_model.get_joint_group_indices("left_hand")] + + 15, + "right_arm_qpos": action[self.robot_model.get_joint_group_indices("right_arm")] + + 15, + "right_hand_qpos": action[ + self.robot_model.get_joint_group_indices("right_hand") + ] + + 15, + }, + time.monotonic(), + ) + + return { + "target_upper_body_pose": action[self.upper_body_joint_indices], + "wrist_pose": wrist_pose, + "navigate_cmd": navigate_cmd, + "base_height_cmd": base_height_cmd, + "timestamp": time.time(), + } + + def action_to_cmd(self, action: dict[str, any]) -> dict[str, any]: + action["target_upper_body_pose"] = action["q"][ + self.robot_model.get_joint_group_indices("upper_body") + ] + del action["q"] + return action + + def set_observation(self, observation: dict[str, any]): + pass + + def get_observation(self) -> dict[str, any]: + return { + "wrist_pose": self.df.iloc[self._ctr - 1].get( + "observation.eef_state", DEFAULT_WRIST_POSE + ), + "timestamp": time.time(), + } + + +if __name__ == "__main__": + policy = LerobotReplayPolicy( + parquet_path="outputs/g1-open-hands-may7/data/chunk-000/episode_000000.parquet" + ) + action = policy.get_action() + print(action) diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/policy/teleop_policy.py b/GR00T-WholeBodyControl/decoupled_wbc/control/policy/teleop_policy.py new file mode 100644 index 0000000000000000000000000000000000000000..e05a43bfb5dc965f8119a2746f3587cc3cf7ac39 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/policy/teleop_policy.py @@ -0,0 +1,207 @@ +from contextlib import contextmanager +import time +from typing import Optional + +import numpy as np +from scipy.spatial.transform import Rotation as R + +from decoupled_wbc.control.base.policy import Policy +from decoupled_wbc.control.robot_model import RobotModel +from decoupled_wbc.control.teleop.teleop_retargeting_ik import TeleopRetargetingIK +from decoupled_wbc.control.teleop.teleop_streamer import TeleopStreamer + + +class TeleopPolicy(Policy): + """ + Robot-agnostic teleop policy. + Clean separation: IK processing vs command passing. + All robot-specific properties are abstracted through robot_model and hand_ik_solvers. + """ + + def __init__( + self, + body_control_device: str, + hand_control_device: str, + robot_model: RobotModel, + retargeting_ik: TeleopRetargetingIK, + body_streamer_ip: str = "192.168.?.?", + body_streamer_keyword: str = "shoulder", + enable_real_device: bool = True, + replay_data_path: Optional[str] = None, + replay_speed: float = 1.0, + wait_for_activation: int = 5, + activate_keyboard_listener: bool = True, + ): + if activate_keyboard_listener: + from decoupled_wbc.control.utils.keyboard_dispatcher import KeyboardListenerSubscriber + + self.keyboard_listener = KeyboardListenerSubscriber() + else: + self.keyboard_listener = None + + self.wait_for_activation = wait_for_activation + + self.teleop_streamer = TeleopStreamer( + robot_model=robot_model, + body_control_device=body_control_device, + hand_control_device=hand_control_device, + enable_real_device=enable_real_device, + body_streamer_ip=body_streamer_ip, + body_streamer_keyword=body_streamer_keyword, + replay_data_path=replay_data_path, + replay_speed=replay_speed, + ) + self.robot_model = robot_model + self.retargeting_ik = retargeting_ik + self.is_active = False + + self.latest_left_wrist_data = np.eye(4) + self.latest_right_wrist_data = np.eye(4) + self.latest_left_fingers_data = {"position": np.zeros((25, 4, 4))} + self.latest_right_fingers_data = {"position": np.zeros((25, 4, 4))} + + def set_goal(self, goal: dict[str, any]): + # The current teleop policy doesn't take higher level commands yet. + pass + + def get_action(self) -> dict[str, any]: + # Get structured data + streamer_output = self.teleop_streamer.get_streamer_data() + + # Handle activation using teleop_data commands + self.check_activation( + streamer_output.teleop_data, wait_for_activation=self.wait_for_activation + ) + + action = {} + + # Process streamer data if active + if self.is_active and streamer_output.ik_data: + body_data = streamer_output.ik_data["body_data"] + left_hand_data = streamer_output.ik_data["left_hand_data"] + right_hand_data = streamer_output.ik_data["right_hand_data"] + + left_wrist_name = self.robot_model.supplemental_info.hand_frame_names["left"] + right_wrist_name = self.robot_model.supplemental_info.hand_frame_names["right"] + self.latest_left_wrist_data = body_data[left_wrist_name] + self.latest_right_wrist_data = body_data[right_wrist_name] + self.latest_left_fingers_data = left_hand_data + self.latest_right_fingers_data = right_hand_data + + # TODO: This stores the same data again + ik_data = { + "body_data": body_data, + "left_hand_data": left_hand_data, + "right_hand_data": right_hand_data, + } + action["ik_data"] = ik_data + + # Wrist poses (pos and quat) + # TODO: This stores the same wrist poses in two different formats + left_wrist_matrix = self.latest_left_wrist_data + right_wrist_matrix = self.latest_right_wrist_data + left_wrist_pose = np.concatenate( + [ + left_wrist_matrix[:3, 3], + R.from_matrix(left_wrist_matrix[:3, :3]).as_quat(scalar_first=True), + ] + ) + right_wrist_pose = np.concatenate( + [ + right_wrist_matrix[:3, 3], + R.from_matrix(right_wrist_matrix[:3, :3]).as_quat(scalar_first=True), + ] + ) + + # Combine IK results with control commands (no teleop_data commands) + action.update( + { + "left_wrist": self.latest_left_wrist_data, + "right_wrist": self.latest_right_wrist_data, + "left_fingers": self.latest_left_fingers_data, + "right_fingers": self.latest_right_fingers_data, + "wrist_pose": np.concatenate([left_wrist_pose, right_wrist_pose]), + **streamer_output.control_data, # Only control & data collection commands pass through + **streamer_output.data_collection_data, + } + ) + + # Run retargeting IK + if "ik_data" in action: + self.retargeting_ik.set_goal(action["ik_data"]) + action["target_upper_body_pose"] = self.retargeting_ik.get_action() + + return action + + def close(self) -> bool: + self.teleop_streamer.stop_streaming() + return True + + def check_activation(self, teleop_data: dict, wait_for_activation: int = 5): + """Activation logic only looks at teleop data commands""" + key = self.keyboard_listener.read_msg() if self.keyboard_listener else "" + toggle_activation_by_keyboard = key == "l" + reset_teleop_policy_by_keyboard = key == "k" + toggle_activation_by_teleop = teleop_data.get("toggle_activation", False) + + if reset_teleop_policy_by_keyboard: + print("Resetting teleop policy") + self.reset() + + if toggle_activation_by_keyboard or toggle_activation_by_teleop: + self.is_active = not self.is_active + if self.is_active: + print("Starting teleop policy") + + if wait_for_activation > 0 and toggle_activation_by_keyboard: + print(f"Sleeping for {wait_for_activation} seconds before starting teleop...") + for i in range(wait_for_activation, 0, -1): + print(f"Starting in {i}...") + time.sleep(1) + + # dda: calibration logic should use current IK data + self.teleop_streamer.calibrate() + print("Teleop policy calibrated") + else: + print("Stopping teleop policy") + + @contextmanager + def activate(self): + try: + yield self + finally: + self.close() + + def handle_keyboard_button(self, keycode): + """ + Handle keyboard input with proper state toggle. + """ + if keycode == "l": + # Toggle start state + self.is_active = not self.is_active + # Reset initialization when stopping + if not self.is_active: + self._initialized = False + if keycode == "k": + print("Resetting teleop policy") + self.reset() + + def activate_policy(self, wait_for_activation: int = 5): + """activate the teleop policy""" + self.is_active = False + self.check_activation( + teleop_data={"toggle_activation": True}, wait_for_activation=wait_for_activation + ) + + def reset(self, wait_for_activation: int = 5, auto_activate: bool = False): + """Reset the teleop policy to the initial state, and re-activate it.""" + self.teleop_streamer.reset() + self.retargeting_ik.reset() + self.is_active = False + self.latest_left_wrist_data = np.eye(4) + self.latest_right_wrist_data = np.eye(4) + self.latest_left_fingers_data = {"position": np.zeros((25, 4, 4))} + self.latest_right_fingers_data = {"position": np.zeros((25, 4, 4))} + + if auto_activate: + self.activate_policy(wait_for_activation) diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/policy/wbc_policy_factory.py b/GR00T-WholeBodyControl/decoupled_wbc/control/policy/wbc_policy_factory.py new file mode 100644 index 0000000000000000000000000000000000000000..bccbd484232e5bf4569ba20491c580a6ad36579e --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/policy/wbc_policy_factory.py @@ -0,0 +1,65 @@ +import os +from pathlib import Path +import time + +import numpy as np + +import decoupled_wbc +from decoupled_wbc.control.main.constants import DEFAULT_BASE_HEIGHT, DEFAULT_NAV_CMD +from decoupled_wbc.control.policy.g1_gear_wbc_policy import G1GearWbcPolicy +from decoupled_wbc.control.policy.identity_policy import IdentityPolicy +from decoupled_wbc.control.policy.interpolation_policy import InterpolationPolicy + +from .g1_decoupled_whole_body_policy import G1DecoupledWholeBodyPolicy + +WBC_VERSIONS = ["gear_wbc"] + + +def get_wbc_policy( + robot_type, + robot_model, + wbc_config, + init_time=time.monotonic(), +): + current_upper_body_pose = robot_model.get_initial_upper_body_pose() + + if robot_type == "g1": + upper_body_policy_type = wbc_config.get("upper_body_policy_type", "interpolation") + if upper_body_policy_type == "identity": + upper_body_policy = IdentityPolicy() + else: + upper_body_policy = InterpolationPolicy( + init_time=init_time, + init_values={ + "target_upper_body_pose": current_upper_body_pose, + "base_height_command": np.array([DEFAULT_BASE_HEIGHT]), + "navigate_cmd": np.array([DEFAULT_NAV_CMD]), + }, + max_change_rate=wbc_config["upper_body_max_joint_speed"], + ) + + lower_body_policy_type = wbc_config.get("VERSION", "gear_wbc") + if lower_body_policy_type not in ["gear_wbc"]: + raise ValueError( + f"Invalid lower body policy version: {lower_body_policy_type}. " + f"Only 'gear_wbc' is supported." + ) + + # Get the base path to decoupled_wbc and convert to Path object + package_path = Path(os.path.dirname(decoupled_wbc.__file__)) + gear_wbc_config = str(package_path / ".." / wbc_config["GEAR_WBC_CONFIG"]) + if lower_body_policy_type == "gear_wbc": + lower_body_policy = G1GearWbcPolicy( + robot_model=robot_model, + config=gear_wbc_config, + model_path=wbc_config["model_path"], + ) + + wbc_policy = G1DecoupledWholeBodyPolicy( + robot_model=robot_model, + upper_body_policy=upper_body_policy, + lower_body_policy=lower_body_policy, + ) + else: + raise ValueError(f"Invalid robot type: {robot_type}") + return wbc_policy diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/robot_model/__init__.py b/GR00T-WholeBodyControl/decoupled_wbc/control/robot_model/__init__.py new file mode 100644 index 0000000000000000000000000000000000000000..774d303ed5f7c5fa77ba80dfa5ce843ace924b8e --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/robot_model/__init__.py @@ -0,0 +1,3 @@ +from .robot_model import ReducedRobotModel, RobotModel + +__all__ = ["RobotModel", "ReducedRobotModel"] diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/robot_model/robot_model.py b/GR00T-WholeBodyControl/decoupled_wbc/control/robot_model/robot_model.py new file mode 100644 index 0000000000000000000000000000000000000000..27a1ed5c66b73cddeb6f3f7dce5a3276e83871fc --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/robot_model/robot_model.py @@ -0,0 +1,772 @@ +from typing import List, Optional, Set, Union + +import numpy as np +import pinocchio as pin + +from decoupled_wbc.control.robot_model.supplemental_info import RobotSupplementalInfo + + +class RobotModel: + def __init__( + self, + urdf_path, + asset_path, + set_floating_base=False, + supplemental_info: Optional[RobotSupplementalInfo] = None, + ): + self.pinocchio_wrapper = pin.RobotWrapper.BuildFromURDF( + filename=urdf_path, + package_dirs=[asset_path], + root_joint=pin.JointModelFreeFlyer() if set_floating_base else None, + ) + self.is_floating_base_model = set_floating_base + + self.joint_to_dof_index = {} + # Assume we only have single-dof joints + # First two names correspond to universe and floating base joints + names = ( + self.pinocchio_wrapper.model.names[2:] + if set_floating_base + else self.pinocchio_wrapper.model.names[1:] + ) + for name in names: + j_id = self.pinocchio_wrapper.model.getJointId(name) + jmodel = self.pinocchio_wrapper.model.joints[j_id] + self.joint_to_dof_index[name] = jmodel.idx_q + + # Store joint limits only for actual joints (excluding floating base) + # if set floating base is true and the robot can move in the world + # then we don't want to impose joint limits for the 7 dofs corresponding + # to the floating base dofs. + root_nq = 7 if set_floating_base else 0 + self.upper_joint_limits = self.pinocchio_wrapper.model.upperPositionLimit[root_nq:].copy() + self.lower_joint_limits = self.pinocchio_wrapper.model.lowerPositionLimit[root_nq:].copy() + + # Set up supplemental info if provided + self.supplemental_info = supplemental_info + if self.supplemental_info is not None: + # Cache indices for body and hand actuated joints separately + self._body_actuated_joint_indices = [ + self.dof_index(name) for name in self.supplemental_info.body_actuated_joints + ] + self._left_hand_actuated_joint_indices = [ + self.dof_index(name) for name in self.supplemental_info.left_hand_actuated_joints + ] + self._right_hand_actuated_joint_indices = [ + self.dof_index(name) for name in self.supplemental_info.right_hand_actuated_joints + ] + self._hand_actuated_joint_indices = ( + self._left_hand_actuated_joint_indices + self._right_hand_actuated_joint_indices + ) + + # Cache indices for joint groups, handling nested groups + self._joint_group_indices = {} + for group_name, group_info in self.supplemental_info.joint_groups.items(): + indices = [] + # Add indices for direct joints + indices.extend([self.dof_index(name) for name in group_info["joints"]]) + # Add indices from subgroups + for subgroup_name in group_info["groups"]: + indices.extend(self.get_joint_group_indices(subgroup_name)) + self._joint_group_indices[group_name] = sorted(set(indices)) + + # Update joint limits from supplemental info if available + if ( + hasattr(self.supplemental_info, "joint_limits") + and self.supplemental_info.joint_limits + ): + for joint_name, limits in self.supplemental_info.joint_limits.items(): + if joint_name in self.joint_to_dof_index: + idx = self.joint_to_dof_index[joint_name] - root_nq + self.lower_joint_limits[idx] = limits[0] + self.upper_joint_limits[idx] = limits[1] + + # Initialize default body pose + self.default_body_pose = self.q_zero.copy() + + # Update with supplemental info if available + if self.supplemental_info is not None: + default_joint_q = self.supplemental_info.default_joint_q + for joint, joint_values in default_joint_q.items(): + # Get the joint name mapping for this type + joint_mapping = self.supplemental_info.joint_name_mapping[joint] + + # Handle both single joint names and left/right mappings + if isinstance(joint_mapping, str): + # Single joint (e.g., waist joints) + if joint_mapping in self.joint_to_dof_index: + joint_idx = self.dof_index(joint_mapping) + self.default_body_pose[joint_idx] = ( + joint_values # joint_values is the value for single joints + ) + else: + # Left/right mapping (e.g., arm joints) + for side, value in joint_values.items(): + if side in joint_mapping and joint_mapping[side] in self.joint_to_dof_index: + joint_idx = self.dof_index(joint_mapping[side]) + self.default_body_pose[joint_idx] = value + + # Initialize initial body pose + self.initial_body_pose = self.default_body_pose.copy() + + @property + def num_dofs(self) -> int: + """Get the number of degrees of freedom of the robot (floating base pose + joints).""" + return self.pinocchio_wrapper.model.nq + + @property + def q_zero(self) -> np.ndarray: + """Get the zero pose of the robot.""" + return self.pinocchio_wrapper.q0 + + @property + def joint_names(self) -> List[str]: + """Get the names of the joints of the robot.""" + return list(self.joint_to_dof_index.keys()) + + @property + def num_joints(self) -> int: + """Get the number of joints of the robot.""" + return len(self.joint_to_dof_index) + + def dof_index(self, joint_name: str) -> int: + """ + Get the index in the degrees of freedom vector corresponding + to the single-DoF joint with name `joint_name`. + """ + if joint_name not in self.joint_to_dof_index: + raise ValueError( + f"Unknown joint name: '{joint_name}'. " + f"Available joints: {list(self.joint_to_dof_index.keys())}" + ) + return self.joint_to_dof_index[joint_name] + + def get_body_actuated_joint_indices(self) -> List[int]: + """ + Get the indices of body actuated joints in the full configuration. + Ordering is that of the actuated joints as defined in the supplemental info. + Requires supplemental_info to be provided. + """ + if self.supplemental_info is None: + raise ValueError("supplemental_info must be provided to use this method") + return self._body_actuated_joint_indices + + def get_hand_actuated_joint_indices(self, side: str = "both") -> List[int]: + """ + Get the indices of hand actuated joints in the full configuration. + Ordering is that of the actuated joints as defined in the supplemental info. + Requires supplemental_info to be provided. + + Args: + side: String specifying which hand to get indices for ('left', 'right', or 'both') + """ + if self.supplemental_info is None: + raise ValueError("supplemental_info must be provided to use this method") + + if side.lower() == "both": + return self._hand_actuated_joint_indices + elif side.lower() == "left": + return self._left_hand_actuated_joint_indices + elif side.lower() == "right": + return self._right_hand_actuated_joint_indices + else: + raise ValueError("side must be 'left', 'right', or 'both'") + + def get_joint_group_indices(self, group_names: Union[str, Set[str]]) -> List[int]: + """ + Get the indices of joints in one or more groups in the full configuration. + Requires supplemental_info to be provided. + The returned indices are sorted in ascending order, so that the joint ordering + of the full model is preserved. + + Args: + group_names: Either a single group name (str) or a set of group names (Set[str]) + + Returns: + List of joint indices in sorted order with no duplicates + """ + if self.supplemental_info is None: + raise ValueError("supplemental_info must be provided to use this method") + + # Convert single string to set for uniform handling + if isinstance(group_names, str): + group_names = {group_names} + + # Collect indices from all groups + all_indices = set() + for group_name in group_names: + if group_name not in self._joint_group_indices: + raise ValueError(f"Unknown joint group: {group_name}") + all_indices.update(self._joint_group_indices[group_name]) + + return sorted(all_indices) + + def cache_forward_kinematics(self, q: np.ndarray, auto_clip=True) -> None: + """ + Perform forward kinematics to update the pose of every joint and frame + in the Pinocchio data structures for the given configuration `q`. + + :param q: A numpy array of shape (num_dofs,) representing the robot configuration. + """ + if q.shape[0] != self.num_dofs: + raise ValueError(f"Expected q of length {self.num_dofs}, got {q.shape[0]} instead.") + + # Apply auto-clip if enabled + if auto_clip: + q = self.clip_configuration(q) + + pin.framesForwardKinematics(self.pinocchio_wrapper.model, self.pinocchio_wrapper.data, q) + + def compute_gravity_compensation_torques( + self, q: np.ndarray, joint_groups: Union[str, List[str], Set[str]] = None, auto_clip=True + ) -> np.ndarray: + """ + Compute gravity compensation torques for specified joint groups using pinocchio. + + :param q: Robot configuration (joint positions) + :param joint_groups: Joint groups to compensate (e.g., "arms", ["left_arm", "waist"], + {"left_arm", "waist"}). If None, compensates all joints + :param auto_clip: Whether to automatically clip joint values to limits + :return: Array of gravity compensation torques for all DOFs (zero for non-compensated joints) + """ + if q.shape[0] != self.num_dofs: + raise ValueError(f"Expected q of length {self.num_dofs}, got {q.shape[0]} instead.") + + # Apply auto-clip if enabled + if auto_clip: + q = self.clip_configuration(q) + + try: + # Cache forward kinematics for the current configuration + self.cache_forward_kinematics(q, auto_clip=False) # Already clipped if needed + + # Compute gravity vector using RNEA with zero velocity and acceleration + v = np.zeros(self.num_dofs) + a = np.zeros(self.num_dofs) + + gravity_torques_full = pin.rnea( + self.pinocchio_wrapper.model, self.pinocchio_wrapper.data, q, v, a + ) + + # If no joint groups specified, return full gravity torques + if joint_groups is None: + return gravity_torques_full + + # Convert list to set for get_joint_group_indices compatibility + if isinstance(joint_groups, list): + joint_groups = set(joint_groups) + + # Get joint indices for specified groups - get_joint_group_indices handles str and Set[str] + try: + compensated_joint_indices = self.get_joint_group_indices(joint_groups) + except ValueError as e: + raise ValueError(f"Error resolving joint groups {joint_groups}: {e}") + + # Create mask for joints that should receive gravity compensation + compensation_mask = np.zeros(self.num_dofs, dtype=bool) + for joint_idx in compensated_joint_indices: + if 0 <= joint_idx < len(compensation_mask): + compensation_mask[joint_idx] = True + + # Apply mask to only compensate specified joints + compensated_torques = np.zeros_like(gravity_torques_full) + compensated_torques[compensation_mask] = gravity_torques_full[compensation_mask] + + return compensated_torques + + except Exception as e: + raise RuntimeError(f"Error computing gravity compensation: {e}") + + def clip_configuration(self, q: np.ndarray, margin: float = 1e-6) -> np.ndarray: + """ + Clip the configuration to stay within joint limits with a small tolerance. + + :param q: Configuration to clip + :param margin: Tolerance to keep away from joint limits + :return: Clipped configuration + """ + q_clipped = q.copy() + + # Only clip joint positions, not floating base + root_nq = 7 if self.is_floating_base_model else 0 + q_clipped[root_nq:] = np.clip( + q[root_nq:], self.lower_joint_limits + margin, self.upper_joint_limits - margin + ) + + return q_clipped + + def frame_placement(self, frame_name: str) -> pin.SE3: + """ + Returns the SE3 transform of the specified frame in the world coordinate system. + Note: make sure cache_forward_kinematics() has been previously called. + + :param frame_name: Name of the frame, e.g. "link_elbow_frame", "hand_imu_frame", etc. + :return: A pin.SE3 object representing the pose of the frame. + """ + model = self.pinocchio_wrapper.model + data = self.pinocchio_wrapper.data + + frame_id = model.getFrameId(frame_name) + if frame_id < 0 or frame_id >= len(model.frames): + valid_frames = [f.name for f in model.frames] + raise ValueError(f"Unknown frame '{frame_name}'. Valid frames: {valid_frames}") + + # Pinocchio's data.oMf[frame_id] is a pin.SE3. + return data.oMf[frame_id].copy() + + def get_body_actuated_joints(self, q: np.ndarray) -> np.ndarray: + """ + Get the configuration of body actuated joints from a full configuration. + + :param q: Configuration in full space + :return: Configuration of body actuated joints + """ + indices = self.get_body_actuated_joint_indices() + + return q[indices] + + def get_hand_actuated_joints(self, q: np.ndarray, side: str = "both") -> np.ndarray: + """ + Get the configuration of hand actuated joints from a full configuration. + + Args: + q: Configuration in full space + side: String specifying which hand to get joints for ('left', 'right', or 'both') + """ + indices = self.get_hand_actuated_joint_indices(side) + return q[indices] + + def get_configuration_from_actuated_joints( + self, + body_actuated_joint_values: np.ndarray, + hand_actuated_joint_values: Optional[np.ndarray] = None, + left_hand_actuated_joint_values: Optional[np.ndarray] = None, + right_hand_actuated_joint_values: Optional[np.ndarray] = None, + ) -> np.ndarray: + """ + Get the full configuration from the body and hand actuated joint configurations. + Can specify either both hands together or left and right hands separately. + + Args: + body_actuated_joint_values: Configuration of body actuated joints + hand_actuated_joint_values: Configuration of both hands' actuated joints (optional) + left_hand_actuated_joint_values: Configuration of left hand actuated joints (optional) + right_hand_actuated_joint_values: Configuration of right hand actuated joints (optional) + + Returns: + Full configuration including body and hand joints + """ + q = self.pinocchio_wrapper.q0.copy() + q[self.get_body_actuated_joint_indices()] = body_actuated_joint_values + + # Handle hand configurations + if hand_actuated_joint_values is not None: + # Use combined hand configuration + q[self.get_hand_actuated_joint_indices("both")] = hand_actuated_joint_values + else: + # Use separate hand configurations + if left_hand_actuated_joint_values is not None: + q[self.get_hand_actuated_joint_indices("left")] = left_hand_actuated_joint_values + if right_hand_actuated_joint_values is not None: + q[self.get_hand_actuated_joint_indices("right")] = right_hand_actuated_joint_values + + return q + + def reset_forward_kinematics(self) -> None: + """ + Reset the forward kinematics to the initial configuration. + """ + self.cache_forward_kinematics(self.q_zero) + + def get_initial_upper_body_pose(self) -> np.ndarray: + """ + Get the initial upper body pose of the robot. + """ + return self.initial_body_pose[self.get_joint_group_indices("upper_body")] + + def get_default_body_pose(self) -> np.ndarray: + """ + Get the default body pose of the robot. + """ + return self.default_body_pose + + def set_initial_body_pose(self, q: np.ndarray, q_idx=None) -> None: + """ + Set the initial body pose of the robot. + """ + if q_idx is None: + self.initial_body_pose = q + else: + self.initial_body_pose[q_idx] = q + + +class ReducedRobotModel(RobotModel): + """ + A class that creates a reduced order robot model by fixing certain joints. + This class maintains a mapping between the reduced state space and the full state space. + """ + + def __init__( + self, + full_robot_model: RobotModel, + fixed_joints: List[str], + fixed_values: Optional[List[float]] = None, + ): + """ + Create a reduced order robot model by fixing specified joints. + + :param full_robot_model: The original robot model + :param fixed_joints: List of joint names to fix + :param fixed_values: Optional list of values to fix the joints to. If None, uses the initial + joint positions (q0) from the full robot model. + """ + self.full_robot = full_robot_model + self.supplemental_info = full_robot_model.supplemental_info + + # If fixed_values is None, use q0 from the full robot model + if fixed_values is None: + fixed_values = [] + for joint_name in fixed_joints: + full_idx = full_robot_model.dof_index(joint_name) + fixed_values.append(full_robot_model.pinocchio_wrapper.q0[full_idx]) + elif len(fixed_joints) != len(fixed_values): + raise ValueError("fixed_joints and fixed_values must have the same length") + + # Store fixed joints and their values + self.fixed_joints = fixed_joints + self.fixed_values = fixed_values + + # Create mapping between reduced and full state spaces + self.reduced_to_full = [] + self.full_to_reduced = {} + + # Initialize with floating base indices if present + if full_robot_model.is_floating_base_model: + self.reduced_to_full.extend(range(7)) # Floating base indices + for i in range(7): + self.full_to_reduced[i] = i + + # Add active joint indices + for joint_name in full_robot_model.joint_names: + if joint_name not in fixed_joints: + full_idx = full_robot_model.dof_index(joint_name) + reduced_idx = len(self.reduced_to_full) + self.reduced_to_full.append(full_idx) + self.full_to_reduced[full_idx] = reduced_idx + + # Create a reduced Pinocchio model using buildReducedModel + # First, get the list of joint IDs to lock + locked_joint_ids = [] + for joint_name in fixed_joints: + joint_id = full_robot_model.pinocchio_wrapper.model.getJointId(joint_name) + if (full_robot_model.is_floating_base_model and joint_id > 1) or ( + not full_robot_model.is_floating_base_model and joint_id > 0 + ): + locked_joint_ids.append(joint_id) + + # First build the reduced kinematic model + reduced_model = pin.buildReducedModel( + full_robot_model.pinocchio_wrapper.model, + locked_joint_ids, + full_robot_model.pinocchio_wrapper.q0, + ) + + # Then build the reduced geometry models using the reduced kinematic model + self.pinocchio_wrapper = pin.RobotWrapper( + model=reduced_model, + ) + + # Create joint to dof index mapping + self.joint_to_dof_index = {} + # Assume we only have single-dof joints + # First two names correspond to universe and floating base joints + names = ( + self.pinocchio_wrapper.model.names[2:] + if self.full_robot.is_floating_base_model + else self.pinocchio_wrapper.model.names[1:] + ) + for name in names: + j_id = self.pinocchio_wrapper.model.getJointId(name) + jmodel = self.pinocchio_wrapper.model.joints[j_id] + self.joint_to_dof_index[name] = jmodel.idx_q + + # Initialize joint limits + root_nq = 7 if self.full_robot.is_floating_base_model else 0 + self.lower_joint_limits = self.pinocchio_wrapper.model.lowerPositionLimit[root_nq:].copy() + self.upper_joint_limits = self.pinocchio_wrapper.model.upperPositionLimit[root_nq:].copy() + + # Update joint limits from supplemental info if available + if self.supplemental_info is not None: + if ( + hasattr(self.supplemental_info, "joint_limits") + and self.supplemental_info.joint_limits + ): + for joint_name, limits in self.supplemental_info.joint_limits.items(): + if joint_name in self.joint_to_dof_index: + idx = self.joint_to_dof_index[joint_name] - root_nq + self.lower_joint_limits[idx] = limits[0] + self.upper_joint_limits[idx] = limits[1] + + # Get full indices for body and hand actuated joints + full_body_indices = full_robot_model.get_body_actuated_joint_indices() + full_hand_indices = full_robot_model.get_hand_actuated_joint_indices("both") + full_left_hand_indices = full_robot_model.get_hand_actuated_joint_indices("left") + full_right_hand_indices = full_robot_model.get_hand_actuated_joint_indices("right") + + # Map to reduced indices + self._body_actuated_joint_indices = [] + for idx in full_body_indices: + if idx in self.full_to_reduced: + self._body_actuated_joint_indices.append(self.full_to_reduced[idx]) + + self._hand_actuated_joint_indices = [] + for idx in full_hand_indices: + if idx in self.full_to_reduced: + self._hand_actuated_joint_indices.append(self.full_to_reduced[idx]) + + self._left_hand_actuated_joint_indices = [] + for idx in full_left_hand_indices: + if idx in self.full_to_reduced: + self._left_hand_actuated_joint_indices.append(self.full_to_reduced[idx]) + + self._right_hand_actuated_joint_indices = [] + for idx in full_right_hand_indices: + if idx in self.full_to_reduced: + self._right_hand_actuated_joint_indices.append(self.full_to_reduced[idx]) + + # Cache indices for joint groups in reduced space + self._joint_group_indices = {} + for group_name in self.supplemental_info.joint_groups: + full_indices = full_robot_model.get_joint_group_indices(group_name) + reduced_indices = [] + for idx in full_indices: + if idx in self.full_to_reduced: + reduced_indices.append(self.full_to_reduced[idx]) + self._joint_group_indices[group_name] = sorted(set(reduced_indices)) + + # Initialize default body pose in reduced space + self.default_body_pose = self.full_to_reduced_configuration( + full_robot_model.default_body_pose + ) + + # Initialize initial body pose in reduced space + self.initial_body_pose = self.full_to_reduced_configuration( + full_robot_model.initial_body_pose + ) + + @property + def num_joints(self) -> int: + """Get the number of active joints in the reduced model.""" + return len(self.joint_names) + + @property + def joint_names(self) -> List[str]: + """Get the names of the active joints in the reduced model.""" + return [name for name in self.full_robot.joint_names if name not in self.fixed_joints] + + @classmethod + def from_fixed_groups( + cls, + full_robot_model: RobotModel, + fixed_group_names: List[str], + fixed_values: Optional[List[float]] = None, + ) -> "ReducedRobotModel": + """ + Create a reduced order robot model by fixing all joints in specified groups. + + :param full_robot_model: The original robot model + :param fixed_group_names: List of joint group names to fix + :param fixed_values: Optional list of values to fix the joints to. If None, uses the initial + joint positions (q0) from the full robot model. + :return: A ReducedRobotModel instance + """ + if full_robot_model.supplemental_info is None: + raise ValueError("supplemental_info must be provided to use this method") + + # Get all joints in the groups, including those from subgroups + fixed_joints = set() # Use a set to avoid duplicates + + for group_name in fixed_group_names: + if group_name not in full_robot_model.supplemental_info.joint_groups: + raise ValueError(f"Unknown joint group: {group_name}") + + group_info = full_robot_model.supplemental_info.joint_groups[group_name] + + # Add direct joints + fixed_joints.update(group_info["joints"]) + + # Add joints from subgroups + for subgroup_name in group_info["groups"]: + subgroup_joints = full_robot_model.get_joint_group_indices(subgroup_name) + fixed_joints.update([full_robot_model.joint_names[idx] for idx in subgroup_joints]) + + # Convert set back to list for compatibility with the original constructor + return cls(full_robot_model, list(fixed_joints), fixed_values) + + @classmethod + def from_fixed_group( + cls, + full_robot_model: RobotModel, + fixed_group_name: str, + fixed_values: Optional[List[float]] = None, + ) -> "ReducedRobotModel": + """ + Create a reduced order robot model by fixing all joints in a specified group. + This is a convenience method that calls from_fixed_groups with a single group. + + :param full_robot_model: The original robot model + :param fixed_group_name: Name of the joint group to fix + :param fixed_values: Optional list of values to fix the joints to. If None, uses the initial + joint positions (q0) from the full robot model. + :return: A ReducedRobotModel instance + """ + return cls.from_fixed_groups(full_robot_model, [fixed_group_name], fixed_values) + + @classmethod + def from_active_group( + cls, + full_robot_model: RobotModel, + active_group_name: str, + fixed_values: Optional[List[float]] = None, + ) -> "ReducedRobotModel": + """ + Create a reduced order robot model by fixing all joints EXCEPT those in the specified group. + This is a convenience method that calls from_active_groups with a single group. + + :param full_robot_model: The original robot model + :param active_group_name: Name of the joint group to keep active (all other joints will be fixed) + :param fixed_values: Optional list of values to fix the joints to. If None, uses the initial + joint positions (q0) from the full robot model. + :return: A ReducedRobotModel instance + """ + return cls.from_active_groups(full_robot_model, [active_group_name], fixed_values) + + @classmethod + def from_active_groups( + cls, + full_robot_model: RobotModel, + active_group_names: List[str], + fixed_values: Optional[List[float]] = None, + ) -> "ReducedRobotModel": + """ + Create a reduced order robot model by fixing all joints EXCEPT those in the specified groups. + This is useful when you want to keep multiple groups active and fix everything else. + + :param full_robot_model: The original robot model + :param active_group_names: List of joint group names to keep active (all other joints will be fixed) + :param fixed_values: Optional list of values to fix the joints to. If None, uses the initial + joint positions (q0) from the full robot model. + :return: A ReducedRobotModel instance + """ + if full_robot_model.supplemental_info is None: + raise ValueError("supplemental_info must be provided to use this method") + + # Get all joints in the active groups, including those from subgroups + active_joints = set() + + def add_group_joints(group_name: str): + if group_name not in full_robot_model.supplemental_info.joint_groups: + raise ValueError(f"Unknown joint group: {group_name}") + + group_info = full_robot_model.supplemental_info.joint_groups[group_name] + + # Add direct joints + if "joints" in group_info: + active_joints.update(group_info["joints"]) + + # Add joints from subgroups + if "groups" in group_info: + for subgroup_name in group_info["groups"]: + add_group_joints(subgroup_name) + + for group_name in active_group_names: + add_group_joints(group_name) + + # Get all joints from the model + all_joints = set(full_robot_model.joint_names) + + # The fixed joints are all joints minus the active joints + fixed_joints = list(all_joints - active_joints) + + return cls(full_robot_model, fixed_joints, fixed_values) + + def reduced_to_full_configuration(self, q_reduced: np.ndarray) -> np.ndarray: + """ + Convert a reduced configuration to the full configuration space. + + :param q_reduced: Configuration in reduced space + :return: Configuration in full space with fixed joints set to their fixed values + """ + if q_reduced.shape[0] != self.num_dofs: + raise ValueError( + f"Expected q_reduced of length {self.num_dofs}, got {q_reduced.shape[0]} instead" + ) + + q_full = np.zeros(self.full_robot.num_dofs) + + # Set active joints + for reduced_idx, full_idx in enumerate(self.reduced_to_full): + q_full[full_idx] = q_reduced[reduced_idx] + + # Set fixed joints + for joint_name, value in zip(self.fixed_joints, self.fixed_values): + full_idx = self.full_robot.dof_index(joint_name) + q_full[full_idx] = value + + return q_full + + def full_to_reduced_configuration(self, q_full: np.ndarray) -> np.ndarray: + """ + Convert a full configuration to the reduced configuration space. + + :param q_full: Configuration in full space + :return: Configuration in reduced space + """ + if q_full.shape[0] != self.full_robot.num_dofs: + raise ValueError( + f"Expected q_full of length {self.full_robot.num_dofs}, got {q_full.shape[0]} instead" + ) + + q_reduced = np.zeros(self.num_dofs) + + # Copy active joints + for reduced_idx, full_idx in enumerate(self.reduced_to_full): + q_reduced[reduced_idx] = q_full[full_idx] + + return q_reduced + + def cache_forward_kinematics(self, q_reduced: np.ndarray, auto_clip=True) -> None: + """ + Perform forward kinematics using the reduced configuration. + + :param q_reduced: Configuration in reduced space + """ + # First update the full robot's forward kinematics + q_full = self.reduced_to_full_configuration(q_reduced) + self.full_robot.cache_forward_kinematics(q_full, auto_clip) + + # Then update the reduced model's forward kinematics + pin.framesForwardKinematics( + self.pinocchio_wrapper.model, self.pinocchio_wrapper.data, q_reduced + ) + + def clip_configuration(self, q_reduced: np.ndarray, margin: float = 1e-6) -> np.ndarray: + """ + Clip the reduced configuration to stay within joint limits with a small tolerance. + + :param q_reduced: Configuration to clip + :param margin: Tolerance to keep away from joint limits + :return: Clipped configuration + """ + q_full = self.reduced_to_full_configuration(q_reduced) + q_full_clipped = self.full_robot.clip_configuration(q_full, margin) + return self.full_to_reduced_configuration(q_full_clipped) + + def reset_forward_kinematics(self): + """ + Reset the forward kinematics to the initial configuration. + """ + # Reset full robot's forward kinematics + self.full_robot.reset_forward_kinematics() + # Reset reduced model's forward kinematics + self.cache_forward_kinematics(self.q_zero) diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/sensor/__init__.py b/GR00T-WholeBodyControl/decoupled_wbc/control/sensor/__init__.py new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/sensor/composed_camera.py b/GR00T-WholeBodyControl/decoupled_wbc/control/sensor/composed_camera.py new file mode 100644 index 0000000000000000000000000000000000000000..675a93563148b01926222c5854149b09d82317b3 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/sensor/composed_camera.py @@ -0,0 +1,440 @@ +from collections import deque +from dataclasses import dataclass +import queue +import threading +import time +from typing import Any, Dict, Optional + +# we need to import these first in this order to avoid TSL segmentation fault +# caused by zed and oak libraries +try: + import cv2 # noqa + import depthai as dai # noqa + import pyzed.sl as sl # noqa +except ImportError: + print( + """ + Some of the camera specific dependencies are not installed. If you are + not running this on the robot, having these libraries is optional. + """ + ) + +import numpy as np # noqa + +from decoupled_wbc.control.base.sensor import Sensor +from decoupled_wbc.control.sensor.sensor_server import ( + ImageMessageSchema, + SensorClient, + SensorServer, + CameraMountPosition, +) + + +def read_qr_code(data): + current_time = time.monotonic() + detector = cv2.QRCodeDetector() + for key, img in data["images"].items(): + decoded_time, bbox, _ = detector.detectAndDecode(img) + if bbox is not None and decoded_time: + print(f"{key} latency: {(current_time - float(decoded_time)) * 1e3:.1f} ms") + else: + print(f"{key} QR code not detected.") + + +@dataclass +class ComposedCameraConfig: + """Camera configuration for composed camera""" + + ego_view_camera: Optional[str] = "oak" + """Camera type for ego view: oak, realsense, zed, or None""" + + ego_view_device_id: Optional[str] = None + """Device ID for ego view camera (optional, used for OAK cameras)""" + + head_camera: Optional[str] = None + """Camera type for head view: oak, oak_mono, realsense, zed or None""" + + head_device_id: Optional[str] = None + """Device ID for head camera (optional, used for OAK cameras)""" + + left_wrist_camera: Optional[str] = None + """Camera type for left wrist view: oak, realsense, zed or None""" + + left_wrist_device_id: Optional[str] = None + """Device ID for left wrist camera (optional, used for OAK cameras)""" + + right_wrist_camera: Optional[str] = None + """Camera type for right wrist view: oak, realsense, zed or None""" + + right_wrist_device_id: Optional[str] = None + """Device ID for right wrist camera (optional, used for OAK cameras)""" + + fps: int = 30 + """Rate at which the composed camera will publish the images. Since composed camera + can read from multiple cameras, it will publish all the images. + Note that OAK can only run at 30 FPS. 20 FPS will cause large latency. + """ + + # Server configuration + run_as_server: bool = True + """Whether to run as server or client""" + + server: bool = True + """Whether to run the camera as a server""" + + port: int = 5555 + """Port number for server/client communication""" + + test_latency: bool = False + """Whether to test latency""" + + # Queue configuration + queue_size: int = 3 + """Size of each camera's image queue""" + + def __post_init__(self): + # runyu: Note that this is a hack to make the config work with G1 camera server in orin + # we should not use this hack in the future + self.run_as_server: bool = self.server + + +class ComposedCameraSensor(Sensor, SensorServer): + + def __init__(self, config: ComposedCameraConfig): + self.config = config + self.camera_queues: Dict[str, queue.Queue] = {} + self.camera_threads: Dict[str, threading.Thread] = {} + self.shutdown_events: Dict[str, threading.Event] = {} + self.error_events: Dict[str, threading.Event] = {} + self.error_messages: Dict[str, str] = {} + self._observation_spaces: Dict[str, Any] = {} + + camera_configs = self._get_camera_configs() + + # Then create worker threads + for mount_position, camera_config in camera_configs.items(): + # Create queue and shutdown event for this camera + camera_queue = queue.Queue(maxsize=config.queue_size) + shutdown_event = threading.Event() + error_event = threading.Event() + + self.camera_queues[mount_position] = camera_queue + self.shutdown_events[mount_position] = shutdown_event + self.error_events[mount_position] = error_event + + # Start camera thread + thread = threading.Thread( + target=self._camera_worker_wrapper, + args=( + mount_position, + camera_config["camera_type"], + camera_config["device_id"], + camera_queue, + shutdown_event, + error_event, + ), + ) + thread.start() + self.camera_threads[mount_position] = thread + + if config.run_as_server: + self.start_server(config.port) + + def _get_camera_configs(self) -> Dict[str, str]: + """Get camera configurations as mount_position -> camera_type mapping""" + camera_configs = {} + + if self.config.ego_view_camera is not None: + camera_configs[CameraMountPosition.EGO_VIEW.value] = { + "camera_type": self.config.ego_view_camera, + "device_id": self.config.ego_view_device_id, + } + + if self.config.head_camera is not None: + camera_configs[CameraMountPosition.HEAD.value] = { + "camera_type": self.config.head_camera, + "device_id": self.config.head_device_id, + } + + if self.config.left_wrist_camera is not None: + camera_configs[CameraMountPosition.LEFT_WRIST.value] = { + "camera_type": self.config.left_wrist_camera, + "device_id": self.config.left_wrist_device_id, + } + + if self.config.right_wrist_camera is not None: + camera_configs[CameraMountPosition.RIGHT_WRIST.value] = { + "camera_type": self.config.right_wrist_camera, + "device_id": self.config.right_wrist_device_id, + } + + return camera_configs + + def _camera_worker_wrapper( + self, + mount_position: str, + camera_type: str, + device_id: Optional[str], + image_queue: queue.Queue, + shutdown_event: threading.Event, + error_event: threading.Event, + ): + """Worker thread that continuously captures from a single camera""" + try: + camera = self._instantiate_camera(mount_position, camera_type, device_id) + self._observation_spaces[mount_position] = camera.observation_space() + + consecutive_failures = 0 + max_consecutive_failures = 5 + + while not shutdown_event.is_set(): + frame = camera.read() + if frame: + consecutive_failures = 0 # Reset on successful read + # Non-blocking queue put with frame dropping + try: + image_queue.put_nowait(frame) + except queue.Full: + # Remove oldest frame and add new one + try: + image_queue.get_nowait() + image_queue.put_nowait(frame) + except queue.Empty: + pass + else: + consecutive_failures += 1 + if consecutive_failures >= max_consecutive_failures: + error_msg = ( + f"Camera {mount_position} ({camera_type}) dropped: " + f"failed to read {consecutive_failures} consecutive frames" + ) + print(f"[ERROR] {error_msg}") + self.error_messages[mount_position] = error_msg + error_event.set() + break + + camera.close() + + except Exception as e: + error_msg = f"Camera {mount_position} ({camera_type}) error: {str(e)}" + print(f"[ERROR] {error_msg}") + self.error_messages[mount_position] = error_msg + error_event.set() + + def _instantiate_camera( + self, mount_position: str, camera_type: str, device_id: Optional[str] = None + ) -> Sensor: + """ + Instantiate a camera sensor based on the camera type. + + Args: + camera_type: Type of camera ("oak", "oak_mono", "realsense", "zed") + device_id: Optional device ID for the camera (used for OAK cameras) + + Returns: + Sensor instance for the specified camera type + """ + if camera_type in ("oak", "oak_mono"): + from decoupled_wbc.control.sensor.oak import OAKConfig, OAKSensor + + oak_config = OAKConfig() + if camera_type == "oak_mono": + oak_config.enable_mono_cameras = True + print("Initializing OAK sensor for camera type: ", camera_type) + return OAKSensor(config=oak_config, mount_position=mount_position, device_id=device_id) + elif camera_type == "realsense": + from decoupled_wbc.control.sensor.realsense import RealSenseSensor + + print("Initializing RealSense sensor for camera type: ", camera_type) + return RealSenseSensor(mount_position=mount_position) + elif camera_type == "zed": + from decoupled_wbc.control.sensor.zed import ZEDSensor + + print("Initializing ZED sensor for camera type: ", camera_type) + return ZEDSensor(mount_position=mount_position) + elif camera_type.endswith(".mp4"): + from decoupled_wbc.control.sensor.dummy import ReplayDummySensor + + print("Initializing Replay Dummy Sensor for camera type: ", camera_type) + return ReplayDummySensor(video_path=camera_type) + else: + raise ValueError(f"Unsupported camera type: {camera_type}") + + def _check_for_errors(self): + """Check if any camera thread has encountered an error and raise exception if so.""" + for mount_position, error_event in self.error_events.items(): + if error_event.is_set(): + error_msg = self.error_messages.get( + mount_position, f"Camera {mount_position} encountered an unknown error" + ) + raise RuntimeError(error_msg) + + def read(self): + """Read frames from all cameras.""" + # Check for errors from camera threads + self._check_for_errors() + + message = {} + for mount_position, camera_queue in self.camera_queues.items(): + frame = self._get_latest_from_queue(camera_queue) + if frame is not None: + message[mount_position] = frame + return message + + def _get_latest_from_queue(self, camera_queue: queue.Queue) -> Optional[Dict[str, Any]]: + """Get most recent frame, discard older ones""" + latest = None + try: + while True: + latest = camera_queue.get_nowait() + except queue.Empty: + pass + return latest + + def close(self): + """Close all cameras.""" + # Signal all worker threads to shutdown + for shutdown_event in self.shutdown_events.values(): + shutdown_event.set() + + # Wait for all threads to finish + for thread in self.camera_threads.values(): + thread.join(timeout=5.0) + + # Clear queues + for camera_queue in self.camera_queues.values(): + try: + while True: + camera_queue.get_nowait() + except queue.Empty: + pass + + # Stop server if running + if self.config.run_as_server: + self.stop_server() + + def serialize_message(self, message: Dict[str, Any]) -> Dict[str, Any]: + """Merge all camera data into a single ImageMessageSchema.""" + all_timestamps = {} + all_images = {} + + for _, camera_data in message.items(): + all_timestamps.update(camera_data.get("timestamps", {})) + all_images.update(camera_data.get("images", {})) + + # Create a single ImageMessageSchema with all data + img_schema = ImageMessageSchema(timestamps=all_timestamps, images=all_images) + return img_schema.serialize() + + def run_server(self): + """Run the server.""" + idx = 0 + server_start_time = time.monotonic() + fps_print_time = time.monotonic() + frame_interval = 1.0 / self.config.fps + + while True: + # Calculate when this frame should ideally complete + target_time = server_start_time + (idx + 1) * frame_interval + + message = self.read() + if message: + if self.config.test_latency: + read_qr_code(message) + + serialized_message = self.serialize_message(message) + self.send_message(serialized_message) + idx += 1 + + if idx % 10 == 0: + print(f"Image sending FPS: {10 / (time.monotonic() - fps_print_time):.2f}") + fps_print_time = time.monotonic() + + # Sleep to maintain precise timing + current_time = time.monotonic() + sleep_time = target_time - current_time + if sleep_time > 0: + time.sleep(sleep_time) + else: + # If we're behind, increment idx to stay on schedule + if not message: + idx += 1 + + def observation_space(self): + """Return the observation space.""" + import gymnasium as gym + + return gym.spaces.Dict(self._observation_spaces) + + +class ComposedCameraClientSensor(Sensor, SensorClient): + """Class that serves as client for multiple different cameras.""" + + def __init__(self, server_ip: str = "localhost", port: int = 5555): + self.start_client(server_ip, port) + + # Initialize tracking variables + self._latest_message = {} + self._avg_time_per_frame = deque(maxlen=20) + self._msg_received_time = 0 + self._start_time = 0.0 # Initialize _start_time + self.idx = 0 + + print("Initialized composed camera client sensor") + + def read(self, **kwargs) -> Optional[Dict[str, Any]]: + self._start_time = time.time() + message = self.receive_message() + if not message: + return None + self.idx += 1 + + self._latest_message = ImageMessageSchema.deserialize(message).asdict() + + # if self.idx % 10 == 0: + # for image_key, image_time in self._latest_message["timestamps"].items(): + # image_latency = (time.time() - image_time) * 1000 + # print(f"Image latency for {image_key}: {image_latency:.2f} ms") + + self._msg_received_time = time.time() + self._avg_time_per_frame.append(self._msg_received_time - self._start_time) + + return self._latest_message + + def close(self): + """Close the client connection.""" + self.stop_client() + + def fps(self) -> float: + """Get the current FPS of the client.""" + if len(self._avg_time_per_frame) == 0: + return 0.0 + return float(1 / np.mean(self._avg_time_per_frame)) + + +if __name__ == "__main__": + """Test function for ComposedCamera.""" + import tyro + + config = tyro.cli(ComposedCameraConfig) + + if config.run_as_server: + composed_camera = ComposedCameraSensor(config) + print("Running composed camera server...") + composed_camera.run_server() + + else: + # Client mode + composed_client = ComposedCameraClientSensor(server_ip="localhost", port=config.port) + + try: + while True: + data = composed_client.read() + if data is not None: + print(f"FPS: {composed_client.fps():.2f}") + if "timestamp" in data: + print(f"Timestamp: {data['timestamp']}") + time.sleep(0.1) + except KeyboardInterrupt: + print("Stopping client...") + composed_client.close() diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/sensor/oak.py b/GR00T-WholeBodyControl/decoupled_wbc/control/sensor/oak.py new file mode 100644 index 0000000000000000000000000000000000000000..ae7b532b8f03135e262801c42ad254bf49dca5d2 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/sensor/oak.py @@ -0,0 +1,324 @@ +import time +from typing import Any, Dict, Optional, Tuple + +import cv2 +import depthai as dai +import gymnasium as gym +import numpy as np + +from decoupled_wbc.control.base.sensor import Sensor +from decoupled_wbc.control.sensor.sensor_server import ( + CameraMountPosition, + ImageMessageSchema, + SensorServer, +) + + +class OAKConfig: + """Configuration for the OAK camera.""" + + color_image_dim: Tuple[int, int] = (640, 480) # RGB camera resolution + monochrome_image_dim: Tuple[int, int] = (640, 480) # Monochrome camera resolution + fps: int = 30 + enable_color: bool = True # Enable CAM_A (RGB) + enable_mono_cameras: bool = False # Enable CAM_B & CAM_C (Monochrome stereo pair) + mount_position: str = CameraMountPosition.EGO_VIEW.value + + +class OAKSensor(Sensor, SensorServer): + """Sensor for the OAK camera family.""" + + def __init__( + self, + run_as_server: bool = False, + port: int = 5555, + config: OAKConfig = OAKConfig(), + device_id: Optional[str] = None, + mount_position: str = CameraMountPosition.EGO_VIEW.value, + ): + """Initialize the OAK camera.""" + self.config = config + self.mount_position = mount_position + self._run_as_server = run_as_server + + device_infos = dai.Device.getAllAvailableDevices() + assert len(device_infos) > 0, f"No OAK devices found for {mount_position}" + print(f"Device infos: {device_infos}") + if device_id is not None: + device_found = False + for device_info in device_infos: + if device_info.getDeviceId() == device_id: + self.device = dai.Device(device_info) + device_found = True + break + if not device_found: + raise ValueError(f"Device with ID {device_id} not found") + else: + self.device = dai.Device() + + print(f"Connected to OAK device: {self.device.getDeviceName(), self.device.getDeviceId()}") + print(f"Device ID: {self.device.getDeviceId()}") + + sockets: list[dai.CameraBoardSocket] = self.device.getConnectedCameras() + print(f"Available cameras: {[str(s) for s in sockets]}") + + # Create pipeline (without context manager to persist across method calls) + self.pipeline = dai.Pipeline(self.device) + self.output_queues = {} + + # Configure RGB camera (CAM_A) + if config.enable_color and dai.CameraBoardSocket.CAM_A in sockets: + self.cam_rgb = self.pipeline.create(dai.node.Camera) + cam_socket = dai.CameraBoardSocket.CAM_A + self.cam_rgb = self.cam_rgb.build(cam_socket) + # Create RGB output queue + self.output_queues["color"] = self.cam_rgb.requestOutput( + config.color_image_dim, + fps=config.fps, + ).createOutputQueue() + print("Enabled CAM_A (RGB)") + + # Configure Monochrome cameras (CAM_B & CAM_C) + if config.enable_mono_cameras: + if dai.CameraBoardSocket.CAM_B in sockets: + self.cam_mono_left = self.pipeline.create(dai.node.Camera) + cam_socket = dai.CameraBoardSocket.CAM_B + self.cam_mono_left = self.cam_mono_left.build(cam_socket) + # Create mono left output queue + self.output_queues["mono_left"] = self.cam_mono_left.requestOutput( + config.monochrome_image_dim, + fps=config.fps, + ).createOutputQueue() + print("Enabled CAM_B (Monochrome Left)") + + if dai.CameraBoardSocket.CAM_C in sockets: + self.cam_mono_right = self.pipeline.create(dai.node.Camera) + cam_socket = dai.CameraBoardSocket.CAM_C + self.cam_mono_right = self.cam_mono_right.build(cam_socket) + # Create mono right output queue + self.output_queues["mono_right"] = self.cam_mono_right.requestOutput( + config.monochrome_image_dim, + fps=config.fps, + ).createOutputQueue() + print("Enabled CAM_C (Monochrome Right)") + + assert len(self.output_queues) > 0, "No output queues enabled" + # auto exposure compensation, for CoRL demo + # cam_q_in = self.cam_rgb.inputControl.createInputQueue() + # ctrl = dai.CameraControl() + # ctrl.setAutoExposureEnable() + # ctrl.setAutoExposureCompensation(-2) + # cam_q_in.send(ctrl) + + # Start pipeline on device + self.pipeline.start() + + if run_as_server: + self.start_server(port) + + def read(self) -> Optional[Dict[str, Any]]: + """Read images from the camera.""" + if not self.pipeline.isRunning(): + print(f"[ERROR] OAK pipeline stopped for {self.mount_position}") + return None + + # Check if device is still connected + if not self.device.isPipelineRunning(): + print(f"[ERROR] OAK device disconnected for {self.mount_position}") + return None + + timestamps = {} + images = {} + rgb_frame_time = None + + # Get color frame if enabled + if "color" in self.output_queues: + try: + rgb_frame = self.output_queues["color"].get() + rgb_frame_time = rgb_frame.getTimestamp() + if rgb_frame is not None: + images[self.mount_position] = rgb_frame.getCvFrame()[..., ::-1] # BGR to RGB + timestamps[self.mount_position] = ( + rgb_frame_time - dai.Clock.now() + ).total_seconds() + time.time() + except Exception as e: + print(f"[ERROR] Failed to read color frame from {self.mount_position}: {e}") + return None + + # Get mono frames if enabled + if "mono_left" in self.output_queues: + try: + mono_left_frame = self.output_queues["mono_left"].get() + mono_left_frame_time = mono_left_frame.getTimestamp() + if mono_left_frame is not None: + key = f"{self.mount_position}_left_mono" + images[key] = mono_left_frame.getCvFrame() + timestamps[key] = ( + mono_left_frame_time - dai.Clock.now() + ).total_seconds() + time.time() + except Exception as e: + print(f"[ERROR] Failed to read mono_left frame from {self.mount_position}: {e}") + return None + + if "mono_right" in self.output_queues: + try: + mono_right_frame = self.output_queues["mono_right"].get() + mono_right_frame_time = mono_right_frame.getTimestamp() + if mono_right_frame is not None: + key = f"{self.mount_position}_right_mono" + images[key] = mono_right_frame.getCvFrame() + timestamps[key] = ( + mono_right_frame_time - dai.Clock.now() + ).total_seconds() + time.time() + except Exception as e: + print(f"[ERROR] Failed to read mono_right frame from {self.mount_position}: {e}") + return None + + if ( + rgb_frame_time is not None + and (rgb_frame_time - dai.Clock.now()).total_seconds() <= -0.2 + ): + print( + f"[{self.mount_position}] OAK latency too large: " + f"{(dai.Clock.now() - rgb_frame_time).total_seconds() * 1000}ms" + ) + + return { + "timestamps": timestamps, + "images": images, + } + + def serialize(self, data: Dict[str, Any]) -> Dict[str, Any]: + """Serialize data using ImageMessageSchema.""" + serialized_msg = ImageMessageSchema(timestamps=data["timestamps"], images=data["images"]) + return serialized_msg.serialize() + + def observation_space(self) -> gym.Space: + spaces = {} + + if self.config.enable_color: + spaces["color_image"] = gym.spaces.Box( + low=0, + high=255, + shape=(self.config.color_image_dim[1], self.config.color_image_dim[0], 3), + dtype=np.uint8, + ) + + if self.config.enable_mono_cameras: + spaces["mono_left_image"] = gym.spaces.Box( + low=0, + high=255, + shape=(self.config.monochrome_image_dim[1], self.config.monochrome_image_dim[0]), + dtype=np.uint8, + ) + spaces["mono_right_image"] = gym.spaces.Box( + low=0, + high=255, + shape=(self.config.monochrome_image_dim[1], self.config.monochrome_image_dim[0]), + dtype=np.uint8, + ) + + return gym.spaces.Dict(spaces) + + def close(self): + """Close the camera connection.""" + if self._run_as_server: + self.stop_server() + if hasattr(self, "pipeline") and self.pipeline.isRunning(): + self.pipeline.stop() + self.device.close() + + def run_server(self): + """Run the server.""" + if not self._run_as_server: + raise ValueError("This function is only available when run_as_server is True") + + while True: + frame = self.read() + if frame is None: + continue + + msg = self.serialize(frame) + self.send_message({self.mount_position: msg}) + + def __del__(self): + self.close() + + +if __name__ == "__main__": + """Test function for OAK camera.""" + + import argparse + + parser = argparse.ArgumentParser() + parser.add_argument("--server", action="store_true", help="Run as server") + parser.add_argument("--client", action="store_true", help="Run as client") + parser.add_argument("--host", type=str, default="localhost", help="Server IP address") + parser.add_argument("--port", type=int, default=5555, help="Port number") + parser.add_argument("--device-id", type=str, default=None, help="Specific device ID") + parser.add_argument( + "--enable-mono", action="store_true", help="Enable monochrome cameras (CAM_B & CAM_C)" + ) + parser.add_argument("--mount-position", type=str, default="ego_view", help="Mount position") + parser.add_argument("--show-image", action="store_true", help="Display images") + args = parser.parse_args() + + oak_config = OAKConfig() + if args.enable_mono: + oak_config.enable_mono_cameras = True + + if args.server: + # Run as server + oak = OAKSensor( + run_as_server=True, + port=args.port, + config=oak_config, + device_id=args.device_id, + mount_position=args.mount_position, + ) + print(f"Starting OAK server on port {args.port}") + oak.run_server() + + else: + # Run standalone + oak = OAKSensor(run_as_server=False, config=oak_config, device_id=args.device_id) + print("Running OAK camera in standalone mode") + + while True: + frame = oak.read() + if frame is None: + print("Waiting for frame...") + time.sleep(0.5) + continue + + if "color_image" in frame: + print(f"Color image shape: {frame['color_image'].shape}") + if "mono_left_image" in frame: + print(f"Mono left image shape: {frame['mono_left_image'].shape}") + if "mono_right_image" in frame: + print(f"Mono right image shape: {frame['mono_right_image'].shape}") + if "depth_image" in frame: + print(f"Depth image shape: {frame['depth_image'].shape}") + + if args.show_image: + if "color_image" in frame: + cv2.imshow("Color Image", frame["color_image"]) + + if "mono_left_image" in frame: + cv2.imshow("Mono Left", frame["mono_left_image"]) + if "mono_right_image" in frame: + cv2.imshow("Mono Right", frame["mono_right_image"]) + + if "depth_image" in frame: + depth_colormap = cv2.applyColorMap( + cv2.convertScaleAbs(frame["depth_image"], alpha=0.03), cv2.COLORMAP_JET + ) + cv2.imshow("Depth Image", depth_colormap) + + if cv2.waitKey(1) == ord("q"): + break + + time.sleep(0.01) + + cv2.destroyAllWindows() + oak.close() diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/sensor/sensor_server.py b/GR00T-WholeBodyControl/decoupled_wbc/control/sensor/sensor_server.py new file mode 100644 index 0000000000000000000000000000000000000000..28453377963567928c5a54c9e44662bfa9650df2 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/sensor/sensor_server.py @@ -0,0 +1,128 @@ +import base64 +from dataclasses import dataclass +from enum import Enum +from typing import Any, Dict + +import cv2 +import msgpack +import msgpack_numpy as m +import numpy as np +import zmq + + +@dataclass +class ImageMessageSchema: + """ + This is a standardized message schema for image data. + Any camera should use this schema to serialize (send to queue) and + deserialize (receive from queue) the image data. + + """ + + timestamps: Dict[str, float] + """Dictionary of timestamps, keyed by image identifier (e.g., {"ego_view": 123.45})""" + images: Dict[str, np.ndarray] + """Dictionary of images, keyed by image identifier (e.g., {"ego_view": array, "ego_view_left_mono": array})""" + + def serialize(self) -> Dict[str, Any]: + """Serialize the message for transmission.""" + serialized_msg = {"timestamps": self.timestamps, "images": {}} + for key, image in self.images.items(): + serialized_msg["images"][key] = ImageUtils.encode_image(image) + return serialized_msg + + @staticmethod + def deserialize(data: Dict[str, Any]) -> "ImageMessageSchema": + """Deserialize received message data.""" + timestamps = data.get("timestamps", {}) + images = {} + for key, value in data.get("images", {}).items(): + if isinstance(value, str): + images[key] = ImageUtils.decode_image(value) + else: + images[key] = value + return ImageMessageSchema(timestamps=timestamps, images=images) + + def asdict(self) -> Dict[str, Any]: + """Convert to dictionary format.""" + return {"timestamps": self.timestamps, "images": self.images} + + +class SensorServer: + def start_server(self, port: int): + self.context = zmq.Context() + self.socket = self.context.socket(zmq.PUB) + self.socket.setsockopt(zmq.SNDHWM, 20) # high water mark + self.socket.setsockopt(zmq.LINGER, 0) + self.socket.bind(f"tcp://*:{port}") + print(f"Sensor server running at tcp://*:{port}") + + self.message_sent = 0 + self.message_dropped = 0 + + def stop_server(self): + self.socket.close() + self.context.term() + + def send_message(self, data: Dict[str, Any]): + try: + packed = msgpack.packb(data, use_bin_type=True) + self.socket.send(packed, flags=zmq.NOBLOCK) + except zmq.Again: + self.message_dropped += 1 + print(f"[Warning] message dropped: {self.message_dropped}") + self.message_sent += 1 + + if self.message_sent % 100 == 0: + print( + f"[Sensor server] Message sent: {self.message_sent}, message dropped: {self.message_dropped}" + ) + + +class SensorClient: + def start_client(self, server_ip: str, port: int): + self.context = zmq.Context() + self.socket = self.context.socket(zmq.SUB) + self.socket.setsockopt_string(zmq.SUBSCRIBE, "") + self.socket.setsockopt(zmq.CONFLATE, True) # last msg only. + self.socket.setsockopt(zmq.RCVHWM, 3) # queue size 3 for receive buffer + self.socket.connect(f"tcp://{server_ip}:{port}") + + def stop_client(self): + self.socket.close() + self.context.term() + + def receive_message(self): + packed = self.socket.recv() + return msgpack.unpackb(packed, object_hook=m.decode) + + +class CameraMountPosition(Enum): + EGO_VIEW = "ego_view" + HEAD = "head" + LEFT_WRIST = "left_wrist" + RIGHT_WRIST = "right_wrist" + + +class ImageUtils: + @staticmethod + def encode_image(image: np.ndarray) -> str: + _, color_buffer = cv2.imencode(".jpg", image, [int(cv2.IMWRITE_JPEG_QUALITY), 80]) + return base64.b64encode(color_buffer).decode("utf-8") + + @staticmethod + def encode_depth_image(image: np.ndarray) -> str: + depth_compressed = cv2.imencode(".png", image)[1].tobytes() + return base64.b64encode(depth_compressed).decode("utf-8") + + @staticmethod + def decode_image(image: str) -> np.ndarray: + color_data = base64.b64decode(image) + color_array = np.frombuffer(color_data, dtype=np.uint8) + return cv2.imdecode(color_array, cv2.IMREAD_COLOR) + + @staticmethod + def decode_depth_image(image: str) -> np.ndarray: + depth_data = base64.b64decode(image) + depth_array = np.frombuffer(depth_data, dtype=np.uint8) + return cv2.imdecode(depth_array, cv2.IMREAD_UNCHANGED) diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/teleop/teleop_retargeting_ik.py b/GR00T-WholeBodyControl/decoupled_wbc/control/teleop/teleop_retargeting_ik.py new file mode 100644 index 0000000000000000000000000000000000000000..0bbf7d070487cc704473de262e3fabe2aa63d536 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/teleop/teleop_retargeting_ik.py @@ -0,0 +1,148 @@ +import time +from typing import List, Optional + +import numpy as np + +from decoupled_wbc.control.base.policy import Policy +from decoupled_wbc.control.robot_model.robot_model import ReducedRobotModel, RobotModel +from decoupled_wbc.control.teleop.solver.body.body_ik_solver import BodyIKSolver +from decoupled_wbc.control.teleop.solver.body.body_ik_solver_settings import BodyIKSolverSettings +from decoupled_wbc.control.teleop.solver.solver import Solver +from decoupled_wbc.control.visualization.humanoid_visualizer import RobotVisualizer + + +class TeleopRetargetingIK(Policy): + """ + Robot-agnostic teleop retargeting inverse kinematics code. + Focus only on IK processing, ignore commands. + """ + + def __init__( + self, + robot_model: RobotModel, + left_hand_ik_solver: Solver, + right_hand_ik_solver: Solver, + enable_visualization=False, + body_active_joint_groups: Optional[List[str]] = None, + body_ik_solver_settings_type: str = "default", + ): + # initialize the body + if body_active_joint_groups is not None: + self.body = ReducedRobotModel.from_active_groups(robot_model, body_active_joint_groups) + self.full_robot = self.body.full_robot + self.using_reduced_robot_model = True + else: + self.body = robot_model + self.full_robot = self.body + self.using_reduced_robot_model = False + if body_ik_solver_settings_type == "default": + body_ik_solver_settings = BodyIKSolverSettings() + else: + raise ValueError( + f"Unknown body_ik_solver_settings_type: {body_ik_solver_settings_type}" + ) + self.body_ik_solver = BodyIKSolver(body_ik_solver_settings) + + # We register the specific robot model to the robot-agnostic body IK solver class + self.body_ik_solver.register_robot(self.body) + + # Hand IK solvers are hand specific, so we pass them in the constructor + self.left_hand_ik_solver = left_hand_ik_solver + self.right_hand_ik_solver = right_hand_ik_solver + + # enable visualizer + self.enable_visualization = enable_visualization + if self.enable_visualization: + self.visualizer = RobotVisualizer(self.full_robot) + self.visualizer.visualize(self.full_robot.q_zero) + time.sleep(1) # wait for the visualizer to start + + self.in_warmup = True + self._most_recent_ik_data = None + self._most_recent_q = self.full_robot.default_body_pose.copy() + + def compute_joint_positions( + self, body_data: dict, left_hand_data: dict, right_hand_data: dict + ) -> np.ndarray: + """Process only IK-related data, return joint positions""" + if self.in_warmup: + # TODO: Warmup is not necessary if we start IK from the current robot qpos, rather than the zero qpos + for _ in range(50): + target_robot_joints = self._inverse_kinematics( + body_data, left_hand_data, right_hand_data + ) + self.in_warmup = False + else: + target_robot_joints = self._inverse_kinematics( + body_data, left_hand_data, right_hand_data + ) + + return target_robot_joints + + def _inverse_kinematics( + self, + body_target_pose, + left_hand_target_pose, + right_hand_target_pose, + ): + """ + Solve the inverse kinematics problem for the given target poses. + Args: + body_target_pose: Dictionary of link names and their corresponding target pose. + left_hand_target_pose: Dictionary with key "position" mapping to a (25, 4, 4) np.ndarray from AVP data + right_hand_target_pose: Dictionary with key "position" mapping to a (25, 4, 4) np.ndarray from AVP data + q: Initial configuration vector. + Returns: + Configuration vector that achieves the target poses. + """ + if body_target_pose: + if self.using_reduced_robot_model: + body_q = self.body.reduced_to_full_configuration( + self.body_ik_solver(body_target_pose) + ) + else: + body_q = self.body_ik_solver(body_target_pose) + else: + # If no body target pose is provided, set the body to the default pose + body_q = self.full_robot.default_body_pose.copy() + + if left_hand_target_pose is not None: + left_hand_actuated_q = self.left_hand_ik_solver(left_hand_target_pose) + body_q[self.full_robot.get_hand_actuated_joint_indices(side="left")] = ( + left_hand_actuated_q + ) + + if right_hand_target_pose is not None: + right_hand_actuated_q = self.right_hand_ik_solver(right_hand_target_pose) + body_q[self.full_robot.get_hand_actuated_joint_indices(side="right")] = ( + right_hand_actuated_q + ) + + if self.enable_visualization: + self.visualizer.visualize(np.array(body_q)) + + return body_q + + def reset(self): + """Reset the robot model and IK solvers to the initial state, and re-activate the warmup procedure.""" + self.body.reset_forward_kinematics() # self.body is the same one as self.body_ik_solver.robot + self.full_robot.reset_forward_kinematics() + self.body_ik_solver.initialize() + # If in the future, the hand IK solver has initialize method, call it + self._most_recent_ik_data = None + self._most_recent_q = self.full_robot.default_body_pose.copy() + self.in_warmup = True + + def set_goal(self, ik_data: dict): + self._most_recent_ik_data = ik_data + + def get_action(self) -> dict[str, any]: + # Process IK if active + if self._most_recent_ik_data is not None: + body_data = self._most_recent_ik_data["body_data"] + left_hand_data = self._most_recent_ik_data["left_hand_data"] + right_hand_data = self._most_recent_ik_data["right_hand_data"] + target_joints = self.compute_joint_positions(body_data, left_hand_data, right_hand_data) + self._most_recent_q = target_joints + + return self._most_recent_q[self.full_robot.get_joint_group_indices("upper_body")] diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/teleop/teleop_streamer.py b/GR00T-WholeBodyControl/decoupled_wbc/control/teleop/teleop_streamer.py new file mode 100644 index 0000000000000000000000000000000000000000..df761a74f021e26438d6dad70b9fbae031278da6 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/teleop/teleop_streamer.py @@ -0,0 +1,240 @@ +from math import floor +import pickle +from typing import Optional + +from decoupled_wbc.control.robot_model.robot_model import RobotModel +from decoupled_wbc.control.teleop.pre_processor.fingers.fingers import FingersPreProcessor +from decoupled_wbc.control.teleop.pre_processor.wrists.wrists import WristsPreProcessor +from decoupled_wbc.control.teleop.streamers.base_streamer import StreamerOutput + + +class TeleopStreamer: + def __init__( + self, + robot_model: RobotModel, + body_control_device: Optional[str] = None, + hand_control_device: Optional[str] = None, + enable_real_device=True, + body_streamer_ip="", + body_streamer_keyword="", + replay_data_path: Optional[str] = None, + replay_speed: float = 1.0, + ): + # initialize the body + self.body = robot_model + + self.body_control_device = body_control_device + self.hand_control_device = hand_control_device + self.body_streamer_ip = body_streamer_ip + self.body_streamer_keyword = body_streamer_keyword + self.replay_speed = replay_speed + + # enable real robot and devices + self.enable_real_device = enable_real_device + if self.enable_real_device: + if body_control_device == "vive": + from decoupled_wbc.control.teleop.streamers.vive_streamer import ViveStreamer + + self.body_streamer = ViveStreamer( + ip=self.body_streamer_ip, keyword=self.body_streamer_keyword + ) + self.body_streamer.start_streaming() + elif body_control_device == "iphone": + from decoupled_wbc.control.teleop.streamers.iphone_streamer import IphoneStreamer + + self.body_streamer = IphoneStreamer() + self.body_streamer.start_streaming() + elif body_control_device == "leapmotion": + from decoupled_wbc.control.teleop.streamers.leapmotion_streamer import ( + LeapMotionStreamer, + ) + + self.body_streamer = LeapMotionStreamer() + self.body_streamer.start_streaming() + elif body_control_device == "joycon": + from decoupled_wbc.control.teleop.streamers.joycon_streamer import JoyconStreamer + + self.body_streamer = JoyconStreamer() + self.body_streamer.start_streaming() + + elif body_control_device == "pico": + from decoupled_wbc.control.teleop.streamers.pico_streamer import PicoStreamer + + self.body_streamer = PicoStreamer() + self.body_streamer.start_streaming() + elif body_control_device == "dummy": + from decoupled_wbc.control.teleop.streamers.dummy_streamer import DummyStreamer + + self.body_streamer = DummyStreamer() + self.body_streamer.start_streaming() + else: + self.body_streamer = None + + if hand_control_device and hand_control_device != body_control_device: + if hand_control_device == "manus": + from decoupled_wbc.control.teleop.streamers.manus_streamer import ManusStreamer + + self.hand_streamer = ManusStreamer() + self.hand_streamer.start_streaming() + elif hand_control_device == "joycon": + from decoupled_wbc.control.teleop.streamers.joycon_streamer import JoyconStreamer + + self.hand_streamer = JoyconStreamer() + self.hand_streamer.start_streaming() + elif hand_control_device == "iphone": + from decoupled_wbc.control.teleop.streamers.iphone_streamer import IphoneStreamer + + self.hand_streamer = IphoneStreamer() + self.hand_streamer.start_streaming() + elif hand_control_device == "pico": + from decoupled_wbc.control.teleop.streamers.pico_streamer import PicoStreamer + + self.hand_streamer = PicoStreamer() + self.hand_streamer.start_streaming() + else: + self.hand_streamer = None + else: + self.hand_streamer = None + else: + self.body_streamer = None + self.hand_streamer = None + + self.raw_replay_data = None + self.replay_calibration_data = None + self.replay_mode = False + if replay_data_path and not self.enable_real_device: + with open(replay_data_path, "rb") as f: + data_ = pickle.load(f) + self.raw_replay_data = data_["replay_data"] + self.replay_calibration_data = data_["calibration_data"] + print("Found teleop replay data in file: ", replay_data_path) + self.replay_idx = 0 + self.replay_mode = True + + # initialize pre_processors + self.body_control_device = body_control_device + if body_control_device or self.replay_mode: + self.body_pre_processor = WristsPreProcessor( + motion_scale=robot_model.supplemental_info.teleop_upper_body_motion_scale + ) + self.body_pre_processor.register(self.body) + else: + self.body_pre_processor = None + + # initialize hand pre-processors and post-processors + self.hand_control_device = hand_control_device + if hand_control_device or self.replay_mode: + self.left_hand_pre_processor = FingersPreProcessor(side="left") + self.right_hand_pre_processor = FingersPreProcessor(side="right") + + else: + self.left_hand_pre_processor = None + self.right_hand_pre_processor = None + + self.is_calibrated = False + + def _get_replay_data(self) -> StreamerOutput: + streamer_data = StreamerOutput() + + if self.replay_idx < len(self.raw_replay_data): + streamer_data.ik_data.update( + self.raw_replay_data[floor(self.replay_idx / self.replay_speed)] + ) + self.replay_idx += 1 + + return streamer_data + + def _get_live_data(self) -> StreamerOutput: + """Get structured data instead of raw dict""" + if self.body_streamer: + streamer_data = self.body_streamer.get() + else: + streamer_data = StreamerOutput() + + if self.hand_streamer and self.hand_streamer != self.body_streamer: + hand_data = self.hand_streamer.get() + + # Merge hand data into body data (hand data takes precedence) + streamer_data.ik_data.update(hand_data.ik_data) + streamer_data.control_data.update(hand_data.control_data) + streamer_data.teleop_data.update(hand_data.teleop_data) + streamer_data.data_collection_data.update(hand_data.data_collection_data) + + return streamer_data + + def get_streamer_data(self) -> StreamerOutput: + if self.enable_real_device: + streamer_data = self._get_live_data() + elif self.replay_mode: + streamer_data = self._get_replay_data() + else: + streamer_data = StreamerOutput() + + if self.is_calibrated and streamer_data.ik_data: + body_data, left_hand_data, right_hand_data = self.pre_process(streamer_data.ik_data) + streamer_data.ik_data = { + "body_data": body_data, + "left_hand_data": left_hand_data, + "right_hand_data": right_hand_data, + } + elif not self.is_calibrated: + streamer_data.ik_data = {} + + return streamer_data + + def calibrate(self): + """Calibrate the pre-processors using only IK data.""" + if self.replay_mode: + ik_data = self.replay_calibration_data + else: + streamer_data = self._get_live_data() + ik_data = streamer_data.ik_data + + if self.body_pre_processor: + self.body_pre_processor.calibrate(ik_data, self.body_control_device) + if self.left_hand_pre_processor: + self.left_hand_pre_processor.calibrate(ik_data, self.hand_control_device) + if self.right_hand_pre_processor: + self.right_hand_pre_processor.calibrate(ik_data, self.hand_control_device) + + self.is_calibrated = True + + def pre_process(self, raw_data): + """Pre-process the raw data.""" + assert ( + self.body_pre_processor or self.left_hand_pre_processor or self.right_hand_pre_processor + ), "Pre-processors are not initialized." + + # Check if finger data is present in raw_data + has_finger_data = "left_fingers" in raw_data and "right_fingers" in raw_data + + if self.body_pre_processor: + body_data = self.body_pre_processor(raw_data) + # Only process hand data if finger keys are present and preprocessors are available + if has_finger_data and self.left_hand_pre_processor and self.right_hand_pre_processor: + left_hand_data = self.left_hand_pre_processor(raw_data) + right_hand_data = self.right_hand_pre_processor(raw_data) + return body_data, left_hand_data, right_hand_data + else: + return body_data, None, None + else: # only hands + if has_finger_data and self.left_hand_pre_processor and self.right_hand_pre_processor: + left_hand_data = self.left_hand_pre_processor(raw_data) + right_hand_data = self.right_hand_pre_processor(raw_data) + return None, left_hand_data, right_hand_data + else: + # No finger data available, return None for hand data + return None, None, None + + def reset(self): + if self.body_streamer is not None: + self.body_streamer.reset_status() + if self.hand_streamer is not None: + self.hand_streamer.reset_status() + + def stop_streaming(self): + if self.body_streamer: + self.body_streamer.stop_streaming() + # Only stop hand_streamer if it's a different instance than body_streamer + if self.hand_streamer and self.hand_streamer is not self.body_streamer: + self.hand_streamer.stop_streaming() diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/utils/__init__.py b/GR00T-WholeBodyControl/decoupled_wbc/control/utils/__init__.py new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/utils/cv_bridge.py b/GR00T-WholeBodyControl/decoupled_wbc/control/utils/cv_bridge.py new file mode 100644 index 0000000000000000000000000000000000000000..c9e7ade227fd86d2a3af917e04d6c9cc2c3f0ec3 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/utils/cv_bridge.py @@ -0,0 +1,396 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2011, Willow Garage, Inc. +# Copyright (c) 2016, Tal Regev. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import sys + +import cv2 +import sensor_msgs.msg + + +def CV_MAT_CNWrap(flags): + return (((flags) & ((63) << 3)) >> 3) + 1 + + +def CV_MAT_DEPTHWrap(flags): + return (flags) & 7 + + +_CV_CONVERSIONS = { + ("mono8", "rgb8"): cv2.COLOR_GRAY2RGB, + ("mono8", "bgr8"): cv2.COLOR_GRAY2BGR, + ("mono8", "rgba8"): cv2.COLOR_GRAY2RGBA, + ("mono8", "bgra8"): cv2.COLOR_GRAY2BGRA, + ("rgb8", "mono8"): cv2.COLOR_RGB2GRAY, + ("rgb8", "bgr8"): cv2.COLOR_RGB2BGR, + ("rgb8", "rgba8"): cv2.COLOR_RGB2RGBA, + ("rgb8", "bgra8"): cv2.COLOR_RGB2BGRA, + ("bgr8", "mono8"): cv2.COLOR_BGR2GRAY, + ("bgr8", "rgb8"): cv2.COLOR_BGR2RGB, + ("bgr8", "rgba8"): cv2.COLOR_BGR2RGBA, + ("bgr8", "bgra8"): cv2.COLOR_BGR2BGRA, + ("rgba8", "mono8"): cv2.COLOR_RGBA2GRAY, + ("rgba8", "rgb8"): cv2.COLOR_RGBA2RGB, + ("rgba8", "bgr8"): cv2.COLOR_RGBA2BGR, + ("rgba8", "bgra8"): cv2.COLOR_RGBA2BGRA, + ("bgra8", "mono8"): cv2.COLOR_BGRA2GRAY, + ("bgra8", "rgb8"): cv2.COLOR_BGRA2RGB, + ("bgra8", "bgr8"): cv2.COLOR_BGRA2BGR, + ("bgra8", "rgba8"): cv2.COLOR_BGRA2RGBA, + ("yuv422", "mono8"): cv2.COLOR_YUV2GRAY_UYVY, + ("yuv422", "rgb8"): cv2.COLOR_YUV2RGB_UYVY, + ("yuv422", "bgr8"): cv2.COLOR_YUV2BGR_UYVY, + ("yuv422", "rgba8"): cv2.COLOR_YUV2RGBA_UYVY, + ("yuv422", "bgra8"): cv2.COLOR_YUV2BGRA_UYVY, + ("bayer_rggb8", "mono8"): cv2.COLOR_BayerBG2GRAY, + ("bayer_rggb8", "rgb8"): cv2.COLOR_BayerBG2RGB, + ("bayer_rggb8", "bgr8"): cv2.COLOR_BayerBG2BGR, + ("bayer_bggr8", "mono8"): cv2.COLOR_BayerRG2GRAY, + ("bayer_bggr8", "rgb8"): cv2.COLOR_BayerRG2RGB, + ("bayer_bggr8", "bgr8"): cv2.COLOR_BayerRG2BGR, + ("bayer_gbrg8", "mono8"): cv2.COLOR_BayerGR2GRAY, + ("bayer_gbrg8", "rgb8"): cv2.COLOR_BayerGR2RGB, + ("bayer_gbrg8", "bgr8"): cv2.COLOR_BayerGR2BGR, + ("bayer_grbg", "mono8"): cv2.COLOR_BayerGB2GRAY, + ("bayer_grbg", "rgb8"): cv2.COLOR_BayerGB2RGB, + ("bayer_grbg", "bgr8"): cv2.COLOR_BayerGB2BGR, +} + +_CV_TYPES = { + "rgb8": cv2.CV_8UC3, + "rgba8": cv2.CV_8UC4, + "rgb16": cv2.CV_16UC3, + "rgba16": cv2.CV_16UC4, + "bgr8": cv2.CV_8UC3, + "bgra8": cv2.CV_8UC4, + "bgr16": cv2.CV_16UC3, + "bgra16": cv2.CV_16UC4, + "mono8": cv2.CV_8UC1, + "mono16": cv2.CV_16UC1, + "8UC1": cv2.CV_8UC1, + "8UC2": cv2.CV_8UC2, + "8UC3": cv2.CV_8UC3, + "8UC4": cv2.CV_8UC4, + "8SC1": cv2.CV_8SC1, + "8SC2": cv2.CV_8SC2, + "8SC3": cv2.CV_8SC3, + "8SC4": cv2.CV_8SC4, + "16UC1": cv2.CV_8UC1, + "16UC2": cv2.CV_8UC2, + "16UC3": cv2.CV_8UC3, + "16UC4": cv2.CV_8UC4, + "16SC1": cv2.CV_16SC1, + "16SC2": cv2.CV_16SC2, + "16SC3": cv2.CV_16SC3, + "16SC4": cv2.CV_16SC4, + "32SC1": cv2.CV_32SC1, + "32SC2": cv2.CV_32SC2, + "32SC3": cv2.CV_32SC3, + "32SC4": cv2.CV_32SC4, + "32FC1": cv2.CV_32FC1, + "32FC2": cv2.CV_32FC2, + "32FC3": cv2.CV_32FC3, + "32FC4": cv2.CV_32FC4, + "64FC1": cv2.CV_64FC1, + "64FC2": cv2.CV_64FC2, + "64FC3": cv2.CV_64FC3, + "64FC4": cv2.CV_64FC4, + "bayer_rggb8": cv2.CV_8UC1, + "bayer_bggr8": cv2.CV_8UC1, + "bayer_gbrg8": cv2.CV_8UC1, + "bayer_grbg8": cv2.CV_8UC1, + "bayer_rggb16": cv2.CV_16UC1, + "bayer_bggr16": cv2.CV_16UC1, + "bayer_gbrg16": cv2.CV_16UC1, + "bayer_grbg16": cv2.CV_16UC1, +} + + +def cvtColor2(img, encoding_in, encoding_out): + if encoding_in == encoding_out: + return img + + conversion = _CV_CONVERSIONS[(encoding_in, encoding_out)] + # depth conversion is not yet implemented + return cv2.cvtColor(img, conversion) + + +def getCvType(encoding): + return _CV_TYPES[encoding] + + +class CvBridgeError(TypeError): + """ + This is the error raised by :class:`cv_bridge.CvBridge` methods when they fail. + """ + + pass + + +class CvBridge(object): + """ + The CvBridge is an object that converts between OpenCV Images and ROS Image messages. + + .. doctest:: + :options: -ELLIPSIS, +NORMALIZE_WHITESPACE + + >>> import cv2 + >>> import numpy as np + >>> from cv_bridge import CvBridge + >>> br = CvBridge() + >>> dtype, n_channels = br.encoding_as_cvtype2('8UC3') + >>> im = np.ndarray(shape=(480, 640, n_channels), dtype=dtype) + >>> msg = br.cv2_to_imgmsg(im) # Convert the image to a message + >>> im2 = br.imgmsg_to_cv2(msg) # Convert the message to a new image + >>> cmprsmsg = br.cv2_to_compressed_imgmsg(im) # Convert the image to a compress message + >>> im22 = br.compressed_imgmsg_to_cv2(msg) # Convert the compress message to a new image + >>> cv2.imwrite("this_was_a_message_briefly.png", im2) + + """ + + def __init__(self): + import cv2 + + self.cvtype_to_name = {} + self.cvdepth_to_numpy_depth = { + cv2.CV_8U: "uint8", + cv2.CV_8S: "int8", + cv2.CV_16U: "uint16", + cv2.CV_16S: "int16", + cv2.CV_32S: "int32", + cv2.CV_32F: "float32", + cv2.CV_64F: "float64", + } + + for t in ["8U", "8S", "16U", "16S", "32S", "32F", "64F"]: + for c in [1, 2, 3, 4]: + nm = "%sC%d" % (t, c) + self.cvtype_to_name[getattr(cv2, "CV_%s" % nm)] = nm + + self.numpy_type_to_cvtype = { + "uint8": "8U", + "int8": "8S", + "uint16": "16U", + "int16": "16S", + "int32": "32S", + "float32": "32F", + "float64": "64F", + } + self.numpy_type_to_cvtype.update( + dict((v, k) for (k, v) in self.numpy_type_to_cvtype.items()) + ) + + def dtype_with_channels_to_cvtype2(self, dtype, n_channels): + return "%sC%d" % (self.numpy_type_to_cvtype[dtype.name], n_channels) + + def cvtype2_to_dtype_with_channels(self, cvtype): + return self.cvdepth_to_numpy_depth[CV_MAT_DEPTHWrap(cvtype)], CV_MAT_CNWrap(cvtype) + + def encoding_to_cvtype2(self, encoding): + try: + return getCvType(encoding) + except RuntimeError as e: + raise CvBridgeError(e) + + def encoding_to_dtype_with_channels(self, encoding): + return self.cvtype2_to_dtype_with_channels(self.encoding_to_cvtype2(encoding)) + + def compressed_imgmsg_to_cv2(self, cmprs_img_msg, desired_encoding="passthrough"): + """ + Convert a sensor_msgs::CompressedImage message to an OpenCV :cpp:type:`cv::Mat`. + + :param cmprs_img_msg: A :cpp:type:`sensor_msgs::CompressedImage` message + :param desired_encoding: The encoding of the image data, one of the following strings: + + * ``"passthrough"`` + * one of the standard strings in sensor_msgs/image_encodings.h + + :rtype: :cpp:type:`cv::Mat` + :raises CvBridgeError: when conversion is not possible. + + If desired_encoding is ``"passthrough"``, then the returned image has the same format as img_msg. + Otherwise desired_encoding must be one of the standard image encodings + + This function returns an OpenCV :cpp:type:`cv::Mat` message on success, or raises + :exc:`cv_bridge.CvBridgeError` on failure. + + If the image only has one channel, the shape has size 2 (width and height) + """ + import cv2 + import numpy as np + + str_msg = cmprs_img_msg.data + buf = np.ndarray(shape=(1, len(str_msg)), dtype=np.uint8, buffer=cmprs_img_msg.data) + im = cv2.imdecode(buf, cv2.IMREAD_ANYCOLOR) + + if desired_encoding == "passthrough": + return im + + try: + res = cvtColor2(im, "bgr8", desired_encoding) + except RuntimeError as e: + raise CvBridgeError(e) + + return res + + def imgmsg_to_cv2(self, img_msg, desired_encoding="passthrough"): + """ + Convert a sensor_msgs::Image message to an OpenCV :cpp:type:`cv::Mat`. + + :param img_msg: A :cpp:type:`sensor_msgs::Image` message + :param desired_encoding: The encoding of the image data, one of the following strings: + + * ``"passthrough"`` + * one of the standard strings in sensor_msgs/image_encodings.h + + :rtype: :cpp:type:`cv::Mat` + :raises CvBridgeError: when conversion is not possible. + + If desired_encoding is ``"passthrough"``, then the returned image has the same format as img_msg. + Otherwise desired_encoding must be one of the standard image encodings + + This function returns an OpenCV :cpp:type:`cv::Mat` message on success, or raises + :exc:`cv_bridge.CvBridgeError` on failure. + + If the image only has one channel, the shape has size 2 (width and height) + """ + import numpy as np + + dtype, n_channels = self.encoding_to_dtype_with_channels(img_msg.encoding) + dtype = np.dtype(dtype) + dtype = dtype.newbyteorder(">" if img_msg.is_bigendian else "<") + if n_channels == 1: + im = np.ndarray(shape=(img_msg.height, img_msg.width), dtype=dtype, buffer=img_msg.data) + else: + im = np.ndarray( + shape=(img_msg.height, img_msg.width, n_channels), dtype=dtype, buffer=img_msg.data + ) + # If the byt order is different between the message and the system. + if img_msg.is_bigendian == (sys.byteorder == "little"): + im = im.byteswap().newbyteorder() + + if desired_encoding == "passthrough": + return im + + try: + res = cvtColor2(im, img_msg.encoding, desired_encoding) + except RuntimeError as e: + raise CvBridgeError(e) + + return res + + def cv2_to_compressed_imgmsg(self, cvim, dst_format="jpg"): + """ + Convert an OpenCV :cpp:type:`cv::Mat` type to a ROS sensor_msgs::CompressedImage message. + + :param cvim: An OpenCV :cpp:type:`cv::Mat` + :param dst_format: The format of the image data, one of the following strings: + + * from http://docs.opencv.org/2.4/modules/highgui/doc/reading_and_writing_images_and_video.html + * from http://docs.opencv.org/2.4/modules/highgui/doc/reading_and_writing_images_and_video.html#Mat + imread(const string& filename, int flags) + * bmp, dib + * jpeg, jpg, jpe + * jp2 + * png + * pbm, pgm, ppm + * sr, ras + * tiff, tif + + :rtype: A sensor_msgs.msg.CompressedImage message + :raises CvBridgeError: when the ``cvim`` has a type that is incompatible with ``format`` + + + This function returns a sensor_msgs::Image message on success, or raises + :exc:`cv_bridge.CvBridgeError` on failure. + """ + import cv2 + import numpy as np + + if not isinstance(cvim, (np.ndarray, np.generic)): + raise TypeError("Your input type is not a numpy array") + cmprs_img_msg = sensor_msgs.msg.CompressedImage() + cmprs_img_msg.format = dst_format + ext_format = "." + dst_format + try: + cmprs_img_msg.data = np.array(cv2.imencode(ext_format, cvim)[1]).tostring() + except RuntimeError as e: + raise CvBridgeError(e) + + return cmprs_img_msg + + def cv2_to_imgmsg(self, cvim, encoding="passthrough"): + """ + Convert an OpenCV :cpp:type:`cv::Mat` type to a ROS sensor_msgs::Image message. + + :param cvim: An OpenCV :cpp:type:`cv::Mat` + :param encoding: The encoding of the image data, one of the following strings: + + * ``"passthrough"`` + * one of the standard strings in sensor_msgs/image_encodings.h + + :rtype: A sensor_msgs.msg.Image message + :raises CvBridgeError: when the ``cvim`` has a type that is incompatible with ``encoding`` + + If encoding is ``"passthrough"``, then the message has the same encoding as the image's OpenCV type. + Otherwise desired_encoding must be one of the standard image encodings + + This function returns a sensor_msgs::Image message on success, or raises + :exc:`cv_bridge.CvBridgeError`on failure. + """ + import numpy as np + + if not isinstance(cvim, (np.ndarray, np.generic)): + raise TypeError("Your input type is not a numpy array") + img_msg = sensor_msgs.msg.Image() + img_msg.height = cvim.shape[0] + img_msg.width = cvim.shape[1] + if len(cvim.shape) < 3: + cv_type = self.dtype_with_channels_to_cvtype2(cvim.dtype, 1) + else: + cv_type = self.dtype_with_channels_to_cvtype2(cvim.dtype, cvim.shape[2]) + if encoding == "passthrough": + img_msg.encoding = cv_type + else: + img_msg.encoding = encoding + # # Verify that the supplied encoding is compatible with the type of the OpenCV image + # if self.cvtype_to_name[self.encoding_to_cvtype2(encoding)] != cv_type: + # raise CvBridgeError( + # "encoding specified as %s, but image has incompatible type %s" + # % (encoding, cv_type) + # ) + if cvim.dtype.byteorder == ">": + img_msg.is_bigendian = True + img_msg.data = cvim.tostring() + img_msg.step = len(img_msg.data) // img_msg.height + + return img_msg diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/utils/episode_state.py b/GR00T-WholeBodyControl/decoupled_wbc/control/utils/episode_state.py new file mode 100644 index 0000000000000000000000000000000000000000..a808a9c5a1d750b1536f819515913e9632dbdb94 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/utils/episode_state.py @@ -0,0 +1,32 @@ +class EpisodeState: + """Episode state controller for data collection. + + Manages the state transitions for episode recording: + - IDLE: Not recording + - RECORDING: Currently recording data + - NEED_TO_SAVE: Recording stopped, waiting to save + """ + + def __init__(self): + self.RECORDING = "recording" + self.IDLE = "idle" + self.NEED_TO_SAVE = "need_to_save" + + self.state = self.IDLE + + def change_state(self): + """Cycle through states: IDLE -> RECORDING -> NEED_TO_SAVE -> IDLE.""" + if self.state == self.IDLE: + self.state = self.RECORDING + elif self.state == self.RECORDING: + self.state = self.NEED_TO_SAVE + elif self.state == self.NEED_TO_SAVE: + self.state = self.IDLE + + def reset_state(self): + """Reset to IDLE state.""" + self.state = self.IDLE + + def get_state(self): + """Get current state.""" + return self.state diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/utils/gear_wbc_utils.py b/GR00T-WholeBodyControl/decoupled_wbc/control/utils/gear_wbc_utils.py new file mode 100644 index 0000000000000000000000000000000000000000..bf036ceb006b77ee88369631d8f56edcc075fd19 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/utils/gear_wbc_utils.py @@ -0,0 +1,100 @@ +import os + +import numpy as np +import yaml + + +def load_config(config_path): + """Load and process the YAML configuration file""" + with open(config_path, "r") as f: + config = yaml.safe_load(f) + + # Set the path to the LEGGED_GYM_ROOT_DIR using relative path + current_file_dir = os.path.dirname(os.path.abspath(config_path)) + LEGGED_GYM_ROOT_DIR = os.path.join(current_file_dir, "..", "GearWbcRL", "legged_gym") + LEGGED_GYM_ROOT_DIR = os.path.abspath(LEGGED_GYM_ROOT_DIR) + + # Process paths with LEGGED_GYM_ROOT_DIR + for path_key in ["policy_path", "xml_path", "onnx_policy_path"]: + if path_key in config: + config[path_key] = config[path_key].format(LEGGED_GYM_ROOT_DIR=LEGGED_GYM_ROOT_DIR) + + # Convert lists to numpy arrays where needed + array_keys = ["kps", "kds", "default_angles", "cmd_scale", "cmd_init"] + for key in array_keys: + if key in config: + config[key] = np.array(config[key], dtype=np.float32) + + return config, LEGGED_GYM_ROOT_DIR + + +def pd_control(target_q, q, kp, target_dq, dq, kd): + """Calculates torques from position commands""" + return (target_q - q) * kp + (target_dq - dq) * kd + + +def quat_rotate_inverse(q, v): + """Rotate vector v by the inverse of quaternion q""" + w = q[..., 0] + x = q[..., 1] + y = q[..., 2] + z = q[..., 3] + + q_conj = np.array([w, -x, -y, -z]) + + return np.array( + [ + v[0] * (q_conj[0] ** 2 + q_conj[1] ** 2 - q_conj[2] ** 2 - q_conj[3] ** 2) + + v[1] * 2 * (q_conj[1] * q_conj[2] - q_conj[0] * q_conj[3]) + + v[2] * 2 * (q_conj[1] * q_conj[3] + q_conj[0] * q_conj[2]), + v[0] * 2 * (q_conj[1] * q_conj[2] + q_conj[0] * q_conj[3]) + + v[1] * (q_conj[0] ** 2 - q_conj[1] ** 2 + q_conj[2] ** 2 - q_conj[3] ** 2) + + v[2] * 2 * (q_conj[2] * q_conj[3] - q_conj[0] * q_conj[1]), + v[0] * 2 * (q_conj[1] * q_conj[3] - q_conj[0] * q_conj[2]) + + v[1] * 2 * (q_conj[2] * q_conj[3] + q_conj[0] * q_conj[1]) + + v[2] * (q_conj[0] ** 2 - q_conj[1] ** 2 - q_conj[2] ** 2 + q_conj[3] ** 2), + ] + ) + + +def get_gravity_orientation(quat): + """Get gravity vector in body frame""" + gravity_vec = np.array([0.0, 0.0, -1.0]) + return quat_rotate_inverse(quat, gravity_vec) + + +def compute_observation(d, config, action, cmd, height_cmd, n_joints): + """Compute the observation vector from current state""" + # Get state from MuJoCo + qj = d.qpos[7 : 7 + n_joints].copy() + dqj = d.qvel[6 : 6 + n_joints].copy() + quat = d.qpos[3:7].copy() + omega = d.qvel[3:6].copy() + + # Handle default angles padding + if len(config["default_angles"]) < n_joints: + padded_defaults = np.zeros(n_joints, dtype=np.float32) + padded_defaults[: len(config["default_angles"])] = config["default_angles"] + else: + padded_defaults = config["default_angles"][:n_joints] + + # Scale the values + qj_scaled = (qj - padded_defaults) * config["dof_pos_scale"] + dqj_scaled = dqj * config["dof_vel_scale"] + gravity_orientation = get_gravity_orientation(quat) + omega_scaled = omega * config["ang_vel_scale"] + + # Calculate single observation dimension + single_obs_dim = 3 + 1 + 3 + 3 + n_joints + n_joints + 12 + + # Create single observation + single_obs = np.zeros(single_obs_dim, dtype=np.float32) + single_obs[0:3] = cmd[:3] * config["cmd_scale"] + single_obs[3:4] = np.array([height_cmd]) + single_obs[4:7] = omega_scaled + single_obs[7:10] = gravity_orientation + single_obs[10 : 10 + n_joints] = qj_scaled + single_obs[10 + n_joints : 10 + 2 * n_joints] = dqj_scaled + single_obs[10 + 2 * n_joints : 10 + 2 * n_joints + 12] = action + + return single_obs, single_obs_dim diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/utils/keyboard_dispatcher.py b/GR00T-WholeBodyControl/decoupled_wbc/control/utils/keyboard_dispatcher.py new file mode 100644 index 0000000000000000000000000000000000000000..e6b91627917cb5fa04137a30553d41b02a259ac3 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/utils/keyboard_dispatcher.py @@ -0,0 +1,255 @@ +import os +import subprocess +import sys +import threading + +import rclpy +from sshkeyboard import listen_keyboard, stop_listening +from std_msgs.msg import String as RosStringMsg + +from decoupled_wbc.control.main.constants import KEYBOARD_INPUT_TOPIC + +# Global variable to store original terminal attributes +_original_terminal_attrs = None + + +def save_terminal_state(): + """Save the current terminal state.""" + global _original_terminal_attrs + try: + import termios + + fd = sys.stdin.fileno() + _original_terminal_attrs = termios.tcgetattr(fd) + except (ImportError, OSError, termios.error): + _original_terminal_attrs = None + + +def restore_terminal(): + """Restore terminal to original state.""" + global _original_terminal_attrs + try: + import termios + + if _original_terminal_attrs is not None: + fd = sys.stdin.fileno() + termios.tcsetattr(fd, termios.TCSANOW, _original_terminal_attrs) + return + except (ImportError, OSError, termios.error): + pass + + # Fallback for non-Unix systems or if termios fails + try: + if os.name == "posix": + os.system("stty sane") + except OSError: + pass + + +class ROSKeyboardDispatcher: + """ROS-based keyboard dispatcher that receives keyboard events via ROS topics.""" + + def __init__(self): + self.listeners = [] + self._active = False + assert rclpy.ok(), "Expected ROS2 to be initialized in this process..." + executor = rclpy.get_global_executor() + self.node = executor.get_nodes()[0] + print("creating keyboard input subscriber...") + self.subscription = self.node.create_subscription( + RosStringMsg, KEYBOARD_INPUT_TOPIC, self._callback, 10 + ) + + def register(self, listener): + if not hasattr(listener, "handle_keyboard_button"): + raise NotImplementedError("handle_keyboard_button is not implemented") + self.listeners.append(listener) + + def start(self): + """Start the ROS keyboard dispatcher.""" + self._active = True + print("ROS keyboard dispatcher started") + + def stop(self): + """Stop the ROS keyboard dispatcher and cleanup.""" + if self._active: + self._active = False + # Clean up subscription + if hasattr(self, "subscription"): + self.node.destroy_subscription(self.subscription) + print("ROS keyboard dispatcher stopped") + + def _callback(self, msg: RosStringMsg): + if self._active: + for listener in self.listeners: + listener.handle_keyboard_button(msg.data) + + def __del__(self): + """Cleanup when object is destroyed.""" + self.stop() + + +class KeyboardDispatcher: + def __init__(self): + self.listeners = [] + self._listening_thread = None + self._stop_event = threading.Event() + self._key = None + + def register(self, listener): + # raise if handle_keyboard_button is not implemented + # TODO(YL): let listener be a Callable instead of a class + if not hasattr(listener, "handle_keyboard_button"): + raise NotImplementedError("handle_keyboard_button is not implemented") + self.listeners.append(listener) + + def handle_key(self, key): + # Check if we should stop + if self._stop_event.is_set(): + stop_listening() + return + + for listener in self.listeners: + listener.handle_keyboard_button(key) + + def start_listening(self): + try: + save_terminal_state() # Save original terminal state before listening + listen_keyboard( + on_press=self.handle_key, + delay_second_char=0.1, + delay_other_chars=0.05, + sleep=0.01, + ) + except Exception as e: + print(f"Keyboard listener stopped: {e}") + finally: + # Ensure terminal is restored even if an exception occurs + self._restore_terminal() + + def start(self): + self._listening_thread = threading.Thread(target=self.start_listening, daemon=True) + self._listening_thread.start() + + def stop(self): + """Stop the keyboard listener and restore terminal settings.""" + if self._listening_thread and self._listening_thread.is_alive(): + self._stop_event.set() + # Force stop_listening to be called + try: + stop_listening() + except Exception: + pass + # Wait a bit for the thread to finish + self._listening_thread.join(timeout=0.5) + # Restore terminal settings + self._restore_terminal() + + def _restore_terminal(self): + """Restore terminal to a sane state.""" + restore_terminal() + + def __del__(self): + """Cleanup when object is destroyed.""" + self.stop() + + +KEYBOARD_LISTENER_TOPIC_NAME = "/Gr00tKeyboardListener" + + +class KeyboardListener: + def __init__(self): + self.key = None + + def handle_keyboard_button(self, key): + self.key = key + + def pop_key(self): + key = self.key + self.key = None + return key + + +class KeyboardListenerPublisher: + def __init__(self, topic_name: str = KEYBOARD_LISTENER_TOPIC_NAME): + """ + Initialize keyboard listener for remote teleop with simplified interface. + + Args: + remote_system: RemoteSystem instance + control_channel_name: Name of the control channel + """ + assert rclpy.ok(), "Expected ROS2 to be initialized in this process..." + executor = rclpy.get_global_executor() + self.node = executor.get_nodes()[0] + self.publisher = self.node.create_publisher(RosStringMsg, topic_name, 1) + + def handle_keyboard_button(self, key): + self.publisher.publish(RosStringMsg(data=key)) + + +class KeyboardListenerSubscriber: + def __init__( + self, + topic_name: str = KEYBOARD_LISTENER_TOPIC_NAME, + node_name: str = "keyboard_listener_subscriber", + ): + assert rclpy.ok(), "Expected ROS2 to be initialized in this process..." + executor = rclpy.get_global_executor() + nodes = executor.get_nodes() + if nodes: + self.node = nodes[0] + self._create_node = False + else: + self.node = rclpy.create_node("KeyboardListenerSubscriber") + executor.add_node(self.node) + self._create_node = True + self.subscriber = self.node.create_subscription(RosStringMsg, topic_name, self._callback, 1) + self._data = None + + def _callback(self, msg: RosStringMsg): + self._data = msg.data + + def read_msg(self): + data = self._data + self._data = None + return data + + +class KeyboardEStop: + def __init__(self): + """Initialize KeyboardEStop with automatic tmux cleanup detection.""" + # Automatically create tmux cleanup if in deployment mode + self.cleanup_callback = self._create_tmux_cleanup_callback() + + def _create_tmux_cleanup_callback(self): + """Create a cleanup callback that kills the tmux session if running in deployment mode.""" + tmux_session = os.environ.get("DECOUPLED_WBC_TMUX_SESSION") + + def cleanup_callback(): + if tmux_session: + print(f"Emergency stop: Killing tmux session '{tmux_session}'...") + try: + subprocess.run(["tmux", "kill-session", "-t", tmux_session], timeout=5) + print("Tmux session terminated successfully.") + except subprocess.TimeoutExpired: + print("Warning: Tmux session termination timed out, forcing kill...") + try: + subprocess.run(["tmux", "kill-session", "-t", tmux_session, "-9"]) + except Exception: + pass + except Exception as e: + print(f"Warning: Error during tmux cleanup: {e}") + # If tmux cleanup fails, fallback to immediate exit + restore_terminal() + os._exit(1) + else: + print("Emergency stop: No tmux session, exiting normally...") + sys.exit(1) + + return cleanup_callback + + def handle_keyboard_button(self, key): + if key == "`": + print("Emergency stop triggered - running cleanup...") + self.cleanup_callback() diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/utils/network_utils.py b/GR00T-WholeBodyControl/decoupled_wbc/control/utils/network_utils.py new file mode 100644 index 0000000000000000000000000000000000000000..105a5cd2e7958e3732cf3ed135576b5d90b81e30 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/utils/network_utils.py @@ -0,0 +1,137 @@ +#!/usr/bin/env python3 +""" +Simple network interface utilities +""" + +import platform +import re +import subprocess + + +def get_network_interfaces(): + """Get network interfaces with their IP addresses""" + try: + result = subprocess.run( + ["/sbin/ip", "addr", "show"], capture_output=True, text=True, check=True + ) + return _parse_ip_output(result.stdout) + except (subprocess.CalledProcessError, FileNotFoundError): + try: + result = subprocess.run(["ifconfig"], capture_output=True, text=True, check=True) + return _parse_ifconfig_output(result.stdout) + except (subprocess.CalledProcessError, FileNotFoundError): + return {} + + +def _parse_ip_output(output): + """Parse 'ip addr' command output""" + interfaces = {} + current_interface = None + + for line in output.split("\n"): + interface_match = re.match(r"^\d+:\s+(\w+):", line) + if interface_match: + current_interface = interface_match.group(1) + interfaces[current_interface] = [] + + ip_match = re.search(r"inet\s+(\d+\.\d+\.\d+\.\d+)", line) + if ip_match and current_interface: + interfaces[current_interface].append(ip_match.group(1)) + + return interfaces + + +def _parse_ifconfig_output(output): + """Parse 'ifconfig' command output""" + interfaces = {} + current_interface = None + + for line in output.split("\n"): + interface_match = re.match(r"^(\w+):", line) + if interface_match: + current_interface = interface_match.group(1) + interfaces[current_interface] = [] + + ip_match = re.search(r"inet\s+(\d+\.\d+\.\d+\.\d+)", line) + if ip_match and current_interface: + interfaces[current_interface].append(ip_match.group(1)) + + return interfaces + + +def find_interface_by_ip(target_ip): + """Find interface name for given IP address""" + interfaces = get_network_interfaces() + for interface, ip_list in interfaces.items(): + if target_ip in ip_list: + return interface + return None + + +def resolve_interface(interface: str) -> tuple[str, str]: + """ + Resolve interface parameter to actual network interface name and environment type + + Args: + interface: "sim", "real", or direct interface name or IP address + + Returns: + tuple: (interface_name, env_type) where env_type is "sim" or "real" + """ + # Check if interface is an IP address + if re.match(r"^\d+\.\d+\.\d+\.\d+$", interface): + if interface == "127.0.0.1": + return interface, "sim" + else: + return interface, "real" + + if interface == "sim": + lo_interface = find_interface_by_ip("127.0.0.1") + if lo_interface: + # macOS uses lo0 instead of lo + if platform.system() == "Darwin" and lo_interface == "lo": + return "lo0", "sim" + return lo_interface, "sim" + return ("lo0" if platform.system() == "Darwin" else "lo"), "sim" + + elif interface == "real": + interfaces = get_network_interfaces() + for iface, ip_list in interfaces.items(): + for ip in ip_list: + if ip.startswith("192.168.123."): + return iface, "real" + return interface, "real" # fallback + + else: + # Direct interface name - check if it has 127.0.0.1 to determine env_type + interfaces = get_network_interfaces() + if interface in interfaces: + for ip in interfaces[interface]: + if ip == "127.0.0.1": + return interface, "sim" + + # macOS lo interface handling + if platform.system() == "Darwin" and interface == "lo": + return "lo0", "sim" + + # Default to real for unknown interfaces + return interface, "real" + + +if __name__ == "__main__": + interfaces = get_network_interfaces() + + if not interfaces: + print("No network interfaces found") + exit(1) + + # Show all interfaces + print("Network interfaces:") + for interface, ip_list in interfaces.items(): + print(f" {interface}: {', '.join(ip_list)}") + + # Test resolve_interface function + print("\nTesting resolve_interface:") + for test_interface in ["sim", "real", "lo", "127.0.0.1"]: + interface_name, env_type = resolve_interface(test_interface) + print(f" {test_interface} -> {interface_name} ({env_type})") diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/utils/ros_utils.py b/GR00T-WholeBodyControl/decoupled_wbc/control/utils/ros_utils.py new file mode 100644 index 0000000000000000000000000000000000000000..f24890fb72dcd00401602eccb772e861f30c72d0 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/utils/ros_utils.py @@ -0,0 +1,201 @@ +import base64 +import signal +import threading +from typing import Optional + +import msgpack +import msgpack_numpy as mnp +import rclpy +from rclpy.executors import SingleThreadedExecutor +from rclpy.node import Node +from sensor_msgs.msg import Image +from std_msgs.msg import ByteMultiArray +from std_srvs.srv import Trigger + +_signal_registered = False + + +def register_keyboard_interrupt_handler(): + """ + Register a signal handler for SIGINT (Ctrl+C) and SIGTERM that raises KeyboardInterrupt. + This ensures consistent exception handling across different termination signals. + """ + global _signal_registered + if not _signal_registered: + + def signal_handler(signum, frame): + raise KeyboardInterrupt + + signal.signal(signal.SIGINT, signal_handler) + signal.signal(signal.SIGTERM, signal_handler) + _signal_registered = True + + +class ROSManager: + """ + Manages the ROS2 node and executor. + + Usage example: + ```python + def main(): + ros_manager = ROSManager() + node = ros_manager.node + + try: + while ros_manager.ok(): + time.sleep(0.1) + except ros_manager.exceptions() as e: + print(f"ROSManager interrupted by user: {e}") + finally: + ros_manager.shutdown() + ``` + """ + + def __init__(self, node_name: str = "ros_manager"): + if not rclpy.ok(): + rclpy.init() + self.node = rclpy.create_node(node_name) + self.thread = threading.Thread(target=rclpy.spin, args=(self.node,), daemon=True) + self.thread.start() + else: + executor = rclpy.get_global_executor() + if len(executor.get_nodes()) > 0: + self.node = executor.get_nodes()[0] + else: + self.node = rclpy.create_node(node_name) + + register_keyboard_interrupt_handler() + + @staticmethod + def ok(): + return rclpy.ok() + + @staticmethod + def shutdown(): + if rclpy.ok(): + rclpy.shutdown() + + @staticmethod + def exceptions(): + return (rclpy.exceptions.ROSInterruptException, KeyboardInterrupt) + + +class ROSMsgPublisher: + """ + Publishes any serializable dict to a topic. + """ + + def __init__(self, topic_name: str): + ros_manager = ROSManager() + self.node = ros_manager.node + self.publisher = self.node.create_publisher(ByteMultiArray, topic_name, 1) + + def publish(self, msg: dict): + payload = msgpack.packb(msg, default=mnp.encode) + payload = tuple(bytes([a]) for a in payload) + msg = ByteMultiArray() + msg.data = payload + self.publisher.publish(msg) + + +class ROSMsgSubscriber: + """ + Subscribes to any topics published by a ROSMsgPublisher. + """ + + def __init__(self, topic_name: str): + ros_manager = ROSManager() + self.node = ros_manager.node + self._msg = None + self.subscription = self.node.create_subscription( + ByteMultiArray, topic_name, self._callback, 1 + ) + + def _callback(self, msg: ByteMultiArray): + self._msg = msg + + def get_msg(self) -> Optional[dict]: + msg = self._msg + if msg is None: + return None + self._msg = None + return msgpack.unpackb(bytes([ab for a in msg.data for ab in a]), object_hook=mnp.decode) + + +class ROSImgMsgSubscriber: + """ + Subscribes to an `Image` topic and returns the image as a numpy array and timestamp. + """ + + def __init__(self, topic_name: str): + ros_manager = ROSManager() + self.node = ros_manager.node + self._msg = None + self.subscription = self.node.create_subscription(Image, topic_name, self._callback, 1) + + from decoupled_wbc.control.utils.cv_bridge import CvBridge + + self.bridge = CvBridge() + + def _callback(self, msg: Image): + self._msg = msg + + def get_image(self) -> Optional[dict]: + """ + Returns the image as a numpy array and the timestamp. + """ + + msg = self._msg + if msg is None: + return None + return { + "image": self.bridge.imgmsg_to_cv2(msg), + "timestamp": msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9, + } + + +class ROSServiceServer: + """ + Generic ROS2 Service server that stores and serves a config dict. + """ + + def __init__(self, service_name: str, config: dict): + ros_manager = ROSManager() + self.node = ros_manager.node + packed = msgpack.packb(config, default=mnp.encode) + self.message = base64.b64encode(packed).decode("ascii") + self.server = self.node.create_service(Trigger, service_name, self._callback) + + def _callback(self, request, response): + try: + response.success = True + response.message = self.message + print("Sending encoded message of length:", len(response.message)) + except Exception as e: + response.success = False + response.message = str(e) + return response + + +class ROSServiceClient(Node): + + def __init__(self, service_name: str, node_name: str = "service_client"): + super().__init__(node_name) + self.cli = self.create_client(Trigger, service_name) + while not self.cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info("service not available, waiting again...") + self.req = Trigger.Request() + + def get_config(self): + future = self.cli.call_async(self.req) + executor = SingleThreadedExecutor() + executor.add_node(self) + executor.spin_until_future_complete(future, timeout_sec=1.0) + executor.remove_node(self) + executor.shutdown() + result = future.result() + if result.success: + decoded = base64.b64decode(result.message.encode("ascii")) + return msgpack.unpackb(decoded, object_hook=mnp.decode) + else: + raise RuntimeError(f"Service call failed: {result.message}") diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/utils/run_real_checklist.py b/GR00T-WholeBodyControl/decoupled_wbc/control/utils/run_real_checklist.py new file mode 100644 index 0000000000000000000000000000000000000000..9ececbc6005f7f72deee7ef201c46113bc46c6de --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/utils/run_real_checklist.py @@ -0,0 +1,121 @@ +#!/usr/bin/env python3 + +import sys + + +def check_real_deployment(extra_args): + """Check if this is a real robot deployment.""" + is_real_deployment = False + + # Check if interface argument is provided and not 'lo' or 'lo0' + for i, arg in enumerate(extra_args): + if arg == "--interface": + # Get the next argument (interface value) + if i + 1 < len(extra_args): + interface_value = extra_args[i + 1] + if interface_value not in ["lo", "lo0"]: + is_real_deployment = True + print(f"Real deployment detected: interface = {interface_value}") + break + else: + print(f"Simulation deployment detected: interface = {interface_value}") + + # If no interface specified, assume simulation (default is 'lo' in deploy_g1.py) + if not is_real_deployment: + print("No interface specified - assuming simulation (default interface = lo)") + + return is_real_deployment + + +def show_deployment_checklist(): + """Show deployment checklist and get confirmation.""" + checklist_content = """═══════════════════════════════════════════════════════════════════════════════ + G1 ROBOT DEPLOYMENT CHECKLIST +═══════════════════════════════════════════════════════════════════════════════ + +⚠️ SAFETY VERIFICATION - Complete ALL checks before deployment + +PRE-DEPLOYMENT CHECKLIST: + +□ Sim2Sim Verification + Test in simulation first with interface set to 'sim' before real deployment + +□ Camera System Check + Test real camera with simulation environment before full deployment + +□ State Reading Validation + • Disable action queue + • Verify sensor readings (IMU, joints, fingers) + • Use rerun for visualization + • Contact: Dennis Da (xda@nvidia.com) for assistance + +□ Low Gain Test + • Start with low kp values (2-5x lower than normal) + • Keep kd values unchanged + +□ Clear Workspace + • Remove obstacles and avoid tables + • Ensure adequate clearance in all directions + +□ Emergency Stop Ready + Ensure access to at least one: + • Keyboard e-stop + • Joycon controller + • External power cutoff + +═══════════════════════════════════════════════════════════════════════════════ +🚨 EMERGENCY: Press ` at any time to stop all processes +📹 RECORDING: Connect a webcam to your computer to record the experiment +═══════════════════════════════════════════════════════════════════════════════ + +Usages: + +- hit ` to stop all processes +- hit Ctrl+C to stop single process +- hit Ctrl+\ to quit the tmux +""" + + print("") + print("🚨 REAL ROBOT DEPLOYMENT DETECTED 🚨") + print("") + print(checklist_content) + print("") + + # Get user confirmation + while True: + user_input = input("Continue with deployment? [Y/n]: ").strip() + + # Default to Y if empty input + if not user_input: + user_input = "Y" + + user_input_upper = user_input.upper() + + if user_input_upper in ["Y", "YES"]: + print("") + print("✅ Deployment confirmed. Proceeding with robot deployment...") + print("") + return True + elif user_input_upper in ["N", "NO"]: + print("") + print("❌ Deployment aborted by user.") + print("") + return False + else: + print( + "❌ Invalid input. Please enter 'Y' for yes, 'N' for no, or press Enter for default (Y)." + ) + + +def main(): + """Main function.""" + # Always show the checklist + if not show_deployment_checklist(): + print("Deployment cancelled.") + sys.exit(1) + + return 0 + + +if __name__ == "__main__": + main() diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/utils/service.py b/GR00T-WholeBodyControl/decoupled_wbc/control/utils/service.py new file mode 100644 index 0000000000000000000000000000000000000000..1d510ab600ae2afbf0728d4589e38205f2122219 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/utils/service.py @@ -0,0 +1,182 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dataclasses import dataclass +from io import BytesIO +from typing import Any, Callable, Dict + +import torch +import zmq + + +class TorchSerializer: + @staticmethod + def to_bytes(data: dict) -> bytes: + buffer = BytesIO() + torch.save(data, buffer) + return buffer.getvalue() + + @staticmethod + def from_bytes(data: bytes) -> dict: + buffer = BytesIO(data) + obj = torch.load(buffer, weights_only=False) + return obj + + +@dataclass +class EndpointHandler: + handler: Callable + requires_input: bool = True + + +class BaseInferenceServer: + """ + An inference server that spin up a ZeroMQ socket and listen for incoming requests. + Can add custom endpoints by calling `register_endpoint`. + """ + + def __init__(self, host: str = "*", port: int = 5555): + self.running = True + self.context = zmq.Context() + self.socket = self.context.socket(zmq.REP) + self.socket.bind(f"tcp://{host}:{port}") + self._endpoints: dict[str, EndpointHandler] = {} + + # Register the ping endpoint by default + self.register_endpoint("ping", self._handle_ping, requires_input=False) + self.register_endpoint("kill", self._kill_server, requires_input=False) + + def _kill_server(self): + """ + Kill the server. + """ + self.running = False + + def _handle_ping(self) -> dict: + """ + Simple ping handler that returns a success message. + """ + return {"status": "ok", "message": "Server is running"} + + def register_endpoint(self, name: str, handler: Callable, requires_input: bool = True): + """ + Register a new endpoint to the server. + + Args: + name: The name of the endpoint. + handler: The handler function that will be called when the endpoint is hit. + requires_input: Whether the handler requires input data. + """ + self._endpoints[name] = EndpointHandler(handler, requires_input) + + def run(self): + addr = self.socket.getsockopt_string(zmq.LAST_ENDPOINT) + print(f"Server is ready and listening on {addr}") + while self.running: + try: + message = self.socket.recv() + request = TorchSerializer.from_bytes(message) + endpoint = request.get("endpoint", "get_action") + + if endpoint not in self._endpoints: + raise ValueError(f"Unknown endpoint: {endpoint}") + + handler = self._endpoints[endpoint] + result = ( + handler.handler(request.get("data", {})) + if handler.requires_input + else handler.handler() + ) + self.socket.send(TorchSerializer.to_bytes(result)) + except Exception as e: + print(f"Error in server: {e}") + import traceback + + print(traceback.format_exc()) + self.socket.send(b"ERROR") + + +class BaseInferenceClient: + def __init__(self, host: str = "localhost", port: int = 5555, timeout_ms: int = 15000): + self.context = zmq.Context() + self.host = host + self.port = port + self.timeout_ms = timeout_ms + self._init_socket() + + def _init_socket(self): + """Initialize or reinitialize the socket with current settings""" + self.socket = self.context.socket(zmq.REQ) + self.socket.connect(f"tcp://{self.host}:{self.port}") + + def ping(self) -> bool: + try: + self.call_endpoint("ping", requires_input=False) + return True + except zmq.error.ZMQError: + self._init_socket() # Recreate socket for next attempt + return False + + def kill_server(self): + """ + Kill the server. + """ + self.call_endpoint("kill", requires_input=False) + + def call_endpoint( + self, endpoint: str, data: dict | None = None, requires_input: bool = True + ) -> dict: + """ + Call an endpoint on the server. + + Args: + endpoint: The name of the endpoint. + data: The input data for the endpoint. + requires_input: Whether the endpoint requires input data. + """ + request: dict = {"endpoint": endpoint} + if requires_input: + request["data"] = data + + self.socket.send(TorchSerializer.to_bytes(request)) + message = self.socket.recv() + if message == b"ERROR": + raise RuntimeError("Server error") + return TorchSerializer.from_bytes(message) + + def __del__(self): + """Cleanup resources on destruction""" + self.socket.close() + self.context.term() + + +class ExternalRobotInferenceClient(BaseInferenceClient): + """ + Client for communicating with the RealRobotServer + """ + + def set_observation(self, observation: dict[str, Any]): + self.call_endpoint("set_observation", data=observation) + + def get_action(self, time: float | None = None) -> Dict[str, Any]: + """ + Get the action from the server. + The exact definition of the observations is defined + by the policy, which contains the modalities configuration. + """ + return self.call_endpoint("get_action", data={"time": time}) + + def get_modality_config(self) -> dict[str, Any]: + return self.call_endpoint("get_modality_config") diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/utils/term_color_constants.py b/GR00T-WholeBodyControl/decoupled_wbc/control/utils/term_color_constants.py new file mode 100644 index 0000000000000000000000000000000000000000..4e13ac1f3a27380881eb98fa4b857acd348c0fe6 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/utils/term_color_constants.py @@ -0,0 +1,19 @@ +GREEN_BOLD = "\033[1;32m" +RED_BOLD = "\033[1;31m" +YELLOW_BOLD = "\033[1;33m" +BLUE_BOLD = "\033[1;34m" +MAGENTA_BOLD = "\033[1;35m" +CYAN_BOLD = "\033[1;36m" +WHITE_BOLD = "\033[1;37m" +GREY_BOLD = "\033[1;90m" + +GREEN = "\033[32m" +RED = "\033[31m" +YELLOW = "\033[33m" +BLUE = "\033[34m" +MAGENTA = "\033[35m" +CYAN = "\033[36m" +WHITE = "\033[37m" +GREY = "\033[90m" + +RESET = "\033[0m" diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/utils/text_to_speech.py b/GR00T-WholeBodyControl/decoupled_wbc/control/utils/text_to_speech.py new file mode 100644 index 0000000000000000000000000000000000000000..4e076c814eaa79452ef123e59ed60a4b77e6d367 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/utils/text_to_speech.py @@ -0,0 +1,28 @@ +# text to speech +import pyttsx3 + + +class TextToSpeech: + def __init__(self, rate: int = 150, volume: float = 1.0): + try: + self.engine = pyttsx3.init(driverName="espeak") + self.engine.setProperty("rate", rate) + self.engine.setProperty("volume", volume) + except Exception as e: + print(f"[Text To Speech] Initialization failed: {e}") + self.engine = None + + def say(self, message: str): + """Speak the message if engine is available.""" + if self.engine: + try: + self.engine.say(message) + self.engine.runAndWait() + except Exception as e: + print(f"[Text To Speech] Failed to say message: {e}") + + def print_and_say(self, message: str, say: bool = True): + """Print message and optionally speak it using Text To Speech.""" + print(message) + if say and self.engine is not None: + self.say(message) diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/visualization/humanoid_visualizer.py b/GR00T-WholeBodyControl/decoupled_wbc/control/visualization/humanoid_visualizer.py new file mode 100644 index 0000000000000000000000000000000000000000..815af250c5a944236301683102c438fa73bcc826 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/visualization/humanoid_visualizer.py @@ -0,0 +1,52 @@ +import time + +import meshcat_shapes +import numpy as np +from pinocchio.visualize import MeshcatVisualizer + +from decoupled_wbc.control.robot_model import RobotModel +from decoupled_wbc.control.robot_model.instantiation.g1 import instantiate_g1_robot_model + + +class RobotVisualizer: + def __init__(self, robot: RobotModel): + self.robot = robot + self.viz = MeshcatVisualizer( + self.robot.pinocchio_wrapper.model, + self.robot.pinocchio_wrapper.collision_model, + self.robot.pinocchio_wrapper.visual_model, + ) + try: + self.viz.initViewer(open=True) + + except ImportError as err: + print("Error while initializing the viewer. It seems you should install Python meshcat") + print(err) + exit(0) + + self.viz.loadViewerModel() + self.viz.display(self.robot.q_zero) + + # Visualize frames + self.viz_frames = [self.robot.supplemental_info.root_frame_name] + for side in ["left", "right"]: + self.viz_frames.append(self.robot.supplemental_info.hand_frame_names[side]) + for frame in self.viz_frames: + meshcat_shapes.frame(self.viz.viewer[frame], opacity=1.0) + + def visualize(self, robot_state: np.ndarray): + # visualize robot state + if robot_state is not None: + self.robot.cache_forward_kinematics(robot_state, auto_clip=False) + self.viz.display(robot_state) + for frame_name in self.viz_frames: + self.viz.viewer[frame_name].set_transform(self.robot.frame_placement(frame_name).np) + + +if __name__ == "__main__": + # robot_model = instantiate_gr1_robot_model() + robot_model = instantiate_g1_robot_model() + visualizer = RobotVisualizer(robot_model) + while True: + visualizer.visualize(robot_model.q_zero) + time.sleep(0.01) diff --git a/GR00T-WholeBodyControl/decoupled_wbc/control/visualization/meshcat_visualizer_env.py b/GR00T-WholeBodyControl/decoupled_wbc/control/visualization/meshcat_visualizer_env.py new file mode 100644 index 0000000000000000000000000000000000000000..c3981d7f83f41bc5ee22f4a0d3c709b74bda6374 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/control/visualization/meshcat_visualizer_env.py @@ -0,0 +1,77 @@ +from contextlib import contextmanager +import time + +import gymnasium as gym +import numpy as np +from pinocchio.visualize import MeshcatVisualizer + +from decoupled_wbc.control.base.env import Env +from decoupled_wbc.control.robot_model import RobotModel + + +class MeshcatVisualizerEnv(Env): + def __init__(self, robot_model: RobotModel): + self.robot_model = robot_model + self.viz = MeshcatVisualizer( + self.robot_model.pinocchio_wrapper.model, + self.robot_model.pinocchio_wrapper.collision_model, + self.robot_model.pinocchio_wrapper.visual_model, + ) + try: + self.viz.initViewer(open=True) + + except ImportError as err: + print("Error while initializing the viewer. It seems you should install Python meshcat") + print(err) + exit(0) + + self.viz.loadViewerModel() + self.visualize(self.robot_model.pinocchio_wrapper.q0) + time.sleep(1.0) + + self._observation_space = gym.spaces.Dict( + { + "q": gym.spaces.Box( + low=-2 * np.pi, high=2 * np.pi, shape=(self.robot_model.num_dofs,) + ) + } + ) + self._action_space = gym.spaces.Dict( + { + "q": gym.spaces.Box( + low=-2 * np.pi, high=2 * np.pi, shape=(self.robot_model.num_dofs,) + ) + } + ) + + def visualize(self, robot_state: np.ndarray): + # visualize robot state + if robot_state is not None: + self.viz.display(robot_state) + + def observe(self): + # Dummy observation + return {"q": self.robot_model.pinocchio_wrapper.q0} + + def queue_action(self, action: dict[str, np.ndarray]): + self.visualize(action["q"]) + + def reset(self, **kwargs): + self.visualize(self.robot_model.pinocchio_wrapper.q0) + return {"q": self.robot_model.pinocchio_wrapper.q0} + + def sensors(self) -> dict[str, any]: + return {} + + def observation_space(self) -> gym.Space: + return self._observation_space + + def action_space(self) -> gym.Space: + return self._action_space + + def close(self): + return + + @contextmanager + def activate(self): + yield diff --git a/GR00T-WholeBodyControl/decoupled_wbc/data/constants.py b/GR00T-WholeBodyControl/decoupled_wbc/data/constants.py new file mode 100644 index 0000000000000000000000000000000000000000..024865f60b8efee1ab529ff58322addb59508787 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/data/constants.py @@ -0,0 +1,5 @@ +# This will be used for both sim and real data collection +RS_VIEW_CAMERA_HEIGHT = 480 +RS_VIEW_CAMERA_WIDTH = 640 + +BUCKET_BASE_PATH = "GearRawDataLeRobotV0" diff --git a/GR00T-WholeBodyControl/decoupled_wbc/data/exporter.py b/GR00T-WholeBodyControl/decoupled_wbc/data/exporter.py new file mode 100644 index 0000000000000000000000000000000000000000..4bb4d2eda5d894d079f4b50b351f0c06cd935d90 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/data/exporter.py @@ -0,0 +1,514 @@ +import copy +from dataclasses import dataclass +from functools import partial +import json +import os +from pathlib import Path +import shutil +from typing import Optional + +import datasets +from datasets import load_dataset +from datasets.utils import disable_progress_bars +from huggingface_hub.errors import RepositoryNotFoundError +from lerobot.common.datasets.lerobot_dataset import ( + LeRobotDataset, + LeRobotDatasetMetadata, + compute_episode_stats, +) +from lerobot.common.datasets.utils import ( + check_timestamps_sync, + get_episode_data_index, + validate_episode_buffer, + validate_frame, +) +import numpy as np +from PIL import Image as PILImage +import torch +from torchvision import transforms + +from decoupled_wbc.control.main.config_template import ArgsConfig +from decoupled_wbc.data.video_writer import VideoWriter + +disable_progress_bars() # Disable HuggingFace progress bars + + +@dataclass +class DataCollectionInfo: + """ + This dataclass stores additional information that is relevant to the data collection process. + """ + + lower_body_policy: Optional[str] = None + wbc_model_path: Optional[str] = None + teleoperator_username: Optional[str] = None + support_operator_username: Optional[str] = None + robot_type: Optional[str] = None + robot_id: Optional[str] = None + + def to_dict(self) -> dict: + """Convert the dataclass to a dictionary for JSON serialization.""" + return { + "lower_body_policy": self.lower_body_policy, + "wbc_model_path": self.wbc_model_path, + "teleoperator_username": self.teleoperator_username, + "support_operator_username": self.support_operator_username, + "robot_type": self.robot_type, + "robot_id": self.robot_id, + } + + @classmethod + def from_dict(cls, data: dict) -> "DataCollectionInfo": + """Create a DataCollectionInfo instance from a dictionary.""" + return cls(**data) + + +class Gr00tDatasetMetadata(LeRobotDatasetMetadata): + """ + Additional metadata on top of LeRobotDatasetMetadata: + - modality_config: Written to `meta/modality.json` + - discarded_episode_indices: List of episode indices that were discarded. Written to `meta/info.json` + """ + + MODALITY_CONFIG_REL_PATH = Path("meta/modality.json") + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + with open(self.root / self.MODALITY_CONFIG_REL_PATH, "rb") as f: + self.modality_config = json.load(f) + + @classmethod + def create( + cls, + modality_config: dict, + script_config: dict, + data_collection_info: DataCollectionInfo, + *args, + **kwargs, + ): + cls.validate_modality_config(modality_config) + + # Create base metadata object using parent class + obj = super().create(*args, **kwargs) + + # we also need to initialize the discarded_episode_indices + obj.info["script_config"] = script_config + obj.info["discarded_episode_indices"] = [] + obj.info["data_collection_info"] = data_collection_info.to_dict() + with open(obj.root / "meta" / "info.json", "w") as f: + json.dump(obj.info, f, indent=4) + + obj.__class__ = cls + with open(obj.root / cls.MODALITY_CONFIG_REL_PATH, "w") as f: + json.dump(modality_config, f, indent=4) + obj.modality_config = modality_config + return obj + + @staticmethod + def validate_modality_config(modality_config: dict) -> None: + # verify if it contains all state, action, video, annotation keys + valid_keys = ["state", "action", "video", "annotation"] + if not all(key in modality_config for key in valid_keys): + raise ValueError( + f"Modality config must contain all of the following keys: {valid_keys}" + ) + + # verify that each key has a modality_config dict + for key in valid_keys: + if key not in modality_config: + raise ValueError(f"Modality config must contain a '{key}' key") + + +class Gr00tDataExporter(LeRobotDataset): + """ + A class for exporting data collected for a single session to LeRobot Dataset. + + Intended life cycle: + 1. Create a Gr00tDataExporter object + 2. Add frames using add_frame() + 3. Save the episode using save_episode() + - This will flush the episode buffer to disk + - This will also close the video writers + - Create a new video writer and ep buffer to start new episode + + If interrupted, here's the indented behavior: + - Interruption before save_episode() is called: loses the current episode + - Interruption after save_episode() is called: keeps completed episodes + """ + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self.video_writers = self.create_video_writer() + + @property + def repo_id(self): + return self.meta.repo_id + + @property + def root(self): + return self.meta.root + + @property + def local_files_only(self): + return self.meta.local_files_only + + @property + def video_keys(self): + return self.meta.video_keys + + @classmethod + def create( + cls, + save_root: str | Path, + fps: int, + features: dict, + modality_config: dict, + task: str, + script_config: ArgsConfig = ArgsConfig(), + data_collection_info: DataCollectionInfo = DataCollectionInfo(), + robot_type: str | None = None, + tolerance_s: float = 1e-4, + vcodec: str = "h264", + overwrite_existing: bool = False, + upload_bucket_path: str | None = None, + ) -> "Gr00tDataExporter": + """ + Create a Gr00tDataExporter object. + + Args: + save_root: The root directory to save the dataset. + fps: The frame rate of the dataset. + features: The features of the dataset. + modality_config: The modality config of the dataset. + task: The task performed during the data collection session. + data_collection_info: The data collection info. + If the dataset already exists, this argument will be ignored. + If data_collection_info is not provided, it will be set to an empty DataCollectionInfo object. + robot_type: The type of robot. + tolerance_s: The tolerance for the dataset. + image_writer_processes: The number of processes to use for the image writer. + image_writer_threads: The number of threads to use for the image writer. + vcodec: The codec to use for the video writer. + """ + + obj = cls.__new__(cls) + repo_id = ( + "tmp/tmp_dataset" # NOTE(fengyuanh): Not relevant since we are not pushing to the hub + ) + if overwrite_existing and (Path(save_root)).exists(): + print( + f"Found existing dataset at {save_root}", + "Cleaning up this directory since overwrite_existing is True.", + ) + shutil.rmtree(save_root) + + if (Path(save_root)).exists(): + # Try to resume from existing dataset + try: + # Load the metadata + obj.meta = Gr00tDatasetMetadata( + repo_id=repo_id, + root=save_root, + ) + + except RepositoryNotFoundError as e: + raise ValueError( + f"Failed to resume from corrupted dataset. Please manually check the dataset at {save_root}" + ) from e + else: + if not isinstance(script_config, dict): + script_config = script_config.to_dict() + obj.meta = Gr00tDatasetMetadata.create( + repo_id=repo_id, + fps=fps, + root=save_root, + # NOTE(fengyuanh): We use "robot_type" instead of this field which requires a Robot object + robot=None, + robot_type=robot_type, + features=features, + modality_config=modality_config, + script_config=script_config, + # NOTE(fengyuanh): Always use videos for exporting + use_videos=True, + data_collection_info=data_collection_info, + ) + obj.tolerance_s = tolerance_s + obj.video_backend = ( + "pyav" # NOTE(fengyuanh): Only used in training, not relevant for exporting + ) + obj.vcodec = vcodec + obj.task = task + obj.image_writer = None + + obj.episode_buffer = obj.create_episode_buffer() + + obj.episodes = None + obj.hf_dataset = obj.create_hf_dataset() + obj.image_transforms = None + obj.delta_timestamps = None + obj.delta_indices = None + obj.episode_data_index = None + obj.upload_bucket_path = upload_bucket_path + obj.video_writers = obj.create_video_writer() + return obj + + def create_video_writer(self) -> dict[str, VideoWriter]: + video_writers = {} + for key in self.meta.video_keys: + video_writers[key] = VideoWriter( + self.root + / self.meta.get_video_file_path(self.episode_buffer["episode_index"], key), + self.meta.shapes[key][1], + self.meta.shapes[key][0], + self.fps, + self.vcodec, + ) + return video_writers + + # @note (k2): This function is copied from LeRobotDataset.add_frame. + # This is done because we want to bypass lerobot's + # image_writer and use our own VideoWriter class. + def add_frame(self, frame: dict) -> None: + """ + This function only adds the frame to the episode_buffer. Videos are handled by the video_writer, + which uses a stream writer to write to disk. + """ + frame = copy.deepcopy(frame) + frame["task"] = frame.get("task", self.task) + + # Convert torch to numpy if needed + for name in frame: + if isinstance(frame[name], torch.Tensor): + frame[name] = frame[name].numpy() + + validate_frame(frame, self.features) + + if self.episode_buffer is None: + self.episode_buffer = self.create_episode_buffer() + + # Automatically add frame_index and timestamp to episode buffer + frame_index = self.episode_buffer["size"] + timestamp = frame.pop("timestamp") if "timestamp" in frame else frame_index / self.fps + self.episode_buffer["frame_index"].append(frame_index) + self.episode_buffer["timestamp"].append(timestamp) + + # Add frame features to episode_buffer + for key in frame: + if key == "task": + # Note: we associate the task in natural language to its task index during `save_episode` + self.episode_buffer["task"].append(frame["task"]) + continue + + if key not in self.features: + raise ValueError( + f"An element of the frame is not in the features. '{key}' not in '{self.features.keys()}'." + ) + + if self.features[key]["dtype"] in ["image", "video"]: + img_path = self._get_image_file_path( + episode_index=self.episode_buffer["episode_index"], + image_key=key, + frame_index=frame_index, + ) + if frame_index == 0: + img_path.parent.mkdir(parents=True, exist_ok=True) + + # @note (k2): using our own VideoWriter class, bypassing the image_writer + self.video_writers[key].add_frame(frame[key]) + self.episode_buffer[key].append(str(img_path)) + else: + self.episode_buffer[key].append(frame[key]) + + self.episode_buffer["size"] += 1 + + def stop_video_writers(self): + if not hasattr(self, "video_writers"): + raise RuntimeError( + "Can't stop video writers because they haven't been initialized. Call create() first." + ) + for key in self.video_writers: + self.video_writers[key].stop() + + def skip_and_start_new_episode( + self, + ) -> None: + """ + Skip the current episode and start a new one. + """ + self.stop_video_writers() + self.episode_buffer = self.create_episode_buffer() + self.video_writers = self.create_video_writer() + + # @note (k2): Code copied from LeRobotDataset.save_episode + # We override this function because we want to bypass lerobot's `compute_episode_stats` on video features + # since `compute_episode_stats` only works when images are written to disk. + def save_episode(self, episode_data: dict | None = None) -> None: + if not episode_data: + episode_buffer = self.episode_buffer + + validate_episode_buffer(episode_buffer, self.meta.total_episodes, self.features) + + # size and task are special cases that won't be added to hf_dataset + episode_length = episode_buffer.pop("size") + tasks = episode_buffer.pop("task") + episode_tasks = list(set(tasks)) + episode_index = episode_buffer["episode_index"] + + episode_buffer["index"] = np.arange( + self.meta.total_frames, self.meta.total_frames + episode_length + ) + episode_buffer["episode_index"] = np.full((episode_length,), episode_index) + + # Add new tasks to the tasks dictionary + for task in episode_tasks: + task_index = self.meta.get_task_index(task) + if task_index is None: + self.meta.add_task(task) + + # Given tasks in natural language, find their corresponding task indices + episode_buffer["task_index"] = np.array([self.meta.get_task_index(task) for task in tasks]) + + for key, ft in self.features.items(): + # index, episode_index, task_index are already processed above, and image and video + # are processed separately by storing image path and frame info as meta data + if key in ["index", "episode_index", "task_index"] or ft["dtype"] in ["image", "video"]: + continue + episode_buffer[key] = np.stack(episode_buffer[key]) + + self._wait_image_writer() + self._save_episode_table(episode_buffer, episode_index) + + # @note (k2): computing only non-video features stats + non_video_features = {k: v for k, v in self.features.items() if v["dtype"] not in ["video"]} + non_vid_ep_buffer = { + k: v for k, v in episode_buffer.items() if k in non_video_features.keys() + } + ep_stats = compute_episode_stats(non_vid_ep_buffer, non_video_features) + + if len(self.meta.video_keys) > 0: + video_paths = self.encode_episode_videos(episode_index) + for key in self.meta.video_keys: + episode_buffer[key] = video_paths[key] + + # `meta.save_episode` be executed after encoding the videos + self.meta.save_episode(episode_index, episode_length, episode_tasks, ep_stats) + + ep_data_index = get_episode_data_index(self.meta.episodes, [episode_index]) + ep_data_index_np = {k: t.numpy() for k, t in ep_data_index.items()} + check_timestamps_sync( + episode_buffer["timestamp"], + episode_buffer["episode_index"], + ep_data_index_np, + self.fps, + self.tolerance_s, + ) + + video_files = list(self.root.rglob("*.mp4")) + assert len(video_files) == self.num_episodes * len(self.meta.video_keys) + + parquet_files = list(self.root.rglob("*.parquet")) + assert len(parquet_files) == self.num_episodes + + # delete images + img_dir = self.root / "images" + if img_dir.is_dir(): + shutil.rmtree(self.root / "images") + + if not episode_data: # Reset the buffer and create new video writers + self.episode_buffer = self.create_episode_buffer() + self.video_writers = self.create_video_writer() + + # check if all video and parquet files exist + for key in self.meta.video_keys: + video_path = os.path.join(self.root, self.meta.get_video_file_path(episode_index, key)) + if not os.path.exists(video_path): + raise FileNotFoundError( + f"Video path: {video_path} does not exist for episode {episode_index}" + ) + + parquet_path = os.path.join(self.root, self.meta.get_data_file_path(episode_index)) + if not os.path.exists(parquet_path): + raise FileNotFoundError( + f"Parquet path: {parquet_path} does not exist for episode {episode_index}" + ) + + # @note (k2): Overriding LeRobotDataset.encode_episode_videos to use our own VideoWriter class + def encode_episode_videos(self, episode_index: int) -> dict: + video_paths = {} + for key in self.meta.video_keys: + video_paths[key] = self.video_writers[key].stop() + return video_paths + + def save_episode_as_discarded(self) -> None: + """ + Flag ongoing episode as discarded and save it to disk. Failed manipulations (grasp, manipulation) are + flagged as discarded. It will add the episode index to the discarded episode indices list in info.json. + """ + self.meta.info["discarded_episode_indices"] = self.meta.info.get( + "discarded_episode_indices", [] + ) + [self.episode_buffer["episode_index"]] + self.save_episode() + + +def hf_transform_to_torch_by_features( + features: datasets.Sequence, items_dict: dict[torch.Tensor | None] +): + """Get a transform function that convert items from Hugging Face dataset (pyarrow) + to torch tensors. Importantly, images are converted from PIL, which corresponds to + a channel last representation (h w c) of uint8 type, to a torch image representation + with channel first (c h w) of float32 type in range [0,1]. + """ + for key in items_dict: + first_item = items_dict[key][0] + if isinstance(first_item, PILImage.Image): + to_tensor = transforms.ToTensor() + items_dict[key] = [to_tensor(img) for img in items_dict[key]] + elif first_item is None: + pass + else: + if isinstance(features[key], datasets.Value): + dtype_str = features[key].dtype + elif isinstance(features[key], datasets.Sequence): + assert isinstance(features[key].feature, datasets.Value) + dtype_str = features[key].feature.dtype + else: + raise ValueError(f"Unsupported feature type for key '{key}': {features[key]}") + dtype_mapping = { + "float32": torch.float32, + "float64": torch.float64, + "int32": torch.int32, + "int64": torch.int64, + } + items_dict[key] = [ + torch.tensor(x, dtype=dtype_mapping[dtype_str]) for x in items_dict[key] + ] + return items_dict + + +# This is a subclass of LeRobotDataset that only fixes the data type when loading +# By default, LeRobotDataset will automatically convert float64 to float32 +class TypedLeRobotDataset(LeRobotDataset): + def __init__(self, load_video=True, *args, **kwargs): + super().__init__(*args, **kwargs) + if not load_video: + video_keys = [] + for key in self.meta.features.keys(): + if self.meta.features[key]["dtype"] == "video": + video_keys.append(key) + for key in video_keys: + self.meta.features.pop(key) + + def load_hf_dataset(self) -> datasets.Dataset: + """hf_dataset contains all the observations, states, actions, rewards, etc.""" + if self.episodes is None: + path = str(self.root / "data") + hf_dataset = load_dataset("parquet", data_dir=path, split="train") + else: + files = [ + str(self.root / self.meta.get_data_file_path(ep_idx)) for ep_idx in self.episodes + ] + hf_dataset = load_dataset("parquet", data_files=files, split="train") + + # TODO(aliberts): hf_dataset.set_format("torch") + hf_dataset.set_transform(partial(hf_transform_to_torch_by_features, hf_dataset.features)) + return hf_dataset diff --git a/GR00T-WholeBodyControl/decoupled_wbc/data/utils.py b/GR00T-WholeBodyControl/decoupled_wbc/data/utils.py new file mode 100644 index 0000000000000000000000000000000000000000..04874bd2759566f61645c0e7799e576ad64c5ca7 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/data/utils.py @@ -0,0 +1,156 @@ +from decoupled_wbc.control.robot_model.robot_model import RobotModel +from decoupled_wbc.data.constants import RS_VIEW_CAMERA_HEIGHT, RS_VIEW_CAMERA_WIDTH + + +def get_modality_config(robot_model: RobotModel, add_stereo_camera: bool = False) -> dict: + """ + Get the modality config for the robot model. + """ + left_hand_indices = sorted(robot_model.get_joint_group_indices("left_hand")) + right_hand_indices = sorted(robot_model.get_joint_group_indices("right_hand")) + left_arm_indices = sorted(robot_model.get_joint_group_indices("left_arm")) + right_arm_indices = sorted(robot_model.get_joint_group_indices("right_arm")) + waist_indices = sorted(robot_model.get_joint_group_indices("waist")) + left_leg_indices = sorted(robot_model.get_joint_group_indices("left_leg")) + right_leg_indices = sorted(robot_model.get_joint_group_indices("right_leg")) + + modality_config = { + "state": { + "left_leg": {"start": left_leg_indices[0], "end": left_leg_indices[-1] + 1}, + "right_leg": {"start": right_leg_indices[0], "end": right_leg_indices[-1] + 1}, + "waist": {"start": waist_indices[0], "end": waist_indices[-1] + 1}, + "left_arm": {"start": left_arm_indices[0], "end": left_arm_indices[-1] + 1}, + "left_hand": {"start": left_hand_indices[0], "end": left_hand_indices[-1] + 1}, + "right_arm": {"start": right_arm_indices[0], "end": right_arm_indices[-1] + 1}, + "right_hand": {"start": right_hand_indices[0], "end": right_hand_indices[-1] + 1}, + "left_wrist_pos": {"start": 0, "end": 3, "original_key": "observation.eef_state"}, + "left_wrist_abs_quat": { + "start": 3, + "end": 7, + "original_key": "observation.eef_state", + "rotation_type": "quaternion", + }, + "right_wrist_pos": {"start": 7, "end": 10, "original_key": "observation.eef_state"}, + "right_wrist_abs_quat": { + "start": 10, + "end": 14, + "original_key": "observation.eef_state", + "rotation_type": "quaternion", + }, + }, + "action": { + "left_leg": {"start": left_leg_indices[0], "end": left_leg_indices[-1] + 1}, + "right_leg": {"start": right_leg_indices[0], "end": right_leg_indices[-1] + 1}, + "waist": {"start": waist_indices[0], "end": waist_indices[-1] + 1}, + "left_arm": {"start": left_arm_indices[0], "end": left_arm_indices[-1] + 1}, + "left_hand": {"start": left_hand_indices[0], "end": left_hand_indices[-1] + 1}, + "right_arm": {"start": right_arm_indices[0], "end": right_arm_indices[-1] + 1}, + "right_hand": {"start": right_hand_indices[0], "end": right_hand_indices[-1] + 1}, + "left_wrist_pos": {"start": 0, "end": 3, "original_key": "action.eef"}, + "left_wrist_abs_quat": { + "start": 3, + "end": 7, + "original_key": "action.eef", + "rotation_type": "quaternion", + }, + "right_wrist_pos": {"start": 7, "end": 10, "original_key": "action.eef"}, + "right_wrist_abs_quat": { + "start": 10, + "end": 14, + "original_key": "action.eef", + "rotation_type": "quaternion", + }, + "base_height_command": { + "start": 0, + "end": 1, + "original_key": "teleop.base_height_command", + }, + "navigate_command": {"start": 0, "end": 3, "original_key": "teleop.navigate_command"}, + }, + "video": {"ego_view": {"original_key": "observation.images.ego_view"}}, + "annotation": {"human.task_description": {"original_key": "task_index"}}, + } + if add_stereo_camera: + modality_config["video"].update( + { + "ego_view_left_mono": {"original_key": "observation.images.ego_view_left_mono"}, + "ego_view_right_mono": {"original_key": "observation.images.ego_view_right_mono"}, + } + ) + + return modality_config + + +def get_dataset_features(robot_model: RobotModel, add_stereo_camera: bool = False) -> dict: + """ + Get the dataset features for the robot model. + """ + dataset_features = { + "observation.images.ego_view": { + "dtype": "video", + "shape": [RS_VIEW_CAMERA_HEIGHT, RS_VIEW_CAMERA_WIDTH, 3], + "names": ["height", "width", "channel"], + }, + "observation.state": { + "dtype": "float64", + "shape": (robot_model.num_joints,), + "names": robot_model.joint_names, + }, + "observation.eef_state": { + "dtype": "float64", + "shape": (14,), + "names": [ + "left_wrist_pos", + "left_wrist_abs_quat", + "right_wrist_pos", + "right_wrist_abs_quat", + ], + }, + "action": { + "dtype": "float64", + "shape": (robot_model.num_joints,), + "names": robot_model.joint_names, + }, + "action.eef": { + "dtype": "float64", + "shape": (14,), + "names": [ + "left_wrist_pos", + "left_wrist_abs_quat", + "right_wrist_pos", + "right_wrist_abs_quat", + ], + }, + "observation.img_state_delta": { + "dtype": "float32", + "shape": (1,), + "names": "img_state_delta", + }, + "teleop.navigate_command": { + "dtype": "float64", + "shape": (3,), + "names": ["lin_vel_x", "lin_vel_y", "ang_vel_z"], + }, + "teleop.base_height_command": { + "dtype": "float64", + "shape": (1,), + "names": "base_height_command", + }, + } + if add_stereo_camera: + dataset_features.update( + { + "observation.images.ego_view_left_mono": { + "dtype": "video", + "shape": [RS_VIEW_CAMERA_HEIGHT, RS_VIEW_CAMERA_WIDTH, 3], + "names": ["height", "width", "channel"], + }, + "observation.images.ego_view_right_mono": { + "dtype": "video", + "shape": [RS_VIEW_CAMERA_HEIGHT, RS_VIEW_CAMERA_WIDTH, 3], + "names": ["height", "width", "channel"], + }, + } + ) + + return dataset_features diff --git a/GR00T-WholeBodyControl/decoupled_wbc/data/video_writer.py b/GR00T-WholeBodyControl/decoupled_wbc/data/video_writer.py new file mode 100644 index 0000000000000000000000000000000000000000..a171c4e2191d8ff81aeb8258615edc4ec86fae9a --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/data/video_writer.py @@ -0,0 +1,102 @@ +import os +import queue +import sys +import threading +import time + +import av +import numpy as np + + +class VideoWriter: + def __init__( + self, + output_path: str, + width: int, + height: int, + fps: float, + codec: str = "h264", + buffer_size: int = 50, + ): + self.output_path = output_path + self._first_frame = True # Track first frame to suppress x264 info output + + # Create output directory if it doesn't exist + output_dir = os.path.dirname(output_path) + if output_dir and not os.path.exists(output_dir): + os.makedirs(output_dir, exist_ok=True) + + self.queue = queue.Queue(maxsize=buffer_size) + self.container = av.open(output_path, mode="w") + self.stream = self.container.add_stream(codec, rate=fps) + self.stream.width = width + self.stream.height = height + thread = threading.Thread(target=self._writer_worker, daemon=True) + thread.start() + + def _assert_dimensions(self, frame: np.ndarray) -> None: + assert ( + frame.shape[1] == self.stream.width and frame.shape[0] == self.stream.height + ), f"""Incorrect frame dimensions. Input dimensions: {frame.shape[1]}x{frame.shape[0]}. + Expected dimensions: {self.stream.width}x{self.stream.height}""" + + def add_frame(self, frame: np.ndarray) -> None: + self._assert_dimensions(frame) + self.queue.put(frame) + + def _writer_worker(self) -> None: + while True: + frame = self.queue.get() + if frame is None: + continue + self._assert_dimensions(frame) + frame = av.VideoFrame.from_ndarray(frame, format="rgb24") + + # Suppress stderr for first frame encoding (x264 prints info then) + if self._first_frame: + stderr_fd = sys.stderr.fileno() + old_stderr = os.dup(stderr_fd) + devnull = os.open(os.devnull, os.O_WRONLY) + os.dup2(devnull, stderr_fd) + try: + packets = self.stream.encode(frame) + for packet in packets: + self.container.mux(packet) + finally: + os.dup2(old_stderr, stderr_fd) + os.close(old_stderr) + os.close(devnull) + self._first_frame = False + else: + packets = self.stream.encode(frame) + for packet in packets: + self.container.mux(packet) + + def _flush_stream(self) -> None: + packets = self.stream.encode() + for packet in packets: + self.container.mux(packet) + + def stop(self) -> str: + """ + Blocking call. Waits until all the frames in the queue have been written to the file + and the video writer has been closed. + """ + if not self.queue.empty(): + print("Waiting for video writer queue to empty...") + while not self.queue.empty(): + time.sleep(0.1) + + print("Video writer queue is empty, flushing stream...") + self._flush_stream() + self.container.close() + return self.output_path + + def cancel(self) -> None: + """Immediately stops writing and deletes the output file""" + if os.path.exists(self.output_path): + os.remove(self.output_path) + self.container.close() + + def __del__(self) -> None: + self.container.close() diff --git a/GR00T-WholeBodyControl/decoupled_wbc/data/viz/rerun_viz.py b/GR00T-WholeBodyControl/decoupled_wbc/data/viz/rerun_viz.py new file mode 100644 index 0000000000000000000000000000000000000000..c649640db30e7d428e44d5c76aec0cb60627a161 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/data/viz/rerun_viz.py @@ -0,0 +1,213 @@ +#!/usr/bin/env python3 +"""Rerun visualization utilities for plotting data and images.""" + +from __future__ import annotations + +import argparse +import time +from typing import Dict, List, Optional + +import cv2 +import numpy as np +import rerun as rr # pip install rerun-sdk +import rerun.blueprint as rrb + + +class RerunViz: + """Class for visualizing data using Rerun.""" + + def __init__( + self, + image_keys: List[str], + tensor_keys: List[str], + app_name: str = "rerun_visualization", + memory_limit: str = "1GB", + window_size: float = 5.0, + port: int = 9876, + in_docker: bool = False, + ): + """Initialize the RerunViz class. + Args: + app_name: Name of the Rerun application + memory_limit: Memory limit for Rerun + window_size: Size of the time window in seconds + image_keys: List of image keys to plot + tensor_keys: List of tensor keys to plot + in_docker: Whether running inside Docker container. If in docker, + forward data to outside of the container to be rendered. + Use `rerun --port 9876` to visualize. Expecting rerun-cli 0.22.1 outside of docker. + Tested with rerun-sdk 0.21.0 inside docker. + """ + self.app_name = app_name + self.memory_limit = memory_limit + self.window_size = window_size + self.tensor_keys = tensor_keys + self.image_keys = image_keys + self.port = port + self.in_docker = in_docker + # Initialize Rerun + self._initialize_rerun() + + def _initialize_rerun(self): + """Initialize Rerun and set up the blueprint.""" + rr.init(self.app_name) + if not self.in_docker: + # support for web visualization + rr.spawn(memory_limit=self.memory_limit, port=self.port, connect=True) + else: + # forward data to outside of the docker container + rr.connect(f"127.0.0.1:{self.port}") + self._create_blueprint() + + def _create_blueprint(self): + # Create a grid of plots + contents = [] + + # Add time series plots + for tensor_key in self.tensor_keys: + contents.append( + rrb.TimeSeriesView( + origin=tensor_key, + time_ranges=[ + rrb.VisibleTimeRange( + "time", + start=rrb.TimeRangeBoundary.cursor_relative(seconds=-self.window_size), + end=rrb.TimeRangeBoundary.cursor_relative(), + ) + ], + ) + ) + + # Add image views + for image_key in self.image_keys: + contents.append(rrb.Spatial2DView(origin=image_key, name=image_key)) + + # Send the blueprint with collapsed panels to hide side/bottom bars + rr.send_blueprint(rrb.Blueprint(rrb.Grid(contents=contents), collapse_panels=True)) + + def set_rerun_keys(self, image_keys: List[str], tensor_keys: List[str]): + """Set the Rerun keys.""" + self.image_keys = image_keys + self.tensor_keys = tensor_keys + self._create_blueprint() + + def plot_images(self, images: Dict[str, np.ndarray], timestamp: Optional[float] = None): + """Plot image data. + + Args: + images: Dictionary mapping image names to image data + timestamp: Timestamp for the data (if None, uses current time) + """ + if timestamp is None: + timestamp = time.time() + + rr.set_time_seconds("time", timestamp) + + for key, image in images.items(): + if image is None: + continue + + if "depth" in key: + # Color jet + depth_colormap = cv2.applyColorMap( + cv2.convertScaleAbs(image, alpha=0.03), cv2.COLORMAP_JET + ) + rr.log(f"{key}", rr.Image(depth_colormap)) + else: + # Convert to RGB + # image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) + rr.log(f"{key}", rr.Image(image)) + + def plot_tensors( + self, data: Optional[Dict[str, np.ndarray]] = None, timestamp: Optional[float] = None + ): + """Plot tensor data. + + Args: + data: Dictionary mapping keys to tensor values + timestamp: Timestamp for the data (if None, uses current time) + """ + if timestamp is None: + timestamp = time.time() + + rr.set_time_seconds("time", timestamp) + + # If no data provided, use random walk generators + for tensor_key in self.tensor_keys: + for i in range(data[tensor_key].shape[0]): + rr.log(f"{tensor_key}/{i}", rr.Scalar(data[tensor_key][i])) + + def close(self): + """Close the RerunViz instance.""" + rr.rerun_shutdown() + + +if __name__ == "__main__": + """Main function to demonstrate the RerunViz class.""" + parser = argparse.ArgumentParser(description="Plot dashboard stress test") + parser.add_argument( + "--freq", type=float, default=20, help="Frequency of logging (applies to all series)" + ) + parser.add_argument( + "--window-size", type=float, default=5.0, help="Size of the window in seconds" + ) + parser.add_argument("--duration", type=float, default=60, help="How long to log for in seconds") + parser.add_argument("--use-rs", action="store_true", help="Use RealSense sensor") + parser.add_argument("--use-zed", action="store_true", help="Use ZED sensor") + parser.add_argument("--in-docker", action="store_true", help="Running inside Docker container") + args = parser.parse_args() + + if args.use_rs: + image_keys = ["color_image", "depth_image"] + from decoupled_wbc.control.sensor.realsense import RealSenseClientSensor + + sensor = RealSenseClientSensor() + elif args.use_zed: + image_keys = ["left_image", "right_image"] + from decoupled_wbc.control.sensor.zed import ZEDClientSensor + + sensor = ZEDClientSensor() + else: + from decoupled_wbc.control.sensor.dummy import DummySensor + + sensor = DummySensor() + image_keys = ["color_image"] + + tensor_keys = ["left_arm_qpos", "left_hand_qpos", "right_arm_qpos", "right_hand_qpos"] + + # Initialize the RerunViz class + viz = RerunViz( + image_keys=image_keys, + tensor_keys=tensor_keys, + window_size=args.window_size, + in_docker=args.in_docker, + ) + + # Run the visualization loop + cur_time = time.time() + end_time = cur_time + args.duration + time_per_tick = 1.0 / args.freq + + while cur_time < end_time: + # Advance time and sleep if necessary + cur_time += time_per_tick + sleep_for = cur_time - time.time() + if sleep_for > 0: + time.sleep(sleep_for) + + if sleep_for < -0.1: + print(f"Warning: missed logging window by {-sleep_for:.2f} seconds") + + # Plot dummy tensor + dummy_tensor = np.random.randn(5) + dummy_tensor_dict = {key: dummy_tensor for key in tensor_keys} + + viz.plot_tensors(dummy_tensor_dict, cur_time) + + # Plot images if available + images = sensor.read() + if images is not None: + img_to_show = {key: images[key] for key in image_keys} + viz.plot_images(img_to_show, cur_time) + + rr.script_teardown(args) diff --git a/GR00T-WholeBodyControl/decoupled_wbc/docker/.bashrc b/GR00T-WholeBodyControl/decoupled_wbc/docker/.bashrc new file mode 100644 index 0000000000000000000000000000000000000000..999e74e973aa66d3a6934fc71c87c967008842f8 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/docker/.bashrc @@ -0,0 +1,163 @@ +# ~/.bashrc: executed by bash(1) for non-login shells. +# see /usr/share/doc/bash/examples/startup-files (in the package bash-doc) +# for examples + +# If not running interactively, don't do anything +case $- in + *i*) ;; + *) return;; +esac + +# don't put duplicate lines or lines starting with space in the history. +# See bash(1) for more options +HISTCONTROL=ignoreboth + +# append to the history file, don't overwrite it +shopt -s histappend + +# for setting history length see HISTSIZE and HISTFILESIZE in bash(1) +HISTSIZE=1000 +HISTFILESIZE=2000 + +# check the window size after each command and, if necessary, +# update the values of LINES and COLUMNS. +shopt -s checkwinsize + +# If set, the pattern "**" used in a pathname expansion context will +# match all files and zero or more directories and subdirectories. +#shopt -s globstar + +# make less more friendly for non-text input files, see lesspipe(1) +[ -x /usr/bin/lesspipe ] && eval "$(SHELL=/bin/sh lesspipe)" + +# set variable identifying the chroot you work in (used in the prompt below) +if [ -z "${debian_chroot:-}" ] && [ -r /etc/debian_chroot ]; then + debian_chroot=$(cat /etc/debian_chroot) +fi + +# set a fancy prompt (non-color, unless we know we "want" color) +case "$TERM" in + xterm-color|*-256color) color_prompt=yes;; +esac + +# uncomment for a colored prompt, if the terminal has the capability; turned +# off by default to not distract the user: the focus in a terminal window +# should be on the output of commands, not on the prompt +force_color_prompt=yes + +if [ -n "$force_color_prompt" ]; then + if [ -x /usr/bin/tput ] && tput setaf 1 >&/dev/null; then + # We have color support; assume it's compliant with Ecma-48 + # (ISO/IEC-6429). (Lack of such support is extremely rare, and such + # a case would tend to support setf rather than setaf.) + color_prompt=yes + else + color_prompt= + fi +fi + +if [ "$color_prompt" = yes ]; then + PS1='${debian_chroot:+($debian_chroot)}\[\033[01;32m\]\u@\h\[\033[00m\]:\[\033[01;34m\]\w\[\033[00m\]\$ ' +else + PS1='${debian_chroot:+($debian_chroot)}\u@\h:\w\$ ' +fi +unset color_prompt force_color_prompt + +# If this is an xterm set the title to user@host:dir +case "$TERM" in +xterm*|rxvt*) + PS1="\[\e]0;${debian_chroot:+($debian_chroot)}\u@\h: \w\a\]$PS1" + ;; +*) + ;; +esac + +# enable color support of ls and also add handy aliases +if [ -x /usr/bin/dircolors ]; then + test -r ~/.dircolors && eval "$(dircolors -b ~/.dircolors)" || eval "$(dircolors -b)" + alias ls='ls --color=auto' + #alias dir='dir --color=auto' + #alias vdir='vdir --color=auto' + + alias grep='grep --color=auto' + alias fgrep='fgrep --color=auto' + alias egrep='egrep --color=auto' +fi + +# colored GCC warnings and errors +export GCC_COLORS='error=01;31:warning=01;35:note=01;36:caret=01;32:locus=01:quote=01' + +# Set terminal type for color support +export TERM=xterm-256color + +# some more ls aliases +alias ll='ls -alF' +alias la='ls -A' +alias l='ls -CF' + +# Add an "alert" alias for long running commands. Use like so: +# sleep 10; alert +alias alert='notify-send --urgency=low -i "$([ $? = 0 ] && echo terminal || echo error)" "$(history|tail -n1|sed -e '\''s/^\s*[0-9]\+\s*//;s/[;&|]\s*alert$//'\'')"' + +# Alias definitions. +# You may want to put all your additions into a separate file like +# ~/.bash_aliases, instead of adding them here directly. +# See /usr/share/doc/bash-doc/examples in the bash-doc package. + +if [ -f ~/.bash_aliases ]; then + . ~/.bash_aliases +fi + +# enable programmable completion features (you don't need to enable +# this, if it's already enabled in /etc/bash.bashrc and /etc/profile +# sources /etc/bash.bashrc). +if ! shopt -oq posix; then + if [ -f /usr/share/bash-completion/bash_completion ]; then + . /usr/share/bash-completion/bash_completion + elif [ -f /etc/bash_completion ]; then + . /etc/bash_completion + fi +fi + +# useful commands +bind '"\e[A": history-search-backward' +bind '"\e[B": history-search-forward' + +# Store the last 10 directories in a history file +CD_HISTFILE=~/.cd_history +CD_HISTSIZE=10 + +cd() { + local histfile="${CD_HISTFILE:-$HOME/.cd_history}" + local max="${CD_HISTSIZE:-10}" + + case "$1" in + --) [ -f "$histfile" ] && tac "$histfile" | nl -w2 -s' ' || echo "No directory history yet."; return ;; + -[0-9]*) + local idx=${1#-} + local dir=$(tac "$histfile" 2>/dev/null | sed -n "${idx}p") + [ -n "$dir" ] && builtin cd "$dir" || echo "Invalid selection: $1" + return ;; + esac + + builtin cd "$@" || return + + [[ $(tail -n1 "$histfile" 2>/dev/null) != "$PWD" ]] && echo "$PWD" >> "$histfile" + tail -n "$max" "$histfile" > "${histfile}.tmp" && mv "${histfile}.tmp" "$histfile" +} + +# Make decoupled_wbc importable +export PYTHONPATH="${DECOUPLED_WBC_DIR}:${PYTHONPATH}" + +# Manus to LD_LIBRARY_PATH +export LD_LIBRARY_PATH=$DECOUPLED_WBC_DIR/decoupled_wbc/control/teleop/device/SDKClient_Linux/ManusSDK/lib:$LD_LIBRARY_PATH + +# CUDA support +export LD_LIBRARY_PATH=/usr/lib/x86_64-linux-gnu:/usr/lib:/lib/x86_64-linux-gnu:/lib64:/lib:$LD_LIBRARY_PATH + +# decoupled_wbc aliases +alias dg="python $DECOUPLED_WBC_DIR/decoupled_wbc/scripts/deploy_g1.py" +alias rsl="python $DECOUPLED_WBC_DIR/decoupled_wbc/control/main/teleop/run_sim_loop.py" +alias rgcl="python $DECOUPLED_WBC_DIR/decoupled_wbc/control/main/teleop/run_g1_control_loop.py" +alias rtpl="python $DECOUPLED_WBC_DIR/decoupled_wbc/control/main/teleop/run_teleop_policy_loop.py" +alias tgcl="pytest $DECOUPLED_WBC_DIR/decoupled_wbc/tests/control/main/teleop/test_g1_control_loop.py -s" diff --git a/GR00T-WholeBodyControl/decoupled_wbc/docker/.tmux.conf b/GR00T-WholeBodyControl/decoupled_wbc/docker/.tmux.conf new file mode 100644 index 0000000000000000000000000000000000000000..0da4a3179e38280c422ea673e6193b6fed83466f --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/docker/.tmux.conf @@ -0,0 +1,84 @@ +# Enable mouse mode +set -g mouse on + +# Start window numbering at 0 (default) +set -g base-index 0 +setw -g pane-base-index 0 + +# Increase scrollback buffer size +set -g history-limit 50000 + +# Use Alt-arrow keys without prefix key to switch panes +bind -n M-Left select-pane -L +bind -n M-Right select-pane -R +bind -n M-Up select-pane -U +bind -n M-Down select-pane -D + +# Use Alt-1,2,3... to switch windows +bind -n M-1 select-window -t 1 +bind -n M-2 select-window -t 2 +bind -n M-3 select-window -t 3 +bind -n M-4 select-window -t 4 +bind -n M-5 select-window -t 5 +bind -n M-6 select-window -t 6 +bind -n M-7 select-window -t 7 +bind -n M-8 select-window -t 8 +bind -n M-9 select-window -t 9 + +# Split panes using Alt-| and Alt-- +bind -n M-| split-window -h +bind -n M-- split-window -v + +# Easy config reload +bind -n M-r source-file ~/.tmux.conf \; display-message "Config reloaded!" + +# Status bar customization +set -g status-style bg=colour240,fg=colour255 +set -g status-left "#[fg=colour255,bg=colour240] #S #[fg=colour240,bg=colour238]" +set -g status-right "#[fg=colour255,bg=colour240] %H:%M #[fg=colour240,bg=colour238]" + +# Window status format +setw -g window-status-format "#[fg=colour255,bg=colour238] #I:#W " +setw -g window-status-current-format "#[fg=colour238,bg=colour255]#[fg=colour238,bg=colour255] #I:#W #[fg=colour255,bg=colour238]" + +# Pane border colors +set -g pane-border-style fg=colour240 +set -g pane-active-border-style fg=colour255 + +# Message text +set -g message-style bg=colour238,fg=colour255 + +# Clock mode +setw -g clock-mode-colour colour255 + +# Enable focus events +set -g focus-events on + +# Increase escape time +set -sg escape-time 0 + +# Enable true color support +set -ga terminal-overrides ",*256col*:Tc" + +# Set default terminal mode to 256 colors +set -g default-terminal "screen-256color" + +# Display a message when a window is created +set -g display-time 4000 + +# Automatically set window title +setw -g automatic-rename on +set -g set-titles on +set -g set-titles-string "#T" + +# Enable clipboard integration +set -g @plugin 'tmux-plugins/tmux-yank' + +# List of plugins +set -g @plugin 'tmux-plugins/tpm' +set -g @plugin 'tmux-plugins/tmux-sensible' +set -g @plugin 'tmux-plugins/tmux-resurrect' +set -g @plugin 'tmux-plugins/tmux-continuum' + +# Initialize TMUX plugin manager (keep this line at the very bottom of tmux.conf) +run '~/.tmux/plugins/tpm/tpm' \ No newline at end of file diff --git a/GR00T-WholeBodyControl/decoupled_wbc/docker/70-manus-hid.rules b/GR00T-WholeBodyControl/decoupled_wbc/docker/70-manus-hid.rules new file mode 100644 index 0000000000000000000000000000000000000000..3c2bb3270d6b329c8a35d6846b8644afeb669d75 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/docker/70-manus-hid.rules @@ -0,0 +1,5 @@ + # HIDAPI/libusb + SUBSYSTEMS=="usb", ATTRS{idVendor}=="3325", MODE:="0666" + + # HIDAPI/hidraw + KERNEL=="hidraw*", ATTRS{idVendor}=="3325", MODE:="0666" \ No newline at end of file diff --git a/GR00T-WholeBodyControl/decoupled_wbc/docker/Dockerfile.deploy b/GR00T-WholeBodyControl/decoupled_wbc/docker/Dockerfile.deploy new file mode 100644 index 0000000000000000000000000000000000000000..239127c85d8be57c5de415f31538f288133e8f05 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/docker/Dockerfile.deploy @@ -0,0 +1,130 @@ +FROM nvgear/ros-2:latest + +# Accept build argument for username +ARG USERNAME +ARG USERID +ARG HOME_DIR +ARG WORKTREE_NAME + +# Create user with the same name as host +RUN if [ "$USERID" != "0" ]; then \ + useradd -m -u ${USERID} -s /bin/bash ${USERNAME} && \ + echo "${USERNAME} ALL=(ALL) NOPASSWD: ALL" >> /etc/sudoers && \ + # Add user to video and render groups for GPU access + usermod -a -G video,render ${USERNAME} || true; \ + fi + +# Copy .bashrc with color settings before switching user +COPY --chown=${USERNAME}:${USERNAME} decoupled_wbc/docker/.bashrc ${HOME_DIR}/.bashrc + +# Install Manus udev rules +COPY --chown=${USERNAME}:${USERNAME} decoupled_wbc/docker/70-manus-hid.rules /etc/udev/rules.d/70-manus-hid.rules + +# Copy tmux configuration +COPY --chown=${USERNAME}:${USERNAME} decoupled_wbc/docker/.tmux.conf ${HOME_DIR}/.tmux.conf + +# Switch to user +USER ${USERNAME} + +# Install tmux plugin manager and uv in parallel +RUN git clone https://github.com/tmux-plugins/tpm ${HOME_DIR}/.tmux/plugins/tpm & \ + curl -LsSf https://astral.sh/uv/install.sh | env UV_INSTALL_DIR=${HOME_DIR}/.cargo/bin sh & \ + wait + +# Install tmux plugins automatically +RUN ${HOME_DIR}/.tmux/plugins/tpm/bin/install_plugins || true + +# Add uv to PATH +ENV PATH="${HOME_DIR}/.cargo/bin:$PATH" +ENV UV_PYTHON=${HOME_DIR}/venv/bin/python + +# Create venv +RUN uv venv --python 3.10 ${HOME_DIR}/venv + +# Install hardware-specific packages (x86 only - not available on ARM64/Orin) +USER root +COPY --chown=${USERNAME}:${USERNAME} decoupled_wbc/control/teleop/device/pico/XRoboToolkit_PC_Service_1.0.0_ubuntu_22.04_amd64.deb ${HOME_DIR}/XRoboToolkit_PC_Service_1.0.0_ubuntu_22.04_amd64.deb +COPY --chown=${USERNAME}:${USERNAME} decoupled_wbc/control/teleop/device/pico/roboticsservice_1.0.0.0_arm64.deb ${HOME_DIR}/roboticsservice_1.0.0.0_arm64.deb + +RUN if [ "$(dpkg --print-architecture)" = "amd64" ]; then \ + # Ultra Leap setup + wget -qO - https://repo.ultraleap.com/keys/apt/gpg | gpg --dearmor | tee /etc/apt/trusted.gpg.d/ultraleap.gpg && \ + echo 'deb [arch=amd64] https://repo.ultraleap.com/apt stable main' | tee /etc/apt/sources.list.d/ultraleap.list && \ + apt-get update && \ + echo "yes" | DEBIAN_FRONTEND=noninteractive apt-get install -y ultraleap-hand-tracking libhidapi-dev && \ + # Space Mouse udev rules + echo 'KERNEL=="hidraw*", SUBSYSTEM=="hidraw", MODE="0664", GROUP="plugdev"' > /etc/udev/rules.d/99-hidraw-permissions.rules && \ + usermod -aG plugdev ${USERNAME}; \ + # Pico setup + apt-get install -y xdg-utils && \ + dpkg -i ${HOME_DIR}/XRoboToolkit_PC_Service_1.0.0_ubuntu_22.04_amd64.deb; \ + else \ + echo "Skipping x86-only hardware packages on $(dpkg --print-architecture)"; \ + fi + +USER ${USERNAME} +# Install hardware Python packages (x86 only) with caching +RUN --mount=type=cache,target=${HOME_DIR}/.cache/uv,uid=${USERID},gid=${USERID} \ + if [ "$(dpkg --print-architecture)" = "amd64" ]; then \ + # Ultra Leap Python bindings + git clone https://github.com/ultraleap/leapc-python-bindings ${HOME_DIR}/leapc-python-bindings && \ + cd ${HOME_DIR}/leapc-python-bindings && \ + UV_CONCURRENT_DOWNLOADS=8 uv pip install -r requirements.txt && \ + MAKEFLAGS="-j$(nproc)" ${HOME_DIR}/venv/bin/python -m build leapc-cffi && \ + uv pip install leapc-cffi/dist/leapc_cffi-0.0.1.tar.gz && \ + uv pip install -e leapc-python-api && \ + # Space Mouse Python package + uv pip install pyspacemouse && \ + # Pico Python bindings + git clone https://github.com/XR-Robotics/XRoboToolkit-PC-Service-Pybind.git ${HOME_DIR}/XRoboToolkit-PC-Service-Pybind && \ + cd ${HOME_DIR}/XRoboToolkit-PC-Service-Pybind && \ + uv pip install setuptools pybind11 && \ + sed -i "s|pip install|uv pip install|g" setup_ubuntu.sh && \ + sed -i "s|pip uninstall|uv pip uninstall|g" setup_ubuntu.sh && \ + sed -i "s|python setup.py install|${HOME_DIR}/venv/bin/python setup.py install|g" setup_ubuntu.sh && \ + bash setup_ubuntu.sh; \ + fi + +# Install Python dependencies using uv with caching +RUN --mount=type=cache,target=${HOME_DIR}/.cache/uv,uid=${USERID},gid=${USERID} \ + UV_CONCURRENT_DOWNLOADS=8 uv pip install --upgrade pip ipython jupyter notebook debugpy + + +# Copy entire project to the workspace directory where it will be mounted at runtime +# NOTE: The build context must be the project root for this to work +# Use dynamic worktree name to match runtime mount path +COPY --chown=${USERNAME}:${USERNAME} . ${HOME_DIR}/Projects/${WORKTREE_NAME} + +# Install Python dependencies inside the venv with caching - split into separate commands +RUN --mount=type=cache,target=${HOME_DIR}/.cache/uv,uid=${USERID},gid=${USERID} \ + UV_CONCURRENT_DOWNLOADS=8 uv pip install \ + -e ${HOME_DIR}/Projects/${WORKTREE_NAME}/external_dependencies/unitree_sdk2_python + +# Unlike pip, uv downloads LFS files by default. There's a bug in uv that causes LFS files +# to fail to download (https://github.com/astral-sh/uv/issues/3312). So we need to set +# UV_GIT_LFS=1 to prevent uv from downloading LFS files. +# Install project packages (decoupled_wbc + gear_sonic) with caching +RUN --mount=type=cache,target=${HOME_DIR}/.cache/uv,uid=${USERID},gid=${USERID} \ + GIT_LFS_SKIP_SMUDGE=1 UV_CONCURRENT_DOWNLOADS=8 uv pip install \ + -e "${HOME_DIR}/Projects/${WORKTREE_NAME}/decoupled_wbc[full,dev]" \ + -e "${HOME_DIR}/Projects/${WORKTREE_NAME}/gear_sonic[sim]" + +# Clone and install robosuite with specific branch +RUN --mount=type=cache,target=${HOME_DIR}/.cache/uv,uid=${USERID},gid=${USERID} \ + git clone https://github.com/xieleo5/robosuite.git ${HOME_DIR}/robosuite && \ + cd ${HOME_DIR}/robosuite && \ + git checkout leo/support_g1_locomanip && \ + UV_CONCURRENT_DOWNLOADS=8 uv pip install -e . + +# Install gr00trobocasa +RUN --mount=type=cache,target=${HOME_DIR}/.cache/uv,uid=${USERID},gid=${USERID} \ + UV_CONCURRENT_DOWNLOADS=8 uv pip install -e ${HOME_DIR}/Projects/${WORKTREE_NAME}/decoupled_wbc/dexmg/gr00trobocasa + +# Configure bash environment with virtual environment and ROS2 setup +RUN echo "source ${HOME_DIR}/venv/bin/activate" >> ${HOME_DIR}/.bashrc && \ + echo "source /opt/ros/humble/setup.bash" >> ${HOME_DIR}/.bashrc && \ + echo "export ROS_LOCALHOST_ONLY=1" >> ${HOME_DIR}/.bashrc && \ + echo "export PYTHONPATH=${HOME_DIR}/Projects/${WORKTREE_NAME}:\${PYTHONPATH}" >> ${HOME_DIR}/.bashrc + +# Default command (can be overridden at runtime) +CMD ["/bin/bash"] diff --git a/GR00T-WholeBodyControl/decoupled_wbc/docker/Dockerfile.deploy.base b/GR00T-WholeBodyControl/decoupled_wbc/docker/Dockerfile.deploy.base new file mode 100644 index 0000000000000000000000000000000000000000..360d2c59c8d0112e661603defa2d95a9a270ae48 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/docker/Dockerfile.deploy.base @@ -0,0 +1,155 @@ +# Multi-architecture Dockerfile for NVIDIA CUDA +# Supports linux/amd64 and linux/arm64 +FROM nvidia/cuda:12.4.0-runtime-ubuntu22.04 + +# Build info - ARGs need to be redeclared after FROM to use in RUN commands +ARG TARGETPLATFORM +ARG BUILDPLATFORM +RUN echo "Building for $TARGETPLATFORM on $BUILDPLATFORM" + +# Avoid prompts from apt +ENV DEBIAN_FRONTEND=noninteractive + +# Install minimal system dependencies +RUN apt-get update && \ + apt-get install -y \ + # Basic tools + build-essential \ + curl \ + gdb \ + git \ + git-lfs \ + net-tools \ + sudo \ + wget \ + iputils-ping \ + vim \ + unzip \ + # System services + udev \ + # Graphics and X11 + libgl1-mesa-dri \ + libgl1-mesa-glx \ + libglu1-mesa \ + mesa-utils \ + libxcb-cursor0 \ + x11-apps \ + xauth \ + # EGL and GPU access + libegl1 \ + libegl1-mesa \ + libegl1-mesa-dev \ + libgl1-mesa-dev \ + libgles2-mesa-dev \ + libglvnd-dev \ + mesa-common-dev \ + # XCB and Qt platform dependencies + libxcb-icccm4 \ + libxcb-image0 \ + libxcb-keysyms1 \ + libxcb-randr0 \ + libxcb-render-util0 \ + libxcb-shape0 \ + libxcb-xfixes0 \ + libxcb-xinerama0 \ + libxcb-xinput0 \ + libxcb-xkb1 \ + libxkbcommon-x11-0 \ + # D-Bus and system dependencies + libdbus-1-3 \ + # Other dependencies + libncurses5-dev \ + libudev-dev \ + libusb-1.0-0-dev \ + # Python 3.10 and pip + python3.10 \ + python3.10-venv \ + python3.10-distutils \ + python3-pip \ + expect \ + # ffmpeg and related libraries + ffmpeg \ + libavcodec-dev \ + libavformat-dev \ + libavdevice-dev \ + libavfilter-dev \ + libavutil-dev \ + libswresample-dev \ + libswscale-dev \ + # opencv + libgtk2.0-dev \ + # Clean up + && rm -rf /var/lib/apt/lists/* + +# --- Install ROS 2 Humble (following official instructions) --- +# Enable required repositories +RUN apt-get update && apt-get install -y software-properties-common \ + && add-apt-repository universe \ + # Add ROS 2 GPG key and repository + && apt-get install -y curl gnupg lsb-release \ + && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg \ + && echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" > /etc/apt/sources.list.d/ros2.list \ + # Upgrade system to avoid removal of critical packages (see ROS 2 docs) + && apt-get update \ + && apt-get upgrade -y \ + # Install ROS 2 Humble desktop + && apt-get install -y ros-humble-desktop \ + # (Optional) Install development tools + && apt-get install -y ros-dev-tools \ + # Install Eclipse Cyclone DDS RMW implementation + && apt-get install -y ros-humble-rmw-cyclonedds-cpp \ + # Clean up + && rm -rf /var/lib/apt/lists/* + +# Source ROS 2 setup in bashrc for all users +RUN echo 'source /opt/ros/humble/setup.bash' >> /etc/bash.bashrc + +# Clone, build, and install CycloneDDS 0.10.x +RUN git clone --branch releases/0.10.x https://github.com/eclipse-cyclonedds/cyclonedds /opt/cyclonedds && \ + mkdir -p /opt/cyclonedds/build /opt/cyclonedds/install && \ + cd /opt/cyclonedds/build && \ + cmake .. -DCMAKE_INSTALL_PREFIX=../install && \ + cmake --build . --target install && \ + # Clean up build files to reduce image size + rm -rf /opt/cyclonedds/build + +# Set CYCLONEDDS_HOME for all users +ENV CYCLONEDDS_HOME=/opt/cyclonedds/install + +# Install uv +RUN curl -LsSf https://astral.sh/uv/install.sh | env UV_INSTALL_DIR=/opt/uv sh + +# Add uv to PATH +ENV PATH="/opt/uv:$PATH" +ENV UV_PYTHON=/opt/venv/bin/python + +# Fix dpkg state and install tmux +RUN dpkg --configure -a && apt-get update && apt-get install -y tmux && rm -rf /var/lib/apt/lists/* + +# Create NVIDIA ICD config for EGL if it doesn't exist +# Note: This might need platform-specific handling for ARM64 +RUN if [ ! -f /usr/share/glvnd/egl_vendor.d/10_nvidia.json ]; then \ + mkdir -p /usr/share/glvnd/egl_vendor.d && \ + printf '{\n "file_format_version" : "1.0.0",\n "ICD" : {\n "library_path" : "libEGL_nvidia.so.0"\n }\n}' | tee /usr/share/glvnd/egl_vendor.d/10_nvidia.json > /dev/null; \ + fi + +# Platform-specific configurations +RUN case "$TARGETPLATFORM" in \ + "linux/arm64") \ + echo "Configuring for ARM64 platform" && \ + # Add any ARM64-specific configurations here + echo "export GPU_FORCE_64BIT_PTR=1" >> /etc/environment \ + ;; \ + "linux/amd64") \ + echo "Configuring for AMD64 platform" && \ + # Add any AMD64-specific configurations here + echo "AMD64 platform configured" \ + ;; \ + *) \ + echo "Unknown platform: $TARGETPLATFORM" \ + ;; \ + esac + +# Add labels for better image management +LABEL org.opencontainers.image.title="Multi-Arch CUDA Runtime with ROS 2 Humble" +LABEL org.opencontainers.image.description="Multi-architecture Docker image with CUDA runtime and ROS 2 Humble" diff --git a/GR00T-WholeBodyControl/decoupled_wbc/docker/build_deploy_base.sh b/GR00T-WholeBodyControl/decoupled_wbc/docker/build_deploy_base.sh new file mode 100644 index 0000000000000000000000000000000000000000..9d4a5d43ac94dc0f3bcbbfaa69d2f75ec7893f13 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/docker/build_deploy_base.sh @@ -0,0 +1,56 @@ +#!/bin/bash + +# Multi-architecture Docker build script +# Supports linux/amd64 + +set -e + +# Configuration +IMAGE_NAME="nvgear/ros-2" +TAG="${1:-latest}" +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +DOCKERFILE="$SCRIPT_DIR/Dockerfile.deploy.base" + +# Colors for output +RED='\033[0;31m' +GREEN='\033[0;32m' +YELLOW='\033[1;33m' +NC='\033[0m' # No Color + +echo -e "${GREEN}Building multi-architecture Docker image: ${IMAGE_NAME}:${TAG}${NC}" + +# Ensure we're using the multiarch builder +echo -e "${YELLOW}Setting up multiarch builder...${NC}" +sudo docker buildx use multiarch-builder 2>/dev/null || { + echo -e "${YELLOW}Creating multiarch builder...${NC}" + sudo docker buildx create --name multiarch-builder --use --bootstrap +} + +# Show supported platforms +echo -e "${YELLOW}Supported platforms:${NC}" +sudo docker buildx inspect --bootstrap | grep Platforms + +# Build for multiple architectures +echo -e "${GREEN}Starting multi-arch build...${NC}" +sudo docker buildx build \ + --platform linux/amd64 \ + --file "${DOCKERFILE}" \ + --tag "${IMAGE_NAME}:${TAG}" \ + --push \ + . + +# Alternative: Build and load locally (only works for single platform) +# docker buildx build \ +# --platform linux/amd64 \ +# --file "${DOCKERFILE}" \ +# --tag "${IMAGE_NAME}:${TAG}" \ +# --load \ +# . + +echo -e "${GREEN}Multi-arch build completed successfully!${NC}" +echo -e "${GREEN}Image: ${IMAGE_NAME}:${TAG}${NC}" +echo -e "${GREEN}Platforms: linux/amd64${NC}" + +# Verify the manifest +echo -e "${YELLOW}Verifying multi-arch manifest...${NC}" +sudo docker buildx imagetools inspect "${IMAGE_NAME}:${TAG}" \ No newline at end of file diff --git a/GR00T-WholeBodyControl/decoupled_wbc/docker/image_name.txt b/GR00T-WholeBodyControl/decoupled_wbc/docker/image_name.txt new file mode 100644 index 0000000000000000000000000000000000000000..10b5635afccc1b018dcf0be55a1837d39c57ce68 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/docker/image_name.txt @@ -0,0 +1 @@ +nvcr.io/nvidian/gr00t_wbc:base diff --git a/GR00T-WholeBodyControl/decoupled_wbc/docker/kill_all_containers.sh b/GR00T-WholeBodyControl/decoupled_wbc/docker/kill_all_containers.sh new file mode 100644 index 0000000000000000000000000000000000000000..1043070f399be790a5e318a6238a6a00fc5d0e67 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/docker/kill_all_containers.sh @@ -0,0 +1,79 @@ +#!/bin/bash + +# Script to kill all running Docker containers +# Usage: ./kill_all_containers.sh [--force] + +set -e + +# Colors for output +RED='\033[0;31m' +GREEN='\033[0;32m' +YELLOW='\033[1;33m' +NC='\033[0m' # No Color + +# Function to print colored output +print_info() { + echo -e "${GREEN}[INFO]${NC} $1" +} + +print_warning() { + echo -e "${YELLOW}[WARNING]${NC} $1" +} + +print_error() { + echo -e "${RED}[ERROR]${NC} $1" +} + +# Check if Docker is running +if ! sudo docker info >/dev/null 2>&1; then + print_error "Docker is not running or not accessible. Please start Docker first." + exit 1 +fi + +# Get list of running containers +RUNNING_CONTAINERS=$(sudo docker ps -q) + +if [ -z "$RUNNING_CONTAINERS" ]; then + print_info "No running containers found." + exit 0 +fi + +# Count running containers +CONTAINER_COUNT=$(echo "$RUNNING_CONTAINERS" | wc -l | tr -d ' ') + +print_info "Found $CONTAINER_COUNT running container(s):" +sudo docker ps --format "table {{.ID}}\t{{.Image}}\t{{.Names}}\t{{.Status}}" + +# Check for --force flag +FORCE_KILL=false +if [ "$1" = "--force" ]; then + FORCE_KILL=true + print_warning "Force mode enabled. Containers will be killed without confirmation." +fi + +# Ask for confirmation unless --force is used +if [ "$FORCE_KILL" = false ]; then + echo + read -p "Are you sure you want to kill all running containers? (y/N): " -n 1 -r + echo + if [[ ! $REPLY =~ ^[Yy]$ ]]; then + print_info "Operation cancelled." + exit 0 + fi +fi + +# Kill all running containers +print_info "Killing all running containers..." +if sudo docker kill $RUNNING_CONTAINERS; then + print_info "Successfully killed all running containers." +else + print_error "Failed to kill some containers. You may need to run with sudo or check Docker permissions." + exit 1 +fi + +# Optional: Remove stopped containers (commented out by default) +# Uncomment the following lines if you also want to remove the stopped containers +# print_info "Removing stopped containers..." +# sudo docker container prune -f + +print_info "Done!" \ No newline at end of file diff --git a/GR00T-WholeBodyControl/decoupled_wbc/docker/kill_decoupled_wbc_processors.sh b/GR00T-WholeBodyControl/decoupled_wbc/docker/kill_decoupled_wbc_processors.sh new file mode 100644 index 0000000000000000000000000000000000000000..b9dd4c1b6e8694520a2b22dc650b04139dde102b --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/docker/kill_decoupled_wbc_processors.sh @@ -0,0 +1,192 @@ +#!/bin/bash + +# kill_decoupled_wbc_processors.sh +# Kill decoupled_wbc processes in current container to prevent message passing conflicts + +# Note: Don't use 'set -e' as tmux/pgrep commands may return non-zero exit codes + +# Configuration +DRY_RUN=false +FORCE=false +QUIET=false +declare -A FOUND_PROCESSES + +# Default to verbose mode if no arguments +[[ $# -eq 0 ]] && { QUIET=false; DRY_RUN=false; } + +# Parse arguments +while [[ $# -gt 0 ]]; do + case $1 in + --dry-run) DRY_RUN=true ;; + --force) FORCE=true ;; + --verbose|-v) VERBOSE=true ;; + --help|-h) + echo "Usage: $0 [--dry-run] [--force] [--verbose] [--help]" + echo "Kill decoupled_wbc processes to prevent message passing conflicts" + exit 0 ;; + *) echo "Unknown option: $1"; exit 1 ;; + esac + shift +done + +# Colors (only if not quiet) +if [[ "$QUIET" != true ]]; then + RED='\033[0;31m'; GREEN='\033[0;32m'; YELLOW='\033[1;33m'; BLUE='\033[0;34m'; NC='\033[0m' +else + RED=''; GREEN=''; YELLOW=''; BLUE=''; NC='' +fi + +# Show processes by pattern (for preview) +show_processes_by_pattern() { + local pattern="$1" desc="$2" + local pids=$(pgrep -f "$pattern" 2>/dev/null || true) + + [[ -z "$pids" ]] && return 0 + + echo -e "${YELLOW}$desc processes:${NC}" + + for pid in $pids; do + local cmd=$(ps -p $pid -o cmd= 2>/dev/null || echo "Process not found") + echo " PID $pid: $cmd" + done +} + +# Kill processes by pattern (silent killing) +kill_by_pattern() { + local pattern="$1" desc="$2" signal="${3:-TERM}" + local pids=$(pgrep -f "$pattern" 2>/dev/null || true) + + [[ -z "$pids" ]] && return 0 + + for pid in $pids; do + # Kill if not dry run + [[ "$DRY_RUN" != true ]] && kill -$signal $pid 2>/dev/null + done +} + +# Show tmux sessions (for preview) +show_tmux() { + local pattern="$1" + local sessions=$(tmux list-sessions 2>/dev/null | grep "$pattern" | cut -d: -f1 || true) + + [[ -z "$sessions" ]] && return 0 + + echo -e "${YELLOW}Tmux sessions:${NC}" + + for session in $sessions; do + echo " Session: $session" + done +} + +# Kill tmux sessions (silent killing) +kill_tmux() { + local pattern="$1" + local sessions=$(tmux list-sessions 2>/dev/null | grep "$pattern" | cut -d: -f1 || true) + + [[ -z "$sessions" ]] && return 0 + + for session in $sessions; do + [[ "$DRY_RUN" != true ]] && tmux kill-session -t "$session" 2>/dev/null + done +} + +# Show processes by port (for preview) +show_processes_by_port() { + local port="$1" desc="$2" + local pids=$(lsof -ti:$port 2>/dev/null || true) + + [[ -z "$pids" ]] && return 0 + + echo -e "${YELLOW}$desc (port $port):${NC}" + + for pid in $pids; do + local cmd=$(ps -p $pid -o cmd= 2>/dev/null || echo "Process not found") + echo " PID $pid: $cmd" + done +} + +# Kill processes by port (silent killing) +kill_by_port() { + local port="$1" desc="$2" + local pids=$(lsof -ti:$port 2>/dev/null || true) + + [[ -z "$pids" ]] && return 0 + + for pid in $pids; do + [[ "$DRY_RUN" != true ]] && kill -TERM $pid 2>/dev/null + done +} + +# Check if any processes exist +has_processes() { + # Check for processes + local has_tmux=$(tmux list-sessions 2>/dev/null | grep "g1_deployment" || true) + local has_control=$(pgrep -f "run_g1_control_loop.py" 2>/dev/null || true) + local has_teleop=$(pgrep -f "run_teleop_policy_loop.py" 2>/dev/null || true) + local has_camera=$(pgrep -f "camera_forwarder.py" 2>/dev/null || true) + local has_rqt=$(pgrep -f "rqt.*image_view" 2>/dev/null || true) + local has_port=$(lsof -ti:5555 2>/dev/null || true) + + [[ -n "$has_tmux" || -n "$has_control" || -n "$has_teleop" || -n "$has_camera" || -n "$has_rqt" || -n "$has_port" ]] +} + +# Main execution +main() { + # Check if any processes exist first + if ! has_processes; then + # No processes to kill, exit silently + exit 0 + fi + + # Show header and processes to be killed + if [[ "$QUIET" != true ]]; then + echo -e "${BLUE}=== decoupled_wbc Process Killer ===${NC}" + [[ "$DRY_RUN" == true ]] && echo -e "${BLUE}=== DRY RUN MODE ===${NC}" + + # Show what will be killed + show_tmux "g1_deployment" + show_processes_by_pattern "run_g1_control_loop.py" "G1 control loop" + show_processes_by_pattern "run_teleop_policy_loop.py" "Teleop policy" + show_processes_by_pattern "camera_forwarder.py" "Camera forwarder" + show_processes_by_pattern "rqt.*image_view" "RQT viewer" + show_processes_by_port "5555" "Inference server" + + # Ask for confirmation + if [[ "$FORCE" != true && "$DRY_RUN" != true ]]; then + echo + echo -e "${RED}WARNING: This will terminate the above decoupled_wbc processes!${NC}" + read -p "Continue? [Y/n]: " -n 1 -r + echo + # Default to Y - only abort if user explicitly types 'n' or 'N' + [[ $REPLY =~ ^[Nn]$ ]] && { echo "Aborted."; exit 0; } + fi + echo + fi + + # Kill processes (silently) + kill_tmux "g1_deployment" + kill_by_pattern "run_g1_control_loop.py" "G1 control loop" + kill_by_pattern "run_teleop_policy_loop.py" "Teleop policy" + kill_by_pattern "camera_forwarder.py" "Camera forwarder" + kill_by_pattern "rqt.*image_view" "RQT viewer" + kill_by_port "5555" "Inference server" + + # Force kill remaining (SIGKILL) + [[ "$DRY_RUN" != true ]] && { + sleep 1 + kill_by_pattern "run_g1_control_loop.py" "G1 control loop" "KILL" + kill_by_pattern "run_teleop_policy_loop.py" "Teleop policy" "KILL" + kill_by_pattern "camera_forwarder.py" "Camera forwarder" "KILL" + } + + # Summary (unless quiet) + [[ "$QUIET" != true ]] && { + if [[ "$DRY_RUN" == true ]]; then + echo -e "${BLUE}=== DRY RUN COMPLETE ===${NC}" + else + echo -e "${GREEN}All decoupled_wbc processes terminated${NC}" + fi + } +} + +main "$@" diff --git a/GR00T-WholeBodyControl/decoupled_wbc/docker/publish.sh b/GR00T-WholeBodyControl/decoupled_wbc/docker/publish.sh new file mode 100644 index 0000000000000000000000000000000000000000..a9d909e8dc30bc627d7270649706a815f050717e --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/docker/publish.sh @@ -0,0 +1,3 @@ +#!/bin/bash +image_name=$(cat image_name.txt) +docker push "$@" $image_name \ No newline at end of file diff --git a/GR00T-WholeBodyControl/decoupled_wbc/docker/run_docker.sh b/GR00T-WholeBodyControl/decoupled_wbc/docker/run_docker.sh new file mode 100644 index 0000000000000000000000000000000000000000..b53db26ff881369d3c12e10f33faaa4c6d0ad333 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/docker/run_docker.sh @@ -0,0 +1,455 @@ +#!/bin/bash + +# Docker run script for decoupled_wbc with branch-based container isolation +# +# Usage: +# ./docker/run_docker.sh [OPTIONS] (run from inside decoupled_wbc/) +# +# Options: +# --build Build Docker image +# --clean Clean containers +# --deploy Run in deploy mode +# --install Pull prebuilt Docker image +# --push Push built image to Docker Hub +# --branch Use branch-specific container names +# +# Branch-based Container Isolation (when --branch flag is used): +# - Each git branch gets its own isolated containers +# - Container names include branch identifier (e.g., decoupled_wbc-deploy-user-main) +# - Works with git worktrees, separate clones, or nested repositories +# - Clean and build operations only affect the current branch + +# Exit on error +set -e + +# Default values +BUILD=false +CLEAN=false +DEPLOY=false +INSTALL=false +# Flag to push the built Docker image to Docker Hub +# This should be used when someone updates the Docker image dependencies +# because this image is used for CI/CD pipelines +# When true, the image will be tagged and pushed to docker.io/nvgear/gr00t_wbc:latest +DOCKER_HUB_PUSH=false +# Flag to build the docker with root user +# This could cause some of your local files to be owned by root +# If you get error like "PermissionError: [Errno 13] Permission denied:" +# You can run `sudo chown -R $USER:$USER .` in local machine to fix it +ROOT=false +BRANCH_MODE=false +EXTRA_ARGS=() +PROJECT_NAME="decoupled_wbc" +PROJECT_SLUG=$(echo "$PROJECT_NAME" | tr '[:upper:]' '[:lower:]') +REMOTE_IMAGE="nvgear/gr00t_wbc:latest" + +# Parse command line arguments +while [[ $# -gt 0 ]]; do + case $1 in + --build) + BUILD=true + shift + ;; + --clean) + CLEAN=true + shift + ;; + --deploy) + DEPLOY=true + shift + ;; + --install) + INSTALL=true + shift + ;; + --push) + DOCKER_HUB_PUSH=true + shift + ;; + --root) + ROOT=true + shift + ;; + --branch) + BRANCH_MODE=true + shift + ;; + *) + # Collect all unknown arguments as extra args for the deployment script + EXTRA_ARGS+=("$1") + shift + ;; + esac +done + +if [ "$INSTALL" = true ] && [ "$BUILD" = true ]; then + echo "Cannot use --install and --build together. Choose one." + exit 1 +fi + + +# Function to get branch name for container naming +function get_branch_id { + # Check if we're in a git repository + if git rev-parse --is-inside-work-tree > /dev/null 2>&1; then + # Get current branch name (returns "HEAD" in detached state) + local branch_name=$(git rev-parse --abbrev-ref HEAD) + # Replace forward slashes with dashes for valid container names + echo "${branch_name//\//-}" + else + # Default: no branch identifier (not in git repo) + echo "" + fi +} + +# Architecture detection helpers +is_arm64() { [ "$(dpkg --print-architecture)" = "arm64" ]; } +is_amd64() { [ "$(dpkg --print-architecture)" = "amd64" ]; } + +# Get current user's username and UID +if [ "$ROOT" = true ]; then + USERNAME=root + USERID=0 + DOCKER_HOME_DIR=/root + CACHE_FROM=${PROJECT_SLUG}-deploy-cache-root +else + USERNAME=$(whoami) + USERID=$(id -u) + DOCKER_HOME_DIR=/home/${USERNAME} + CACHE_FROM=${PROJECT_SLUG}-deploy-cache +fi +# Get input group ID for device access +INPUT_GID=$(getent group input | cut -d: -f3) + +# Get script directory for path calculations +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" + +# Function to get the actual project directory (worktree-aware) +function get_project_dir { + # For worktrees, use the actual worktree root path + if git rev-parse --is-inside-work-tree > /dev/null 2>&1; then + git rev-parse --show-toplevel + else + # Fallback to script-based detection (go up two levels: docker/ -> decoupled_wbc/ -> project root) + dirname "$(dirname "$SCRIPT_DIR")" + fi +} + +# Get branch identifier +BRANCH_ID=$(get_branch_id) + +# Set project directory (needs to be after branch detection) +PROJECT_DIR="$(get_project_dir)" + +# Function to generate container name with optional branch support +function get_container_name { + local container_type="$1" + if [[ -n "$BRANCH_ID" ]] && [[ "$BRANCH_MODE" = true ]]; then + echo "${PROJECT_SLUG}-${container_type}-${USERNAME}-${BRANCH_ID}" + else + echo "${PROJECT_SLUG}-${container_type}-${USERNAME}" + fi +} + +# Set common variables used throughout the script +DEPLOY_CONTAINER=$(get_container_name "deploy") +BASH_CONTAINER=$(get_container_name "bash") +WORKTREE_NAME=$(basename "$PROJECT_DIR") + +# Debug output for branch detection +if [[ -n "$BRANCH_ID" ]] && [[ "$BRANCH_MODE" = true ]]; then + echo "Branch mode enabled - using branch: $BRANCH_ID" + echo "Project directory: $PROJECT_DIR" +elif [[ -n "$BRANCH_ID" ]]; then + echo "Branch mode disabled - using default containers" + echo "Project directory: $PROJECT_DIR" +else + echo "Running outside git repository" + echo "Project directory: $PROJECT_DIR" +fi + +# Get host's hostname and append -docker +HOSTNAME=$(hostname)-docker + +function clean_container { + echo "Cleaning up Docker containers..." + + # Stop containers + sudo docker stop $DEPLOY_CONTAINER 2>/dev/null || true + sudo docker stop $BASH_CONTAINER 2>/dev/null || true + # Remove containers + echo "Removing containers..." + sudo docker rm $DEPLOY_CONTAINER 2>/dev/null || true + sudo docker rm $BASH_CONTAINER 2>/dev/null || true + echo "Containers cleaned!" +} + + +# Function to install Docker Buildx if needed +function install_docker_buildx { + # Check if Docker Buildx is already installed + if sudo docker buildx version &> /dev/null; then + echo "Docker Buildx is already installed." + return 0 + fi + + echo "Installing Docker Buildx..." + + # Create directories and detect architecture + mkdir -p ~/.docker/cli-plugins/ && sudo mkdir -p /root/.docker/cli-plugins/ + ARCH=$(dpkg --print-architecture) + [[ "$ARCH" == "arm64" ]] && BUILDX_ARCH="linux-arm64" || BUILDX_ARCH="linux-amd64" + + # Get version (with fallback) + BUILDX_VERSION=$(curl -s https://api.github.com/repos/docker/buildx/releases/latest | grep tag_name | cut -d '"' -f 4) + BUILDX_VERSION=${BUILDX_VERSION:-v0.13.1} + + # Download and install for both user and root + curl -L "https://github.com/docker/buildx/releases/download/${BUILDX_VERSION}/buildx-${BUILDX_VERSION}.${BUILDX_ARCH}" -o ~/.docker/cli-plugins/docker-buildx + sudo cp ~/.docker/cli-plugins/docker-buildx /root/.docker/cli-plugins/docker-buildx + chmod +x ~/.docker/cli-plugins/docker-buildx && sudo chmod +x /root/.docker/cli-plugins/docker-buildx + + # Create builder + sudo docker buildx create --use --name mybuilder || true + sudo docker buildx inspect --bootstrap + + echo "Docker Buildx installation complete!" +} + +# Function to install NVIDIA Container Toolkit if needed +function install_nvidia_toolkit { + # Check if NVIDIA Container Toolkit is already installed + if command -v nvidia-container-toolkit &> /dev/null; then + echo "NVIDIA Container Toolkit is already installed." + return 0 + fi + + echo "Installing NVIDIA Container Toolkit..." + + # Add the package repositories + distribution=$(. /etc/os-release;echo $ID$VERSION_ID) + + # Check if GPG key exists and remove it if it does + if [ -f "/usr/share/keyrings/nvidia-container-toolkit-keyring.gpg" ]; then + echo "Removing existing NVIDIA GPG key..." + sudo rm /usr/share/keyrings/nvidia-container-toolkit-keyring.gpg + fi + + # Add new GPG key + curl -fsSL https://nvidia.github.io/libnvidia-container/gpgkey | sudo gpg --dearmor -o /usr/share/keyrings/nvidia-container-toolkit-keyring.gpg + + # Add repository + curl -s -L https://nvidia.github.io/nvidia-container-runtime/$distribution/nvidia-container-runtime.list | \ + sed 's#deb https://#deb [signed-by=/usr/share/keyrings/nvidia-container-toolkit-keyring.gpg] https://#g' | \ + sudo tee /etc/apt/sources.list.d/nvidia-container-runtime.list + + # Install nvidia-container-toolkit and docker if needed + sudo apt-get update + sudo apt-get install -y nvidia-container-toolkit + + # Install docker if not already installed + if ! command -v docker &> /dev/null; then + sudo apt-get install -y docker.io + fi + + # Configure Docker to use the NVIDIA runtime + sudo nvidia-ctk runtime configure --runtime=docker + + # Restart the Docker daemon + sudo systemctl restart docker + + echo "NVIDIA Container Toolkit installation complete!" +} + + +# Function to build Docker image for current branch +function build_docker_image { + echo "Building Docker image: $DEPLOY_CONTAINER" + + sudo docker buildx build \ + --build-arg USERNAME=$USERNAME \ + --build-arg USERID=$USERID \ + --build-arg HOME_DIR=$DOCKER_HOME_DIR \ + --build-arg WORKTREE_NAME=$WORKTREE_NAME \ + --cache-from $CACHE_FROM \ + -t $DEPLOY_CONTAINER \ + -f "$SCRIPT_DIR/Dockerfile.deploy" \ + --load \ + "$PROJECT_DIR" + + # Tag for persistent cache + # sudo docker tag $DEPLOY_CONTAINER $CACHE_FROM + echo "Docker image build complete!" +} + +# Build function +function build_with_cleanup { + echo "Building Docker image..." + echo "Removing existing containers and images..." + clean_container + # Tag for persistent cache before deleting the image + sudo docker tag $DEPLOY_CONTAINER $CACHE_FROM 2>/dev/null || true + sudo docker rmi $DEPLOY_CONTAINER 2>/dev/null || true + echo "Images cleaned!" + + install_docker_buildx + install_nvidia_toolkit + build_docker_image +} + +function install_remote_image { + echo "Installing Docker image from remote registry: $REMOTE_IMAGE" + echo "Removing existing containers to ensure a clean install..." + clean_container + sudo docker pull "$REMOTE_IMAGE" + sudo docker tag "$REMOTE_IMAGE" "$DEPLOY_CONTAINER" + sudo docker tag "$REMOTE_IMAGE" "$CACHE_FROM" 2>/dev/null || true + echo "Docker image install complete!" +} + +# Clean up if requested +if [ "$CLEAN" = true ]; then + clean_container + exit 0 +fi + +# Build if requested +if [ "$BUILD" = true ]; then + build_with_cleanup +fi + +if [ "$INSTALL" = true ]; then + install_remote_image +fi + +if [ "$DOCKER_HUB_PUSH" = true ]; then + echo "Pushing Docker image to Docker Hub: docker.io/${REMOTE_IMAGE}" + sudo docker tag $DEPLOY_CONTAINER docker.io/${REMOTE_IMAGE} + sudo docker push docker.io/${REMOTE_IMAGE} + echo "Docker image pushed to Docker Hub!" + exit 0 +fi + +# Setup X11 display forwarding +setup_x11() { + # Set display if missing and X server available + if [ -z "$DISPLAY" ] && command -v xset >/dev/null 2>&1 && xset q >/dev/null 2>&1; then + export DISPLAY=:1 + echo "No DISPLAY set, using :1" + fi + + # Enable X11 forwarding if possible + if [ -n "$DISPLAY" ] && command -v xhost >/dev/null 2>&1 && xhost +local:docker 2>/dev/null; then + echo "X11 forwarding enabled" + return 0 + else + echo "Headless environment - X11 disabled" + export DISPLAY="" + return 1 + fi +} + +X11_ENABLED=false +setup_x11 && X11_ENABLED=true + +# Mount entire /dev directory for dynamic device access (including hidraw for joycon) +# This allows JoyCon controllers to be detected even when connected after container launch +sudo chmod g+r+w /dev/input/* + +# Detect GPU setup and set appropriate environment variables +echo "Detecting GPU setup..." +GPU_ENV_VARS="" + +# Check if we have both integrated and discrete GPUs (hybrid/Optimus setup) +HAS_AMD_GPU=$(lspci | grep -i "vga\|3d\|display" | grep -i amd | wc -l) +HAS_INTEL_GPU=$(lspci | grep -i "vga\|3d\|display" | grep -i intel | wc -l) +HAS_NVIDIA_GPU=$(lspci | grep -i "vga\|3d\|display" | grep -i nvidia | wc -l) + +if [[ "$HAS_INTEL_GPU" -gt 0 ]] || [[ "$HAS_AMD_GPU" -gt 0 ]] && [[ "$HAS_NVIDIA_GPU" -gt 0 ]]; then + echo "Detected hybrid GPU setup (Intel/AMD integrated + NVIDIA discrete)" + echo "Setting NVIDIA Optimus environment variables for proper rendering offload..." + GPU_ENV_VARS="-e __NV_PRIME_RENDER_OFFLOAD=1 \ + -e __VK_LAYER_NV_optimus=NVIDIA_only" +else + GPU_ENV_VARS="" +fi + +# Set GPU runtime based on architecture +if is_arm64; then + echo "Detected ARM64 architecture (Jetson Orin), using device access instead of nvidia runtime..." + GPU_RUNTIME_ARGS="--device /dev/nvidia0 --device /dev/nvidiactl --device /dev/nvidia-modeset --device /dev/nvidia-uvm --device /dev/nvidia-uvm-tools" +else + GPU_RUNTIME_ARGS="--gpus all --runtime=nvidia" +fi + +# Common Docker run parameters +DOCKER_RUN_ARGS="--hostname $HOSTNAME \ + --user $USERNAME \ + --group-add $INPUT_GID \ + $GPU_RUNTIME_ARGS \ + --ipc=host \ + --network=host \ + --privileged \ + --device=/dev \ + $GPU_ENV_VARS \ + -p 5678:5678 \ + -e DISPLAY=$DISPLAY \ + -e NVIDIA_VISIBLE_DEVICES=all \ + -e NVIDIA_DRIVER_CAPABILITIES=graphics,compute,utility \ + -e __GLX_VENDOR_LIBRARY_NAME=nvidia \ + -e USERNAME=$USERNAME \ + -e DECOUPLED_WBC_DIR="$DOCKER_HOME_DIR/Projects/$WORKTREE_NAME" \ + -e PYTHONPATH="$DOCKER_HOME_DIR/Projects/$WORKTREE_NAME" \ + -v /dev/bus/usb:/dev/bus/usb \ + -v /tmp/.X11-unix:/tmp/.X11-unix \ + -v $HOME/.ssh:$DOCKER_HOME_DIR/.ssh \ + -v $HOME/.gear:$DOCKER_HOME_DIR/.gear \ + -v $HOME/.Xauthority:$DOCKER_HOME_DIR/.Xauthority \ + -v $PROJECT_DIR:$DOCKER_HOME_DIR/Projects/$(basename "$PROJECT_DIR") + --device /dev/snd \ + --group-add audio \ + -e PULSE_SERVER=unix:/run/user/$(id -u)/pulse/native \ + -v /run/user/$(id -u)/pulse/native:/run/user/$(id -u)/pulse/native \ + -v $HOME/.config/pulse/cookie:/home/$USERNAME/.config/pulse/cookie" + +# Check if RL mode first, then handle container logic +if [ "$DEPLOY" = true ]; then + # Deploy mode - use decoupled_wbc-deploy-${USERNAME} container + + # Always clean up old processes and create a new container + # Kill all decoupled_wbc processes across containers to prevent message passing conflicts + "$SCRIPT_DIR/kill_decoupled_wbc_processors.sh" + echo "Creating new deploy container..." + + # Clean up old processes and create a fresh deploy container + # Remove existing deploy container if it exists + if sudo docker ps -a --format '{{.Names}}' | grep -q "^$DEPLOY_CONTAINER$"; then + echo "Removing existing deploy container..." + sudo docker rm -f $DEPLOY_CONTAINER + fi + sudo docker run -it --rm $DOCKER_RUN_ARGS \ + -w $DOCKER_HOME_DIR/Projects/$WORKTREE_NAME \ + --name $DEPLOY_CONTAINER \ + $DEPLOY_CONTAINER \ + /bin/bash -ic 'exec "$0" "$@"' \ + "${DOCKER_HOME_DIR}/Projects/${WORKTREE_NAME}/decoupled_wbc/docker/entrypoint/deploy.sh" \ + "${EXTRA_ARGS[@]}" +else + # Bash mode - use decoupled_wbc-bash-${USERNAME} container + if sudo docker ps -a --format '{{.Names}}' | grep -q "^$BASH_CONTAINER$"; then + echo "Bash container exists, starting it..." + sudo docker start $BASH_CONTAINER > /dev/null + sudo docker exec -it $BASH_CONTAINER /bin/bash + else + echo "Creating new bash container with auto-install decoupled_wbc..." + sudo docker run -it $DOCKER_RUN_ARGS \ + -w $DOCKER_HOME_DIR/Projects/$WORKTREE_NAME \ + --name $BASH_CONTAINER \ + $DEPLOY_CONTAINER \ + /bin/bash -ic 'exec "$0"' \ + "${DOCKER_HOME_DIR}/Projects/${WORKTREE_NAME}/decoupled_wbc/docker/entrypoint/bash.sh" + fi +fi + +# Cleanup X11 permissions +$X11_ENABLED && xhost -local:docker 2>/dev/null diff --git a/GR00T-WholeBodyControl/decoupled_wbc/scripts/deploy_g1.py b/GR00T-WholeBodyControl/decoupled_wbc/scripts/deploy_g1.py new file mode 100644 index 0000000000000000000000000000000000000000..b2fa6be9c42f4bf6f10cb90ec1d6e70cb51ebf07 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/scripts/deploy_g1.py @@ -0,0 +1,472 @@ +from pathlib import Path +import signal +import subprocess +import sys +import time + +import tyro + +from decoupled_wbc.control.main.teleop.configs.configs import DeploymentConfig +from decoupled_wbc.control.utils.run_real_checklist import show_deployment_checklist + + +class G1Deployment: + """ + Unified deployment manager for G1 robot with one-click operation. + Handles camera setup, control loop, teleoperation, and data collection. + Uses tmux for process management and I/O handling. + """ + + def __init__(self, config: DeploymentConfig): + self.config = config + + # Process directories + self.project_root = Path(__file__).resolve().parent.parent + + # Tmux session name + self.session_name = "g1_deployment" + + # Create tmux session if it doesn't exist + self._create_tmux_session() + + def _create_tmux_session(self): + """Create a new tmux session if it doesn't exist""" + # Check if session exists + result = subprocess.run( + ["tmux", "has-session", "-t", self.session_name], capture_output=True, text=True + ) + + if result.returncode != 0: + # Create new session + subprocess.run(["tmux", "new-session", "-d", "-s", self.session_name]) + print(f"Created new tmux session: {self.session_name}") + + # Set up the default window for control, data collection, and teleop + # First rename the default window (which is 0) to our desired name + subprocess.run( + ["tmux", "rename-window", "-t", f"{self.session_name}:0", "control_data_teleop"] + ) + # Split the window horizontally (left and right) + subprocess.run(["tmux", "split-window", "-t", f"{self.session_name}:0", "-h"]) + # Split the right pane vertically (top and bottom) + subprocess.run(["tmux", "split-window", "-t", f"{self.session_name}:0.1", "-v"]) + # Select the left pane (control) + subprocess.run(["tmux", "select-pane", "-t", f"{self.session_name}:0.0"]) + + def _run_in_tmux(self, name, cmd, wait_time=2, pane_index=None): + """Run a command in a new tmux window or pane""" + if pane_index is not None: + # Run in existing window's pane + target = f"{self.session_name}:0.{pane_index}" + else: + # Create new window + subprocess.run(["tmux", "new-window", "-t", self.session_name, "-n", name]) + target = f"{self.session_name}:{name}" + + # Set up trap for Ctrl+\ in the window + trap_cmd = f"trap 'tmux kill-session -t {self.session_name}' QUIT" + + # Set environment variable for the tmux session name + env_cmd = f"export DECOUPLED_WBC_TMUX_SESSION={self.session_name}" + + # Construct the command with proper escaping and trap + cmd_str = " ".join(str(x) for x in cmd) + full_cmd = f"{trap_cmd}; {env_cmd}; {cmd_str}" + + # Send command to tmux window/pane + subprocess.run(["tmux", "send-keys", "-t", target, full_cmd, "C-m"]) + + # Wait for process to start + time.sleep(wait_time) + + # Check if process is still running + result = subprocess.run( + ["tmux", "list-panes", "-t", target, "-F", "#{pane_dead}"], + capture_output=True, + text=True, + ) + + if result.stdout.strip() == "1": + print(f"ERROR: {name} failed to start!") + return False + + return True + + def start_camera_sensor(self): + """Start the camera sensor in local mode if we are using replay video""" + if self.config.egoview_replay_dummy is None and self.config.head_replay_dummy is None: + return + + print("Starting camera sensor in local mode...") + cmd = [ + sys.executable, + str(self.project_root / "control/sensor/composed_camera.py"), + "--egoview_camera", + self.config.egoview_replay_dummy, + "--head_camera", + self.config.head_replay_dummy, + "--port", + str(self.config.camera_port), + "--host", + "localhost", + ] + + if not self._run_in_tmux("camera_sensor", cmd): + print("ERROR: Camera sensor failed to start!") + print("Continuing without camera sensor...") + else: + print("Camera sensor started successfully.") + + def start_camera_viewer(self): + """Start the ROS rqt camera viewer""" + if not self.config.view_camera: + return + + print("Starting camera viewer...") + # Use rqt directly instead of ros2 run + cmd = [ + sys.executable, + str(self.project_root / "control/main/teleop/run_camera_viewer.py"), + "--camera_host", + self.config.camera_host, + "--camera_port", + str(self.config.camera_port), + "--fps", + str(self.config.fps), + ] + + if not self._run_in_tmux("camera_viewer", cmd): + print("ERROR: Camera viewer failed to start!") + print("Continuing without camera viewer...") + else: + print("Camera viewer started successfully.") + + def start_sim_loop(self): + """Start the simulation loop in a separate process""" + print("Starting simulation loop...") + cmd = [ + sys.executable, + str(self.project_root / "control/main/teleop/run_sim_loop.py"), + "--wbc_version", + self.config.wbc_version, + "--interface", + self.config.interface, + "--simulator", + self.config.simulator, + "--sim_frequency", + str(self.config.sim_frequency), + "--env_name", + self.config.env_name, + "--camera_port", + str(self.config.camera_port), + ] + + # Handle boolean flags + if self.config.enable_waist: + cmd.append("--enable_waist") + else: + cmd.append("--no-enable_waist") + + if self.config.with_hands: + cmd.append("--with_hands") + else: + cmd.append("--no-with_hands") + + if self.config.image_publish: + cmd.append("--enable_image_publish") + cmd.append("--enable_offscreen") + else: + cmd.append("--no-enable_image_publish") + + if self.config.enable_onscreen: + cmd.append("--enable_onscreen") + else: + cmd.append("--no-enable_onscreen") + + if not self._run_in_tmux("sim_loop", cmd, wait_time=5): + print("ERROR: Simulation loop failed to start!") + self.cleanup() + sys.exit(1) + + print("Simulation loop started successfully. Waiting for warmup for 10 seconds...") + time.sleep(10) # Wait for sim loop to warm up + + def start_control_loop(self): + """Start the G1 control loop""" + print("Starting G1 control loop...") + cmd = [ + sys.executable, + str(self.project_root / "control/main/teleop/run_g1_control_loop.py"), + "--wbc_version", + self.config.wbc_version, + "--wbc_model_path", + self.config.wbc_model_path, + "--wbc_policy_class", + self.config.wbc_policy_class, + "--interface", + self.config.interface, + "--simulator", + "None" if self.config.sim_in_single_process else self.config.simulator, + "--control_frequency", + str(self.config.control_frequency), + ] + + # Handle boolean flag using presence/absence pattern + if self.config.enable_waist: + cmd.append("--enable_waist") + else: + cmd.append("--no-enable_waist") + + if self.config.with_hands: + cmd.append("--with_hands") + else: + cmd.append("--no-with_hands") + + if self.config.high_elbow_pose: + cmd.append("--high_elbow_pose") + else: + cmd.append("--no-high_elbow_pose") + + # Gravity compensation configuration + # Note: This is where gravity compensation is actually applied since the control loop + # contains the G1Body that interfaces directly with the robot motors + if self.config.enable_gravity_compensation: + cmd.append("--enable_gravity_compensation") + # Add joint groups if specified + if self.config.gravity_compensation_joints: + cmd.extend( + ["--gravity_compensation_joints"] + self.config.gravity_compensation_joints + ) + else: + cmd.append("--no-enable_gravity_compensation") + + if not self._run_in_tmux("control", cmd, wait_time=3, pane_index=0): + print("ERROR: Control loop failed to start!") + self.cleanup() + sys.exit(1) + + print("Control loop started successfully.") + print("Controls: 'i' for initial pose, ']' to activate locomotion") + + def start_policy(self): + """Start either teleop or inference policy based on configuration""" + if not self.config.enable_upper_body_operation: + print("Upper body operation disabled in config.") + return + + self.start_teleop() + + def start_teleop(self): + """Start the teleoperation policy""" + print("Starting teleoperation policy...") + cmd = [ + sys.executable, + str(self.project_root / "control/main/teleop/run_teleop_policy_loop.py"), + "--body_control_device", + self.config.body_control_device, + "--hand_control_device", + self.config.hand_control_device, + "--body_streamer_ip", + self.config.body_streamer_ip, + "--body_streamer_keyword", + self.config.body_streamer_keyword, + ] + + # Handle boolean flags using tyro syntax + if self.config.enable_waist: + cmd.append("--enable_waist") + else: + cmd.append("--no-enable_waist") + + if self.config.high_elbow_pose: + cmd.append("--high_elbow_pose") + else: + cmd.append("--no-high_elbow_pose") + + if self.config.enable_visualization: + cmd.append("--enable_visualization") + else: + cmd.append("--no-enable_visualization") + + if self.config.enable_real_device: + cmd.append("--enable_real_device") + else: + cmd.append("--no-enable_real_device") + + if not self._run_in_tmux("teleop", cmd, pane_index=2): + print("ERROR: Teleoperation policy failed to start!") + print("Continuing without teleoperation...") + else: + print("Teleoperation policy started successfully.") + print("Press 'l' in the control loop terminal to start teleoperation.") + + def start_data_collection(self): + """Start the data collection process""" + if not self.config.data_collection: + print("Data collection disabled in config.") + return + + print("Starting data collection...") + cmd = [ + sys.executable, + str(self.project_root / "control/main/teleop/run_g1_data_exporter.py"), + "--data_collection_frequency", + str(self.config.data_collection_frequency), + "--root_output_dir", + self.config.root_output_dir, + "--lower_body_policy", + self.config.wbc_version, + "--wbc_model_path", + self.config.wbc_model_path, + "--camera_host", + self.config.camera_host, + "--camera_port", + str(self.config.camera_port), + ] + + if not self._run_in_tmux("data", cmd, pane_index=1): + print("ERROR: Data collection failed to start!") + print("Continuing without data collection...") + else: + print("Data collection started successfully.") + print("Press 'c' in the control loop terminal to start/stop recording data.") + + def start_webcam_recording(self): + """Start webcam recording for real robot deployment monitoring""" + if not self.config.enable_webcam_recording or self.config.env_type != "real": + return + + print("Starting webcam recording for deployment monitoring...") + cmd = [ + sys.executable, + str(self.project_root / "scripts/run_webcam_recorder.py"), + "--output_dir", + self.config.webcam_output_dir, + ] + + if not self._run_in_tmux("webcam", cmd): + print("ERROR: Webcam recording failed to start!") + print("Continuing without webcam recording...") + else: + print("Webcam recording started successfully.") + print("External camera recording deployment activities to logs_experiment/") + + def deploy(self): + """ + Run the complete deployment process + """ + print("Starting G1 deployment with config:") + print(f" Robot IP: {self.config.robot_ip}") + print(f" WBC Version: {self.config.wbc_version}") + print(f" Interface: {self.config.interface}") + print(f" Policy Mode: {self.config.upper_body_operation_mode}") + print(f" With Hands: {self.config.with_hands}") + print(f" View Camera: {self.config.view_camera}") + print(f" Enable Waist: {self.config.enable_waist}") + print(f" High Elbow Pose: {self.config.high_elbow_pose}") + print(f" Gravity Compensation: {self.config.enable_gravity_compensation}") + if self.config.enable_gravity_compensation: + print(f" Gravity Comp Joints: {self.config.gravity_compensation_joints}") + print( + f" Webcam Recording: {self.config.enable_webcam_recording and self.config.env_type == 'real'}" + ) + print(f" Sim in Single Process: {self.config.sim_in_single_process}") + if self.config.sim_in_single_process: + print(f" Image Publish: {self.config.image_publish}") + + # Check if this is a real robot deployment and run safety checklist + if self.config.env_type == "real": + if not show_deployment_checklist(): + sys.exit(1) + + # Register signal handler for clean shutdown + signal.signal(signal.SIGINT, self.signal_handler) + + # Start components in sequence + # Start sim loop first if sim_in_single_process is enabled + if self.config.sim_in_single_process: + self.start_sim_loop() + + self.start_control_loop() + self.start_camera_viewer() + self.start_policy() # This will start either teleop or inference policy + self.start_data_collection() + self.start_webcam_recording() # Only runs for real robot deployment + + print("\n--- G1 DEPLOYMENT COMPLETE ---") + print("All systems running in tmux session:", self.session_name) + print("Press Ctrl+b then d to detach from the session") + print("Press Ctrl+\\ in any window to shutdown all components.") + + try: + # Automatically attach to the tmux session and switch to control window + subprocess.run( + [ + "tmux", + "attach", + "-t", + self.session_name, + ";", + "select-window", + "-t", + "control_data_teleop", + ] + ) + except KeyboardInterrupt: + print("\nShutdown requested...") + self.cleanup() + sys.exit(0) + + # Keep main thread alive to handle signals + try: + while True: + # Check if tmux session still exists + result = subprocess.run( + ["tmux", "has-session", "-t", self.session_name], capture_output=True, text=True + ) + + if result.returncode != 0: + print("Tmux session terminated. Exiting.") + break + + time.sleep(1) + except KeyboardInterrupt: + print("\nShutdown requested...") + finally: + self.cleanup() + + def cleanup(self): + """Clean up tmux session""" + print("Cleaning up tmux session...") + try: + # Kill the tmux session + subprocess.run(["tmux", "kill-session", "-t", self.session_name], timeout=5) + print("Tmux session terminated successfully.") + except subprocess.TimeoutExpired: + print("Warning: Tmux session termination timed out, forcing kill...") + subprocess.run(["tmux", "kill-session", "-t", self.session_name, "-9"]) + except Exception as e: + print(f"Warning: Error during cleanup: {e}") + print("Cleanup complete.") + + def signal_handler(self, sig, frame): + """Handle SIGINT (Ctrl+C) gracefully""" + print("\nShutdown signal received...") + self.cleanup() + sys.exit(0) + + +def main(): + """Main entry point with automatic CLI generation from G1Config dataclass""" + # This single line automatically generates a complete CLI from the dataclass! + config = tyro.cli(DeploymentConfig) + + # Run deployment with the configured settings + deployment = G1Deployment(config) + deployment.deploy() + + +if __name__ == "__main__": + # Edited outside docker + + main() diff --git a/GR00T-WholeBodyControl/decoupled_wbc/scripts/leap_tracking_example.py b/GR00T-WholeBodyControl/decoupled_wbc/scripts/leap_tracking_example.py new file mode 100644 index 0000000000000000000000000000000000000000..04fde4b504afc4307f3fc221450166406d8ccf43 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/scripts/leap_tracking_example.py @@ -0,0 +1,50 @@ +"""Prints the palm position of each hand, every frame. When a device is +connected we set the tracking mode to desktop and then generate logs for +every tracking frame received. The events of creating a connection to the +server and a device being plugged in also generate logs. +""" + +import time + +import leap + + +class MyListener(leap.Listener): + def on_connection_event(self, event): + print("Connected") + + def on_device_event(self, event): + try: + with event.device.open(): + info = event.device.get_info() + except leap.LeapCannotOpenDeviceError: + info = event.device.get_info() + + print(f"Found device {info.serial}") + + def on_tracking_event(self, event): + print(f"Frame {event.tracking_frame_id} with {len(event.hands)} hands.") + for hand in event.hands: + hand_type = "left" if str(hand.type) == "HandType.Left" else "right" + print( + f"Hand id {hand.id} is a {hand_type} hand with position ({hand.palm.position.x}, " + f"{hand.palm.position.y}, {hand.palm.position.z})." + ) + + +def main(): + my_listener = MyListener() + + connection = leap.Connection() + connection.add_listener(my_listener) + + running = True + + with connection.open(): + connection.set_tracking_mode(leap.TrackingMode.Desktop) + while running: + time.sleep(1) + + +if __name__ == "__main__": + main() diff --git a/GR00T-WholeBodyControl/decoupled_wbc/scripts/run_webcam_recorder.py b/GR00T-WholeBodyControl/decoupled_wbc/scripts/run_webcam_recorder.py new file mode 100644 index 0000000000000000000000000000000000000000..18e1965d93bd509ca02519293c57da306162ea34 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/scripts/run_webcam_recorder.py @@ -0,0 +1,392 @@ +#!/usr/bin/env python3 +""" +Simple webcam recorder with optional preview window. +Usage: + webcam_recorder.py [--output-dir DIR] [--no-preview] + webcam_recorder.py --help +""" + +import argparse +from datetime import datetime +import os +from pathlib import Path +import signal +import subprocess +import sys +import time + + +class WebcamRecorder: + def __init__(self, debug=False): + self.process = None + self.debug = debug + + def _debug_print(self, message): + """Print debug message only if debug mode is enabled.""" + if self.debug: + print(f"[webcam_recorder] {message}", file=sys.stderr) + + def _info_print(self, message): + """Print info message always.""" + print(f"[webcam_recorder] {message}", file=sys.stderr) + + def find_webcam(self): + """Find available webcam device.""" + # Check container status only in debug mode + if self.debug and (os.path.exists("/.dockerenv") or os.environ.get("CONTAINER")): + self._debug_print("Running in Docker container") + # Check if we have video group permissions + try: + import grp + + groups = os.getgroups() + video_gid = grp.getgrnam("video").gr_gid + if video_gid in groups: + self._debug_print("Has video group permissions ✓") + else: + self._debug_print("Warning: Not in video group") + except Exception: + pass + + # Check /dev/v4l/by-id for Logitech webcam first + by_id_path = Path("/dev/v4l/by-id") + if by_id_path.exists(): + self._debug_print("Checking /dev/v4l/by-id for devices...") + for device in by_id_path.iterdir(): + if device.is_symlink(): + device_name = device.name + if "Logitech" in device_name or "046d" in device_name: + resolved_device = str(device.resolve()) + self._debug_print( + f"Found Logitech device: {device_name} -> {resolved_device}" + ) + if self._is_video_capture_device(resolved_device): + return resolved_device + + # No Logitech found, try any webcam + for device in by_id_path.iterdir(): + if device.is_symlink() and "metadata" not in device.name: + resolved_device = str(device.resolve()) + self._debug_print(f"Checking device: {device.name} -> {resolved_device}") + if self._is_video_capture_device(resolved_device): + return resolved_device + + # Fallback to /dev/video* - prioritize external cameras + self._debug_print("Scanning /dev/video* devices...") + external_devices = [] + integrated_devices = [] + + for i in range(20): + device = f"/dev/video{i}" + if os.path.exists(device): + # Check if we can access the device + if not os.access(device, os.R_OK): + self._debug_print(f"Found {device} but no read access") + continue + + # Check if it's a video capture device (not metadata) + if self._is_video_capture_device(device): + # Check device name to prioritize external cameras + device_name = self._get_device_name(device) + if device_name: + if any( + keyword in device_name.lower() + for keyword in ["logitech", "brio", "c920", "c930", "c925"] + ): + self._info_print(f"Found external camera: {device} ({device_name})") + external_devices.append(device) + elif ( + "integrated" in device_name.lower() or "internal" in device_name.lower() + ): + self._debug_print(f"Found integrated camera: {device} ({device_name})") + integrated_devices.append(device) + else: + self._debug_print(f"Found unknown camera: {device} ({device_name})") + external_devices.append(device) # Assume external if unknown + else: + self._debug_print(f"Found camera: {device} (no name info)") + external_devices.append(device) + + # Return external camera first, then integrated as fallback + if external_devices: + self._info_print(f"Using external camera: {external_devices[0]}") + return external_devices[0] + elif integrated_devices: + self._info_print( + "No external camera found - integrated camera available but not preferred" + ) + self._info_print( + "Please connect an external camera (Logitech, Brio, etc.) for recording" + ) + return None + + return None + + def _get_device_name(self, device): + """Get the friendly name of a video device.""" + try: + # Extract device number from /dev/videoX + device_num = device.split("video")[-1] + name_path = f"/sys/class/video4linux/video{device_num}/name" + if os.path.exists(name_path): + with open(name_path, "r") as f: + return f.read().strip() + except Exception: + pass + return None + + def _is_video_capture_device(self, device): + """Check if a device is a video capture device (not metadata).""" + try: + # Try v4l2-ctl first if available + if subprocess.run(["which", "v4l2-ctl"], capture_output=True).returncode == 0: + result = subprocess.run( + ["v4l2-ctl", "--device=" + device, "--info"], capture_output=True, timeout=2 + ) + if result.returncode == 0: + output = result.stdout.decode() + # Look for "Video Capture" capability in Device Caps, not just general Capabilities + lines = output.split("\n") + in_device_caps = False + for line in lines: + if "Device Caps" in line: + in_device_caps = True + continue + if in_device_caps: + if line.strip().startswith("Video Capture"): + # Double-check with ffmpeg + ffmpeg_result = subprocess.run( + [ + "ffmpeg", + "-f", + "v4l2", + "-i", + device, + "-frames:v", + "1", + "-f", + "null", + "-", + ], + capture_output=True, + timeout=2, + ) + return ffmpeg_result.returncode == 0 + # Stop checking if we hit another section + elif line.strip() and not line.startswith("\t"): + break + + # Fallback: try ffmpeg directly if v4l2-ctl not available + self._debug_print(f"Testing {device} with ffmpeg...") + ffmpeg_result = subprocess.run( + [ + "ffmpeg", + "-f", + "v4l2", + "-i", + device, + "-frames:v", + "1", + "-f", + "null", + "-", + ], + capture_output=True, + timeout=5, + ) + if ffmpeg_result.returncode == 0: + self._debug_print(f"{device} is a working video capture device") + return True + else: + stderr_output = ffmpeg_result.stderr.decode() + # Check if it's just a metadata device + if ( + "metadata" in stderr_output.lower() + or "not a capture device" in stderr_output.lower() + ): + self._debug_print(f"{device} is metadata device, skipping") + return False + else: + self._debug_print(f"{device} failed ffmpeg test: {stderr_output[:100]}") + return False + except Exception as e: + self._debug_print(f"Error testing {device}: {e}") + pass + return False + + def test_camera_format(self, device): + """Test camera and determine best format.""" + # Try default format + result = subprocess.run( + ["ffmpeg", "-f", "v4l2", "-i", device, "-frames:v", "1", "-f", "null", "-"], + capture_output=True, + ) + if result.returncode == 0: + return [] # Default format works + + # Try MJPEG format + result = subprocess.run( + [ + "ffmpeg", + "-f", + "v4l2", + "-input_format", + "mjpeg", + "-i", + device, + "-frames:v", + "1", + "-f", + "null", + "-", + ], + capture_output=True, + ) + if result.returncode == 0: + return ["-input_format", "mjpeg"] + + raise RuntimeError(f"Cannot access camera at {device}") + + def record(self, output_dir="./logs_experiment", preview=True): + """Start recording from webcam.""" + # Find webcam + device = self.find_webcam() + if not device: + self._info_print("No webcam found") + return False + + self._info_print(f"Recording from: {device}") + + # Test camera format + try: + format_args = self.test_camera_format(device) + except RuntimeError as e: + self._info_print(str(e)) + return False + + # Create output directory with date subfolder and filename + now = datetime.now() + date_folder = now.strftime("%Y_%m_%d") + output_path = Path(output_dir) / date_folder + output_path.mkdir(parents=True, exist_ok=True) + timestamp = now.strftime("%Y_%m_%d_%H_%M_%S") + output_file = output_path / f"robot_video_{timestamp}.mp4" + + self._info_print(f"Saving to: {output_file}") + if self.debug: + self._debug_print(f"Absolute path: {output_file.absolute()}") + + # Check if preview is possible + if preview and not os.environ.get("DISPLAY"): + self._debug_print("No DISPLAY found, disabling preview") + preview = False + + # Build ffmpeg command + cmd = ( + ["ffmpeg", "-loglevel", "error", "-f", "v4l2"] + + format_args + + ["-i", device, "-c:v", "libx264", "-preset", "ultrafast"] + ) + + if preview: + # Try with preview first + preview_cmd = cmd + ["-f", "tee", "-map", "0:v", f"[f=mp4]{output_file}|[f=nut]pipe:"] + + try: + ffmpeg_proc = subprocess.Popen( + preview_cmd, stdout=subprocess.PIPE, stderr=subprocess.DEVNULL + ) + ffplay_proc = subprocess.Popen( + [ + "ffplay", + "-loglevel", + "quiet", + "-f", + "nut", + "-i", + "pipe:", + "-window_title", + "Webcam Preview", + ], + stdin=ffmpeg_proc.stdout, + stdout=subprocess.DEVNULL, + stderr=subprocess.DEVNULL, + ) + + # Wait a bit to see if it starts successfully + time.sleep(0.5) + if ffmpeg_proc.poll() is None and ffplay_proc.poll() is None: + self.process = ffplay_proc # Kill ffplay to stop both + self._info_print("Recording with preview started") + return True + else: + self._debug_print("Preview failed, falling back to no-preview mode") + # Clean up failed processes + try: + ffmpeg_proc.kill() + ffplay_proc.kill() + except Exception: + pass + except Exception as e: + self._debug_print(f"Preview error: {e}, using no-preview mode") + + # Record without preview + cmd.append(str(output_file)) + self.process = subprocess.Popen(cmd, stdout=subprocess.DEVNULL, stderr=subprocess.PIPE) + + # Verify it's running + time.sleep(0.5) + if self.process.poll() is None: + self._info_print("Recording started (no preview)") + return True + else: + stderr = self.process.stderr.read().decode() if self.process.stderr else "" + self._info_print(f"Failed to start recording: {stderr}") + return False + + def stop(self): + """Stop recording.""" + if self.process: + self.process.terminate() + try: + self.process.wait(timeout=5) + except subprocess.TimeoutExpired: + self.process.kill() + self._info_print("Recording stopped") + self.process = None + + +def main(): + parser = argparse.ArgumentParser(description="Simple webcam recorder") + parser.add_argument( + "--output-dir", default="./logs_experiment", help="Output directory for recordings" + ) + parser.add_argument("--no-preview", action="store_true", help="Disable preview window") + parser.add_argument("--test", action="store_true", help="Enable debug output") + args = parser.parse_args() + + recorder = WebcamRecorder(debug=args.test) + + # Set up signal handlers for clean shutdown + def signal_handler(signum, frame): + print("\nStopping recording...", file=sys.stderr) + recorder.stop() + sys.exit(0) + + signal.signal(signal.SIGINT, signal_handler) + signal.signal(signal.SIGTERM, signal_handler) + + # Start recording + if recorder.record(args.output_dir, preview=not args.no_preview): + print("Press Ctrl+C to stop recording", file=sys.stderr) + # Keep running until interrupted + try: + signal.pause() + except KeyboardInterrupt: + pass + else: + sys.exit(1) + + +if __name__ == "__main__": + main() diff --git a/GR00T-WholeBodyControl/decoupled_wbc/sim2mujoco/.gitattributes b/GR00T-WholeBodyControl/decoupled_wbc/sim2mujoco/.gitattributes new file mode 100644 index 0000000000000000000000000000000000000000..c94badc6679d5e0d0f3d067543d6aa329df62b89 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/sim2mujoco/.gitattributes @@ -0,0 +1 @@ +*.stl filter=lfs diff=lfs merge=lfs -text diff --git a/GR00T-WholeBodyControl/decoupled_wbc/sim2mujoco/.gitignore b/GR00T-WholeBodyControl/decoupled_wbc/sim2mujoco/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..c01bd8c24797ba34341797d59e804c99da7016a8 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/sim2mujoco/.gitignore @@ -0,0 +1 @@ +*/__pycache__/ diff --git a/GR00T-WholeBodyControl/decoupled_wbc/sim2mujoco/README.md b/GR00T-WholeBodyControl/decoupled_wbc/sim2mujoco/README.md new file mode 100644 index 0000000000000000000000000000000000000000..588b2a4f5eac2567b6bbe2b749510d4076ba283b --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/sim2mujoco/README.md @@ -0,0 +1,5 @@ +urdf to mjcf: +``` +pip install urdf2mjcf +urdf2mjcf path_to_urdf +``` \ No newline at end of file diff --git a/GR00T-WholeBodyControl/decoupled_wbc/sim2mujoco/requirements.txt b/GR00T-WholeBodyControl/decoupled_wbc/sim2mujoco/requirements.txt new file mode 100644 index 0000000000000000000000000000000000000000..40bc58afbb3313013234e3d845fbfd3c3d2dd728 --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/sim2mujoco/requirements.txt @@ -0,0 +1,8 @@ +mujoco==3.3.4 +matplotlib==3.10.3 +numpy==2.2.6 +trimesh +ipdb +open3d +onnxruntime +keyboard \ No newline at end of file diff --git a/GR00T-WholeBodyControl/decoupled_wbc/tests/conftest.py b/GR00T-WholeBodyControl/decoupled_wbc/tests/conftest.py new file mode 100644 index 0000000000000000000000000000000000000000..d614373289a85f3d006369b3da67029e71d8cc3b --- /dev/null +++ b/GR00T-WholeBodyControl/decoupled_wbc/tests/conftest.py @@ -0,0 +1,15 @@ +import pytest + + +def pytest_addoption(parser): + parser.addoption( + "--tensorboard-log-dir", + action="store", + default="logs/Gr00t_TRL_loco/.tensorboard", + help="Directory containing tensorboard logs", + ) + + +@pytest.fixture +def tensorboard_log_dir(request): + return request.config.getoption("--tensorboard-log-dir") diff --git a/GR00T-WholeBodyControl/docs/source/.nojekyll b/GR00T-WholeBodyControl/docs/source/.nojekyll new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/GR00T-WholeBodyControl/docs/source/_static/NVIDIA-logo-black.png b/GR00T-WholeBodyControl/docs/source/_static/NVIDIA-logo-black.png new file mode 100644 index 0000000000000000000000000000000000000000..12e95458e7c22ec8a63f14640cd012c8ea065e00 Binary files /dev/null and b/GR00T-WholeBodyControl/docs/source/_static/NVIDIA-logo-black.png differ diff --git a/GR00T-WholeBodyControl/docs/source/_static/Pipeline.jpg b/GR00T-WholeBodyControl/docs/source/_static/Pipeline.jpg new file mode 100644 index 0000000000000000000000000000000000000000..51989f0cf1f0ff5137993ad03eb890f84fe2323b Binary files /dev/null and b/GR00T-WholeBodyControl/docs/source/_static/Pipeline.jpg differ diff --git a/GR00T-WholeBodyControl/docs/source/_static/css/custom.css b/GR00T-WholeBodyControl/docs/source/_static/css/custom.css new file mode 100644 index 0000000000000000000000000000000000000000..d2e8f2bdf93b5a3776560386a41dd6cf8b4a906d --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/_static/css/custom.css @@ -0,0 +1,102 @@ +/* + * NVIDIA-styled theme for GR00T-WholeBodyControl + * Based on IsaacLab documentation styling + * Colors reference: https://clrs.cc/ + */ + +/* anything related to the light theme */ +html[data-theme="light"] { + --pst-color-primary: #76B900; + --pst-color-secondary: #5b8e03; + --pst-color-secondary-highlight: #5b8e03; + --pst-color-inline-code-links: #76B900; + --pst-color-info: var(--pst-color-primary); + --pst-color-info-highlight: var(--pst-color-primary); + --pst-color-info-bg: #daedb9; + --pst-color-attention: #ffc107; + --pst-color-text-base: #323232; + --pst-color-text-muted: #646464; + --pst-color-shadow: #d8d8d8; + --pst-color-border: #c9c9c9; + --pst-color-inline-code: #76B900; + --pst-color-target: #fbe54e; + --pst-color-background: #fff; + --pst-color-on-background: #fff; + --pst-color-surface: #f5f5f5; + --pst-color-on-surface: #e1e1e1; + --pst-color-link: var(--pst-color-primary); + --pst-color-link-hover: #789841; + --pst-color-table-row-hover-bg: #daedb9; + --pst-color-accent: var(--pst-color-primary); +} + +/* anything related to the dark theme */ +html[data-theme="dark"] { + --pst-color-primary: #76B900; + --pst-color-secondary: #c2f26f; + --pst-color-secondary-highlight: #c2f26f; + --pst-color-inline-code-links: #b6e664; + --pst-color-info: var(--pst-color-primary); + --pst-color-info-highlight: var(--pst-color-primary); + --pst-color-info-bg: #3a550b; + --pst-color-attention: #dca90f; + --pst-color-text-base: #cecece; + --pst-color-text-muted: #a6a6a6; + --pst-color-shadow: #212121; + --pst-color-border: silver; + --pst-color-inline-code: #76B900; + --pst-color-target: #472700; + --pst-color-background: #121212; + --pst-color-on-background: #1e1e1e; + --pst-color-surface: #212121; + --pst-color-on-surface: #373737; + --pst-color-link: var(--pst-color-primary); + --pst-color-link-hover: #aee354; + --pst-color-table-row-hover-bg: #3a550b; + --pst-color-accent: var(--pst-color-primary); +} + +a { + text-decoration: none !important; +} + +/* for the announcement link */ +.bd-header-announcement a, +.bd-header-version-warning a { + color: #7FDBFF; +} + +/* for the search box in the navbar */ +.form-control { + border-radius: 0 !important; + border: none !important; + outline: none !important; +} + +/* reduce padding for logo */ +.navbar-brand { + padding-top: 0.0rem !important; + padding-bottom: 0.0rem !important; +} + +.navbar-icon-links { + padding-top: 0.0rem !important; + padding-bottom: 0.0rem !important; +} + +/* Remove "Built with Sphinx" footer */ +.footer-item__copyright, +.footer-item__theme { + display: none !important; +} + +/* Hide Sphinx attribution in footer */ +div.footer-item:has(.theme-switcher) ~ div.footer-item { + display: none !important; +} + +/* Cleaner footer styling */ +footer.bd-footer { + border-top: 1px solid var(--pst-color-border); + padding-top: 1rem; +} diff --git a/GR00T-WholeBodyControl/docs/source/_static/favicon.ico b/GR00T-WholeBodyControl/docs/source/_static/favicon.ico new file mode 100644 index 0000000000000000000000000000000000000000..7b39f8af7af0831df98713dd0c28347049e4041f Binary files /dev/null and b/GR00T-WholeBodyControl/docs/source/_static/favicon.ico differ diff --git a/GR00T-WholeBodyControl/docs/source/api/index.md b/GR00T-WholeBodyControl/docs/source/api/index.md new file mode 100644 index 0000000000000000000000000000000000000000..991b7c36ce265e225c9e77e43043cac83154588e --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/api/index.md @@ -0,0 +1,3 @@ +# API Reference + +Coming soon... diff --git a/GR00T-WholeBodyControl/docs/source/api/teleop.md b/GR00T-WholeBodyControl/docs/source/api/teleop.md new file mode 100644 index 0000000000000000000000000000000000000000..32f9c302221bb694ced8bcf88abd056d4940725d --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/api/teleop.md @@ -0,0 +1,3 @@ +# Teleoperation API + +Coming soon... diff --git a/GR00T-WholeBodyControl/docs/source/conf.py b/GR00T-WholeBodyControl/docs/source/conf.py new file mode 100644 index 0000000000000000000000000000000000000000..f80cabaf1dc4ab528ff7597a04c14dbcd119e040 --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/conf.py @@ -0,0 +1,145 @@ +# Configuration file for the Sphinx documentation builder. +# +# For the full list of built-in configuration values, see the documentation: +# https://www.sphinx-doc.org/en/master/usage/configuration.html + +import os +import sys + +# -- Project information ----------------------------------------------------- + +project = 'GR00T-WholeBodyControl' +copyright = '2026, NVIDIA' +author = 'NVIDIA GEAR Team' +release = '1.0.0' +version = '1.0' + +# -- General configuration --------------------------------------------------- + +extensions = [ + 'autodocsumm', + 'myst_parser', + 'sphinx.ext.napoleon', + 'sphinxemoji.sphinxemoji', + 'sphinx.ext.autodoc', + 'sphinx.ext.autosummary', + 'sphinx.ext.githubpages', + 'sphinx.ext.intersphinx', + 'sphinx.ext.mathjax', + 'sphinx.ext.todo', + 'sphinx.ext.viewcode', + 'sphinxcontrib.bibtex', + 'sphinx_copybutton', + 'sphinx_design', + 'sphinxcontrib.video', +] + +# mathjax hacks +mathjax3_config = { + "tex": { + "inlineMath": [["\\(", "\\)"]], + "displayMath": [["\\[", "\\]"]], + }, +} + +# emoji style +sphinxemoji_style = "twemoji" + +# supported file extensions for source files +source_suffix = { + '.rst': 'restructuredtext', + '.md': 'markdown', +} + +# BibTeX configuration +bibtex_bibfiles = [] + +# generate autosummary even if no references +autosummary_generate = True +autosummary_generate_overwrite = False + +# generate links to the documentation of objects in external projects +intersphinx_mapping = { + 'python': ('https://docs.python.org/3', None), + 'numpy': ('https://numpy.org/doc/stable/', None), + 'torch': ('https://pytorch.org/docs/stable/', None), +} + +templates_path = ['_templates'] +exclude_patterns = ['_build', '_templates', 'Thumbs.db', '.DS_Store'] + +# List of zero or more Sphinx-specific warning categories to be squelched +suppress_warnings = [ + "ref.python", +] + +# -- MyST Parser configuration ----------------------------------------------- + +myst_enable_extensions = [ + "colon_fence", + "deflist", + "tasklist", +] + +# -- Options for HTML output ------------------------------------------------- + +import sphinx_book_theme + +html_title = "GR00T-WholeBodyControl Documentation" +html_theme_path = [sphinx_book_theme.get_html_theme_path()] +html_theme = "sphinx_book_theme" +html_favicon = "_static/favicon.ico" +html_show_copyright = True +html_show_sphinx = False # This removes "Built with Sphinx" footer +html_last_updated_fmt = "" + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +html_static_path = ["_static"] +html_css_files = ["css/custom.css"] + +html_theme_options = { + "path_to_docs": "docs/", + "collapse_navigation": True, + "repository_url": "https://github.com/NVlabs/GR00T-WholeBodyControl", + "use_repository_button": True, + "use_issues_button": True, + "use_edit_page_button": True, + "show_toc_level": 1, + "use_sidenotes": True, + "logo": { + "text": "GR00T-WholeBodyControl Documentation", + "image_light": "_static/NVIDIA-logo-white.png", + "image_dark": "_static/NVIDIA-logo-black.png", + }, + "icon_links": [ + { + "name": "GitHub", + "url": "https://github.com/NVlabs/GR00T-WholeBodyControl", + "icon": "fa-brands fa-square-github", + "type": "fontawesome", + }, + { + "name": "GEAR-SONIC Website", + "url": "https://nvlabs.github.io/GEAR-SONIC/", + "icon": "fa-solid fa-globe", + "type": "fontawesome", + }, + { + "name": "Paper", + "url": "https://arxiv.org/abs/2511.07820", + "icon": "fa-solid fa-file-pdf", + "type": "fontawesome", + }, + ], + "icon_links_label": "Quick Links", +} + +templates_path = [ + "_templates", +] + +# -- Internationalization ---------------------------------------------------- + +language = "en" diff --git a/GR00T-WholeBodyControl/docs/source/getting_started/download_models.md b/GR00T-WholeBodyControl/docs/source/getting_started/download_models.md new file mode 100644 index 0000000000000000000000000000000000000000..67f92285ae941adf9993b6e88304a37268ab8149 --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/getting_started/download_models.md @@ -0,0 +1,490 @@ +# Downloading Model Checkpoints + +Pre-trained GEAR-SONIC checkpoints (ONNX format) are hosted on Hugging Face: + +**[nvidia/GEAR-SONIC](https://huggingface.co/nvidia/GEAR-SONIC)** + +## Quick Download + +### Install the dependency + +```bash +pip install huggingface_hub +``` + +### Run the download script + +From the repo root: + +```bash +# Deployment (ONNX models + planner → gear_sonic_deploy/) +python download_from_hf.py + +# Low-latency teleoperation checkpoint (ONNX models + planner → gear_sonic_deploy/) +python download_from_hf.py --low-latency + +# SONIC v1.1 checkpoint (ONNX models + planner → gear_sonic_deploy/) +python download_from_hf.py --sonic-v1-1 + +# Training (checkpoint + SMPL data → sonic_release/ + data/smpl_filtered/) +python download_from_hf.py --training + +# Low-latency PyTorch checkpoint + config only +python download_from_hf.py --training --low-latency + +# SONIC v1.1 PyTorch checkpoint + configs only +python download_from_hf.py --training --sonic-v1-1 --no-smpl + +# Sample data only (1 walking sequence for quick testing) +python download_from_hf.py --sample + +# Training checkpoint only (skip 30GB SMPL download) +python download_from_hf.py --training --no-smpl +``` + +This downloads the **latest** policy encoder + decoder + kinematic planner into +`gear_sonic_deploy/`, preserving the same directory layout the deployment binary expects. + +--- + +## Options + +| Flag | Description | +|------|-------------| +| `--training` | Download training checkpoint + SMPL motion data (~30 GB) | +| `--low-latency` | Download the low-latency teleoperation checkpoint. For deployment, ONNX files go to `gear_sonic_deploy/policy/low_latency/`; with `--training`, the PyTorch checkpoint and configs go to `low_latency/`. | +| `--sonic-v1-1` | Download SONIC v1.1, which uses robot-heading-normalized targets and wrist-pose augmentation. Deployment files go to `gear_sonic_deploy/policy/sonic_v1_1/`; training files go to `sonic_v1_1/`. | +| `--sample` | Download sample motion data only (~4 MB) | +| `--no-planner` | Skip the kinematic planner download | +| `--no-smpl` | With `--training`, skip SMPL data (checkpoint only) | +| `--output-dir PATH` | Override the destination directory | +| `--token TOKEN` | HF token (alternative to `hf auth login`) | + +### Examples + +```bash +# Policy + planner (default) +python download_from_hf.py + +# Policy only +python download_from_hf.py --no-planner + +# Low-latency teleoperation policy only +python download_from_hf.py --low-latency --no-planner + +# SONIC v1.1 policy only +python download_from_hf.py --sonic-v1-1 --no-planner + +# Download into a custom directory +python download_from_hf.py --output-dir /data/gear-sonic +``` + +--- + +## Low-Latency Teleoperation Checkpoint + +The checkpoint published under `low_latency/` in +[`nvidia/GEAR-SONIC`](https://huggingface.co/nvidia/GEAR-SONIC) is configured +for responsive whole-body teleoperation. Its SMPL encoder uses **4 future +reference frames**, compared with **10 frames** in the default release. At +50 Hz (20 ms per frame), this reduces SMPL reference lookahead from +approximately **200 ms to 80 ms**. + +This is the controller's reference lookahead, not a measurement of total +end-to-end system latency. The checkpoint does not replace the default +top-level deployment policy. + +Download the deployment ONNX files: + +```bash +python download_from_hf.py --low-latency +``` + +This creates: + +``` +gear_sonic_deploy/ +└── policy/low_latency/ + ├── model_encoder.onnx + ├── model_decoder.onnx + └── observation_config.yaml +``` + +### C++ deployment inference + +Run the low-latency ONNX controller in simulation: + +```bash +cd gear_sonic_deploy +./deploy.sh \ + --cp policy/low_latency/model \ + --obs-config policy/low_latency/observation_config.yaml \ + sim +``` + +Run it for VLA or teleoperation on the real robot: + +```bash +cd gear_sonic_deploy +./deploy.sh \ + --cp policy/low_latency/model \ + --obs-config policy/low_latency/observation_config.yaml \ + --input-type zmq_manager \ + real +``` + +`deploy.sh` expects `--cp` to be the shared model prefix; it appends +`_encoder.onnx` and `_decoder.onnx` internally. The low-latency PyTorch +checkpoint is available as `low_latency/last.pt`: + +```bash +python download_from_hf.py --training --low-latency +``` + +### Python inference and evaluation + +For Python-side checkpoint evaluation in Isaac Lab, download the PyTorch +checkpoint and sample motions: + +```bash +python download_from_hf.py --training --low-latency +python download_from_hf.py --sample +``` + +Then run the low-latency checkpoint with `eval_agent_trl.py`: + +```bash +python gear_sonic/eval_agent_trl.py \ + +checkpoint=low_latency/last.pt \ + +headless=False \ + ++num_envs=1 \ + ++manager_env.observations.policy.enable_corruption=False \ + ++manager_env.observations.tokenizer.enable_corruption=False \ + "++manager_env.commands.motion.motion_lib_cfg.motion_file=sample_data/robot_filtered" \ + "++manager_env.commands.motion.motion_lib_cfg.smpl_motion_file=sample_data/smpl_filtered" +``` + +For the Python VLA tmux launcher, pass the same low-latency C++ deploy files +through launcher flags: + +```bash +python gear_sonic/scripts/launch_inference.py \ + --deploy-checkpoint policy/low_latency/model \ + --deploy-obs-config policy/low_latency/observation_config.yaml \ + --camera-host 192.168.123.164 \ + --prompt "pick up the cup" +``` + +The launcher still runs the ONNX controller through the C++ deployment pane; +the Python process coordinates the VLA client, camera client, keyboard control, +and optional data exporter. + +--- + +## SONIC v1.1 Checkpoint + +The checkpoint under `sonic_v1_1/` uses robot-heading-normalized target +orientations and was trained with wrist-pose augmentation. It is intended for +heading-stable 3-point teleoperation and SONIC-backed VLA policies trained +against this controller. + +Its SMPL and wrist encoders use **10 future frames at 20 ms spacing** +(approximately **200 ms** of reference lookahead). G1 and teleoperation +references use 10 frames at `step5`. This is not the low-latency checkpoint. + +Download the matching ONNX encoder, decoder, observation config, and planner: + +```bash +python download_from_hf.py --sonic-v1-1 +``` + +This creates: + +``` +gear_sonic_deploy/ +└── policy/sonic_v1_1/ + ├── model_encoder.onnx + ├── model_decoder.onnx + └── observation_config.yaml +``` + +Run the controller in simulation: + +```bash +cd gear_sonic_deploy +./deploy.sh \ + --cp policy/sonic_v1_1/model \ + --obs-config policy/sonic_v1_1/observation_config.yaml \ + sim +``` + +For the VLA launcher: + +```bash +python gear_sonic/scripts/launch_inference.py \ + --deploy-checkpoint policy/sonic_v1_1/model \ + --deploy-obs-config policy/sonic_v1_1/observation_config.yaml \ + --camera-host 192.168.123.164 \ + --prompt "pick up the cup" +``` + +Download the PyTorch checkpoint and configs without the shared 30 GB SMPL +dataset: + +```bash +python download_from_hf.py --training --sonic-v1-1 --no-smpl +``` + +Evaluate it with the matching release recipe: + +```bash +python gear_sonic/eval_agent_trl.py \ + +exp=manager/universal_token/all_modes/sonic_v1_1 \ + +checkpoint=sonic_v1_1/last.pt \ + +headless=False \ + ++num_envs=1 \ + ++manager_env.observations.policy.enable_corruption=False \ + ++manager_env.observations.tokenizer.enable_corruption=False +``` + +Use the same `+exp` and `+checkpoint` values with `train_agent_trl.py` for +continued training. + +--- + +## Manual download via CLI + +If you prefer the Hugging Face CLI: + +```bash +pip install huggingface_hub[cli] + +# Policy only +hf download nvidia/GEAR-SONIC \ + model_encoder.onnx \ + model_decoder.onnx \ + observation_config.yaml \ + --local-dir gear_sonic_deploy + +# Everything (policy + planner) +hf download nvidia/GEAR-SONIC --local-dir gear_sonic_deploy +``` + +--- + +## Manual download via Python + +```python +from huggingface_hub import hf_hub_download + +REPO_ID = "nvidia/GEAR-SONIC" + +encoder = hf_hub_download(repo_id=REPO_ID, filename="model_encoder.onnx") +decoder = hf_hub_download(repo_id=REPO_ID, filename="model_decoder.onnx") +config = hf_hub_download(repo_id=REPO_ID, filename="observation_config.yaml") +planner = hf_hub_download(repo_id=REPO_ID, filename="planner_sonic.onnx") + +print("Policy encoder :", encoder) +print("Policy decoder :", decoder) +print("Obs config :", config) +print("Planner :", planner) +``` + +--- + +## SONIC Training Checkpoint + +The SONIC release training checkpoint and config are also available on Hugging Face, for evaluation or fine-tuning: + +### Download via CLI + +```bash +hf download nvidia/GEAR-SONIC \ + sonic_release/last.pt \ + sonic_release/config.yaml \ + --local-dir models +``` + +### Download via Python + +```python +from huggingface_hub import hf_hub_download + +REPO_ID = "nvidia/GEAR-SONIC" + +checkpoint = hf_hub_download(repo_id=REPO_ID, filename="sonic_release/last.pt") +config = hf_hub_download(repo_id=REPO_ID, filename="sonic_release/config.yaml") + +print("Checkpoint :", checkpoint) +print("Config :", config) +``` + +### Evaluate the checkpoint + +```bash +python gear_sonic/eval_agent_trl.py \ + +checkpoint=models/sonic_release/last.pt \ + +num_envs=1 headless=False +``` + +--- + +## Sample Motion Data (Quick Start) + +A small sample dataset (1 walking sequence) is included for quick testing without downloading the full Bones-SEED dataset. It contains all three data types needed for training: robot retargeted, SOMA skeleton, and SMPL. + +### Download via CLI + +```bash +# Sample data only +hf download nvidia/GEAR-SONIC \ + --include "sample_data/*" \ + --local-dir . + +# Sample data + training checkpoint +hf download nvidia/GEAR-SONIC \ + --include "sample_data/*" \ + --include "sonic_release/*" \ + --local-dir . +``` + +This creates: + +``` +sample_data/ +├── robot_filtered/210531/ # G1 retargeted motion (for motion tracking) +│ ├── walk_forward_amateur_001__A001.pkl +│ └── walk_forward_amateur_001__A001_M.pkl +├── soma_filtered/210531/ # SOMA skeleton motion +│ ├── walk_forward_amateur_001__A001.pkl +│ └── walk_forward_amateur_001__A001_M.pkl +└── smpl_filtered/ # SMPL human motion + ├── walk_forward_amateur_001__A001.pkl + └── walk_forward_amateur_001__A001_M.pkl +``` + +### Test training with sample data + +```bash +python gear_sonic/train_agent_trl.py \ + +exp=manager/universal_token/all_modes/sonic_release \ + num_envs=16 headless=True \ + manager_env.commands.motion.motion_lib_cfg.motion_file=sample_data/robot_filtered \ + manager_env.commands.motion.motion_lib_cfg.smpl_motion_file=sample_data/smpl_filtered +``` + +For full-scale training, download the complete [Bones-SEED](https://huggingface.co/datasets/bones-studio/seed) dataset and follow the [Training Guide](../user_guide/training.md). + +--- + +## SMPL Motion Data (Bones-SEED Filtered) + +The SMPL retargeted motion data used for training (131K sequences, filtered from the Bones-SEED dataset) is available as a split tar archive (~30GB total). + +### Download and extract + +```bash +# Download all parts +hf download nvidia/GEAR-SONIC --include "bones_seed_smpl/*" --local-dir . + +# Reassemble and extract +cat bones_seed_smpl/bones_seed_smpl.tar.part_* | tar xf - -C data/ +``` + +This extracts to `data/smpl_filtered/` with 131K `.pkl` files. + +Then point training to it: + +```bash +python gear_sonic/train_agent_trl.py \ + +exp=manager/universal_token/all_modes/sonic_release \ + +checkpoint=sonic_release/last.pt \ + num_envs=4096 headless=True \ + ++manager_env.commands.motion.motion_lib_cfg.smpl_motion_file=data/smpl_filtered +``` + +--- + +## Available files + +``` +nvidia/GEAR-SONIC/ +├── model_encoder.onnx # Policy encoder (ONNX, for deployment) +├── model_decoder.onnx # Policy decoder (ONNX, for deployment) +├── observation_config.yaml # Observation configuration (deployment) +├── planner_sonic.onnx # Kinematic planner (ONNX) +├── low_latency/ +│ ├── model_encoder.onnx # Low-latency policy encoder (ONNX) +│ ├── model_decoder.onnx # Low-latency policy decoder (ONNX) +│ ├── observation_config.yaml # Low-latency observation configuration +│ ├── last.pt # Low-latency training checkpoint +│ ├── config.yaml # Low-latency training config +│ └── model_config.yaml # Low-latency model config +├── sonic_v1_1/ +│ ├── model_encoder.onnx # SONIC v1.1 policy encoder (ONNX) +│ ├── model_decoder.onnx # SONIC v1.1 policy decoder (ONNX) +│ ├── observation_config.yaml # Matching deployment observations +│ ├── last.pt # SONIC v1.1 training checkpoint +│ ├── config.yaml # Resolved training config +│ └── model_config.yaml # Model architecture config +├── bones_seed_smpl/ # SMPL motion data (131K sequences, ~30GB split tar) +│ ├── bones_seed_smpl.tar.part_aa +│ ├── ... +│ └── bones_seed_smpl.tar.part_ag +├── sonic_release/ +│ ├── last.pt # Training checkpoint (for eval/fine-tuning) +│ └── config.yaml # Training config +└── sample_data/ # Sample motion data (1 walking sequence) + ├── robot_filtered/ # G1 retargeted motion + ├── soma_filtered/ # SOMA skeleton motion + └── smpl_filtered/ # SMPL human motion +``` + +The download script places deployment files into the layout the deployment binary expects: + +``` +gear_sonic_deploy/ +├── policy/release/ +│ ├── model_encoder.onnx +│ ├── model_decoder.onnx +│ └── observation_config.yaml +├── policy/low_latency/ +│ ├── model_encoder.onnx +│ ├── model_decoder.onnx +│ └── observation_config.yaml +├── policy/sonic_v1_1/ +│ ├── model_encoder.onnx +│ ├── model_decoder.onnx +│ └── observation_config.yaml +└── planner/target_vel/V2/ + └── planner_sonic.onnx +``` + +--- + +## Authentication + +The repository is **public** — no token required for downloading. + +If you hit rate limits or need to access private forks: + +```bash +# Option 1: CLI login (recommended — token is saved once) +hf login + +# Option 2: environment variable +export HF_TOKEN="hf_..." +python download_from_hf.py + +# Option 3: pass token directly +python download_from_hf.py --token hf_... +``` + +Get a free token at [huggingface.co/settings/tokens](https://huggingface.co/settings/tokens). + +--- + +## Next steps + +After downloading, follow the [Quick Start](quickstart.md) guide to run the +deployment stack in MuJoCo simulation or on real hardware. diff --git a/GR00T-WholeBodyControl/docs/source/getting_started/installation_deploy.md b/GR00T-WholeBodyControl/docs/source/getting_started/installation_deploy.md new file mode 100644 index 0000000000000000000000000000000000000000..5b55a06e4307b1d97d024268d0a293b519d49439 --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/getting_started/installation_deploy.md @@ -0,0 +1,145 @@ +# Installation (Deployment) + +## Prerequisites + +**Required for all setups:** +- **Ubuntu 20.04/22.04/24.04** or other Debian-based Linux distributions +- **CUDA Toolkit** (for GPU acceleration) +- **TensorRT** (for inference optimization) — **Install this first!** +- **Jetpack 6** (for onboard deployment) +- Python 3.8+ +- Git with LFS support + +**Download TensorRT** from [NVIDIA Developer](https://developer.nvidia.com/tensorrt/download/10x): + +| Platform | TensorRT Version | +|---|---| +| x86_64 (Desktop) | **10.13** (required) | +| Jetson / G1 onboard Orin | **10.7** (required; requires JetPack 6 — [flashing guide](../references/jetpack6.md)) | + +```{tip} +Download the **TAR** package (not the DEB one) so you can extract TensorRT to any location. The archive is ~10 GB; consider using `pv` to monitor progress: +``` + +```{danger} +You **must** use the exact TensorRT versions listed above. Using a different version is known to produce incorrect inference results — the planner will output wrong motion, which can cause dangerous robot behavior. +``` + +```sh +sudo apt-get install -y pv +pv TensorRT-*.tar.gz | tar -xz -f - +``` + +Move the unzipped TensorRT to `~/TensorRT` (or similar) and add to your `~/.bashrc`: + +```sh +export TensorRT_ROOT=$HOME/TensorRT +``` + +## Clone the Repository + +```bash +git clone https://github.com/NVlabs/GR00T-WholeBodyControl.git +cd GR00T-WholeBodyControl +git lfs pull # make sure all large files are fetched +``` + +## Setup + +### Native Development (Recommended) + +**Advantages:** Direct system installation, faster builds, production-ready. + +```{warning} +For G1 onboard deployment, we require the onboard Orin to be upgraded to Jetpack 6 to support TensorRT. Please follow the [flashing guide](../references/jetpack6.md) for upgrading! +``` + +**Prerequisites:** +- Basic development tools (cmake, git, etc.) +- (Optional) ROS2 if you plan to use ROS2-based input/output + +**Setup steps:** + +1. **Install system dependencies:** + +```sh +cd gear_sonic_deploy +chmod +x scripts/install_deps.sh +./scripts/install_deps.sh +``` + +2. **Set up environment:** + +```sh +source scripts/setup_env.sh +``` + +The setup script will automatically: +- Configure TensorRT environment +- Set up all necessary paths + +For convenience, you can add the environment setup to your shell profile: + +```sh +echo "source $(pwd)/scripts/setup_env.sh" >> ~/.bashrc +``` + +3. **Build the project:** + +```sh +just build +``` + +### Docker (ROS2 Development Environment) + +We provide a unified Docker environment with ROS2 Humble, supporting x86_64 and Jetson platforms. + +**Prerequisites:** +- Docker installed and user added to docker group +- `TensorRT_ROOT` environment variable set on host +- For Jetson: JetPack 6.1+ (CUDA 12.6) + +**Quick Setup:** + +```sh +# 1. Add user to docker group (one-time setup) +sudo usermod -aG docker $USER +newgrp docker + +# 2. Set TensorRT path (add to ~/.bashrc for persistence) +export TensorRT_ROOT=/path/to/TensorRT + +# 3. Launch container +cd gear_sonic_deploy +./docker/run-ros2-dev.sh +``` + +**Options:** + +```sh +./docker/run-ros2-dev.sh # Standard build (fast) +./docker/run-ros2-dev.sh --rebuild # Force rebuild +./docker/run-ros2-dev.sh --with-opengl # Include OpenGL for visualization (RViz, Gazebo) +``` + +**Architecture Support:** +- **x86_64**: CUDA 12.4.1 (requires NVIDIA driver 550+) +- **Jetson**: CUDA 12.4.1 container on CUDA 12.6 host (forward compatible) + +**Inside the container:** + +```sh +source scripts/setup_env.sh # set up dependency +just build # Build +just --list # Show all commands +``` + +**Troubleshooting:** +- If you get "permission denied", ensure you're in the docker group +- TensorRT must be set on the **host** before starting container +- For Jetson: Run `source scripts/setup_env.sh` on host first (sets jetson_clocks) + + + + + diff --git a/GR00T-WholeBodyControl/docs/source/getting_started/installation_training.md b/GR00T-WholeBodyControl/docs/source/getting_started/installation_training.md new file mode 100644 index 0000000000000000000000000000000000000000..e19321040bd11f99f1aeeca3670e40fd08af6a7e --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/getting_started/installation_training.md @@ -0,0 +1,125 @@ +# Installation (Training) + +This guide walks through setting up the SONIC training environment for whole-body humanoid control. + +## Prerequisites + +- **GPU**: NVIDIA GPU with CUDA 12.x (L40 recommended) +- **OS**: Ubuntu 22.04+ +- **Python**: 3.11 (required by Isaac Lab; sim/teleop/deploy scripts work on 3.10+) +- **Isaac Lab**: 2.3+ (required for simulation environments) + +## Install Isaac Lab + +SONIC training uses Isaac Lab for physics simulation. Follow the official +[Isaac Lab installation guide](https://isaac-sim.github.io/IsaacLab/main/source/setup/installation/index.html) +to install Isaac Lab. + +After installation, verify: + +```bash +python -c "import isaaclab; print(isaaclab.__version__)" +``` + +## Install gear_sonic (Training) + +From the repository root: + +```bash +pip install -e "gear_sonic/[training]" +``` + +This installs the training dependencies (Hydra, W&B, HuggingFace TRL, etc.) +on top of the Isaac Lab environment. + +## Download Model and Data from Hugging Face + +SONIC model checkpoints and SMPL motion data are hosted on +[Hugging Face](https://huggingface.co/nvidia/GEAR-SONIC). + +```bash +pip install huggingface_hub +python download_from_hf.py --training +``` + +This downloads: + +- **PyTorch checkpoint** (`sonic_release/last.pt`) for finetuning +- **SMPL motion data** (`data/smpl_filtered/`) for the SMPL encoder + +## Prepare Robot Motion Data + +SONIC trains on the [Bones-SEED](https://huggingface.co/datasets/bones-studio/seed) motion capture dataset +(142K+ motion sequences retargeted to the Unitree G1). + +### Step 1: Download and convert + +Download the **G1 retargeted CSVs** (29 DOF, 120 FPS) from +[Bones-SEED on HuggingFace](https://huggingface.co/datasets/bones-studio/seed), then convert: + +```bash +python gear_sonic/data_process/convert_soma_csv_to_motion_lib.py \ + --input /path/to/bones_seed/g1/csv/ \ + --output data/motion_lib_bones_seed/robot \ + --fps 30 --fps_source 120 --individual --num_workers 16 +``` + +### Step 2: Filter motions + +Remove motions the G1 robot cannot perform: + +```bash +python gear_sonic/data_process/filter_and_copy_bones_data.py \ + --source data/motion_lib_bones_seed/robot \ + --dest data/motion_lib_bones_seed/robot_filtered --workers 16 +``` + +This removes ~8.7% of motions (~130K of 142K remain). See the +[Training Guide](../user_guide/training.md) for details. + +Your data directory should look like: + +``` +/ +├── data/ +│ ├── motion_lib_bones_seed/ +│ │ └── robot_filtered/ # Filtered G1 motions (~130K PKLs) +│ └── smpl_filtered/ # SMPL motion data (from Hugging Face) +└── sonic_release/ # Released checkpoint (from Hugging Face) +``` + +> **Note**: Data processing scripts (`gear_sonic/data_process/`) do **not** require +> Isaac Lab and can be run on any machine with `pip install -e gear_sonic/`. + +## Verify Installation + +First, run the pre-flight check to verify all dependencies: + +```bash +python check_environment.py --training +``` + +Then run a quick smoke test with a small number of environments: + +```bash +# Interactive (with viewer) +python gear_sonic/train_agent_trl.py \ + +exp=manager/universal_token/all_modes/sonic_release \ + num_envs=16 headless=False \ + ++algo.config.num_learning_iterations=5 + +# Headless (server / no display) +python gear_sonic/train_agent_trl.py \ + +exp=manager/universal_token/all_modes/sonic_release \ + num_envs=16 headless=True \ + ++algo.config.num_learning_iterations=5 +``` + +After a minute of initialization you should see training metrics (rewards, errors) +printing to the console. + +## Full Training + +Once installation is verified, see the [Training Guide](../user_guide/training.md) +for full training commands (64+ GPU recommended), evaluation, ONNX export, and +SOMA encoder setup. diff --git a/GR00T-WholeBodyControl/docs/source/getting_started/quickstart.md b/GR00T-WholeBodyControl/docs/source/getting_started/quickstart.md new file mode 100644 index 0000000000000000000000000000000000000000..43383a4887734bacd1ee3e7547ac778a49693ba5 --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/getting_started/quickstart.md @@ -0,0 +1,155 @@ +# Quick Start + +Get started with SONIC in minutes! + +```{admonition} Prerequisites +:class: note +1. **Completed the [Installation Guide](installation_deploy)** — TensorRT is installed, the repo is cloned, and the C++ deployment is built. +2. **Downloaded the model checkpoints** — run `python download_from_hf.py` from the repo root. See [Downloading Model Checkpoints](download_models) for details. +``` + +```{admonition} Safety Warning +:class: danger +Robots can be dangerous. Ensure a clear safety zone, keep a safety operator ready to trigger an emergency stop in front of the keyboard, and use this software at your own risk. The authors and contributors are not responsible for any damage, injury, or loss caused by use or misuse of this project. +``` + +## Isaac Lab Eval + +Use Isaac Lab to sanity-check the released PyTorch checkpoint in simulation. Run these commands from the repo root inside your Isaac Lab Python environment. + +If you only downloaded the deployment ONNX files, first fetch the eval checkpoint and the small sample motion set: + +```sh +python download_from_hf.py --training --no-smpl +python download_from_hf.py --sample +``` + +To open the Isaac Sim viewer and watch the policy: + +```sh +python gear_sonic/eval_agent_trl.py \ + +checkpoint=sonic_release/last.pt \ + +headless=False \ + ++num_envs=1 \ + ++manager_env.observations.policy.enable_corruption=False \ + ++manager_env.observations.tokenizer.enable_corruption=False \ + "++manager_env.commands.motion.motion_lib_cfg.motion_file=sample_data/robot_filtered" \ + "++manager_env.commands.motion.motion_lib_cfg.smpl_motion_file=sample_data/smpl_filtered" +``` + +Leave this running while you inspect the viewer, then stop it with `Ctrl+C`. + +For a quick metrics run: + +```sh +python gear_sonic/eval_agent_trl.py \ + +checkpoint=sonic_release/last.pt \ + +headless=True \ + ++eval_callbacks=im_eval \ + ++run_eval_loop=False \ + ++num_envs=128 \ + ++manager_env.observations.policy.enable_corruption=False \ + ++manager_env.observations.tokenizer.enable_corruption=False \ + "+manager_env/terminations=tracking/eval" \ + "++manager_env.commands.motion.motion_lib_cfg.max_unique_motions=512" \ + "++manager_env.commands.motion.motion_lib_cfg.motion_file=sample_data/robot_filtered" \ + "++manager_env.commands.motion.motion_lib_cfg.smpl_motion_file=sample_data/smpl_filtered" +``` + +To render videos instead: + +```sh +python gear_sonic/eval_agent_trl.py \ + +checkpoint=sonic_release/last.pt \ + +headless=True \ + ++eval_callbacks=im_eval \ + ++run_eval_loop=False \ + ++num_envs=8 \ + ++manager_env.config.render_results=True \ + "++manager_env.config.save_rendering_dir=/tmp/sonic_renders" \ + ++manager_env.config.env_spacing=10.0 \ + "~manager_env/recorders=empty" "+manager_env/recorders=render" \ + ++manager_env.observations.policy.enable_corruption=False \ + ++manager_env.observations.tokenizer.enable_corruption=False \ + "++manager_env.commands.motion.motion_lib_cfg.motion_file=sample_data/robot_filtered" \ + "++manager_env.commands.motion.motion_lib_cfg.smpl_motion_file=sample_data/smpl_filtered" +``` + +Videos are written to `/tmp/sonic_renders`. For full-dataset evaluation and expected metrics, see the [Training Guide](../user_guide/training.md#evaluation). + + +## Sim2Sim in MuJoCo + + + +For testing in a MuJoCo simulator, run the simulation loop and deployment script in separate terminals. + +```{note} +The MuJoCo simulator (Terminal 1) runs on the **host** in a Python virtual environment — it is **not** inside the Docker container. The deployment binary (Terminal 2) can run either natively on the host or inside the Docker container. If you are using Docker, run Terminal 1 on the host and Terminal 2 inside the container. +``` + +### One-time setup: install the MuJoCo sim environment + +On the **host** (outside Docker), from the **repo root** (`GR00T-WholeBodyControl/`), run: + +```sh +bash install_scripts/install_mujoco_sim.sh +``` + +This creates a lightweight `.venv_sim` virtual environment with only the packages needed for the simulator (MuJoCo, Pinocchio, Unitree SDK2, etc.). + +### Running the sim2sim loop + +We highly recommend running through this process and getting familiar with the controls in simulation before deploying on real hardware. + +**Terminal 1 — MuJoCo simulator** (host, from repo root): + +```sh +source .venv_sim/bin/activate +python gear_sonic/scripts/run_sim_loop.py +``` + +**Terminal 2 — Deployment** (host or Docker, from `gear_sonic_deploy/`): + +```sh +bash deploy.sh sim +``` + +**Starting Control:** + +1. In Terminal 2 (deploy.sh), press **`]`** to start the policy. +2. Click on the MuJoCo viewer window, press **`9`** to drop the robot to the ground. +3. Go back to Terminal 2. Press **`T`** to play the current reference motion — the robot will execute it to completion. +4. Press **`N`** or **`P`** to switch to the next or previous motion sequence. +5. Press **`T`** again to play the new motion. +6. You can press **`T`** again to replay the same motion once it has finished. If you want to stop and go back to the first frame of the current motion, press **`R`** to restart it from the beginning. This can be used to stop the motion without terminating the policy. +7. When you are done or need an **emergency stop**, press **`O`** to stop control and exit. + +For more controls, see the tutorials for [Keyboard](../tutorials/keyboard.md), [Gamepad](../tutorials/gamepad.md), [ZMQ Streaming](../tutorials/zmq.md), and [Interface Manager](../tutorials/manager.md). + +## Real Robot + +To deploy on the real G1 robot, run: + +```sh +./deploy.sh real +``` + +## Online Visualization + +Start the visualizer and connect to a running `g1_deploy` executable: + +```sh +python visualize_motion.py --realtime_debug_url tcp://localhost:5557 +``` + +Notes: +- Default port: 5557 (change with `--zmq-out-port `) +- Default topic: `g1_debug` (change with `--zmq-out-topic ` on executable, `--realtime_debug_topic ` on visualizer) +- For physical robots, replace `localhost` with the robot's IP address + +For offline motion CSV visualization and logging details, see [Deployment Code & Program Flow](../references/deployment_code.md). + +For more advanced usage, see the tutorials for [Keyboard](../tutorials/keyboard.md), [Gamepad](../tutorials/gamepad.md), [ZMQ Streaming](../tutorials/zmq.md), [VR Whole-Body Teleop](../tutorials/vr_wholebody_teleop.md), and [Interface Manager](../tutorials/manager.md). diff --git a/GR00T-WholeBodyControl/docs/source/getting_started/vr_teleop_setup.md b/GR00T-WholeBodyControl/docs/source/getting_started/vr_teleop_setup.md new file mode 100644 index 0000000000000000000000000000000000000000..e31ea1403cf548096230c1484c80ddb0cdcd8c11 --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/getting_started/vr_teleop_setup.md @@ -0,0 +1,151 @@ +# VR Teleop Setup (PICO) + +This page covers the one-time hardware and software setup for PICO VR whole-body teleoperation. After completing these steps, proceed to the [ZMQ Manager tutorial](../tutorials/vr_wholebody_teleop.md) to run teleop in sim or on real hardware. + +--- + +## Required Hardware + +- [PICO 4 / PICO 4 Pro headset](https://www.picoxr.com/global/products/pico4) +- [2x PICO controllers](https://www.picoxr.com/global/products/pico4) +- [2x PICO motion trackers](https://www.picoxr.com/global/products/pico-motion-tracker) (strapped to ankles) +- A high-speed, low-latency Wi-Fi connection; teleoperation performance is heavily dependent on network quality. + +--- + +## Step 1: Install XRoboToolkit + +XRoboToolkit consists of a PC service (running on your workstation) and a PICO app (running on the headset) that streams body-tracking data. + +### PC Service + +The PC service must be installed and running on your workstation **before** the PICO can connect. + +**Ubuntu 22.04 (x86_64 workstation):** + +```bash +wget https://github.com/XR-Robotics/XRoboToolkit-PC-Service/releases/download/v1.0.0/XRoboToolkit_PC_Service_1.0.0_ubuntu_22.04_amd64.deb +sudo dpkg -i XRoboToolkit_PC_Service_1.0.0_ubuntu_22.04_amd64.deb +``` + +**Ubuntu 24.04 (x86_64 workstation):** + +```bash +wget https://github.com/XR-Robotics/XRoboToolkit-PC-Service/releases/download/v1.0.0/XRoboToolkit_PC_Service_1.0.0_ubuntu_24.04_amd64.deb +sudo dpkg -i XRoboToolkit_PC_Service_1.0.0_ubuntu_24.04_amd64.deb +``` + +**Jetson (aarch64, onboard):** + +```bash +sudo dpkg -i gear_sonic_deploy/thirdparty/roboticsservice_1.0.0.0_arm64.deb +``` + +See [XRoboToolkit-PC-Service releases](https://github.com/XR-Robotics/XRoboToolkit-PC-Service/releases) for other platforms or newer versions. + +### PICO App + +1. Wear the PICO headset to begin the setup and installation process. +2. Complete the quick setup on PICO. +3. Make sure the PICO is connected to Wi-Fi. +4. Open the browser application in the PICO. +5. Type **"xrobotoolkit"** in the search bar and select the GitHub page [https://github.com/XR-Robotics](https://github.com/XR-Robotics). + +```{image} ../_static/pico_setup/google_search_screenshot.png +:width: 600px +:align: center +``` + +6. Make sure **Developer Mode** is enabled (Settings → Developer). +7. **[INSIDE PICO]** Scroll down in the GitHub page until you see the APK download option and click with the PICO trigger to download it. + +```{tip} +Download [XRoboToolkit-PICO-1.1.1.apk](https://github.com/XR-Robotics/XRoboToolkit-Unity-Client/releases/download/v1.1.1/XRoboToolkit-PICO-1.1.1.apk) on PICO using the browser. ([Other Versions](https://github.com/XR-Robotics/XRoboToolkit-Unity-Client/releases)) +``` + +8. **[INSIDE PICO]** Open the manage downloads option on the top right section of the browser page and click to open the `XRoboToolkit-PICO-1.1.1.apk` download. +9. **[INSIDE PICO]** Select **Install** — the application will appear in the **Unknown** section of your library. + +--- + +## Step 2: Motion Tracker Setup + +```{image} ../_static/pico_setup/pico_setup_screenshot.png +:width: 600px +:align: center +``` + +1. Strap one PICO motion tracker to your left ankle and one to your right ankle. **Scrunch** down any baggy clothing so the trackers are visible. Make sure the side with the light indicator faces up. +2. Go to PICO settings. In the menu on the left, scroll down to the last option: **"Developer"**. Make sure **"Safeguard"** is turned off. + - If the Developer option is not active, tap on "Software" until it appears. +3. Click the **Wi-Fi icon** in the PICO menu. A picture of the headset will appear. Above the headset, there will be a small circular logo for the motion trackers. If there is no logo, open the **"Motion Tracker"** app itself. + - Headset and 2 controllers will populate — select **Motion Tracker** (small circle). +4. Next to each tracker, there is an **"i"** icon. Click on this and **unpair all trackers**. +5. Once all trackers are cleared, click the **"Pair"** button in the top right corner. +6. Press and hold the button on the top of each motion tracker for **6 seconds**. Once in pairing mode, the lights will flash red and blue. + +### Motion Tracker Calibration + +1. Wear the PICO headset over your eyes. +2. Press the blue **"Calibrate"** button and follow the two calibration sequences: + - **Sequence 1:** Stand stiff with the handheld controllers down by your sides. + - **Sequence 2:** Look down at the foot motion trackers until the headset cameras recognize them. +3. Once calibrated, wear the PICO headset around your forehead (ensuring PICO faces forward to continue detecting motion trackers). + +--- + +## Step 3: Install the PICO Teleop Environment + +From the **repo root**: + +```bash +bash install_scripts/install_pico.sh +``` + +This creates a `.venv_teleop` virtual environment (Python 3.10) that includes: +- `teleop` extra (ZMQ, Pinocchio, PyVista) +- `sim` extra (MuJoCo, tyro) +- XRoboToolkit SDK +- Unitree SDK2 Python bindings + +Activate it with: + +```bash +source .venv_teleop/bin/activate # prompt: (gear_sonic_teleop) +``` + +--- + +## Step 4: Connect the PICO to Your Workstation + +1. Open the Wi-Fi settings on both the laptop/PC and PICO and ensure they are on the **same Wi-Fi network**. Take note of the Wi-Fi IPv4 address. + - To find the PICO's Wi-Fi, select the control center on the bottom right of the menu. + +```{image} ../_static/pico_setup/internet.png +:width: 600px +:align: center +``` + +```{image} ../_static/pico_setup/pico_vr_screenshot.png +:width: 600px +:align: center +``` + +2. Open the **XRoboToolKit** application. Enter the IP address of the laptop by clicking **"Enter"** next to "PC Service:". You will know it is properly connected if **WORKING** appears next to "Status:". + - If your IP address is already inputted, select **"Reconnect"** where it says "Status:" in the Network section. + +3. Make sure the following boxes are ticked as shown in the picture below: + - **"Head"** and **"Controller"** under the "Tracking" section. + - For Data/Control, make sure to select the **"Send"** button. + - For "Pico Motion Tracker" make sure to select **"Full body"**. + +```{image} ../_static/pico_setup/xrrobot_setup.png +:width: 600px +:align: center +``` + +--- + +## Next Steps + +Your PICO hardware and software are now ready. Proceed to the [ZMQ Manager (`zmq_manager`) tutorial](../tutorials/vr_wholebody_teleop.md) to run whole-body teleoperation in simulation or on the real robot. diff --git a/GR00T-WholeBodyControl/docs/source/index.rst b/GR00T-WholeBodyControl/docs/source/index.rst new file mode 100644 index 0000000000000000000000000000000000000000..193fa32cdc292aabc7a060923b3cceac24fa66d7 --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/index.rst @@ -0,0 +1,157 @@ +GR00T-WholeBodyControl Documentation +==================================== + +.. image:: https://img.shields.io/badge/License-Apache%202.0%20%7C%20NVIDIA%20Open%20Model-blue.svg + :target: resources/license.html + :alt: License + +.. image:: https://img.shields.io/badge/IsaacLab-2.3.0-blue.svg + :target: https://github.com/isaac-sim/IsaacLab/releases/tag/v2.3.0 + :alt: IsaacLab + +Welcome to the official documentation for **GR00T Whole-Body Control (WBC)**! This is a unified platform for developing and deploying advanced humanoid controllers. + + +What is GR00T-WholeBodyControl? +-------------------------------- + +This codebase serves as the foundation for: + +- **Decoupled WBC** models used in NVIDIA Isaac-Gr00t, Gr00t N1.5 and N1.6 (see :doc:`detailed reference `) +- **GEAR-SONIC Series**: State-of-the-art controllers from the GEAR team + +News +---- + +- **[2026-07-23]** **SONIC v1.1 checkpoint** — released a robot-heading-normalized SONIC controller trained with wrist-pose augmentation for 3-point teleoperation and SONIC-backed VLA execution. See the `Model Card `_ and `Download Models `_. +- **[06/16]** **Isaac Teleop Setup (CloudXR / DeviceIO, in-process)** — added bring-up docs for the in-process CloudXR path via ``isaacteleop[cloudxr]``, with no separate publisher container. See `Isaac Teleop Setup `_. +- **[2026-06-16]** **Low-latency teleoperation checkpoint** — released a SONIC checkpoint with 4-frame SMPL reference lookahead for more responsive whole-body teleoperation. See the `Model Card `_, `Download Models `_, and `VLA Inference `_ for usage. +- **[2026-05-07]** **End-to-end VLA workflow on G1** — collect teleop data, fine-tune Isaac-GR00T N1.7, and deploy with SONIC whole-body control. See `Data Collection `_, `VLA Workflow `_, and `VLA Inference `_. +- **[2026-04-14]** `Live web demo `_ — try SONIC interactively in your browser. Features `Kimodo `_ text-to-motion generation. +- **[2026-04-10]** Released **SONIC training code and checkpoint** on `HuggingFace `_. Train from scratch or finetune. **Additional embodiment support** and **VLA data collection pipeline**. See `Training Guide `_. +- **[2026-03-24]** C++ inference stack update: motor error monitoring, TTS alerts, ZMQ protocol v4, idle-mode readaptation. **ZMQ header size changed to 1280 bytes.** +- **[2026-03-16]** `BONES-SEED `_ open-sourced — 142K+ human motions (~288 hours) with G1 MuJoCo trajectories. +- **[2026-02-19]** Released GEAR-SONIC: pretrained checkpoints, C++ inference, VR teleoperation, and documentation. +- **[2025-11-12]** Initial release with Decoupled WBC for GR00T N1.5 and N1.6. + +GEAR-SONIC +---------- +.. image:: _static/sonic-preview-gif-480P.gif + :width: 100% + :align: center + + +.. raw:: html + +

+ Website + Paper + GitHub +

+ +**SONIC** is a humanoid behavior foundation model that gives robots a core set of motor skills learned from large-scale human motion data. Rather than building separate controllers for every motion, SONIC uses motion tracking as a scalable training task so a single unified policy can produce natural, whole-body movement and support a wide range of behaviors. + +🎯 Key Features: + +- 🚶 Natural whole-body locomotion (walking, crawling, dynamic movements) +- 🎮 Real-time VR teleoperation support +- 🤖 Foundation for higher-level planning and interaction +- 📦 Ready-to-deploy C++ inference stack + +Quick Start: Sim2Sim +-------------------- + +Quickly test the SONIC deployment stack in MuJoCo before deploying on real hardware. + +.. raw:: html + + + +.. tip:: + + **Get running in minutes!** Follow the :doc:`Installation ` and :doc:`Quickstart ` guides to see this in action on your machine. + +Documentation +------------- + +.. toctree:: + :maxdepth: 2 + :caption: Getting Started + + model_card + getting_started/installation_deploy + getting_started/download_models + getting_started/quickstart + getting_started/vr_teleop_setup + +.. toctree:: + :maxdepth: 2 + :caption: Tutorials + + tutorials/keyboard + tutorials/gamepad + tutorials/zmq + tutorials/manager + tutorials/isaac_teleop_publisher_setup + tutorials/vr_wholebody_teleop + tutorials/live_camera_teleop + tutorials/data_collection + tutorials/vla_workflow + tutorials/vla_inference + +.. toctree:: + :maxdepth: 2 + :caption: Training + + getting_started/installation_training + user_guide/training + user_guide/training_data + user_guide/new_embodiments + +.. toctree:: + :maxdepth: 2 + :caption: Best Practices + + user_guide/teleoperation + user_guide/troubleshooting + +.. toctree:: + :maxdepth: 2 + :caption: API Reference + +.. api/index +.. api/teleop + +.. toctree:: + :maxdepth: 2 + :caption: Reference Documentation + + references/index + user_guide/configuration + references/conventions + references/training_code + references/deployment_code + references/observation_config + references/motion_reference + references/planner_onnx + references/jetpack6 + references/decoupled_wbc + + +.. toctree:: + :maxdepth: 1 + :caption: Additional Resources + + resources/citations + resources/license + resources/support +.. resources/contributing + +Indices and tables +================== + +* :ref:`genindex` +* :ref:`modindex` +* :ref:`search` diff --git a/GR00T-WholeBodyControl/docs/source/model_card.md b/GR00T-WholeBodyControl/docs/source/model_card.md new file mode 100644 index 0000000000000000000000000000000000000000..fb39f2b4fac8072dbda0a804f5e333e1ed1cbd3d --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/model_card.md @@ -0,0 +1,131 @@ +# Model Card + +SONIC provides three released whole-body controller checkpoints for the +Unitree G1. Choose the model based on its reference representation and intended +deployment. + +## Available Models + +| Model | Hugging Face location | SMPL reference input | Intended use and comments | +|---|---|---|---| +| **Default SONIC (original release)** | Top-level `model_encoder.onnx`, `model_decoder.onnx`, and `observation_config.yaml`; training checkpoint at `sonic_release/last.pt` | 10 future frames at 20 ms spacing, approximately 200 ms of reference lookahead | Default general-purpose SONIC controller for motion tracking, planning, teleoperation, and compatibility with existing deployments. G1 and teleoperation future-reference observations use `step5`. | +| **Low-latency teleoperation** | [`low_latency/`](https://huggingface.co/nvidia/GEAR-SONIC/tree/main/low_latency) | 4 future frames at 20 ms spacing, approximately 80 ms of reference lookahead | Intended for more responsive whole-body teleoperation and VLA execution. G1 and teleoperation future-reference observations use `step1`. Use its encoder, decoder, and observation config together. | +| **SONIC v1.1** | [`sonic_v1_1/`](https://huggingface.co/nvidia/GEAR-SONIC/tree/main/sonic_v1_1) | 10 future frames at 20 ms spacing, approximately 200 ms of reference lookahead | Uses robot-heading-normalized target orientation and was trained with wrist-pose augmentation. Intended for heading-stable 3-point teleoperation and SONIC-backed VLA policies that use this controller. G1 and teleoperation future-reference observations use `step5`; this is not the low-latency model. | + +All three models use the SONIC universal-token controller, produce 64-dimensional +latent motion tokens, run the controller at 50 Hz, and support SMPL pose, G1 +motion reference, and VR 3-point inputs. Deployment uses C++ and TensorRT; the +PyTorch checkpoints support Isaac Lab evaluation and continued training. + +```{note} +The lookahead values describe the reference horizon presented to the +controller. They are not measurements of total end-to-end teleoperation +latency, which also includes sensing, networking, preprocessing, and inference. +``` + +## Released Files + +| Model | Deployment files | PyTorch and configuration files | +|---|---|---| +| Default SONIC | `model_encoder.onnx`, `model_decoder.onnx`, `observation_config.yaml` | `sonic_release/last.pt`, `sonic_release/config.yaml` | +| Low-latency teleoperation | `low_latency/model_encoder.onnx`, `low_latency/model_decoder.onnx`, `low_latency/observation_config.yaml` | `low_latency/last.pt`, `low_latency/config.yaml`, `low_latency/model_config.yaml` | +| SONIC v1.1 | `sonic_v1_1/model_encoder.onnx`, `sonic_v1_1/model_decoder.onnx`, `sonic_v1_1/observation_config.yaml` | `sonic_v1_1/last.pt`, `sonic_v1_1/config.yaml`, `sonic_v1_1/model_config.yaml` | + +All files are hosted in +[`nvidia/GEAR-SONIC`](https://huggingface.co/nvidia/GEAR-SONIC). Model weights +are covered by the [NVIDIA Open Model License](resources/license.md). + +## Choosing a Model + +Use **Default SONIC** when you want the original release, the broadest +compatibility with existing deployment setups, or the standard motion-tracking +and planning controller. + +Use **Low-latency teleoperation** when responsiveness to streamed SMPL, VR, or +VLA commands is the priority. Its shorter reference horizon reduces commanded +motion lookahead, but it does not remove latency elsewhere in the system. + +Use **SONIC v1.1** for robot-heading-normalized 3-point +teleoperation or a SONIC-backed VLA policy trained against this controller. It +retains the 10-frame SMPL horizon and was trained with wrist-pose augmentation. + +## Usage + +Install the Hugging Face dependency from the repository root: + +```bash +pip install huggingface_hub +``` + +### Default SONIC + +```bash +python download_from_hf.py + +cd gear_sonic_deploy +./deploy.sh --input-type zmq_manager real +``` + +### Low-Latency Teleoperation + +```bash +python download_from_hf.py --low-latency + +cd gear_sonic_deploy +./deploy.sh \ + --cp policy/low_latency/model \ + --obs-config policy/low_latency/observation_config.yaml \ + --input-type zmq_manager \ + real +``` + +### SONIC v1.1 + +```bash +python download_from_hf.py --sonic-v1-1 + +cd gear_sonic_deploy +./deploy.sh \ + --cp policy/sonic_v1_1/model \ + --obs-config policy/sonic_v1_1/observation_config.yaml \ + --input-type zmq_manager \ + real +``` + +### Python VLA Launcher + +For the default model: + +```bash +python gear_sonic/scripts/launch_inference.py \ + --camera-host 192.168.123.164 \ + --prompt "pick up the cup" +``` + +For the low-latency model: + +```bash +python gear_sonic/scripts/launch_inference.py \ + --deploy-checkpoint policy/low_latency/model \ + --deploy-obs-config policy/low_latency/observation_config.yaml \ + --camera-host 192.168.123.164 \ + --prompt "pick up the cup" +``` + +For SONIC v1.1, replace the two `policy/low_latency/` paths above with +`policy/sonic_v1_1/`. + +See [Downloading Model Checkpoints](getting_started/download_models.md) for +PyTorch checkpoint evaluation and additional download options. + +## Limitations and Safety + +- The low-latency name refers to reduced controller reference lookahead, not a + benchmark of total system latency. +- SONIC v1.1 is not a low-latency checkpoint; it uses the + 10-frame SMPL reference horizon. +- Each ONNX encoder and decoder must be used with its matching observation + configuration. +- These checkpoints target the Unitree G1 embodiment. +- Test in simulation before deployment and keep a safety operator ready to + stop a physical robot. diff --git a/GR00T-WholeBodyControl/docs/source/references/conventions.md b/GR00T-WholeBodyControl/docs/source/references/conventions.md new file mode 100644 index 0000000000000000000000000000000000000000..01d15b1f746b67463fe819f2e9cc35c6601d48fb --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/references/conventions.md @@ -0,0 +1,142 @@ +# Coordinate Frame and Rotation Conventions + +This page documents the coordinate frame, quaternion, and rotation conventions +used throughout the SONIC codebase. Getting these wrong causes silent bugs — +the robot will move but in the wrong direction or with wrong orientation. + +## Coordinate Frames + +### Isaac Lab / MuJoCo (simulation) + +- **Z-up**: Gravity is along -Z. Ground plane is XY. +- **Right-handed**: X forward, Y left, Z up. +- This is the convention used during training and evaluation. + +### SMPL / BVH (human motion data) + +- **Y-up**: Gravity is along -Y. Ground plane is XZ. +- When loading SMPL or BVH data, set `smpl_y_up: true` in the motion library + config. The motion library automatically converts Y-up to Z-up internally. + +### Summary + +| System | Up axis | Convention | +|--------|---------|------------| +| Isaac Lab | Z | Z-up, right-handed | +| MuJoCo | Z | Z-up, right-handed | +| SMPL body model | Y | Y-up | +| BVH motion files | Y | Y-up | +| Retargeted PKL data | Z | Z-up (already converted) | + +## Quaternion Convention + +### Scalar-first (wxyz) — default throughout SONIC + +The SONIC codebase uses **scalar-first (wxyz)** quaternions everywhere: + +``` +q = [w, x, y, z] +``` + +This applies to: + +- `gear_sonic/trl/utils/torch_transform.py` — all rotation utilities +- `gear_sonic/isaac_utils/rotations.py` — Isaac Lab rotation helpers (use `w_last=False`) +- Isaac Lab APIs (`body_quat_w`, `root_quat_w`, etc.) +- Motion library internal storage +- Retargeted PKL data (`root_rot` field) + +### Scalar-last (xyzw) — scipy only + +[SciPy's Rotation class](https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Rotation.html) +uses **scalar-last (xyzw)** convention: + +``` +q = [x, y, z, w] +``` + +This is only used in the **data processing scripts** (`data_process/`) when +calling `scipy.spatial.transform.Rotation`. The scripts convert to wxyz +before saving: + +```python +# In data processing (scipy xyzw → wxyz for storage) +root_quat_xyzw = Rotation.from_euler("xyz", euler_angles).as_quat() # scipy: xyzw +root_quat_wxyz = root_quat_xyzw[:, [3, 0, 1, 2]] # convert to wxyz +``` + +### The `w_last` parameter + +Functions in `gear_sonic/isaac_utils/rotations.py` accept a `w_last` boolean: + +```python +quat_rotate(q, v, w_last=False) # q is wxyz (scalar-first) — this is the default +quat_rotate(q, v, w_last=True) # q is xyzw (scalar-last) +``` + +**Always use `w_last=False`** unless you're interfacing with scipy or a system +that explicitly uses xyzw. + +### Quick reference + +| System | Convention | Order | Identity | +|--------|-----------|-------|----------| +| SONIC (torch_transform.py) | wxyz | `[w, x, y, z]` | `[1, 0, 0, 0]` | +| Isaac Lab | wxyz | `[w, x, y, z]` | `[1, 0, 0, 0]` | +| SciPy | xyzw | `[x, y, z, w]` | `[0, 0, 0, 1]` | +| MuJoCo | wxyz | `[w, x, y, z]` | `[1, 0, 0, 0]` | +| ROS | xyzw | `[x, y, z, w]` | `[0, 0, 0, 1]` | + +### Converting between conventions + +```python +# wxyz → xyzw +q_xyzw = q_wxyz[..., [1, 2, 3, 0]] + +# xyzw → wxyz +q_wxyz = q_xyzw[..., [3, 0, 1, 2]] +``` + +## Rotation Representations + +The codebase uses multiple rotation representations depending on context: + +| Representation | Shape | Used in | +|---------------|-------|---------| +| Quaternion (wxyz) | `(..., 4)` | Simulation, motion library, observations | +| Axis-angle | `(..., 3)` | `pose_aa` field in motion PKLs | +| Rotation matrix | `(..., 3, 3)` | Forward kinematics, 6D rotation encoding | +| 6D rotation | `(..., 6)` | Some observation terms (first 2 columns of rotation matrix) | +| Euler angles | `(..., 3)` | CSV motion data input (converted immediately) | + +### Axis-angle in motion data + +The `pose_aa` field in retargeted PKL files stores per-body **local** rotations +as axis-angle vectors. The direction is the rotation axis, the magnitude is +the angle in radians: + +```python +pose_aa # (T, num_bodies, 3) — axis-angle per body, MuJoCo body order +``` + +## Joint Ordering + +Isaac Lab and MuJoCo traverse the kinematic tree in different orders. The +codebase provides bidirectional index mappings per robot: + +```python +from gear_sonic.envs.manager_env.robots.g1 import ( + G1_ISAACLAB_TO_MUJOCO_DOF, # Reorder DOFs: IsaacLab → MuJoCo + G1_MUJOCO_TO_ISAACLAB_DOF, # Reorder DOFs: MuJoCo → IsaacLab +) + +# Convert joint positions from IsaacLab order to MuJoCo order: +mujoco_joints = isaaclab_joints[..., G1_ISAACLAB_TO_MUJOCO_DOF] +``` + +Motion PKL data (`dof`, `pose_aa`) is stored in **MuJoCo order**. Isaac Lab +simulation uses **IsaacLab order**. The training pipeline handles the conversion +automatically via `order_converter.py`. + +See [Training on New Embodiments](../user_guide/new_embodiments.md) for how to +define these mappings for a new robot. diff --git a/GR00T-WholeBodyControl/docs/source/references/decoupled_wbc.md b/GR00T-WholeBodyControl/docs/source/references/decoupled_wbc.md new file mode 100644 index 0000000000000000000000000000000000000000..2ff38a02dc1ff12a55177fbe9b829b5802cec41f --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/references/decoupled_wbc.md @@ -0,0 +1,155 @@ +# Decoupled WBC + +Software stack for loco-manipulation experiments across multiple humanoid platforms, with primary support for the Unitree G1. This repository provides whole-body control policies, a teleoperation stack, and a data exporter. + +--- + +## System Installation + +### Prerequisites +- Ubuntu 22.04 +- NVIDIA GPU with a recent driver +- Docker and NVIDIA Container Toolkit (required for GPU access inside the container) + +### Repository Setup + +Install Git and Git LFS: + +```bash +sudo apt update +sudo apt install git git-lfs +git lfs install +``` + +Clone the repository: + +```bash +mkdir -p ~/Projects +cd ~/Projects +git clone https://github.com/NVlabs/GR00T-WholeBodyControl.git +cd decoupled_wbc +``` + +### Docker Environment + +We provide a Docker image with all dependencies pre-installed. + +Install a fresh image and start a container: + +```bash +./docker/run_docker.sh --install --root +``` + +This pulls the latest `decoupled_wbc` image from `docker.io/nvgear`. + +Start or re-enter a container: + +```bash +./docker/run_docker.sh --root +``` + +Use `--root` to run as the `root` user. To run as a normal user, build the image locally: + +```bash +./docker/run_docker.sh --build +``` + +--- + +## Running the Control Stack + +Once inside the container, the control policies can be launched directly. + +- Simulation: + +```bash +python decoupled_wbc/control/main/teleop/run_g1_control_loop.py +``` + +- Real robot: Ensure the host machine network is configured per the [G1 SDK Development Guide](https://support.unitree.com/home/en/G1_developer) and set a static IP at `192.168.123.222`, subnet mask `255.255.255.0`: + +```bash +python decoupled_wbc/control/main/teleop/run_g1_control_loop.py --interface real +``` + +Keyboard shortcuts (terminal window): +- `]`: Activate policy +- `o`: Deactivate policy +- `9`: Release / Hold the robot +- `w` / `s`: Move forward / backward +- `a` / `d`: Strafe left / right +- `q` / `e`: Rotate left / right +- `z`: Zero navigation commands +- `1` / `2`: Raise / lower the base height +- `backspace` (viewer): Reset the robot in the visualizer + +--- + +## Running the Teleoperation Stack + +The teleoperation policy primarily uses Pico controllers for coordinated hand and body control. It also supports other teleoperation devices, including LeapMotion and HTC Vive with Nintendo Switch Joy-Con controllers. + +Keep `run_g1_control_loop.py` running, and in another terminal run: + +```bash +python decoupled_wbc/control/main/teleop/run_teleop_policy_loop.py --hand_control_device=pico --body_control_device=pico +``` + +### Pico Setup and Controls + +Configure the teleop app on your Pico headset by following the [XR Robotics guidelines](https://github.com/XR-Robotics). + +The necessary PC software is pre-installed in the Docker container. Only the [XRoboToolkit-PC-Service](https://github.com/XR-Robotics/XRoboToolkit-PC-Service) component is needed. + +Prerequisites: Connect the Pico to the same network as the host computer. + +Controller bindings: +- `menu + left trigger`: Toggle lower-body policy +- `menu + right trigger`: Toggle upper-body policy +- `Left stick`: X/Y translation +- `Right stick`: Yaw rotation +- `L/R triggers`: Control hand grippers + +Pico unit test: + +```bash +python decoupled_wbc/control/teleop/streamers/pico_streamer.py +``` + +--- + +## Running the Data Collection Stack + +Run the full stack (control loop, teleop policy, and camera forwarder) via the deployment helper: + +```bash +python decoupled_wbc/scripts/deploy_g1.py \ + --interface sim \ + --camera_host localhost \ + --sim_in_single_process \ + --simulator robocasa \ + --image-publish \ + --enable-offscreen \ + --env_name PnPBottle \ + --hand_control_device=pico \ + --body_control_device=pico +``` + +The `tmux` session `g1_deployment` is created with panes for: +- `control_data_teleop`: Main control loop, data collection, and teleoperation policy +- `camera`: Camera forwarder +- `camera_viewer`: Optional live camera feed + +Operations in the `controller` window (`control_data_teleop` pane, left): +- `]`: Activate policy +- `o`: Deactivate policy +- `k`: Reset the simulation and policies +- `` ` ``: Terminate the tmux session +- `ctrl + d`: Exit the shell in the pane + +Operations in the `data exporter` window (`control_data_teleop` pane, right top): +- Enter the task prompt + +Operations on Pico controllers: +- `A`: Start/Stop recording +- `B`: Discard trajectory diff --git a/GR00T-WholeBodyControl/docs/source/references/deployment_code.md b/GR00T-WholeBodyControl/docs/source/references/deployment_code.md new file mode 100644 index 0000000000000000000000000000000000000000..3cb1ce9edf0fa52ffed2754d46621306412fab61 --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/references/deployment_code.md @@ -0,0 +1,276 @@ +# C++ Deployment Program Flow + +This document describes how the main executables run, their arguments, and the logging/configuration options. + +## Program Pipeline + +High-level flow (matches current code): +- Input interfaces: `keyboard | gamepad | gamepad_manager | zmq | zmq_manager | ros2 | manager` +- Optional planner (when enabled) generates target animations +- Motion reader provides reference motions for non-planner mode +- Policy inference (TensorRT; optional encoder → decoder) +- Output publishing via `--output-type ` + +## Available Commands + +```sh +just build # Build main project +just clean # Clean build artifacts +just --list # Show all available commands +``` + +## Run + +### Frequency Test + +Load an ONNX model and print input/output info. This is a sanity check for model loading; the reported frequency is not TensorRT inference speed. + +```sh +# Basic usage with default settings (1000 iterations, random data) +just run freq_test policy/example/model_step_000000.onnx + +# Custom iterations and data mode +just run freq_test policy/example/model_step_000000.onnx 5000 random +``` + +**Usage:** `just run freq_test [iterations] [data_mode]` +- `model_file`: Path to ONNX model file (required) +- `iterations`: Number of inference iterations (default: 1000) +- `data_mode`: Input data type — `zeros|random|ones` (default: random) + +### Policy Deployment + +Deploy ONNX policy on G1 robot with motion reference control: + +```sh +# Example command (real robot) +just run g1_deploy_onnx_ref enP8p1s0 policy/release/model_decoder.onnx reference/example/ \ + --obs-config policy/release/observation_config.yaml \ + --encoder-file policy/release/model_encoder.onnx \ + --planner-file planner/target_vel/V2/planner_sonic.onnx \ + --input-type manager \ + --enable-motion-recording \ + --enable-csv-logs + +# MuJoCo simulation (disables CRC validation) +python ../gear_sonic/scripts/run_sim_loop.py +just run g1_deploy_onnx_ref lo policy/release/model_decoder.onnx reference/example/ \ + --obs-config policy/release/observation_config.yaml \ + --encoder-file policy/release/model_encoder.onnx \ + --planner-file planner/target_vel/V2/planner_sonic.onnx \ + --input-type manager \ + --enable-motion-recording \ + --enable-csv-logs \ + --disable-crc-check +``` + +**Usage:** `just run g1_deploy_onnx_ref [options...]` + +**Required Arguments:** +- `network_interface`: Network interface for DDS communication (e.g., `eth0`, `enp5s0`, `enP8p1s0`, `lo`) +- `model_file`: Path to ONNX policy model file +- `motion_data_path`: Path to motion data directory containing reference motions + +**Optional Arguments:** + +**Model Configuration:** +- `--obs-config `: Path to observation configuration YAML file +- `--encoder-file `: Path to ONNX encoder model file (optional, for token-based policies) +- `--planner-file `: Path to ONNX planner model file (required for ROS2, `gamepad_manager`, and `zmq_manager` planner mode) +- `--planner-precision <16|32>`: Floating point precision for planner (default: 32) +- `--policy-precision <16|32>`: Floating point precision for policy (default: 32) + +**Output Mode:** +- `--output-type `: Output interface for publishing control results + - `zmq` — Publish via ZMQ (default) + - `ros2` — Publish via ROS2 (only if built with ROS2 support) + - `all` — Create all available output interfaces simultaneously + +**Input Mode:** +- `--input-type `: Input interface type (default: `keyboard`) + - `keyboard` — Direct keyboard input + - `gamepad` — Wireless controller + - `gamepad_manager` — Gamepad + quick switching to ZMQ/ROS2 + - `zmq` — Network motion streaming + - `zmq_manager` — Dynamic switching between planner and network motion streaming + - `manager` — Dynamic switching between keyboard, gamepad, ZMQ, and ROS2 + - `ros2` — ROS2 topic control (requires planner, only if built with ROS2 support) + +**ZMQ Configuration (when using `--input-type zmq`, `zmq_manager`, `demo_gamepad_manager`, or `manager`):** +- `--zmq-host `: ZMQ server host (default: `localhost`) +- `--zmq-port `: ZMQ server port (default: `5556`) +- `--zmq-topic `: ZMQ topic/prefix (default: `pose`) +- `--zmq-conflate`: Enable ZMQ CONFLATE mode +- `--zmq-verbose`: Enable verbose ZMQ subscriber logging +- `--zmq-out-port`: Port to which control results will be published when using `--output-type zmq` (default: `5557`) +- `--zmq-out-topic`: Topic to which control results will be published when using `--output-type zmq` (default: `g1_debug`) + +**Simulation:** +- `--disable-crc-check`: Disable CRC validation (required for MuJoCo simulation) + +**Hand & Compliance Control:** +- `--set-compliance `: Set initial VR 3-point compliance (0.01 = rigid, 0.5 = compliant; default: `0.5,0.5,0.0`). Can specify 1 value (applied to both hands) or 3 comma-separated values (`left_wrist,right_wrist,head`). Runtime keyboard controls: `g/h` = left hand ±0.1, `b/v` = right hand ±0.1. +- `--max-close-ratio `: Set initial hand max close ratio (0.2–1.0; default: 1.0 = full closure allowed). Runtime keyboard controls: `x/c` = ±0.1. + +**Logging (CLI flags):** +- **Debug / analysis logs (write a single CSV file)**: + - `--target-motion-logfile `: Log the target motion tracked by the controller (visualize with `visualize_motion.py`) + - `--planner-motion-logfile `: Log planner-generated animation sequences + - `--policy-input-logfile `: Log policy input (observation) tensors + - `--record-input-file `: Record operator control inputs to CSV for later playback + - `--playback-input-file `: Play back previously recorded control inputs from CSV +- **State CSV logs (write a timestamped directory)**: + - `--logs-dir `: Base directory for state CSV logs (default: `logs/dd-mm-yy/hh-mm-ss`) + - `--enable-csv-logs`: Enable robot state CSV logging (default: OFF) + - `--enable-motion-recording`: Record the active motion stream(s) to `reference/recorded_motion/...` (default: OFF) + +## Logging (Details) + +The system provides multiple logging capabilities for debugging, analysis, and replay. + +### Motion Logging + +**Target Motion (`--target-motion-logfile `):** +- Logs the motion the controller is tracking each control frame (~50 Hz) +- CSV columns: `pos_x, pos_y, pos_z, rot_qw, rot_qx, rot_qy, rot_qz, dof_0, dof_1, ... dof_28` + - Global position (xyz) + - Global rotation quaternion (w, x, y, z) + - 29 joint angles (DoF) + +**Planner Motion (`--planner-motion-logfile `):** +- Logs animation sequences generated by the planner (~10 Hz planning updates) +- Each planner update produces a short sequence (e.g., ~100 frames) that is appended to the CSV +- Same CSV format as target motion +- Contains motion blending and replanning results + +**Motion Recording (`--enable-motion-recording`):** +- Automatically records the currently active motion stream(s) into timestamped folders under `reference/recorded_motion/YYYYMMDD/` + - **Streamed motion** (ZMQ pose topic): saved as `streamed_HHMMSS/` + - **Planner motion** (planner-generated sequence): saved as `planner_motion_HHMMSS/` +- Each recording folder contains `joint_pos.csv`, `joint_vel.csv`, `body_pos.csv`, `body_quat.csv`, etc. +- Useful for offline inspection / regression comparisons of closed-loop behavior + +### Visualization + +All motion CSV files (logged data and reference motions) can be visualized using the `visualize_motion.py` script: + +```sh +# Visualize logged motion data (single CSV file) +python visualize_motion.py --csv_path target_motion.csv + +# Visualize reference motion from motion data directory +python visualize_motion.py --motion_dir reference/example/high_jump_full_turn/ +``` + +The visualizer script can connect to a running `g1_deploy` executable to visualize target/measured robot motions in real time: + +```sh +python visualize_motion.py --realtime_debug_url tcp://localhost:5557 +``` + +This displays four G1 robots: target animation (colored), target with zero translation (green), measured sensor data (red), and motor temperature heatmap (white, with per-joint color indicators: green → yellow → orange → red/flashing by temperature). + +**Configuration:** +- Default port: 5557 (change with `--zmq-out-port `) +- Default topic: `g1_debug` (change with `--zmq-out-topic ` on executable, `--realtime_debug_topic ` on visualizer) +- For physical robots, replace `localhost` with the robot's IP address + +**Playback Controls:** +- **Space**: Pause/resume playback +- **`.`** (period): Step forward one frame +- **`,`** (comma): Step backward one frame +- **`r`**: Reset to frame 0 + +### Policy Input Logging + +**Policy Input (`--policy-input-logfile `):** +- Logs the raw observation tensor fed to the neural network policy +- Output: a single CSV file (one row per control step, all observation values) +- Useful for debugging observation configuration and input drift + +### Control Input Recording/Playback + +**Recording (`--record-input-file `):** +- Records control inputs (motion index, frame, operator state, planner state, movement commands) +- Logging starts when the control system is activated +- Tip: Wait a few seconds after lowering from gantry before starting control to give yourself setup time during playback + +**Playback (`--playback-input-file `):** +- Replays recorded control inputs for reproducible experiments +- Playback starts when the control system is activated +- Useful for testing policy changes with identical inputs + +### Robot State CSV Logger + +When enabled with `--enable-csv-logs`, the system logs detailed robot state at each control step (50 Hz). + +**Output Directory:** +- Default: `logs/dd-mm-yy/hh-mm-ss` (auto-generated timestamp) +- Custom: Use `--logs-dir ` to specify directory + +**Files Generated (split by signal type):** +- `base_quat.csv` — Base IMU quaternion (4 values: w, x, y, z) +- `base_ang_vel.csv` — Base angular velocity (3 values: x, y, z) +- `torso_quat.csv` — Torso IMU quaternion (4 values) +- `torso_ang_vel.csv` — Torso angular velocity (3 values) +- `q.csv` — Joint positions (29 joints) +- `dq.csv` — Joint velocities (29 joints) +- `action.csv` — Policy actions (29 joints) + +**CSV Format:** +- Columns: `index,time_ms,...` +- `time_ms`: Milliseconds since first log (0.0 at start, fractional allowed) +- Synchronized across all files using the same index/timestamp + +**Example:** + +```sh +just run g1_deploy_onnx_ref enp5s0 policy/model.onnx reference/motions/ \ + --obs-config policy/obs_config.yaml \ + --enable-csv-logs \ + --logs-dir logs/my_experiment +``` + +## Observation Configuration + +The system uses YAML configuration files to define which observations are fed to the policy. This allows flexible policy designs without code changes. + +**Basic Structure (`--obs-config `):** + +```yaml +observations: + - name: "body_joint_positions" + enabled: true + - name: "base_angular_velocity" + enabled: true + # ... other observations +``` + +**With Encoder (Token-Based Policies):** + +For policies that use encoded tokens, add an `encoder:` section: + +```yaml +observations: + - name: "token_state" # Encoder outputs (64-dim tokens) + enabled: true + - name: "base_angular_velocity" # Direct observations + enabled: true + +encoder: + dimension: 64 # Token output dimension + use_fp16: false # TensorRT precision (optional) + encoder_observations: + - name: "motion_joint_positions_10frame_step5" + enabled: true + # ... observations fed to encoder +``` + +Then run with `--encoder-file ` to load the encoder model. If omitted, tokens can be set externally via ROS2/ZMQ. + +**Complete Observation Reference:** + +For the full list of all available observation names, dimensions, and example configurations, see [Observation Configuration](observation_config.md). + +**Examples:** +- See `policy/observation_config_example.yaml` diff --git a/GR00T-WholeBodyControl/docs/source/references/index.md b/GR00T-WholeBodyControl/docs/source/references/index.md new file mode 100644 index 0000000000000000000000000000000000000000..5c3cd242bccf85747a4709769e012e344073a4cf --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/references/index.md @@ -0,0 +1,11 @@ +# Reference Documentation + +These pages contain the original standalone documentation that previously lived as individual `README_*.md` files. They are preserved here as detailed references. + +- [Training Code Structure](training_code.md) — training codebase architecture, pipeline flow, configuration system, and key classes +- [Deployment Code & Program Flow](deployment_code.md) — CLI arguments, logging, observation config, and visualization tools +- [Observation Configuration](observation_config.md) — YAML config format, all observation types with dimensions, and how to create custom observations +- [Motion Reference Data](motion_reference.md) — reference motion file format, conversion, verification, and usage +- [Kinematic Planner ONNX Model](planner_onnx.md) — detailed input/output specification for the ONNX-exported kinematic planner +- [JetPack 6 Flashing Guide](jetpack6.md) — flash the Orin NX on the Unitree G1 +- [Decoupled WBC (N1.5 / N1.6)](decoupled_wbc.md) — the earlier Decoupled WBC stack used in Gr00t N1.5 and N1.6 diff --git a/GR00T-WholeBodyControl/docs/source/references/jetpack6.md b/GR00T-WholeBodyControl/docs/source/references/jetpack6.md new file mode 100644 index 0000000000000000000000000000000000000000..61b4f791db9c9bc71e33394432a6a042269293a4 --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/references/jetpack6.md @@ -0,0 +1,126 @@ +# G1 JetPack 6 Flashing Guide + +## 1. Download Image + +1. **Download the required files** — get both the `.tar` file and the image file from [Jetpack 6.2](https://drive.google.com/drive/folders/1ho17ectOxi7FbaRFdpAbP4tet8BJWjbm). + +## 2. Unmount Orin NX's NVMe + +### Steps to Remove the NVMe SSD + +1. **Remove the back handle screws** + + Use a 5 mm T-handle Allen key to unscrew the two screws located at the back of the robot near the handle. + +2. **Remove the foam and plastic back cover** + + - Use the 2 mm hex tool from the Fanttik tool kit to remove the four screws holding the foam and plastic backing in place. + - Lift the backing off to expose the internal components. + +```{image} ../_static/screws.png +:width: 600px +:align: center +``` + +3. **Remove the NVMe screw on the Orin NX module** + + Use a Phillips screwdriver (also in the Fanttik tool kit) to remove the single screw securing the Orin NX's NVMe SSD. + +```{image} ../_static/ssd.png +:width: 600px +:align: center +``` + +4. **Remove the SSD card** + + Carefully slide out and remove the NVMe SSD from its slot. + +## 3. Flash the NVMe SSD + +**Mount the NVMe SSD from the Orin NX into the NVMe SSD enclosure adapter.** +(Adapter needed when burning image from a laptop) + +1. Check that the robot's SSD is unmounted. Run the following command to make sure the external SSD (where you will burn the image) is not mounted: + +```bash +sudo umount /dev/sda* +``` + +2. If the SSD was mounted, this command will safely unmount it so it's ready for imaging. + +3. Navigate to the folder where you have the image (`cd robot_NXUpgrade/`), then run the following command: + +```bash +bzip2 -dc g1-nx-j6.2.img.bz2 | sudo dd of=/dev/sda bs=4M status=progress conv=fsync +``` + +4. After it's done, eject the card with the following commands to safely unplug it: + +```bash +sudo sync +sudo udisksctl power-off -b /dev/sda +``` + +5. **Set the SSD card to the side and proceed with the second part of the flashing process!** + +## 4. Put the Robot Into Flashing Mode + +1. **Power on the G1** and wait until all three power indicator lights remain steadily lit. + +2. **Connect the robot to your laptop/desktop** using a USB-C cable. + +3. **Press and hold both white buttons** on the robot at the same time for two seconds. + +4. While still holding them, **release the top white button** and continue holding the **bottom button** for 2 seconds until the **three green lights change to two green lights**. + +```{image} ../_static/flashing.png +:width: 600px +:align: center +``` + +5. When only two lights are on, the robot is **now in flashing mode**. Open a new terminal on your computer and enter `lsusb`. You should see text containing `NVIDIA Corp. APX`. + +6. You can now proceed to run the following commands: + +```bash +sudo tar -xjvf Jetpack_6.2_nx.tar.bz2 +cd Jetpack_6.2_nx/Linux_for_Tegra +sudo ./flash_nx_module.sh +``` + +Wait patiently for about 8 minutes until it shows success. + +## 5. Reassemble the Robot + +1. After the flashing is complete, **power off the robot**. + +2. **Reinstall the Orin NX's NVMe SSD** back into its slot on the G1 robot and secure it with its screw. + +3. **Reattach the foam and plastic backing**, using the same tools you used to remove it. + +4. **Tighten all screws** to ensure the back cover and handle are securely in place. + +5. Turn on `maxn` mode on Jetson Orin using the command: + + +``` +sudo nvpmodel -m 0 +``` + +and use + + +``` +sudo jetson_clocks +sudo jetson_clocks --show +``` + +to check if it is already in Maxn model. + +## 6. Install Required JetPack Packages + +Install the packages needed for deployment: + +``` +sudo apt-get install -y nvidia-l4t-dla-compiler libcudla-dev-12-6 +``` diff --git a/GR00T-WholeBodyControl/docs/source/references/motion_reference.md b/GR00T-WholeBodyControl/docs/source/references/motion_reference.md new file mode 100644 index 0000000000000000000000000000000000000000..8e218fea93aba2d2a6102ef20db6bd018f9b626c --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/references/motion_reference.md @@ -0,0 +1,295 @@ +# Motion Reference Data + +This page describes the motion reference data format used by the C++ deployment stack, how to create your own reference motions, and how to verify and deploy them. + +The deployment stack plays back pre-loaded **reference motions** — sequences of joint positions, velocities, and full body kinematics that the policy tracks. These motions are stored as CSV files in a structured folder hierarchy. You can generate them from any source (motion capture, simulation, retargeting pipeline, etc.) as long as the output matches the format described below. + +--- + +## Folder Structure + +Each motion dataset is a directory containing one subfolder per motion clip. The C++ reader (`MotionDataReader`) auto-discovers all subfolders at startup. + +``` +reference/my_motions/ +├── motion_name_1/ +│ ├── joint_pos.csv # Joint positions +│ ├── joint_vel.csv # Joint velocities +│ ├── body_quat.csv # Body quaternions +│ ├── body_pos.csv # Body positions +│ ├── metadata.txt # Body part indexes +│ ├── body_lin_vel.csv # Body linear velocities +│ ├── body_ang_vel.csv # Body angular velocities +│ ├── smpl_joint.csv # SMPL joint positions +│ ├── smpl_pose.csv # SMPL body poses +│ └── info.txt # Detailed motion information +└── motion_name_2/ + └── ... +``` + +The C++ reader scans the base directory for subfolders, reads each subfolder as one motion, and validates frame-count consistency across all files in that folder. +--- + +## File Formats + +The C++ reader loads whichever files are present and skips missing files gracefully. However, **in practice**, most policies require: +- `joint_pos.csv`, `joint_vel.csv` — for joint-based motion tracking +- `body_quat.csv` — for anchor orientation observations (the control loop will stop if this is missing when gathering observations) +- `body_pos.csv` — for heading computation and VR 3-point observations +- `metadata.txt` — for body part index alignment when body data is present + +A motion must have **at least one valid data source** (joint, body, or SMPL) to load at startup. + +### `joint_pos.csv` + +Joint positions in **IsaacLab order** (29 joints). Each row is one timestep at 50 Hz. The first row is a header. + +| Column | Description | +|--------|-------------| +| `joint_0` … `joint_28` | Joint angles in radians (IsaacLab ordering) | + +Shape: `(timesteps, 29)` + +### `joint_vel.csv` + +Joint velocities in **IsaacLab order** (29 joints). Each row is one timestep at 50 Hz. Frame count must match `joint_pos.csv`. + +| Column | Description | +|--------|-------------| +| `joint_vel_0` … `joint_vel_28` | Joint angular velocities in rad/s (IsaacLab ordering) | + +Shape: `(timesteps, 29)` + +### `body_pos.csv` + +Body part positions in the **world frame**. Each body contributes 3 columns (x, y, z). The number of bodies varies per motion. Needed for heading computation and VR 3-point observations. + +| Column | Description | +|--------|-------------| +| `body_0_x`, `body_0_y`, `body_0_z` | Position of body 0 (root/pelvis) in meters | +| `body_1_x`, `body_1_y`, `body_1_z` | Position of body 1 in meters | +| … | … | + +Shape: `(timesteps, num_bodies * 3)` + +**We assume the root/pelvis is always at column group 0** (the first 3 columns). + +### `body_quat.csv` + +Body part orientations as quaternions in the **world frame**. Each body contributes 4 columns. The quaternion ordering is **(w, x, y, z)**. **Required for most policies** — the `motion_anchor_orientation` observation (used by most policies) will fail and stop the control system if this file is missing. + +| Column | Description | +|--------|-------------| +| `body_0_w`, `body_0_x`, `body_0_y`, `body_0_z` | Quaternion of body 0 (root/pelvis) | +| `body_1_w`, `body_1_x`, `body_1_y`, `body_1_z` | Quaternion of body 1 | +| … | … | + +Shape: `(timesteps, num_bodies * 4)` + +**We assume the root/pelvis is always at column group 0** (the first 4 columns). + +```{note} +The number of bodies in `body_quat.csv` can differ from `body_pos.csv`. The C++ reader tracks them independently (`num_bodies` vs `num_body_quaternions`). However, the root body (first column group) must be present for heading computation to work. You can use zero if you don't need root pos. +``` + +### `metadata.txt` + +Contains the **body part indexes** array, which maps each column group in `body_pos.csv` / `body_quat.csv` to the corresponding IsaacLab body index. This is needed when body data is present. + +``` +Metadata for: motion_name +============================== + +Body part indexes: +[ 0 4 10 18 5 11 19 9 16 22 28 17 23 29] + +Total timesteps: 497 +``` + +The C++ reader parses `Body part indexes:` followed by a line of space-separated integers in brackets. For example, `[0, 4, 10, 18, ...]` means column group 0 → IsaacLab body 0 (pelvis/root), column group 1 → body 4, etc. + +For a **root-only** motion (only 1 body), use: + +``` +Body part indexes: +[0] +``` + +### `body_lin_vel.csv` / `body_ang_vel.csv` + +Body part linear and angular velocities in the world frame. Same layout as `body_pos.csv` (3 columns per body). The number of bodies must match `body_pos.csv`. + +### `smpl_joint.csv` + +SMPL joint positions (typically 24 joints × 3 coordinates). Each row is one timestep. + +Shape: `(timesteps, num_smpl_joints * 3)` + +### `smpl_pose.csv` + +SMPL body poses in axis-angle representation (typically 21 poses × 3 coordinates). Each row is one timestep. + +Shape: `(timesteps, num_smpl_poses * 3)` + +```{note} +The **current reference motion tracking pipeline uses joint-based tracking only** (encoder mode 0). To enable SMPL-based reference tracking (encoder mode 2), you would need to modify the code to detect the presence of SMPL data and switch the encoder mode accordingly. +``` + +### `info.txt` + +Human-readable summary with shapes, dtypes, and value ranges. Not read by the C++ stack — purely for documentation. + +--- + +## Creating Your Own Reference Motions + +You can generate reference motions from any source — the only requirement is producing CSV files in the format above. Common approaches: + +1. **Motion capture retargeting** — retarget human mocap to the G1 model, export joint positions/velocities and body kinematics. +2. **Simulation recording** — record joint states from an IsaacLab or MuJoCo simulation at 50 Hz. +3. **Procedural generation** — programmatically create joint trajectories. + +### Minimal Files Needed + +The **minimum** set of files to create a working motion for SONIC policy: + +1. **`joint_pos.csv`** — 29 joint positions (IsaacLab order), header + one row per timestep +2. **`joint_vel.csv`** — 29 joint velocities (IsaacLab order), header + one row per timestep +3. **`body_quat.csv`** — Root quaternion (w, x, y, z), header + one row per timestep +4. **`body_pos.csv`** — Root position (x, y, z), header + one row per timestep. You can use all zeros if you don't need position tracking. +5. **`metadata.txt`** — Body part indexes (just `[0]` for root-only) + +**Example files:** + +`joint_pos.csv`: +``` +joint_0,joint_1,joint_2,...,joint_28 +0.128441,0.102713,0.020116,...,0.045231 +0.130124,0.104532,0.021045,...,0.046112 +... +``` + +`joint_vel.csv`: +``` +joint_vel_0,joint_vel_1,...,joint_vel_28 +0.143671,0.143864,...,0.012345 +... +``` + +`body_quat.csv` (root quaternion only): +``` +body_0_w,body_0_x,body_0_y,body_0_z +0.999123,0.000456,0.001234,0.040567 +... +``` + +`body_pos.csv` (root position, can be all zeros): +``` +body_0_x,body_0_y,body_0_z +0.000000,0.000000,0.000000 +... +``` + +`metadata.txt`: +``` +Metadata for: my_motion +============================== + +Body part indexes: +[0] + +Total timesteps: 100 +``` + +This gives you a **root-only** motion (1 body = pelvis/root) that most policies can track. + + +### Provided Conversion Script + +A convenience script `reference/convert_motions.py` is included for converting **joblib pickle** (`.pkl`) files to this format. This is just one possible source — you can use any tool or pipeline that produces the correct CSV output. + +```bash +cd gear_sonic_deploy +python reference/convert_motions.py [output_dir] +``` + +The pickle should be a dictionary where each key is a motion name and each value contains `joint_pos`, `joint_vel`, `body_pos_w`, `body_quat_w`, `body_lin_vel_w`, `body_ang_vel_w`, `_body_indexes`, and `time_step_total`. + +--- + +## Verifying Reference Motions + +### MuJoCo Visualization + +Use the included visualizer to check that the motion looks correct on the G1 model: + +```bash +cd gear_sonic_deploy +python visualize_motion.py --motion_dir reference/my_motions/motion_name_1/ +``` + +**Controls:** +- **Space**: Pause / resume playback +- **R**: Reset to frame 0 +- **,** / **.**: Step backward / forward one frame +- **-** / **=**: Previous / next motion (if multiple loaded) + +Verify that: +- The robot stands upright and does not clip through the floor +- Joint angles look reasonable (no extreme poses) +- The motion plays smoothly without sudden jumps +- Body positions track the expected trajectory + +--- + +## Using Reference Motions + +### With `deploy.sh` + +Pass the motion directory via `--motion-data`: + +```bash +./deploy.sh --motion-data reference/my_motions/ sim +``` + +Or use the default motions (configured in `deploy.sh`): + +```bash +./deploy.sh sim +``` + +### At Runtime + +Once deployed, use the keyboard or gamepad to browse and play motions: + +- **T**: Play current motion +- **N / P**: Next / Previous motion +- **R**: Restart from frame 0 + +See the [Keyboard tutorial](../tutorials/keyboard.md) for the full control reference. + +--- + +## Validation Rules + +The C++ reader enforces the following during loading: + +- **Frame count consistency**: All CSV files within a motion folder must have the same number of rows (excluding headers). Mismatches cause the motion to be skipped with an error. +- **Joint count consistency**: `joint_pos.csv` and `joint_vel.csv` must have the same number of columns. +- **Body count consistency**: `body_lin_vel.csv` and `body_ang_vel.csv` must have the same number of body columns as `body_pos.csv`. +- **At least one data source**: A motion must have at least some valid data (joint, body, or SMPL) to be loaded. +- **Metadata parsing**: The `metadata.txt` file must contain a `Body part indexes:` line followed by a bracketed list of integers for the motion to have correct body-part alignment. + +If a motion fails validation, it is skipped and a warning is printed. The deployment continues with the remaining valid motions. + +--- + +## Notes + +- All data is at **50 Hz** (0.02 s per timestep), matching the control loop frequency. +- Joint ordering follows **IsaacLab convention** (not MuJoCo). The C++ stack handles the conversion internally when sending motor commands. +- Body quaternions use **(w, x, y, z)** ordering. +- The first body (column group 0) must correspond to the root/pelvis for heading computation and anchor orientation observations to work correctly. +- While the C++ reader can load motions without `body_quat.csv`, the control loop will fail during observation gathering if the policy observes `motion_anchor_orientation` (which most policies do). +- CSV files must have a **header row** as the first line — the C++ reader skips the first line of every CSV. +- Values are parsed as `double` precision internally. diff --git a/GR00T-WholeBodyControl/docs/source/references/observation_config.md b/GR00T-WholeBodyControl/docs/source/references/observation_config.md new file mode 100644 index 0000000000000000000000000000000000000000..d0bba48367b6ad53caa5f48a313ea30278329987 --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/references/observation_config.md @@ -0,0 +1,437 @@ +# Observation Configuration + +This page is the complete reference for configuring observations in the deployment system. It covers the YAML configuration format, the encoder system, every available observation type, and how to create your own custom observations. + +(obs-config-format)= +## Configuration Format + +Observations are configured via a YAML file passed with `--obs-config `. Each observation has a `name` (must match a registered observation) and an `enabled` flag. + +### Basic Structure + +```yaml +observations: + - name: "motion_joint_positions" + enabled: true + - name: "motion_joint_velocities" + enabled: true + - name: "motion_anchor_orientation" + enabled: true + - name: "base_angular_velocity" + enabled: true + - name: "body_joint_positions" + enabled: true + - name: "body_joint_velocities" + enabled: true + - name: "last_actions" + enabled: true +``` + +**Key rules:** + +- Observations are concatenated **in the order listed** to form the policy input vector. +- Offsets are calculated automatically — no manual offset management needed. +- The **total dimension** of all enabled observations must match your ONNX model's input size. +- Disabled observations (`enabled: false`) are skipped entirely. +- Reordering entries changes the layout of the input tensor (offsets shift accordingly). + +(obs-config-encoder)= +### With Encoder (Token-Based Policies) + +For policies that use an encoder to compress observations into a compact token, add an `encoder:` section: + +```yaml +observations: + - name: "token_state" # Encoder outputs (dimension set below) + enabled: true + - name: "base_angular_velocity" # Direct observations + enabled: true + - name: "body_joint_positions" + enabled: true + - name: "body_joint_velocities" + enabled: true + - name: "last_actions" + enabled: true + +encoder: + dimension: 64 # Token output dimension + use_fp16: false # TensorRT precision for encoder (optional) + encoder_observations: + - name: "motion_joint_positions_10frame_step5" + enabled: true + - name: "motion_joint_velocities_10frame_step5" + enabled: true + - name: "motion_anchor_orientation_10frame_step5" + enabled: true + - name: "motion_root_z_position_10frame_step5" + enabled: true + encoder_modes: # Optional: per-mode observation requirements + - name: "g1" + mode_id: 0 + required_observations: + - motion_joint_positions_10frame_step5 + - motion_joint_velocities_10frame_step5 + - motion_anchor_orientation_10frame_step5 + - motion_root_z_position_10frame_step5 +``` + +**Encoder fields:** + +| Field | Description | +|---|---| +| `dimension` | Token output dimension (must match encoder ONNX model output). Set to 0 or omit to disable encoder. | +| `use_fp16` | Use FP16 precision for encoder TensorRT engine (default: false). | +| `encoder_observations` | Observations fed to the encoder (superset of all modes). Same name/enabled format as policy observations. | +| `encoder_modes` | *(Optional)* Per-mode observation requirements. Observations not in a mode's `required_observations` are zero-filled, saving computation. | + +Run with `--encoder-file ` to load the encoder model. If omitted, `token_state` can be set externally via ROS2/ZMQ. + +See `policy/observation_config_example.yaml` for a complete annotated example. + +### Naming Convention + +Multi-frame observations follow the pattern: `{base_name}_{N}frame_step{S}` + +- **N** = number of frames gathered (temporal window size) +- **S** = step size between frames (in control ticks at 50 Hz, so step5 = 0.1 s apart) +- Without the suffix = single current frame only + +For example, `motion_joint_positions_10frame_step5` gathers 10 frames of joint positions, sampled every 5 ticks (0.1 s), giving a 0.9 s look-ahead window. If future frames exceed the motion length, the last frame is repeated. + +--- + +## Encoder & Token Observations + +These observations relate to the encoder (tokenizer) system. See [With Encoder](obs-config-encoder) above for the YAML format. + +| Name | Dim | Description | +|---|---|---| +| `token_state` | config | Encoder output tokens (dimension set by `encoder.dimension` in YAML). Populated by local encoder inference or externally via ZMQ/ROS2. | +| `encoder_mode` | 3 | Current encoder mode ID + 2 zero-padding values. | +| `encoder_mode_4` | 4 | Current encoder mode ID + 3 zero-padding values. | + +--- + +## Motion Reference Observations + +Gathered from the currently-active motion sequence (reference motions, planner output, or ZMQ stream). All joint data uses **IsaacLab joint ordering** (29 joints). + +### Joint Positions (from motion) + +| Name | Dim | Frames | Step | Description | +|---|---|---|---|---| +| `motion_joint_positions` | 29 | 1 | — | Current frame joint positions (rad) | +| `motion_joint_positions_3frame_step1` | 87 | 3 | 1 | 3-frame window, consecutive | +| `motion_joint_positions_5frame_step5` | 145 | 5 | 5 | 5-frame window, 0.1 s apart | +| `motion_joint_positions_10frame_step1` | 290 | 10 | 1 | 10-frame window, consecutive | +| `motion_joint_positions_10frame_step5` | 290 | 10 | 5 | 10-frame window, 0.1 s apart | +| `motion_joint_positions_lowerbody_10frame_step1` | 120 | 10 | 1 | Lower-body joints only (12 joints), consecutive | +| `motion_joint_positions_lowerbody_10frame_step5` | 120 | 10 | 5 | Lower-body joints only, 0.1 s apart | +| `motion_joint_positions_wrists_10frame_step1` | 60 | 10 | 1 | Wrist joints only (6 joints), consecutive | +| `motion_joint_positions_wrists_2frame_step1` | 12 | 2 | 1 | Wrist joints only, 2 consecutive frames | + +```{note} +When upper-body control is active (e.g., via ZMQ/ROS2 teleoperation), the upper-body joint positions in these observations are replaced with the externally-provided targets. +``` + +### Joint Velocities (from motion) + +| Name | Dim | Frames | Step | Description | +|---|---|---|---|---| +| `motion_joint_velocities` | 29 | 1 | — | Current frame joint velocities (rad/s). Zero when not playing. | +| `motion_joint_velocities_3frame_step1` | 87 | 3 | 1 | 3-frame window, consecutive | +| `motion_joint_velocities_5frame_step5` | 145 | 5 | 5 | 5-frame window, 0.1 s apart | +| `motion_joint_velocities_10frame_step1` | 290 | 10 | 1 | 10-frame window, consecutive | +| `motion_joint_velocities_10frame_step5` | 290 | 10 | 5 | 10-frame window, 0.1 s apart | +| `motion_joint_velocities_lowerbody_10frame_step1` | 120 | 10 | 1 | Lower-body joints only, consecutive | +| `motion_joint_velocities_lowerbody_10frame_step5` | 120 | 10 | 5 | Lower-body joints only, 0.1 s apart | +| `motion_joint_velocities_wrists_10frame_step1` | 60 | 10 | 1 | Wrist joints only, consecutive | + +### Anchor Orientation (from motion) + +Heading-corrected relative rotation from the robot's current base orientation to the reference motion orientation. Output is the first two columns of the 3×3 rotation matrix (6 values per frame). + +| Name | Dim | Frames | Step | Description | +|---|---|---|---|---| +| `motion_anchor_orientation` | 6 | 1 | — | Current frame anchor orientation (full base quaternion) | +| `motion_anchor_orientation_10frame_step1` | 60 | 10 | 1 | 10-frame window, consecutive | +| `motion_anchor_orientation_10frame_step5` | 60 | 10 | 5 | 10-frame window, 0.1 s apart | +| `motion_anchor_orientation_heading` | 6 | 1 | — | Current frame, heading-only quaternion (yaw extracted from robot base) | +| `motion_anchor_orientation_heading_10frame_step1` | 60 | 10 | 1 | Heading-only, 10-frame window, consecutive | +| `motion_anchor_orientation_heading_10frame_step5` | 60 | 10 | 5 | Heading-only, 10-frame window, 0.1 s apart | +| `motion_anchor_orientation_refheading` | 6 | 1 | — | Current frame, reference-heading quaternion (yaw from first future ref frame) | +| `motion_anchor_orientation_refheading_10frame_step1` | 60 | 10 | 1 | Ref-heading, 10-frame window, consecutive | +| `motion_anchor_orientation_refheading_10frame_step5` | 60 | 10 | 5 | Ref-heading, 10-frame window, 0.1 s apart | + +### Root Z Position (from motion) + +| Name | Dim | Frames | Step | Description | +|---|---|---|---|---| +| `motion_root_z_position` | 1 | 1 | — | Current frame root height (m) | +| `motion_root_z_position_3frame_step1` | 3 | 3 | 1 | 3-frame window, consecutive | +| `motion_root_z_position_10frame_step1` | 10 | 10 | 1 | 10-frame window, consecutive | +| `motion_root_z_position_10frame_step5` | 10 | 10 | 5 | 10-frame window, 0.1 s apart | + +--- + +## SMPL Observations + +Gathered from SMPL data in the motion sequence (optional — requires motions with `smpl_joint.csv` / `smpl_pose.csv`). + +### SMPL Joint Positions + +3D positions per SMPL joint (24 joints × 3 = 72 per frame). + +| Name | Dim | Frames | Step | Description | +|---|---|---|---|---| +| `smpl_joints` | 72 | 1 | — | Current frame, all 24 SMPL joints | +| `smpl_joints_2frame_step1` | 144 | 2 | 1 | 2 consecutive frames | +| `smpl_joints_5frame_step5` | 360 | 5 | 5 | 5-frame window, 0.1 s apart | +| `smpl_joints_10frame_step1` | 720 | 10 | 1 | 10-frame window, consecutive | +| `smpl_joints_10frame_step5` | 720 | 10 | 5 | 10-frame window, 0.1 s apart | +| `smpl_joints_lower_10frame_step1` | 270 | 10 | 1 | Lower-body SMPL joints only (9 joints), consecutive | + +### SMPL Poses (Axis-Angle) + +3D axis-angle per SMPL body part (21 poses × 3 = 63 per frame). + +| Name | Dim | Frames | Step | Description | +|---|---|---|---|---| +| `smpl_pose` | 63 | 1 | — | Current frame, all 21 SMPL poses | +| `smpl_pose_5frame_step5` | 315 | 5 | 5 | 5-frame window, 0.1 s apart | +| `smpl_pose_10frame_step1` | 630 | 10 | 1 | 10-frame window, consecutive | +| `smpl_pose_10frame_step5` | 630 | 10 | 5 | 10-frame window, 0.1 s apart | +| `smpl_elbow_wrist_poses_10frame_step1` | 120 | 10 | 1 | Elbow + wrist poses only (4 parts), consecutive | + +### SMPL Aliases + +These use the same gatherers as the motion observations but are intended for SMPL-based policies: + +| Name | Dim | Frames | Step | Description | +|---|---|---|---|---| +| `smpl_root_z_10frame_step1` | 10 | 10 | 1 | Root height, 10 consecutive frames | +| `smpl_anchor_orientation_10frame_step1` | 60 | 10 | 1 | Anchor orientation, 10 consecutive frames | +| `smpl_anchor_orientation_2frame_step1` | 12 | 2 | 1 | Anchor orientation, 2 consecutive frames | + +--- + +## VR Tracking Observations + +VR 3-point and 5-point tracking data. When an external source (ZMQ/ROS2) provides VR data, buffered values are used directly. Otherwise, positions and orientations are computed from the motion sequence's body data and normalised to the root body frame. + +### VR 3-Point + +| Name | Dim | Description | +|---|---|---| +| `vr_3point_local_target` | 9 | 3-point positions in root frame: `[left_wrist xyz, right_wrist xyz, head xyz]` | +| `vr_3point_local_target_compliant` | 9 | Same as above (identical during teleoperation) | +| `vr_3point_local_orn_target` | 12 | 3-point orientations in root frame: `[left quat wxyz, right quat wxyz, head quat wxyz]` | +| `vr_3point_compliance` | 3 | Compliance values: `[left_arm, right_arm, head]`. Keyboard-controlled (g/h/b/v keys), range [0.0, 0.5]. | + +### VR 5-Point + +| Name | Dim | Description | +|---|---|---| +| `vr_5point_local_target` | 15 | 5-point positions in root frame: `[left_wrist, right_wrist, head, left_ankle, right_ankle]` × xyz | +| `vr_5point_local_orn_target` | 20 | 5-point orientations in root frame: 5 quaternions × wxyz | + +--- + +## Robot State History Observations + +Gathered from the StateLogger ring buffer (measured sensor data from the real robot). These provide temporal context by sampling past states. + +### Single-Frame (Current State) + +| Name | Dim | Description | +|---|---|---| +| `base_angular_velocity` | 3 | IMU angular velocity (rad/s): `[roll_rate, pitch_rate, yaw_rate]` | +| `body_joint_positions` | 29 | Current joint positions from encoders (rad, IsaacLab order) | +| `body_joint_velocities` | 29 | Current joint velocities from encoders (rad/s, IsaacLab order) | +| `last_actions` | 29 | Previous policy output (normalised action values) | +| `gravity_dir` | 3 | Gravity direction in body frame (computed from base IMU quaternion) | + +### Multi-Frame History (4 frames, step 1) + +| Name | Dim | Description | +|---|---|---| +| `his_body_joint_positions_4frame_step1` | 116 | Joint positions: 4 consecutive ticks (29 × 4) | +| `his_body_joint_velocities_4frame_step1` | 116 | Joint velocities: 4 consecutive ticks | +| `his_last_actions_4frame_step1` | 116 | Past actions: 4 consecutive ticks | +| `his_base_angular_velocity_4frame_step1` | 12 | Angular velocity: 4 consecutive ticks (3 × 4) | +| `his_gravity_dir_4frame_step1` | 12 | Gravity direction: 4 consecutive ticks | + +### Multi-Frame History (10 frames, step 1) + +| Name | Dim | Description | +|---|---|---| +| `his_body_joint_positions_10frame_step1` | 290 | Joint positions: 10 consecutive ticks (29 × 10) | +| `his_body_joint_velocities_10frame_step1` | 290 | Joint velocities: 10 consecutive ticks | +| `his_last_actions_10frame_step1` | 290 | Past actions: 10 consecutive ticks | +| `his_base_angular_velocity_10frame_step1` | 30 | Angular velocity: 10 consecutive ticks (3 × 10) | +| `his_gravity_dir_10frame_step1` | 30 | Gravity direction: 10 consecutive ticks | + +--- + +## Creating Custom Observations + +You can add your own observation types by modifying the C++ source. The observation system is built around a **registry pattern** — you write a gatherer function, register it with a name and dimension, and then use that name in your YAML config. + +All observation code lives in `gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp` inside the `G1Deploy` class. + +### Step 1: Write a Gatherer Function + +A gatherer function reads from internal state (sensor data, motion data, etc.) and writes its output into a target buffer at a given offset. The signature is: + +```cpp +bool MyObservation(std::vector& target_buffer, size_t offset) { + // Write your observation values into target_buffer starting at offset. + // Return true on success, false on failure (will stop the control loop). +} +``` + +**Available data sources inside G1Deploy** (see member variables in `g1_deploy_onnx_ref.cpp` for the full list): + +| Source | Description | +|---|---| +| `state_logger_` | Ring buffer of past robot states — IMU, joints, velocities, actions, hand states, token state | +| `current_motion_` / `current_frame_` | Currently-active motion sequence and playback cursor | +| `operator_state` | Operator control flags (`.play`, `.start`, `.stop`) | +| `vr_*_buffer_`, `left_hand_joint_buffer_`, etc. | Buffered input interface data — VR tracking, hand joints, compliance, upper-body targets | +| `heading_state_buffer_`, `movement_state_buffer_` | Thread-safe buffers for heading and planner movement commands | + +**Example** — a custom observation that outputs the torso IMU angular velocity (3 values): + +```cpp +bool GatherTorsoAngularVelocity(std::vector& target_buffer, size_t offset) { + if (!state_logger_) { return false; } + + auto hist = state_logger_->GetLatest(1); + if (hist.empty()) { return false; } + + const auto& entry = hist[0]; + target_buffer[offset + 0] = entry.body_torso_ang_vel[0]; + target_buffer[offset + 1] = entry.body_torso_ang_vel[1]; + target_buffer[offset + 2] = entry.body_torso_ang_vel[2]; + return true; +} +``` + +### Step 2: Register in the Observation Registry + +Add your observation to the `GetObservationRegistry()` method in `g1_deploy_onnx_ref.cpp`. Each entry is a tuple of `{name, dimension, gatherer_lambda}`: + +```cpp +std::vector GetObservationRegistry() { + return { + // ... existing observations ... + + // Your custom observation: + {"torso_angular_velocity", 3, + [this](std::vector& buf, size_t offset) { + return GatherTorsoAngularVelocity(buf, offset); + }}, + }; +} +``` + +The **name** is the string you'll use in the YAML config. The **dimension** must be exact — the system validates that the total of all enabled observations matches the ONNX model input size. + +### Step 3: Use in YAML Config + +Once registered, your observation is available like any built-in one: + +```yaml +observations: + - name: "torso_angular_velocity" + enabled: true + # ... other observations ... +``` + +### Tips + +- **Dimension must be fixed.** The observation dimension is set at registration time and cannot change at runtime. If you need variable-size data, pad to a fixed maximum. +- **Don't allocate in the hot path.** Gatherer functions run at 50 Hz in the control loop. Avoid `new`, `malloc`, or resizing vectors. Pre-allocate buffers in the constructor or use stack arrays. +- **Return `false` carefully.** Returning `false` from a gatherer stops the entire control loop. Only return `false` for unrecoverable errors. For missing optional data, write zeros and return `true`. +- **Thread safety.** Gatherers run on the control thread. Reading from `state_logger_` and `DataBuffer` objects is thread-safe. Accessing `current_motion_` and `current_frame_` is protected by `current_motion_mutex_` (already held when `GatherObservations()` is called). +- **Multi-frame pattern.** If your observation needs temporal windows, follow the existing `GatherHis*` or `GatherMotion*MultiFrame` patterns — they accept `num_frames` and `step_size` parameters and register multiple variants (e.g., `my_obs`, `my_obs_4frame_step1`, `my_obs_10frame_step5`). +- **Encoder observations.** Custom observations can also be used as encoder inputs. Register them in the same registry — they'll be available for both `observations:` and `encoder_observations:` in the YAML config. +- **Rebuild after changes.** After modifying the C++ source, rebuild with `just build` from the `gear_sonic_deploy/` directory. + +--- + +## Example Configurations + +### Minimal (154D — default policy) + +```yaml +observations: + - name: "motion_joint_positions" # 29D + enabled: true + - name: "motion_joint_velocities" # 29D + enabled: true + - name: "motion_anchor_orientation" # 6D + enabled: true + - name: "base_angular_velocity" # 3D + enabled: true + - name: "body_joint_positions" # 29D + enabled: true + - name: "body_joint_velocities" # 29D + enabled: true + - name: "last_actions" # 29D + enabled: true +# Total: 154D +``` + +### Token-Based Policy with Encoder + +```yaml +observations: + - name: "token_state" # 64D (from encoder) + enabled: true + - name: "base_angular_velocity" # 3D + enabled: true + - name: "body_joint_positions" # 29D + enabled: true + - name: "body_joint_velocities" # 29D + enabled: true + - name: "last_actions" # 29D + enabled: true + +encoder: + dimension: 64 + use_fp16: false + encoder_observations: + - name: "motion_joint_positions_10frame_step5" # 290D + enabled: true + - name: "motion_joint_velocities_10frame_step5" # 290D + enabled: true + - name: "motion_anchor_orientation_10frame_step5" # 60D + enabled: true + - name: "motion_root_z_position_10frame_step5" # 10D + enabled: true +``` + +### VR Teleoperation Policy + +```yaml +observations: + - name: "token_state" # 64D + enabled: true + - name: "vr_3point_local_target" # 9D + enabled: true + - name: "vr_3point_local_orn_target" # 12D + enabled: true + - name: "vr_3point_compliance" # 3D + enabled: true + - name: "base_angular_velocity" # 3D + enabled: true + - name: "body_joint_positions" # 29D + enabled: true + - name: "body_joint_velocities" # 29D + enabled: true + - name: "last_actions" # 29D + enabled: true +``` + +See [Configuration Format](obs-config-format) above for YAML syntax details. diff --git a/GR00T-WholeBodyControl/docs/source/references/planner_onnx.md b/GR00T-WholeBodyControl/docs/source/references/planner_onnx.md new file mode 100644 index 0000000000000000000000000000000000000000..d0d2e8c6fd567723af31894582ccb81ce5743cdb --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/references/planner_onnx.md @@ -0,0 +1,493 @@ +# Kinematic Planner ONNX Model Reference + +This page provides a detailed specification of the **Kinematic Planner** ONNX model inputs and outputs. The kinematic planner is the core motion generation component of the GEAR-SONIC system: given the robot's current state and high-level navigation commands, it produces a sequence of future whole-body poses (MuJoCo `qpos` frames) that the low-level whole-body controller then tracks. + +The ONNX model is part of the **C++ inference stack** and is called by the deployment runtime during operation. The C++ stack manages input construction, timing, and state management — certain combinations of inputs are invalid and are handled by the C++ layer to ensure safe operation. This page is intended for developers who want to understand the model interface at a deeper level or build custom integrations beyond the standard deployment pipeline. + +```{admonition} Training Code & Technical Report +:class: note +The kinematic planner training code and technical report will be released soon. This page documents the ONNX model interface for deployment integration. +``` + +--- + +## Overview + +The planner takes **11 input tensors** and produces **2 output tensors**. The 6 primary inputs are listed below; the remaining 5 are advanced inputs managed by the C++ stack and should not need to be modified in most cases. + +**Primary inputs:** + +| Tensor Name | Shape | Dtype | Default | +|-------------|-------|-------|---------| +| `context_mujoco_qpos` | `[1, 4, 36]` | `float32` | Required | +| `target_vel` | `[1]` | `float32` | `-1.0` (use mode default velocity) | +| `mode` | `[1]` | `int64` | Required | +| `movement_direction` | `[1, 3]` | `float32` | Required | +| `facing_direction` | `[1, 3]` | `float32` | Required | +| `height` | `[1]` | `float32` | `-1.0` (disable height control) | + +**Outputs:** + +| Tensor Name | Shape | Dtype | +|-------------|-------|-------| +| `mujoco_qpos` | `[1, N, 36]` | `float32` | +| `num_pred_frames` | scalar | `int64` | + +Where: +- **K** = `max_tokens - min_tokens + 1` (model-dependent; the range of allowed prediction horizons) +- **N** = maximum number of output frames (padded); only the first `num_pred_frames` frames are valid + +--- + +## Coordinate System + +The model operates in **MuJoCo's Z-up coordinate convention**: + +- **X** — forward +- **Y** — left +- **Z** — up + +All position and direction vectors in the inputs and outputs follow this convention. + +--- + +## Input Tensors + +(context_mujoco_qpos)= +### `context_mujoco_qpos` + +| Property | Value | +|----------|-------| +| **Shape** | `[1, 4, 36]` | +| **Dtype** | `float32` | +| **Description** | The planner's context input consisting of 4 consecutive MuJoCo `qpos` frames representing the recent states of the robot | + +This is the primary context input. It provides 4 frames of the robot's recent joint configuration at the simulation framerate. +The 36 dimensions of each frame are the standard MuJoCo `qpos` vector for the Unitree G1 (29-DOF) model: + +| Index | Field | Description | +|-------|-------|-------------| +| 0–2 | Root position | `(x, y, z)` in meters, Z-up world frame | +| 3–6 | Root quaternion | `(w, x, y, z)` orientation — MuJoCo convention | +| 7–35 | DOF positions | 29 joint angles in radians, following MuJoCo body tree order | + +```{admonition} Coordinate Frame +:class: note +All inputs — including `context_mujoco_qpos`, `movement_direction`, `facing_direction`, `specific_target_positions`, and `specific_target_headings` — should be provided in the **world coordinate frame**. The root quaternion uses MuJoCo's `(w, x, y, z)` ordering at indices 3 to 6. The model handles canonicalization internally. +``` + +### `target_vel` + +| Property | Value | +|----------|-------| +| **Shape** | `[1]` | +| **Dtype** | `float32` | +| **Description** | Desired locomotion speed override | + +Controls the target movement speed. When set to **zero or below** (e.g., `-1.0`), the model uses the default velocity for the selected mode. When set to a **positive value**, it overrides the mode's default speed (in meters per second). Note that the actual achieved speed may differ from the target due to the critically damped spring model and motion dynamics. + +| Value | Behavior | +|-------|----------| +| `<= 0.0` | Use the default velocity for the selected `mode` | +| `> 0.0` | Override with this target velocity (m/s) | + + +### `mode` + +| Property | Value | +|----------|-------| +| **Shape** | `[1]` | +| **Dtype** | `int64` | +| **Description** | Index selecting the motion style/behavior | + +Selects the motion style from the pre-loaded clip library. The mode index is clamped to the number of available clips at runtime. The default planner ships with the following modes: + +**Locomotion set:** + +| Index | Mode | Description | +|-------|------|-------------| +| 0 | `idle` | Standing still | +| 1 | `slowWalk` | Slow forward locomotion | +| 2 | `walk` | Normal walking speed | +| 3 | `run` | Running | + +**Squat / ground set:** + +| Index | Mode | Description | +|-------|------|-------------| +| 4 | `squat` | Squatting — requires `height` input (range ~0.4–0.8m) | +| 5 | `kneelTwoLeg` | Kneeling on both knees — requires `height` input (0.2m-0.4m) | +| 6 | `kneelOneLeg` | Kneeling on one knee — requires `height` input (0.2m-0.4m) | +| 7 | `lyingFacedown` | Lying face down — requires `height` input | +| 8 | `handCrawling` | Crawling on hands and knees | +| 14 | `elbowCrawling` | Crawling on elbows (more likely to overheat) | + +**Boxing set:** + +| Index | Mode | Description | +|-------|------|-------------| +| 9 | `idleBoxing` | Boxing stance (idle) | +| 10 | `walkBoxing` | Walking with boxing guard | +| 11 | `leftJab` | Left jab | +| 12 | `rightJab` | Right jab | +| 13 | `randomPunches` | Random punch sequence | +| 15 | `leftHook` | Left hook | +| 16 | `rightHook` | Right hook | + +**Style walks:** + +| Index | Mode | Description | +|-------|------|-------------| +| 17 | `happy` | Happy walking | +| 18 | `stealth` | Stealthy walking | +| 19 | `injured` | Limping walk | +| 20 | `careful` | Cautious walking | +| 21 | `objectCarrying` | Walking with hands reaching out | +| 22 | `crouch` | Crouched walking | +| 23 | `happyDance` | Dancing walk (only walk forward) | +| 24 | `zombie` | Zombie walk | +| 25 | `point` | Walking with hands pointing | +| 26 | `scared` | Scared walk | + +### `movement_direction` + +| Property | Value | +|----------|-------| +| **Shape** | `[1, 3]` | +| **Dtype** | `float32` | +| **Description** | Desired direction of movement in the MuJoCo world frame | + +A 3D direction vector `(x, y, z)` in the Z-up world coordinate system indicating where the robot should move. It is recommended to pass a normalized vector for good practice, though the model normalizes internally. Speed is controlled by `target_vel` and `mode`, not by the magnitude of this vector. + +- The planner uses the `(x, y)` components (horizontal plane) for computing the target root trajectory via a critically-damped spring model. +- When the magnitude is near zero (`< 1e-5`), the model falls back to using the `facing_direction` with a small scaling factor for in-place turning. + + +### `facing_direction` + +| Property | Value | +|----------|-------| +| **Shape** | `[1, 3]` | +| **Dtype** | `float32` | +| **Description** | Desired facing (heading) direction in the MuJoCo world frame | + +A 3D direction vector `(x, y, z)` indicating which direction the robot's torso should face. The target heading angle is computed as `atan2(y, x)` from this vector. Like `movement_direction`, this does not need to be normalized. + +This is independent of `movement_direction` — the robot can walk in one direction while facing another (e.g., strafing). + + +### `height` + +| Property | Value | +|----------|-------| +| **Shape** | `[1]` | +| **Dtype** | `float32` | +| **Description** | Desired root height for height-aware behaviors | + +Controls the target pelvis height for modes that support variable height (e.g., `squat`, `kneelTwoLeg`, `kneelOneLeg`, `lyingFacedown`). When a positive value is provided, the model searches the reference clip's keyframes and selects the one whose root height is closest to the requested value, using it as the target pose for motion generation. + +| Value | Behavior | +|-------|----------| +| `< 0.0` | Height control disabled; use the randomly-selected keyframe from the reference clip | +| `>= 0.0` | Find the closest height keyframe in the reference clip and use it as the target pose (meters) | + + +
+ +## Advanced Inputs + +These inputs are managed internally by the C++ deployment stack and **should not be modified** under normal operation. They are documented here for completeness and for advanced users who need to build custom integrations. + +### `random_seed` + +| Property | Value | +|----------|-------| +| **Shape** | `[1]` | +| **Dtype** | `int64` | +| **Description** | Seed for controlling network randomness | + + +### `has_specific_target` + +| Property | Value | +|----------|-------| +| **Shape** | `[1, 1]` | +| **Dtype** | `int64` | +| **Description** | Flag indicating whether specific waypoint targets are provided | + +| Value | Behavior | +|-------|----------| +| `0` | Ignore `specific_target_positions` and `specific_target_headings`; use `movement_direction` / `facing_direction` | +| `1` | Use the provided specific target positions and headings as waypoint constraints | + +When enabled, the spring model's target root position and heading are overridden by the values in `specific_target_positions` and `specific_target_headings`. + + +### `specific_target_positions` + +| Property | Value | +|----------|-------| +| **Shape** | `[1, 4, 3]` | +| **Dtype** | `float32` | +| **Description** | 4 waypoint positions in MuJoCo world coordinates | + +Each waypoint is a 3D position `(x, y, z)` in the Z-up world frame. The 4 waypoints correspond to 4 frames (one token's worth) of target root positions. Only used when `has_specific_target = 1`. + + +### `specific_target_headings` + +| Property | Value | +|----------|-------| +| **Shape** | `[1, 4]` | +| **Dtype** | `float32` | +| **Description** | 4 waypoint heading angles in radians | + +Target heading (yaw) angles for each of the 4 waypoint frames. These are absolute angles in the Z-up world frame, measured as rotation around the Z-axis. Only used when `has_specific_target = 1`. The last waypoint's heading (`[:, -1]`) is used as the primary target heading for the spring model. + + +### `allowed_pred_num_tokens` + +| Property | Value | +|----------|-------| +| **Shape** | `[1, K]` where `K = max_tokens - min_tokens + 1` | +| **Dtype** | `int64` | +| **Description** | Binary mask controlling the allowed prediction horizon | + +A binary mask where each element corresponds to a possible number of predicted tokens. Index `i` maps to `min_tokens + i` tokens. A value of `1` means that prediction length is allowed; `0` means it is disallowed. + +Since each token represents 4 frames, the prediction horizon in frames is `num_tokens * 4`. In our default planner we have `min_tokens = 6` and `max_tokens = 16`: + +| Index | Tokens | Frames | +|-------|--------|--------| +| 0 | 6 | 24 | +| 1 | 7 | 28 | +| 2 | 8 | 32 | +| 3 | 9 | 36 | +| 4 | 10 | 40 | +| 5 | 11 | 44 | +| 6 | 12 | 48 | +| 7 | 13 | 52 | +| 8 | 14 | 56 | +| 9 | 15 | 60 | +| 10 | 16 | 64 | + +--- + +## Output Tensors + +### `mujoco_qpos` + +| Property | Value | +|----------|-------| +| **Shape** | `[1, N, 36]` | +| **Dtype** | `float32` | +| **Description** | Predicted motion sequence as MuJoCo `qpos` frames | + +The primary output: a sequence of whole-body pose frames in the same 36-dimensional MuJoCo `qpos` format as the input (see {ref}`context_mujoco_qpos ` for the dimension layout). + +```{admonition} Important: Use num_pred_frames to Truncate +:class: warning +The output tensor `mujoco_qpos` is **not truncated** — it contains the full padded buffer. Only the first `num_pred_frames` frames are valid predictions. When consuming this output, always slice: +``` + +```python +valid_qpos = mujoco_qpos[:, :num_pred_frames, :] +``` + +The poses are in the **global MuJoCo world frame** (not canonicalized). The model internally handles canonicalization, inference, and coordinate conversion, then transforms the output back to the original world frame. The first 4 predicted frames are blended with the input context for smooth transitions. + +The root quaternion in the output uses `(w, x, y, z)` ordering (MuJoCo convention). + + +### `num_pred_frames` + +| Property | Value | +|----------|-------| +| **Shape** | scalar | +| **Dtype** | `int64` | +| **Description** | Number of valid predicted frames in the `mujoco_qpos` output | + +This value equals `num_pred_tokens * 4`, where `num_pred_tokens` is the number of motion tokens the model decided to generate (constrained by `allowed_pred_num_tokens`). Use this value to slice the `mujoco_qpos` output. + +--- + +## Internal Pipeline + +1. **Canonicalization** — The input qpos is transformed to a body-relative frame by removing the first frame's heading rotation and horizontal position. This helps the model generalize across different starting orientations and positions. + +2. **Spring Model** — A critically-damped spring model generates smooth target root trajectories and heading angles from the high-level commands, using mode-dependent average velocities from the training clips. + +3. **Target Pose Selection** — Based on the `mode` and `random_seed`, a target pose is fetched from the pre-loaded clip library and aligned (rotated/translated) to match the spring model's predicted target position and heading. + +4. **Motion Inference** — The core motion model fills in the motion between the context (current state) and target (desired future state), producing a natural transition. + +5. **Post-processing** — The output is converted back to MuJoCo qpos in the original world frame, and the first 4 frames are blended with the input context for smooth transitions. + +--- + +## Deployment Integration + +This section describes how the C++ deployment stack uses the planner at runtime. Understanding this is useful for building custom integrations or modifying the replan behavior. + +### Threading Model + +The planner runs on a **dedicated thread at 10 Hz** (`planner_dt = 0.1s`), separate from the control loop (50 Hz) and input thread (100 Hz). The planner thread: + +1. Reads the latest `MovementState` from a thread-safe buffer (written by the input interface). +2. Decides whether a replan is needed. +3. If so, calls `UpdatePlanning()` which runs TensorRT inference. +4. Stores the result in a shared buffer that the control thread picks up on its next tick. + +### Initialization + +When the planner is first enabled (e.g., pressing **ENTER** on the keyboard interface), the planner thread: + +1. Reads the robot's current base quaternion and joint positions from the latest `LowState`. +2. Calls `Initialize()`, which: + - Sets up a 4-frame context at the default standing height with zero-yaw orientation. + - Runs an initial inference with `IDLE` mode and no movement. + - Resamples the 30 Hz output to 50 Hz. +3. The control thread detects the new planner motion and switches `current_motion_` to the planner output. + +### Context Construction + +The planner requires a 4-frame context (`context_mujoco_qpos` of shape `[1, 4, 36]`). During operation, this context is sampled from the **current planner motion** (not the robot state): + +- The context starts at `gen_frame = current_frame + motion_look_ahead_steps` (default look-ahead = 2 frames at 50 Hz). +- 4 frames are sampled at 30 Hz intervals from this starting point. +- Joint positions, body positions, and quaternions are linearly interpolated (quaternions via slerp) between 50 Hz motion frames to produce the 30 Hz context samples. + +### Replan Logic + +Not every planner tick triggers a replan. The decision follows this priority: + +**1. Always replan when** (regardless of static/non-static mode): +- Locomotion mode changed +- Facing direction changed +- Height changed + +**2. For non-static modes only**, also replan when any of the following is true: +- Movement speed changed +- Movement direction changed +- Periodic replan timer expired **and** movement speed is non-zero + +Static modes (Idle, Squat, Kneel, Lying, Idle Boxing) **never** trigger replans from the second category — they only replan on mode/facing/height changes from the first category. + +**Replan intervals** (periodic timer) vary by locomotion type to balance responsiveness and computational cost: + +| Locomotion Type | Replan Interval | +|----------------|-----------------| +| Running | 0.1 s (every planner tick) | +| Crawling | 0.2 s | +| Boxing (punches, hooks) | 1.0 s | +| All others (walk, squat, styled, etc.) | 1.0 s | + +The periodic timer only triggers a replan if the current movement speed is non-zero — a stationary robot in a non-static mode (e.g., Walk mode with speed 0) will not replan on the timer. + +### Output Resampling (30 Hz → 50 Hz) + +The planner model outputs frames at **30 Hz**. The deployment stack resamples them to **50 Hz** (the control loop rate) using linear interpolation: + +- For each 50 Hz frame, compute the corresponding fractional 30 Hz frame index. +- Linearly interpolate joint positions and body positions between the two nearest 30 Hz frames. +- Slerp-interpolate body quaternions. +- Compute joint velocities by finite differencing the resampled positions (`(pos[t+1] - pos[t]) * 50`). + +The resampled motion is stored in `planner_motion_50hz_` and has `num_pred_frames * 50/30` frames (rounded down). + +### Animation Blending + +When a new planner output arrives while the previous one is still playing, the control thread **blends** the old and new animations over an 8-frame cross-fade: + +1. The old animation is rebased so `current_frame` maps to frame 0. +2. The new animation is aligned to start at `gen_frame - current_frame` in the rebased timeline. +3. Over 8 frames starting from the blend point, a linearly increasing weight `w_new` (0 → 1) is applied: + - Joint positions/velocities: `w_old * old + w_new * new` + - Body positions: `w_old * old + w_new * new` + - Body quaternions: `slerp(old, new, w_new)` +4. After the blend region, the new animation takes over completely. +5. `current_frame` is reset to 0 on the blended result. + +This ensures smooth transitions between successive planner outputs without visible discontinuities. + +### TensorRT Acceleration + +The planner runs via **TensorRT** with CUDA graph capture for low-latency inference: + +1. At startup, the ONNX model is converted to a TensorRT engine (cached on disk). +2. A **CUDA graph** is captured during initialization — this records the entire inference pass (input copy → kernel launches → output copy) as a single replayable graph. +3. On each replan, inputs are copied to GPU via pinned memory (`TPinnedVector`), the CUDA graph is launched, and outputs are copied back. +4. FP16 precision is supported via `--planner-precision 16` (default is FP32). + +### Planner Model Versions + +The deployment stack supports multiple planner model versions, auto-detected from the model filename: + +| Version | Inputs | Modes | Description | +|---------|--------|-------|-------------| +| V0 | 6 | 4 (Idle, Slow Walk, Walk, Run) | Basic locomotion only | +| V1 | 11 | 20 | Adds squat/kneel/boxing/styled walks + height control + waypoint targets | +| V2 | 11 | 27 | All V1 modes + additional styled walking modes | + +The version is determined by the presence of `V0`, `V1`, or `V2` in the planner model filename. Version determines: +- The number of input tensors (6 vs 11) +- The valid range of `mode` values +- Whether `height`, `has_specific_target`, `specific_target_positions`, `specific_target_headings`, and `allowed_pred_num_tokens` inputs are used + +--- + +## Model Properties + +The exported ONNX model has the following properties: + +- **ONNX opset version**: 17 +- **Batch size**: 1 (fixed) + +The model is distributed as a single `.onnx` file, along with a `.pt` file containing reference input/output tensors that can be used for validation. + +```{admonition} Coming Soon +:class: note +The training code, export tooling, and a full technical report will be released soon. Stay tuned for updates. +``` + +--- + +## Usage Example + +```python +import onnxruntime as ort +import numpy as np + +# Load the ONNX model +session = ort.InferenceSession("kinematic_planner.onnx") + +# Primary inputs +inputs = { + "context_mujoco_qpos": current_qpos_buffer.astype(np.float32), # [1, 4, 36] + "target_vel": np.array([-1.0], dtype=np.float32), # -1.0 = use mode default + "mode": np.array([2], dtype=np.int64), # 2 = walk + "movement_direction": np.array([[1.0, 0.0, 0.0]], dtype=np.float32), # forward + "facing_direction": np.array([[1.0, 0.0, 0.0]], dtype=np.float32), # face forward + "height": np.array([-1.0], dtype=np.float32), # -1.0 = disabled + + # Advanced inputs (typically managed by the C++ stack) + "random_seed": np.array([1234], dtype=np.int64), + "has_specific_target": np.array([[0]], dtype=np.int64), + "specific_target_positions": np.zeros([1, 4, 3], dtype=np.float32), + "specific_target_headings": np.zeros([1, 4], dtype=np.float32), + "allowed_pred_num_tokens": np.ones([1, 11], dtype=np.int64), # K = 11 for default model +} + +# Run inference +mujoco_qpos, num_pred_frames = session.run(None, inputs) + +# Extract valid frames only +num_frames = int(num_pred_frames) +predicted_motion = mujoco_qpos[0, :num_frames, :] # [num_frames, 36] + +# Each row: [x, y, z, qw, qx, qy, qz, 29 joint angles in radians] +for frame in predicted_motion: + root_pos = frame[:3] + root_quat = frame[3:7] # (w, x, y, z) + joint_angles = frame[7:36] # 29 DOF positions +``` diff --git a/GR00T-WholeBodyControl/docs/source/references/training_code.md b/GR00T-WholeBodyControl/docs/source/references/training_code.md new file mode 100644 index 0000000000000000000000000000000000000000..6c8bd5c6e58adae2ff674b05e64764216661913e --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/references/training_code.md @@ -0,0 +1,375 @@ +# Training Code Structure + +This page describes the Python training codebase under `gear_sonic/`, covering directory layout, the training pipeline, configuration system, key modules, and evaluation scripts. + +--- + +## Directory Layout + +``` +gear_sonic/ +├── train_agent_trl.py # Main training entry point +├── eval_agent_trl.py # Single-checkpoint evaluation +├── eval_exp.py # Checkpoint monitor (continuous eval) +├── config/ # Hydra configuration hierarchy +│ ├── base.yaml # Global defaults (seed, num_envs, paths) +│ ├── base_eval.yaml # Eval-specific global defaults +│ ├── eval_exp.yaml # Checkpoint monitor config +│ ├── base/ # Hydra plumbing (output dirs, resolvers) +│ ├── algo/ # PPO hyperparameters +│ ├── actor_critic/ # Actor-critic architecture configs +│ │ ├── encoders/ # Per-encoder MLP configs (g1, smpl, teleop) +│ │ ├── decoders/ # Decoder MLP configs (g1_kin, g1_dyn) +│ │ ├── critics/ # Critic backbone configs +│ │ ├── quantizers/ # FSQ quantizer config +│ │ └── universal_token/ # Assembled encoder+decoder+quantizer presets +│ ├── aux_losses/ # Auxiliary loss definitions +│ ├── callbacks/ # Training callback configs +│ ├── exp/ # Experiment presets (compose all pieces) +│ ├── manager_env/ # Environment MDP component configs +│ ├── opt/ # Logging options (wandb) +│ └── trainer/ # Trainer class selection +├── envs/ # IsaacLab environment wrappers +│ ├── manager_env/ +│ │ ├── modular_tracking_env_cfg.py # Scene, sensors, robot articulation +│ │ ├── robots/ # Per-robot configs (g1.py, h2.py) +│ │ └── mdp/ # MDP components (see below) +│ ├── wrapper/ +│ │ └── manager_env_wrapper.py # RL-facing env wrapper +│ └── env_utils/ # Joint ordering utilities +├── trl/ # Training modules (PPO, actor-critic, losses) +│ ├── trainer/ +│ │ ├── ppo_trainer.py # Base PPO trainer +│ │ └── ppo_trainer_aux_loss.py # PPO + auxiliary losses (SONIC) +│ ├── modules/ +│ │ ├── actor_critic_modules.py # Actor, Critic classes +│ │ ├── universal_token_modules.py # UniversalTokenModule (SONIC ATM) +│ │ ├── base_module.py # Shared MLP building blocks +│ │ └── data_utils.py # Batch/data helpers +│ ├── losses/ +│ │ └── token_losses.py # Reconstruction & latent auxiliary losses +│ ├── callbacks/ # Runtime callbacks +│ │ ├── im_eval_callback.py # Imitation evaluation metrics +│ │ ├── im_resample_callback.py # Adaptive motion resampling +│ │ ├── model_save_callback.py # Checkpoint saving +│ │ ├── wandb_callback.py # W&B logging +│ │ └── read_eval_callback.py # Read eval results from disk +│ └── utils/ # Math, rotation, scheduling utilities +├── utils/ # Shared utilities +│ ├── motion_lib/ # Motion library loading (PKL format) +│ ├── mujoco_sim/ # MuJoCo sim-to-sim bridge +│ └── teleop/ # VR teleoperation helpers +├── data/ # Robot models, URDF/USD assets +├── data_process/ # Motion data conversion scripts +└── scripts/ # MuJoCo sim loop, misc tools +``` + +--- + +## Training Pipeline + +Running `python gear_sonic/train_agent_trl.py +exp=manager/universal_token/all_modes/sonic_release` executes the following steps: + +### 1. Configuration Loading + +The entry point uses `@hydra.main(config_path="config", config_name="base")`. The `+exp=...` argument selects an experiment preset that composes all sub-configs: + +``` +base.yaml # Global defaults + └── +exp=manager/universal_token/all_modes/sonic_release + ├── /algo: ppo_im_phc # PPO hyperparameters + ├── /actor_critic: universal_token/all_mlp_v1 + │ ├── encoders/g1_mf_mlp, smpl_mlp, teleop_mlp + │ ├── decoders/g1_kin_mf_mlp, g1_dyn_mlp + │ ├── quantizers/fsq + │ └── critics/mlp + ├── /manager_env: base_env # Environment config + │ ├── observations/{tokenizer, policy, critic} + │ ├── rewards/tracking/base_5point_local_feet_acc + │ ├── terminations/tracking/base_adaptive_strict_ori_foot_xyz + │ └── events/tracking/level0_4 + ├── /aux_losses: universal_token/g1_recon_and_all_latent + ├── /trainer: trl_ppo_aux + └── /callbacks: model_save, wandb, read_eval, im_resample +``` + +### 2. Simulator and Accelerator Init + +After config resolution, the script: +1. Parses TRL `PPOConfig` / `ScriptArguments` / `ModelConfig` from the config dict. +2. Creates a HuggingFace `Accelerator` for multi-GPU support (DDP). +3. Launches the IsaacLab `AppLauncher` to start the Isaac Sim runtime. +4. Saves `config.yaml` and `meta.yaml` to the experiment directory. + +### 3. Environment Creation + +`create_manager_env()` instantiates the IsaacLab `ManagerBasedRLEnv` from the composed environment config, then wraps it with `ManagerEnvWrapper`: + +``` +ManagerBasedRLEnv (IsaacLab) + └── ManagerEnvWrapper + ├── Observation spaces (policy, critic, tokenizer groups) + ├── Motion command manager (motion_lib) + ├── Action transform module (optional, for pretrained ATM) + └── Keyboard / visualization hooks +``` + +### 4. Policy and Value Model Creation + +The actor and critic are instantiated from the algo config. For SONIC training, the actor backbone is `UniversalTokenModule`: + +```python +# Simplified from train_agent_trl.py +policy = custom_instantiate(config.algo.config.actor, env_config=env.config, ...) +value_model = custom_instantiate(config.algo.config.critic, env_config=env.config, ...) +``` + +The `Actor` wraps `UniversalTokenModule` as its backbone and adds a diagonal Gaussian distribution for exploration. The `Critic` wraps a separate MLP backbone. + +### 5. PPO Training Loop + +The `TRLAuxLossPPOTrainer.train()` method runs the main loop: + +``` +for iteration in range(num_learning_iterations): + # 1. Rollout: collect num_steps_per_env transitions + for step in range(num_steps_per_env): + actions = policy.rollout(obs_dict) + obs_dict, rewards, dones, infos = env.step(actions) + store(obs, actions, rewards, values, log_probs) + + # 2. GAE: compute advantages and returns + advantages = generalized_advantage_estimation(rewards, values, dones) + + # 3. PPO update: num_ppo_epochs over mini-batches + for epoch in range(num_ppo_epochs): + for mini_batch in shuffle_and_split(rollout_data): + policy_loss = clipped_surrogate_objective(...) + value_loss = clipped_value_loss(...) + aux_loss = sum(coef_i * aux_loss_i) # encoder reconstruction, etc. + total_loss = policy_loss + value_loss_coef * value_loss + + aux_loss_scale * aux_loss + optimizer.step(total_loss) + + # 4. Post-update: sync running stats, adaptive sampling, callbacks + update_scheduled_params(...) # learning rate, domain randomization + callbacks.on_step_end(...) # checkpointing, evaluation, logging +``` + +--- + +## Configuration System + +The configuration system uses [Hydra](https://hydra.cc/) with config groups and composition. + +### Hierarchy + +| Level | Path | Purpose | +|---|---|---| +| **Global** | `config/base.yaml` | Seed, num_envs, paths, wandb toggle | +| **Algorithm** | `config/algo/ppo_im_phc.yaml` | PPO hyperparameters, learning rates, epochs | +| **Actor-Critic** | `config/actor_critic/` | Network architecture (encoders, decoders, critic) | +| **Environment** | `config/manager_env/` | Observations, rewards, terminations, events | +| **Auxiliary Losses** | `config/aux_losses/` | Reconstruction and latent alignment losses | +| **Trainer** | `config/trainer/` | Trainer class selection (PPO or PPO+AuxLoss) | +| **Callbacks** | `config/callbacks/` | Checkpointing, evaluation, W&B logging | +| **Experiment** | `config/exp/` | Preset that composes all the above | + +### Experiment Presets + +Experiment configs live under `config/exp/` and use the `@package _global_` directive to set values at the root level. They compose all component configs via `defaults`: + +```yaml +# config/exp/manager/universal_token/all_modes/sonic_release.yaml +defaults: + - /algo: ppo_im_phc + - /manager_env: base_env + - override /actor_critic: universal_token/all_mlp_v1 + - override /manager_env/observations/tokenizer: unitoken_all_noz + - override /manager_env/observations/policy: local_dir_hist + - override /manager_env/rewards: tracking/base_5point_local_feet_acc + - override /manager_env/terminations: tracking/base_adaptive_strict_ori_foot_xyz + - override /manager_env/events: tracking/level0_4 + # ... +``` + +### Key Config Parameters + +| Parameter | Default | Description | +|---|---|---| +| `num_envs` | 4096 | Number of parallel simulation environments | +| `algo.config.num_learning_iterations` | 100000 | Total training iterations | +| `algo.config.num_steps_per_env` | 32 | Rollout horizon per iteration | +| `algo.config.num_learning_epochs` | 5 | PPO epochs per iteration | +| `algo.config.num_mini_batches` | 4 | Mini-batches per PPO epoch | +| `algo.config.actor_learning_rate` | 2e-5 | Actor learning rate | +| `algo.config.critic_learning_rate` | 1e-3 | Critic learning rate | +| `algo.config.clip_param` | 0.2 | PPO clipping parameter | +| `algo.config.init_noise_std` | 0.05 | Initial exploration noise std | +| `algo.config.save_interval` | 500 | Checkpoint save frequency (iterations) | + +--- + +## Universal Token Module + +The `UniversalTokenModule` implements SONIC's action transform module (ATM) -- the core architecture that maps diverse motion inputs into a shared token space. + +### Architecture + +``` + ┌─────────────┐ + G1 obs ───► │ G1 Encoder │──┐ + └─────────────┘ │ + ┌─────────────┐ │ ┌─────────┐ ┌─────────────┐ + Teleop obs───► │Teleop Encdr │──┼──► │ FSQ │──► │ G1 Dynamic │──► joint actions + └─────────────┘ │ │Quantizer│ │ Decoder │ + ┌─────────────┐ │ └─────────┘ └─────────────┘ + SMPL obs ───► │ SMPL Encoder│──┘ │ + └─────────────┘ │ ┌─────────────┐ + └───────► │G1 Kinematic │──► (aux loss only) + │ Decoder │ + └─────────────┘ +``` + +**Encoders** map different observation modalities into a shared latent space. Each encoder is an MLP that takes modality-specific tokenizer observations and outputs a fixed-size latent vector. During training, one encoder is sampled per environment according to `encoder_sample_probs`. + +**FSQ Quantizer** discretizes the continuous latent into a finite set of tokens using Finite Scalar Quantization. Each latent dimension is independently quantized to one of `fsq_level_list` discrete levels. This produces a compact, discrete token representation. + +**Decoders** reconstruct outputs from the quantized tokens plus proprioception: +- **G1 Dynamic Decoder** (`g1_dyn`): Produces joint-space actions fed to the actuators. This is the only decoder used at deployment time. +- **G1 Kinematic Decoder** (`g1_kin`): Reconstructs future motion frames from tokens. Used only during training to compute reconstruction auxiliary losses. + +### Latent Residual Mode + +For downstream tasks (e.g., object manipulation), an external policy can inject corrections into the token space without retraining the base ATM: + +| Mode | Behavior | +|---|---| +| `post_quantization` (default) | Residual added after FSQ quantization | +| `pre_quantization` | Residual added before FSQ; the sum gets quantized | +| `pre_quantization_replace` | Latent is replaced entirely by the residual | + +### Encoder Sampling + +During training, each environment is randomly assigned an encoder per episode according to `encoder_sample_probs`. The `encoder_index` observation tells the module which encoder produced the current token. At deployment, only one encoder is active (selected by the observation configuration). + +--- + +## Environment Structure + +The training environment is built on IsaacLab's `ManagerBasedRLEnv` and uses a modular MDP design where each component is configured independently via YAML. + +### MDP Components + +All MDP components live in `gear_sonic/envs/manager_env/mdp/`: + +| Module | Config path | Description | +|---|---|---| +| `observations.py` | `config/manager_env/observations/` | Observation terms for policy, critic, and tokenizer groups | +| `actions.py` | `config/manager_env/actions/` | Joint position action space | +| `rewards.py` | `config/manager_env/rewards/` | Reward terms (tracking, regularization) | +| `terminations.py` | `config/manager_env/terminations/` | Episode termination conditions | +| `events.py` | `config/manager_env/events/` | Domain randomization events | +| `commands.py` | `config/manager_env/commands/` | Motion command generation (motion library) | +| `curriculum.py` | `config/manager_env/curriculum/` | Curriculum schedules | +| `terrain.py` | (inline) | Terrain generation | +| `recorders.py` | `config/manager_env/recorders/` | Video recording | + +### Observation Groups + +Observations are split into groups, each with its own config file: + +| Group | Purpose | Example terms | +|---|---|---| +| **policy** | Direct input to the policy MLP | joint_pos, joint_vel, base_ang_vel, gravity_dir, last_actions | +| **critic** | Privileged observations for the value function | All policy obs + base_lin_vel, body_pos, body_ori | +| **tokenizer** | Input to the UniversalTokenModule encoders | Multi-future joint commands, SMPL joints, VR targets, anchor orientations | + +### Reward Terms + +Reward configs compose individual terms from `config/manager_env/rewards/terms/`. Key tracking rewards: + +| Term | Description | +|---|---| +| `tracking_relative_body_pos` | Track reference body positions (5-point: root, wrists, feet) | +| `tracking_relative_body_ori` | Track reference body orientations | +| `tracking_anchor_pos` | Track root anchor position | +| `tracking_anchor_ori` | Track root anchor orientation | +| `tracking_body_linvel` | Track reference body linear velocities | +| `tracking_body_angvel` | Track reference body angular velocities | +| `action_rate_l2` | Penalize action jerk | +| `feet_acc` | Penalize foot acceleration (smoothness) | + +### ManagerEnvWrapper + +`ManagerEnvWrapper` bridges the IsaacLab environment with the RL training loop. It handles: +- Flattening observation dicts for the policy +- Applying the optional pretrained action transform module +- Motion replay mode +- Debug visualization and keyboard controls + +--- + +## Evaluation Scripts + +### eval_agent_trl.py -- Single Checkpoint + +Loads a single checkpoint and runs evaluation in Isaac Sim. Automatically reads the training `config.yaml` from the checkpoint directory to reconstruct the full configuration. + +```bash +# Interactive visualization +python gear_sonic/eval_agent_trl.py +checkpoint=path/to/model.pt +headless=False ++num_envs=1 + +# Headless with video rendering +python gear_sonic/eval_agent_trl.py +checkpoint=path/to/model.pt +headless=True \ + ++num_envs=16 +run_once=True \ + ++manager_env.config.save_rendering_dir=path/to/output \ + ++manager_env.config.render_results=True \ + +manager_env/recorders=render +``` + +Key features: +- Merges training config with eval overrides (`eval_overrides` in config) +- Removes train-only events and terminations automatically +- Supports `+run_once=True` to exit after all environments complete one episode +- Handles `+metrics_file` to render worst-performing motions from a prior eval + +### eval_exp.py -- Checkpoint Monitor + +`CheckpointEvaluator` continuously monitors an experiment directory for new checkpoints and evaluates them sequentially. It runs as a companion process alongside training. + +```bash +python gear_sonic/eval_exp.py ++experiment_dir=path/to/experiment +``` + +For each new checkpoint, it: +1. Runs metrics evaluation (launches `eval_agent_trl.py` via subprocess) +2. Runs video rendering for the hardest motions +3. Logs results and videos to W&B (resuming the training run) +4. Marks each checkpoint as evaluated to avoid redundant work + +Configuration (`config/eval_exp.yaml`): + +| Parameter | Description | +|---|---| +| `experiment_dir` | Path to the training experiment directory | +| `scan_interval` | Seconds between checkpoint scans (default: 60) | +| `num_eval_envs` | Number of environments for metric evaluation | +| `num_render_videos` | Number of videos to render per checkpoint | +| `eval_frequency` | Only evaluate every N-th checkpoint (default: all) | +| `single_pass` | Evaluate pending checkpoints once and exit | + +--- + +## Key Classes Reference + +| Class | Module | Description | +|---|---|---| +| `Actor` | `trl/modules/actor_critic_modules.py` | Policy network: backbone + diagonal Gaussian. Maintains observation buffer for temporal models. | +| `Critic` | `trl/modules/actor_critic_modules.py` | Value function network: backbone + scalar output. Supports running mean/std normalization. | +| `UniversalTokenModule` | `trl/modules/universal_token_modules.py` | SONIC ATM: multi-encoder, FSQ quantizer, multi-decoder. Computes auxiliary reconstruction losses. | +| `TRLPPOTrainer` | `trl/trainer/ppo_trainer.py` | Base PPO trainer adapted from HuggingFace TRL. Handles rollout collection, GAE, and gradient updates. | +| `TRLAuxLossPPOTrainer` | `trl/trainer/ppo_trainer_aux_loss.py` | Extends `TRLPPOTrainer` with auxiliary loss support (reconstruction, latent alignment). | +| `PolicyAndValueWrapper` | `trl/trainer/ppo_trainer.py` | Wraps policy + value model into a single `nn.Module` for DDP-safe forward passes. | +| `ManagerEnvWrapper` | `envs/wrapper/manager_env_wrapper.py` | Bridges IsaacLab `ManagerBasedRLEnv` with the training loop. Handles obs flattening, action transforms, replay. | +| `CheckpointEvaluator` | `eval_exp.py` | Monitors experiment directory, evaluates new checkpoints, logs to W&B. | diff --git a/GR00T-WholeBodyControl/docs/source/resources/citations.md b/GR00T-WholeBodyControl/docs/source/resources/citations.md new file mode 100644 index 0000000000000000000000000000000000000000..b84152f007cba7088223c0d18fb342e524942266 --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/resources/citations.md @@ -0,0 +1,12 @@ +# Citations + +If you use GEAR-SONIC in your research, please cite: + +```bibtex +@article{luo2025sonic, + title={SONIC: Supersizing Motion Tracking for Natural Humanoid Whole-Body Control}, + author={Luo, Zhengyi and Yuan, Ye and Wang, Tingwu and Li, Chenran and Chen, Sirui and Casta\~neda, Fernando and Cao, Zi-Ang and Li, Jiefeng and Minor, David and Ben, Qingwei and Da, Xingye and Ding, Runyu and Hogg, Cyrus and Song, Lina and Lim, Edy and Jeong, Eugene and He, Tairan and Xue, Haoru and Xiao, Wenli and Wang, Zi and Yuen, Simon and Kautz, Jan and Chang, Yan and Iqbal, Umar and Fan, Linxi and Zhu, Yuke}, + journal={arXiv preprint arXiv:2511.07820}, + year={2025} +} +``` diff --git a/GR00T-WholeBodyControl/docs/source/resources/contributing.md b/GR00T-WholeBodyControl/docs/source/resources/contributing.md new file mode 100644 index 0000000000000000000000000000000000000000..5da54b5ac2074175560eca185d76361bf3dc1e0f --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/resources/contributing.md @@ -0,0 +1,3 @@ +# Contributing + +We welcome contributions! Coming soon... diff --git a/GR00T-WholeBodyControl/docs/source/resources/license.md b/GR00T-WholeBodyControl/docs/source/resources/license.md new file mode 100644 index 0000000000000000000000000000000000000000..5339979a63e890d419d78e757eca4b9e150d437f --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/resources/license.md @@ -0,0 +1,17 @@ +# License + +This project uses dual licensing. + +## Source Code + +The source code is licensed under the **Apache License 2.0**. + +## Model Weights + +The trained model checkpoints and weights are licensed under the **NVIDIA Open Model License**. + +See the complete [LICENSE](https://github.com/NVlabs/GR00T-WholeBodyControl/blob/main/LICENSE) file for the full text of both licenses. + +The NVIDIA Open Model License permits commercial use and allows you to create derivative models. When distributing models, you must include attribution and comply with [NVIDIA Trustworthy AI Terms](https://www.nvidia.com/en-us/agreements/trustworthy-ai/terms/). + +For licensing questions, contact [gear-wbc@nvidia.com](mailto:gear-wbc@nvidia.com). diff --git a/GR00T-WholeBodyControl/docs/source/resources/support.md b/GR00T-WholeBodyControl/docs/source/resources/support.md new file mode 100644 index 0000000000000000000000000000000000000000..0135ea3a6a8301eeed5c921e33f6b1662c04533a --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/resources/support.md @@ -0,0 +1,3 @@ +# Support + +For questions and issues, please contact the GEAR WBC team at [gear-wbc@nvidia.com](mailto:gear-wbc@nvidia.com). diff --git a/GR00T-WholeBodyControl/docs/source/tutorials/data_collection.md b/GR00T-WholeBodyControl/docs/source/tutorials/data_collection.md new file mode 100644 index 0000000000000000000000000000000000000000..4f7a40c9af029d8faefc466483505d941b1b45b2 --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/tutorials/data_collection.md @@ -0,0 +1,564 @@ +# Data Collection for VLA + +Record teleop demonstrations as [LeRobot](https://github.com/huggingface/lerobot) datasets for post-training with [Isaac-GR00T](https://github.com/NVIDIA/Isaac-GR00T). The data exporter runs alongside the SONIC deployment and VR teleop stack, capturing robot state, SMPL teleop poses, and camera images at a configurable frequency. + +```{admonition} Deployment model +:class: important +Everything runs **offboard on your workstation** except the **camera server**, which runs **onboard the robot computer** (e.g., Jetson Orin) where the physical cameras are connected. The camera server publishes JPEG frames over ZMQ to the workstation. +``` + +```{admonition} Supported cameras +:class: note +The tested and supported camera setup uses **Luxonis OAK cameras** (OAK-D, OAK-1, etc.). This includes a head/ego-view OAK camera and optional OAK wrist cameras. Other camera drivers (RealSense, USB webcam) are included in the codebase but have not been tested recently. + +A 3D-printable mount for the head/ego-view **OAK-D W** camera is available under [`hardware/camera_mount/`](https://github.com/NVlabs/GR00T-WholeBodyControl/blob/main/hardware/camera_mount/README.md) — see its README for print settings, the bill of materials, and how it mounts on the G1. +``` + +```{admonition} Prerequisites +:class: note +1. **Completed the [Quick Start](../getting_started/quickstart.md)** — you can run the sim2sim loop (includes [installing the deployment](../getting_started/installation_deploy.md) and [downloading model checkpoints](../getting_started/download_models.md)). +2. **Completed the [VR Teleop Setup](../getting_started/vr_teleop_setup.md)** — PICO hardware is calibrated and `.venv_teleop` is ready. +3. **Camera server running on the robot** — see [Camera Server Setup](#camera-server-setup-on-robot) below. For simulation, the MuJoCo sim loop publishes camera images automatically — no camera server needed. +``` + +--- + +## One-Time Setup (Workstation) + +On your **workstation** (where you run the C++ deployment, teleop, and data exporter), run the install script from the repo root to create a dedicated virtual environment with all data collection dependencies (LeRobot, PyAV, OpenCV, etc.): + +```sh +bash install_scripts/install_data_collection.sh +``` + +This creates `.venv_data_collection` using Python 3.10 via `uv`. It installs `gear_sonic[data_collection]` which includes `lerobot`, `av`, `opencv-python`, and other required packages. It also installs `espeak` (system package) for voice feedback during recording. + +```{tip} +This environment is separate from `.venv_teleop` and `.venv_sim` — the data exporter has heavier ML dependencies that are not needed for teleop or simulation. +``` + +--- + +## Camera Server Setup (On-Robot) + +The camera server is the **only component that runs on the robot computer** (e.g., Jetson Orin). Everything else — the C++ deployment, PICO teleop streamer, data exporter, and camera viewer — runs on your workstation. + +The camera server captures frames from the OAK cameras physically connected to the robot and publishes them over ZMQ to the workstation. + +### Step 1: Clone the repo on the robot + +SSH into your robot computer and clone this repository: + +```sh +git clone https://github.com/NVlabs/GR00T-WholeBodyControl.git +cd GR00T-WholeBodyControl +``` + +### Step 2: Run the install script + +The install script handles everything: creates the virtual environment, installs all +dependencies (including the DepthAI SDK for OAK cameras), detects connected cameras, +and optionally installs a systemd service so the camera server starts automatically +on boot. + +```sh +bash install_scripts/install_camera_server.sh +``` + +The script will: + +1. Create `.venv_camera` with `gear_sonic[camera]` (DepthAI, ZMQ, msgpack, OpenCV, tyro). +2. Detect connected OAK cameras and list their MxIDs. +3. Prompt you for each camera position (ego view, and optionally left/right wrist) and its device ID. +4. Ask whether to install the camera server as a **systemd service** (recommended). If you answer **y**, it generates the unit file, installs, enables, and starts the service automatically. + +After the script finishes, verify the service is running: + +```sh +sudo systemctl status composed_camera_server.service +journalctl -u composed_camera_server.service -f +``` + +```{note} +Other camera drivers (RealSense, USB webcam) are included in the codebase but have not been tested recently for data collection. If you need RealSense, install `pyrealsense2` into the venv after setup. See the driver files in `gear_sonic/camera/drivers/` for details. +``` + +### Manual setup (alternative) + +If you prefer not to use the install script, or need to reconfigure: + +**Finding camera device IDs:** + +Each OAK camera has a unique MxID. List all connected OAK devices: + +```sh +source .venv_camera/bin/activate +python -c "import depthai as dai; print(dai.Device.getAllAvailableDevices())" +``` + +Example output: + +```text +[XLinkDeviceState.X_LINK_BOOTED, MxId: 18443010E1ABC12300, ...] +``` + +**Starting the camera server manually:** + +```sh +source .venv_camera/bin/activate + +# Single camera (ego view only) +python -m gear_sonic.camera.composed_camera \ + --ego-view-camera oak \ + --ego-view-device-id \ + --port 5555 + +# Multiple cameras (ego view + wrist cameras) +python -m gear_sonic.camera.composed_camera \ + --ego-view-camera oak --ego-view-device-id \ + --left-wrist-camera oak --left-wrist-device-id \ + --right-wrist-camera oak --right-wrist-device-id \ + --port 5555 +``` + +Run `python -m gear_sonic.camera.composed_camera --help` for all options including `--fps`, `--use-mjpeg`, and `--mjpeg-quality`. + +**Manual systemd setup:** + +```sh +# 1. Edit the service file to match your camera setup +nano systemd/composed_camera_server.service + +# 2. Copy to systemd, enable, and start +sudo cp systemd/composed_camera_server.service /etc/systemd/system/ +sudo systemctl daemon-reload +sudo systemctl enable composed_camera_server.service +sudo systemctl start composed_camera_server.service +``` + +Once the systemd service is running, the camera server starts automatically whenever the robot boots — no manual intervention needed. + +### Connecting from the workstation + +On your workstation, the data exporter and camera viewer connect to the robot's camera server over the network. Pass the robot's IP address (the G1 robot's default IP is `192.168.123.164`): + +```sh +# Data exporter +python gear_sonic/scripts/run_data_exporter.py \ + --task-prompt "pick up the cup" \ + --camera-host 192.168.123.164 --camera-port 5555 + +# Camera viewer (to verify the feed) +python gear_sonic/scripts/run_camera_viewer.py \ + --camera-host 192.168.123.164 --camera-port 5555 +``` + +The tmux launcher also accepts `--camera-host`: + +```sh +python gear_sonic/scripts/launch_data_collection.py \ + --camera-host 192.168.123.164 \ + --task-prompt "pick up the cup" +``` + +### ZMQ message format + +The camera server publishes a single msgpack-encoded payload per frame cycle containing all camera images: + +```python +{ + "timestamps": {"ego_view": 1712345678.123, "left_wrist": 1712345678.125}, + "images": {"ego_view": "", "left_wrist": ""} +} +``` + +Images are JPEG-compressed (quality 80) and either base64-encoded strings or raw JPEG bytes (when MJPEG on-device encoding is enabled). The data exporter's `ComposedCameraClientSensor` handles both formats automatically. + +--- + +## Architecture + +The data exporter receives data from three ZMQ sources. The C++ deployment, PICO teleop, and data exporter all run **offboard on the workstation**. The camera server runs **onboard the robot** and streams frames to the workstation over the network. + +```text + Workstation (offboard) Robot (onboard) +┌──────────────────────┐ ┌──────────────────────┐ ┌───────────────┐ +│ C++ deploy │ │ pico_manager │ │ Camera │ +│ (zmq_output_handler)│ │ _thread_server.py │ │ server │ +│ │ │ │ │ (OAK cameras)│ +│ port 5557 │ │ port 5556 │ │ port 5555 │ +│ topics: g1_debug, │ │ topic: pose │ │ (JPEG/ZMQ) │ +│ robot_config│ │ (SMPL body params) │ │ │ +└──────────┬───────────┘ └──────────┬────────────┘ └──────┬────────┘ + │ │ │ + └────────────┬────────────┘───────────────────────┘ + │ (network) + ┌────────▼────────┐ + │ run_data_ │ + │ exporter.py │ + │ (workstation) │ + │ │ + │ LeRobot dataset│ + │ (parquet + mp4)│ + └─────────────────┘ +``` + +| Source | Runs on | ZMQ Topic | Default Port | Provides | +|---|---|---|---|---| +| C++ deployment | Workstation | `g1_debug` | 5557 | Joint positions, velocities, IMU quaternion | +| C++ deployment | Workstation | `robot_config` | 5557 | One-shot robot configuration at startup | +| PICO teleop streamer | Workstation | `pose` | 5556 | SMPL body parameters (teleop target poses) | +| Camera server | Robot | *(raw TCP)* | 5555 | JPEG-compressed camera images (ego view + optional wrist views) | + +--- + +## Running Data Collection + +There are two ways to run the data collection stack: an **all-in-one tmux launcher** (recommended) or **manual multi-terminal setup**. + +### Option A: All-in-One Tmux Launch (Recommended) + +The launcher starts all components in a single tmux session with four panes: + +```text +┌───────────────────────┬───────────────────────┐ +│ Pane 0: C++ Deploy │ Pane 2: Data Exporter │ +│ (gear_sonic_deploy) │ (.venv_data_collection)│ +├───────────────────────┼───────────────────────┤ +│ Pane 1: PICO Teleop │ Pane 3: Camera Viewer │ +│ (.venv_teleop) │ (.venv_data_collection)│ +└───────────────────────┴───────────────────────┘ +``` + +```{note} +Requires `tmux` to be installed (`sudo apt install tmux`). +``` + +**For simulation** (the launcher starts `run_sim_loop.py` in a separate tmux window automatically): + +```bash +python gear_sonic/scripts/launch_data_collection.py --sim +``` + +**For real robot** (camera server running on robot at `192.168.123.164`): + +```bash +python gear_sonic/scripts/launch_data_collection.py \ + --camera-host 192.168.123.164 \ + --task-prompt "pick up the cup" +``` + +**With wrist cameras** (records ego view + left/right wrist camera streams): + +```bash +python gear_sonic/scripts/launch_data_collection.py \ + --camera-host 192.168.123.164 \ + --task-prompt "pick up the cup" \ + --record-wrist-cameras +``` + +```{tip} +No need to activate a virtual environment first — the launcher automatically detects and uses `.venv_data_collection` if the required dependencies are not in the current Python. +``` + +The launcher auto-attaches to the tmux session. Use `Ctrl+b` then arrow keys to switch between panes. + +Common options: + +| Flag | Default | Description | +|---|---|---| +| `--task-prompt` | `"demo"` | Language task description (e.g., `"pick up the cup"`) | +| `--dataset-name` | *(auto: timestamp)* | Dataset name; omit to auto-generate | +| `--sim / --no-sim` | `False` | Run deploy.sh in sim mode (also starts the sim loop) | +| `--camera-host` | `localhost` | Camera server host (e.g., `192.168.123.164` for real robot) | +| `--camera-port` | `5555` | Camera server port | +| `--no-camera-viewer` | *(viewer on)* | Disable the camera viewer pane | +| `--data-exporter-frequency` | `50` | Recording frequency (Hz) | +| `--deploy-checkpoint` | *(default)* | Custom checkpoint path for deploy.sh | +| `--deploy-obs-config` | *(default)* | Custom observation config for deploy.sh | +| `--deploy-planner` | *(default)* | Custom planner model path for deploy.sh | +| `--deploy-motion-data` | *(default)* | Custom motion data path for deploy.sh | +| `--record-wrist-cameras` | `False` | Record left/right wrist camera streams in the dataset | +| `--no-text-to-speech` | *(on)* | Disable voice feedback via espeak | + +Run `python gear_sonic/scripts/launch_data_collection.py --help` for all options. + +```{tip} +The launcher automatically enables **mouse support** in the tmux session — click to select panes, scroll with the mouse wheel, and drag to resize pane borders. +``` + +**Session management:** + +| Action | Command | +|---|---| +| Switch panes | `Ctrl+b`, then arrow keys | +| Detach (keep running) | `Ctrl+b`, then `d` | +| Reattach | `tmux attach -t sonic_data_collection` | +| Kill session | `Ctrl+\` in any pane, or `tmux kill-session -t sonic_data_collection` | + +### Option B: Manual Multi-Terminal Setup + +If you prefer individual control over each process, run them in separate terminals: + +**Terminal 1 — MuJoCo Simulator** *(skip for real robot)*: + +```bash +source .venv_sim/bin/activate +python gear_sonic/scripts/run_sim_loop.py \ + --enable-image-publish --enable-offscreen --camera-port 5555 +``` + +The `--enable-image-publish` and `--enable-offscreen` flags are required so the +sim renders camera images and streams them over ZMQ on the specified port. +The data exporter subscribes to this port the same way it subscribes to a +physical camera server. + +For real robot deployment, skip this terminal and see [VR Whole-Body Teleop](vr_wholebody_teleop.md) instead. + +**Terminal 2 — C++ Deployment** (from `gear_sonic_deploy/`): + +```bash +cd gear_sonic_deploy +source scripts/setup_env.sh +./deploy.sh --input-type zmq_manager sim +# Wait until you see "Init done" +``` + +**Terminal 3 — PICO Teleop Streamer:** + +```bash +source .venv_teleop/bin/activate +python gear_sonic/scripts/pico_manager_thread_server.py --manager +``` + +**Terminal 4 — Data Exporter:** + +```bash +source .venv_data_collection/bin/activate +python gear_sonic/scripts/run_data_exporter.py --task-prompt "pick up the cup" +``` + +**Terminal 5 (optional) — Camera Viewer:** + +```bash +source .venv_data_collection/bin/activate +python gear_sonic/scripts/run_camera_viewer.py +``` + +All options are provided via CLI flags — no interactive prompts. Key flags: + +| Flag | Default | Description | +|---|---|---| +| `--task-prompt` | `"demo"` | Language task description for this session | +| `--dataset-name` | *(auto: timestamp)* | Dataset name. Omit to create a new one, or pass an existing name to append episodes | +| `--data-collection-frequency` | `50` | Recording frequency (Hz) | +| `--root-output-dir` | `outputs` | Parent directory for saved datasets | + +```{tip} +Datasets are saved under `//`. If `--dataset-name` +is not specified, a timestamped name is generated automatically. +``` + +### Recording Controls + +There are two ways to control recording: **PICO VR controllers** (recommended during teleop) or **keyboard over ZMQ**. + +**PICO VR Controllers (via `manager_state` topic):** + +| Input | Action | +|---|---| +| **Left Grip + A** | **Toggle** recording — starts a new episode, or stops and saves the current one | +| **Left Grip + B** | **Discard** the current episode (saved to disk but flagged for removal during post-processing) | + +These buttons work in any manager mode (POSE, PLANNER, etc.) and are independent of the mode-switching controls. + +**Keyboard over ZMQ:** + +| Key | Action | +|---|---| +| `c` | **Toggle** recording (same as Left Grip + A) | +| `x` | **Discard** episode (same as Left Grip + B — flagged for removal) | + +```{note} +Keyboard commands are sent via a separate ZMQ publisher (default port `5580`). The data exporter subscribes to this channel automatically. You can send keys from any ZMQ publisher on that port, or integrate with the C++ deployment's keyboard handler. +``` + +--- + +## Camera Viewer + +A standalone camera viewer is available for monitoring camera feeds and recording raw video independently of the data exporter. + +```bash +source .venv_data_collection/bin/activate +python gear_sonic/scripts/run_camera_viewer.py --camera-host localhost --camera-port 5555 +``` + +The viewer connects to the same ZMQ camera server used by the data exporter and displays all detected camera streams in a tiled OpenCV window. + +**Controls** (OpenCV window must be focused): + +| Key | Action | +|---|---| +| `R` | Start/stop video recording | +| `Q` | Quit | + +Recordings are saved to `camera_recordings/rec_/` with one MP4 per camera stream. This is useful for: +- Verifying camera placement and image quality before starting data collection +- Recording reference videos alongside the LeRobot dataset +- Debugging camera server connectivity + +Run `python gear_sonic/scripts/run_camera_viewer.py --help` for all options. + +--- + +## CLI Options + +All options can be viewed with `--help`: + +```bash +python gear_sonic/scripts/run_data_exporter.py --help +``` + +Key options: + +| Flag | Default | Description | +|---|---|---| +| `--task-prompt` | `"demo"` | Language task description for annotation | +| `--dataset-name` | *(auto: timestamp)* | Dataset name; omit to auto-generate, or reuse an existing name to append | +| `--data-collection-frequency` | `50` | Recording frequency in Hz | +| `--camera-host` | `localhost` | Camera server hostname | +| `--camera-port` | `5555` | Camera server port | +| `--sonic-zmq-host` | `localhost` | SMPL pose publisher host | +| `--sonic-zmq-port` | `5556` | SMPL pose publisher port | +| `--state-zmq-host` | `localhost` | Robot state publisher host | +| `--state-zmq-port` | `5557` | Robot state publisher port | +| `--root-output-dir` | `outputs` | Root directory for saved datasets | +| `--text-to-speech / --no-text-to-speech` | `True` | Voice feedback via espeak | + +--- + +## Output Format + +Datasets are saved in the [LeRobot v2.1](https://github.com/huggingface/lerobot) format under `//`: + +```text +outputs/2026-04-03-14-30-00-G1-robot01/ +├── data/ +│ ├── train-00000.parquet # Tabular data (joint states, actions, annotations) +│ └── ... +├── videos/ +│ ├── observation.images.ego_view/ +│ │ ├── episode_000000.mp4 # H264-encoded ego camera video +│ │ └── ... +│ ├── observation.images.left_wrist/ # (only with --record-wrist-cameras) +│ └── observation.images.right_wrist/ # (only with --record-wrist-cameras) +└── meta/ + ├── info.json # Dataset metadata (fps, features, sizes) + ├── modality.json # GR00T modality configuration + ├── episodes.jsonl # Per-episode metadata + └── tasks.jsonl # Task prompt definitions +``` + +### Recorded Data Channels + +Each frame contains: + +| Feature | Shape | Description | +|---|---|---| +| `observation.state.joint_position` | `(N,)` | Actuated joint positions (rad) | +| `observation.state.joint_velocity` | `(N,)` | Actuated joint velocities (rad/s) | +| `observation.state.body_rotation_6d` | `(6,)` | Base orientation (6D rotation) | +| `observation.state.projected_gravity` | `(3,)` | Gravity vector in body frame | +| `observation.images.ego_view` | `(480, 640, 3)` | Ego camera image (saved as MP4 video) | +| `observation.images.left_wrist` | `(480, 640, 3)` | Left wrist camera (only with `--record-wrist-cameras`) | +| `observation.images.right_wrist` | `(480, 640, 3)` | Right wrist camera (only with `--record-wrist-cameras`) | +| `action.joint_position` | `(N,)` | Teleop target joint positions | +| `action.body_rotation_6d` | `(6,)` | Teleop target body rotation | +| `annotation.human.action.task_description` | string | Task prompt for this frame | + +--- + +## Post-Processing Datasets + +After recording, you can clean and merge datasets using the processing script. +All commands below run in the **data collection virtual environment**: + +```bash +source .venv_data_collection/bin/activate +``` + +### Remove Discarded Episodes + +Episodes discarded during collection (`x` key or Left Grip + B) are saved to disk +but flagged in `meta/info.json`. By default, the processing script removes these +flagged episodes so they are excluded from fine-tuning: + +```bash +# Clean a single dataset (removes discarded episodes + stale SMPL frames) +python gear_sonic/scripts/process_dataset.py \ + --dataset-path outputs/my_dataset \ + --output-path outputs/my_dataset_cleaned +``` + +To keep discarded episodes (e.g., for inspection), pass `--no-remove-discarded`. + +### Remove Stale SMPL Frames + +Teleop pauses or ZMQ frame drops create frames where `teleop.smpl_pose` is all +zeros. The processing script detects these and also removes consecutive +frozen (identical) lead-in frames that precede them: + +```bash +# Clean a single dataset in-place +python gear_sonic/scripts/process_dataset.py \ + --dataset-path outputs/my_dataset + +# Clean and write to a new directory (non-destructive) +python gear_sonic/scripts/process_dataset.py \ + --dataset-path outputs/my_dataset \ + --output-path outputs/my_dataset_cleaned +``` + +```{warning} +If you collected data using **VR 3-point tracking mode** (VR_3PT), the +`teleop.smpl_pose` column will be all zeros because VR_3PT uses raw VR +positions/orientations instead of SMPL body parameters. In this case, you +**must** disable SMPL cleaning to avoid dropping all frames: + + python gear_sonic/scripts/process_dataset.py \ + --dataset-path outputs/my_dataset \ + --output-path outputs/my_dataset_cleaned \ + --no-remove-stale-smpl +``` + +### Merge Multiple Datasets + +Combine several recording sessions into a single dataset. The script +validates that all sessions share the same `script_config` (robot +configuration) before merging: + +```bash +# Merge by listing datasets on the command line +python gear_sonic/scripts/process_dataset.py \ + --dataset-path outputs/session1 outputs/session2 outputs/session3 \ + --output-path outputs/merged_dataset + +# Or use a text file (one dataset path per line, # for comments) +python gear_sonic/scripts/process_dataset.py \ + --dataset-list datasets.txt \ + --output-path outputs/merged_dataset +``` + +SMPL cleaning is applied by default during merging. When enabled, the script +removes entire frames where the SMPL teleop pose is stuck at zeros — this +happens during operator pauses or ZMQ packet-drop periods where the SMPL +stream stops updating. Consecutive frozen (identical) frames that lead into +a zero block are also removed, since they represent stale data right before +the dropout. To skip this cleaning and merge only, add `--no-remove-stale-smpl`. + +--- + +## Next Steps: Fine-tune and Deploy + +The output dataset is directly compatible with the [Isaac-GR00T](https://github.com/NVIDIA/Isaac-GR00T) post-training pipeline. To fine-tune a VLA model on your collected data and deploy it for autonomous inference, see the [VLA Workflow tutorial](vla_workflow.md). diff --git a/GR00T-WholeBodyControl/docs/source/tutorials/gamepad.md b/GR00T-WholeBodyControl/docs/source/tutorials/gamepad.md new file mode 100644 index 0000000000000000000000000000000000000000..a0b0541dfd4ec806320c8f24d54d48c5b559e3e1 --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/tutorials/gamepad.md @@ -0,0 +1,117 @@ +# Motion Tracking and Kinematic Planner with Gamepad Controls + +Control the robot using a Unitree wireless gamepad for reference motion playback and planner-based locomotion (using `--input-type gamepad`). + +```{admonition} Prerequisites +:class: note +Complete the [Installation Guide](../getting_started/installation_deploy) and build the project before proceeding. +``` + +```{warning} +The gamepad interface requires a physical Unitree wireless gamepad connected to the robot. It is **not available in sim2sim** — use the [keyboard interface](keyboard.md) to test in simulation first. +``` + +```{admonition} Emergency Stop +:class: danger +Press **Select** at any time to immediately stop control and exit. Always keep a hand ready to press **Select**. +``` + +## Launch + +```bash +# From gear_sonic_deploy/ +bash deploy.sh --input-type gamepad real +``` + +## Step-by-Step: Normal Mode (Reference Motion Tracking) + +Normal Mode plays back pre-loaded reference motions. This is the default mode when the program starts. + +1. Press **Start** to start the control system. +2. Press **A** to play the current reference motion — the robot executes it to completion. +4. Press **R1** to switch to the next motion sequence, or **L1** for the previous one. +5. Press **A** again to play the new motion. +6. To stop mid-motion and return to the first frame, press **B** — the robot pauses without terminating the policy. +7. Use **D-pad Left / Right** to nudge the heading (±0.1 rad per press). +8. Press **X** or **Y** to reinitialize the base quaternion and reset the heading to zero, i.e. robot will think the current facing is the facing at the first frame of the reference. +9. When done, press **Select** to stop control and exit. + +## Step-by-Step: Planner Mode (Real-time Motion Generation) + +Planner Mode gives you analog stick control — steer with the left stick, aim the facing direction with the right stick, and cycle through locomotion modes. + +1. From Normal Mode, press **F1** to switch to Planner Mode. The terminal will print `Planner enabled`. +2. The robot starts in Slow Walk mode (mode 1). Push the **left stick forward** to walk — movement direction is computed from the stick angle relative to the current facing direction. +3. Steer the **right stick left / right** to smoothly rotate the facing direction (continuous, ±0.02 rad per frame). +4. Press **R1** to cycle to the next movement mode (Idle → Slow Walk → Walk → Run → Squat → Kneel Two Legs → Kneel → Idle → …). Press **L1** to cycle backward. +5. Hold **R2** to increase speed (for standing modes 1–3) or height (for squat modes 4–6). Hold **L2** to decrease. Speed/height changes by ±0.02 per frame while held. +6. When the left stick is in the dead zone, standing modes automatically switch to Idle; squat/kneel modes hold their pose with speed 0. +7. To pause, press **B** — the robot resets to Idle immediately. +8. Press **F1** again to return to Normal Mode, or **Select** to stop and exit. + +## Control Reference + +### System Controls (Both Modes) + +| Button | Action | +|--------|--------| +| **Start** | Start control system | +| **Select** | Stop control and exit (emergency stop) | +| **F1** | Toggle between Normal / Planner modes | +| **X** or **Y** | Reinitialize base quaternion and reset heading | +| **D-pad Left / Right** | Adjust delta heading (±0.1 rad) | + +### Normal Mode Buttons + +| Button | Action | +|--------|--------| +| **A** | Play current motion to completion | +| **B** | Restart current motion from beginning (pause at frame 0) | +| **L1** / **R1** | Previous / Next motion sequence | + +### Planner Mode Buttons + +**Movement:** + +| Input | Action | +|-------|--------| +| **Left Stick** | Movement direction (computed from stick angle + facing direction) | +| **Right Stick** | Facing direction (continuous rotation, ±0.02 rad/frame) | + +**Mode & Speed:** + +| Button | Action | +|--------|--------| +| **L1** / **R1** | Previous / Next movement mode (cycles 0–6) | +| **L2** (hold) | Decrease speed (modes 1–3) or height (modes 4–6), ±0.02/frame | +| **R2** (hold) | Increase speed (modes 1–3) or height (modes 4–6), ±0.02/frame | +| **A** | Play / resume motion | + +**Emergency:** + +| Button | Action | +|--------|--------| +| **B** | Pause (reset to Idle) | +| **Select** | Stop control and exit | + +## Movement Modes + +The gamepad cycles through 7 modes with **L1** / **R1**. Use **L2** / **R2** (hold) to adjust speed or height. + +| ID | Mode | L2/R2 Adjusts | Range | +|----|------|---------------|-------| +| 0 | Idle | — | — | +| 1 | Slow Walk | speed | 0.2–0.8 m/s | +| 2 | Walk | speed | 0.8–1.5 m/s | +| 3 | Run | speed | 1.5–3.0 m/s | +| 4 | Squat | height | 0.1–0.8 m | +| 5 | Kneel (two legs) | — | — | +| 6 | Kneel | — | — | + +```{tip} +For lateral (side-stepping) movement, we recommend keeping the target velocity at around **0.4 m/s**. Higher velocities during strafing can cause the robot's feet to collide due to the cross-legged foot placement required for lateral steps. +``` + +```{note} +When entering Squat (mode 4) from an adjacent mode, the height automatically initializes to 0.8 m. +``` diff --git a/GR00T-WholeBodyControl/docs/source/tutorials/isaac_teleop_publisher_setup.md b/GR00T-WholeBodyControl/docs/source/tutorials/isaac_teleop_publisher_setup.md new file mode 100644 index 0000000000000000000000000000000000000000..3becfc4aedb09743c85817f5e45ec94fb8a5d9ff --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/tutorials/isaac_teleop_publisher_setup.md @@ -0,0 +1,197 @@ +# Isaac Teleop Setup (CloudXR / DeviceIO, in-process) + +This page documents the Isaac Teleop / CloudXR bring-up for **G1 with a Thor backpack** that drives `GR00T-WholeBodyControl` directly from the headset. Using `pico_manager_thread_server.py --input-source isaac-teleop`, the CloudXR runtime is hosted **in-process** via the `isaacteleop[cloudxr]` Python package. + +```{admonition} Scope +:class: important +Real-robot deployment is supported only on **G1 + Thor backpack**. Sim2Sim (MuJoCo) can run on both Thor and x86_64 workstations. +``` + +## Prerequisites + +1. **Completed the [Quick Start](../getting_started/quickstart.md)** — you can run the Sim2Sim loop (includes [installing the deployment](../getting_started/installation_deploy.md) and [downloading model checkpoints](../getting_started/download_models.md)). +2. **Completed the [VR Teleop Setup](../getting_started/vr_teleop_setup.md)** — `.venv_teleop` is ready and `install_pico.sh` has been run (on Thor for real-robot deployment; on your workstation for Sim2Sim). + +This page is a condensed, repo-specific version of the upstream [Isaac Teleop](https://nvidia.github.io/IsaacTeleop/) docs: + +- [Quick Start](https://nvidia.github.io/IsaacTeleop/main/getting_started/quick_start.html) +- [`isaacteleop[cloudxr]` Python API](https://nvidia.github.io/IsaacTeleop/main/) + +## Step 1: Prepare the Thor Host + +Install the prerequisites on Thor (skip any you've already done for the rest of the deploy): + +```bash +sudo apt install -y build-essential curl git-lfs +git lfs install +``` + +```{note} +The remainder of this step (max power mode, thermal check) uses Thor/Jetson-specific tools. If you are running Sim2Sim (MuJoCo) on an x86_64 workstation instead of real G1 hardware, skip ahead to Step 2. +``` + +For Thor performance, enable max power mode before teleoperation: + +```bash +sudo nvpmodel -m 0 +sudo jetson_clocks +``` + +Optional thermal / over-current check: + +```bash +cat /sys/class/hwmon/hwmon*/oc*_event_cnt +``` + +## Step 2: Confirm `isaacteleop[cloudxr]` Installed + +```{note} +This step is a checkpoint, not a new action. `install_pico.sh`, run during [VR Teleop Setup](../getting_started/vr_teleop_setup.md) Step 3, already installs `isaacteleop[cloudxr]`. If that step is complete, skip to Step 3 below; otherwise, complete it before continuing. +``` + +For reference, `install_pico.sh` installs the package from the public NVIDIA index: + +```bash +# Already wired into install_pico.sh; shown here for reference +uv pip install 'isaacteleop[cloudxr]~=1.3.0' --prerelease=allow \ + --extra-index-url https://pypi.nvidia.com +``` + +It also seeds `~/cloudxr.env` with `NV_DEVICE_PROFILE=Quest3` (override by editing the file). `CloudXRLauncher` reads this on startup. + +## Step 3: Start the C++ Deployment + +From `gear_sonic_deploy/`: + +```bash +export TensorRT_ROOT=$HOME/TensorRT # only if not already in ~/.bashrc +./docker/run-ros2-dev.sh + +# inside the container (setup_env.sh is sourced automatically): +just build # first run only + +# run one of these: +./deploy.sh --input-type zmq_manager real # real robot +./deploy.sh --input-type zmq_manager sim # Sim2Sim (MuJoCo) +# Wait until you see "Init done" +``` + +```{note} +For Sim2Sim, first start the MuJoCo simulator on the host as shown in the [Quick Start](../getting_started/quickstart.md) Sim2Sim section, or the deployment will have nothing to control. +``` + +## Step 4: Launch the Teleop Streamer + +From the **repo root**: + +```bash +source .venv_teleop/bin/activate +python gear_sonic/scripts/pico_manager_thread_server.py --manager \ + --input-source isaac-teleop + +# If running offboard with a display, add visualization: +# --vis_vr3pt --vis_smpl +``` + +On startup the streamer brings up the in-process CloudXR runtime and logs `Isaac Teleop session initialized.`, then repeats `waiting for Isaac Teleop body data (connect the headset to CloudXR)...` until you connect the client in Step 5. + +## Step 5: Connect the XR Client + +Connecting the client starts the body-data stream the streamer is waiting for. + +- Open the [Isaac Teleop Web Client](https://nvidia.github.io/IsaacTeleop/client/) in the headset browser +- Enter the IP address of the host running the streamer +- Accept the self-signed certificate at `https://:48322` +- On Thor, change **Video Codec** from the default **AV1** to **H.264** or **H.265 (HEVC)**. +- Return to the client page and click **Connect** + +Once connected, stand in the [calibration pose](vr_wholebody_teleop.md#calibration-pose) and press **A+B+X+Y** on the PICO controllers to start the policy; the first press also runs the startup calibration and enters PLANNER (locomotion) mode. Then press **A+X** to switch to POSE mode for whole-body teleop, where your motion maps directly to the robot. See [Complete PICO Controls](vr_wholebody_teleop.md#pico-controls) for the other modes and the emergency stop. + +For quick validation, the same client URL can also be opened in a desktop browser. + +If you prefer to run the WebXR client from source instead of the hosted client, follow the CloudXR/WebXR build instructions linked from the [Isaac Teleop Quick Start](https://nvidia.github.io/IsaacTeleop/main/getting_started/quick_start.html). + +## Step 6: Start Camera Visualization + +Stream cameras to the headset via upstream IsaacTeleop's `camera_viz.sh`. If you don't have the IsaacTeleop repo yet, clone it first: + +```bash +git clone --recurse-submodules https://github.com/NVIDIA/IsaacTeleop.git +``` + +Then create the environment for the camera visualization streamer: + +```bash +cd IsaacTeleop +examples/camera_viz/camera_viz.sh setup +source examples/camera_viz/.venv/bin/activate +cd examples/camera_viz +``` +### Optional: Camera Preview in a Window +If you have video preview available such as on Thor or desktop simulation, it might be best to test your camera first with the "window" mode: +```bash +./camera_viz.sh run configs/YOUR_CAMERA.yaml --mode window +``` + +Choosing the correct yaml configuration camera for your setup is critical. The v4l2.yaml configuration is default but other configs are available: +```bash +# run just one of these that best matches your camera +./camera_viz.sh run configs/v4l2.yaml --mode window +./camera_viz.sh run configs/oakd.yaml --mode window +./camera_viz.sh run configs/zed.yaml --mode window +./camera_viz.sh run configs/realsense.yaml --mode window +``` +If you do not see correct output for your camera, you may need to modify the closest yaml file to your setup or create your own in the configs folder. Consider the following: +- Having more than one camera connected will modify what channel the video should be served from. +- First, try the command "lsusb" to see if your camera is connected. +- Next, use "ls -1 /dev/video* to list all video device modes. +- If you are using a v4l2 setup (Video4Linux2), these cammands will help to debug what is possible: + - To see the association of channels to cameras: "v4l2-ctl --list-devices" + - For each node, see what it captures and in what pixel formats: "v4l2-ctl --device=/dev/video0 --list-formats" + - To see full details on formats and available resolutions: "v4l2-ctl --device=/dev/video0 --list-formats-ext" +Once you have this information, make sure the settings in your yaml config match. + +### XR Camera Streaming Command + +Once you are certain you have valid yaml configured, shut down any preview windows and run instead with "--mode xr", which will stream frames to Isaac Teleop: + +```bash +./camera_viz.sh run configs/[YOUR_CAMERA].yaml --mode xr +``` +This should also be consistent with the [instructions found at IsaacTeleop](https://nvidia.github.io/IsaacTeleop/main/references/camera_streaming.html). + +## Troubleshooting + +### `RuntimeError: Failed to get OpenXR system: -35` + +In this setup, that error usually means the XR client is not connected yet. Re-check: + +- The headset / web client is fully connected to `https://:48322` +- The CloudXR runtime subprocess is still alive (look for the `Isaac Teleop session initialized.` log) +- `~/cloudxr.env` exists and has the right `NV_DEVICE_PROFILE` for your headset + +### Client connects but the video encoder fails to initialize + +On Thor, this usually means the client's **Video Codec** is still set to its default, **AV1**. Switch the client to **H.264** or **H.265 (HEVC)** and reconnect (see Step 5). + +### `gear_sonic_deploy` build error + +If `just build` fails, rebuild from scratch inside the container (Step 3): + +```bash +rm -rf build +just build +``` + +### `isaacteleop` import error + +Re-run `install_pico.sh` to reinstall `isaacteleop[cloudxr]~=1.3.0` into `.venv_teleop`. If `pypi.nvidia.com` is unreachable, check your network and the `--extra-index-url` flag. + +### Body data not arriving + +The streamer logs `[IsaacTeleopReader] No DeviceIO data for 5.0s, flagging disconnect` if the headset stops feeding body data. Confirm: + +1. The headset is still connected to CloudXR (Step 5). +2. The Pico body trackers are paired and calibrated (see [VR Teleop Setup → Motion Tracker Setup](../getting_started/vr_teleop_setup.md)). +3. The first time the schema runs, watch for `[IsaacTeleopReader] Unrecognised body_data schema: type=...` — if you see it, the upstream `FullBodyTrackerPico.get_body_pose().data` shape changed and `_body_data_to_24x7()` in `gear_sonic/utils/teleop/input_readers.py` needs an extra branch for the new layout. + diff --git a/GR00T-WholeBodyControl/docs/source/tutorials/keyboard.md b/GR00T-WholeBodyControl/docs/source/tutorials/keyboard.md new file mode 100644 index 0000000000000000000000000000000000000000..7177c56329b9f0e952fce3d66bb6fbe7a66fa3e7 --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/tutorials/keyboard.md @@ -0,0 +1,253 @@ +# Motion Tracking and Kinematic Planner with Keyboard Controls + +
+ +
In-the-wild navigation demo using the kinematic planner with keyboard controls.
+
+ +```{video} ../_static/Keyboard_Guidance.mp4 +:width: 100% +``` +*Video: Keyboard control walkthrough — starting the control system, playing reference motions, and using planner mode for real-time locomotion.* + +Control the robot using keyboard commands for reference motion playback and planner-based locomotion (using `--input-type keyboard`). + +```{admonition} Prerequisites +:class: note +Complete the [Quick Start](../getting_started/quickstart.md) to have the sim2sim loop running. +``` + +```{admonition} Emergency Stop +:class: danger +Press **`O`** at any time to immediately stop control and exit. Always keep a hand near the keyboard ready to press **`O`**. +``` + +## Launch + +**Sim2Sim (MuJoCo):** + +```bash +# Terminal 1 — MuJoCo simulator (from repo root) +source .venv_sim/bin/activate +python gear_sonic/scripts/run_sim_loop.py + +# Terminal 2 — C++ deployment (from gear_sonic_deploy/) +bash deploy.sh --input-type keyboard sim +``` + +**Real Robot:** + +```bash +# From gear_sonic_deploy/ +bash deploy.sh --input-type keyboard real +``` + +## Step-by-Step: Normal Mode (Reference Motion Tracking) + +Normal Mode plays back pre-loaded reference motions. This is the default mode when the program starts. + +1. In Terminal 2, press **`]`** to start the control system. +2. In the MuJoCo window, press **`9`** to drop the robot to the ground. +3. Go back to Terminal 2, press **`T`** to play the current reference motion — the robot executes it to completion. +4. Press **`N`** to switch to the next motion sequence, or **`P`** for the previous one. +5. Press **`T`** again to play the new motion. +6. To replay the same motion, press **`T`** again after it finishes. To stop mid-motion and return to the first frame, press **`R`** — the robot pauses at the first frame without terminating the policy. +7. Use **`Q`** / **`E`** to nudge the heading left or right (±π/12 rad per press). +8. Press **`I`** to reinitialize the base quaternion and reset the heading to zero, i.e. robot will think the current facing is the facing at the first frame of the reference. +9. When done, press **`O`** to stop control and exit. + +## Step-by-Step: Planner Mode (Real-time Motion Generation) + +Planner Mode lets you control the robot in real time — choose a locomotion style, steer with WASD, and adjust speed and height on the fly. + +1. From Normal Mode, press **`ENTER`** to switch to Planner Mode. The terminal will print `Planner enabled`. +2. The robot starts in the **Locomotion** motion set. Press **`1`** for slow walk, **`2`** for walk, or **`3`** for run. +3. Press **`W`** to walk forward. The robot uses a momentum system — holding a direction key sets momentum to full; releasing it lets the robot gradually decelerate and return to idle. +4. Steer with **`A`** / **`D`** (adjust heading and moving direction together) or turn in place with **`Q`** / **`E`** (±π/6 rad per press, only facing direction). +5. Press **`,`** / **`.`** to strafe left / right. +6. Press **`S`** to move backward. +7. Adjust speed with **`9`** (decrease) / **`0`** (increase). Speed ranges depend on the current mode (see tables below). +8. Press **`N`** to cycle to the next motion set (Locomotion → Squat → Boxing → Styled Walking → …). Use **`P`** to go back. +9. Within a motion set, press **`1`**–**`8`** to pick a specific mode (see the Motion Sets section below). +10. For squat-type modes, adjust body height with **`-`** (lower) / **`=`** (higher), clamped to 0.2–0.8 m. +11. If you need an immediate halt, press **`R`**, **`` ` ``**, or **`~`** — this resets movement momentum to zero instantly. +12. Press **`ENTER`** again to return to Normal Mode, or **`O`** to stop and exit. + +## Control Reference + +### System Controls (Both Modes) + +| Key | Action | +|-----|--------| +| **]** | Start control system | +| **O** | Stop control and exit (emergency stop) | +| **ENTER** | Toggle between Normal / Planner modes | +| **I** | Reinitialize base quaternion and reset heading | +| **Z** | Toggle encoder mode (between mode 0 and mode 1, if encoder loaded) | +| **F** | Report motor temperatures (TTS voice alert) | + +### Normal Mode Keys + +| Key | Action | +|-----|--------| +| **T** | Play current motion to completion | +| **R** | Restart current motion from beginning (pause at frame 0) | +| **P** / **N** | Previous / Next motion sequence | +| **Q** / **E** | Adjust delta heading (at policy level) left / right (±π/12 rad) | + +### Planner Mode Keys + +**Movement:** + +| Key | Action | +|-----|--------| +| **W** / **S** | Move forward / backward | +| **A** / **D** | Adjust heading slightly and move forward (left / right) | +| **,** / **.** | Strafe left / right | + +**Heading:** + +| Key | Action | +|-----|--------| +| **Q** / **E** | Adjust facing direction (at planner level) left / right (±π/6 rad) | +| **J** / **L** | Adjust delta heading (at policy level) left / right (±π/12 rad) | + +**Mode & Speed:** + +| Key | Action | +|-----|--------| +| **N** / **P** | Next / Previous motion set | +| **1**–**8** | Select mode within the current set | +| **9** / **0** | Decrease / Increase movement speed | +| **-** / **=** | Decrease / Increase height (non-standing sets, 0.2–0.8 m) | +| **T** | Play motion | + +**Emergency:** + +| Key | Action | +|-----|--------| +| **R** / **`** / **~** | Emergency stop (immediate momentum reset) | + +## Motion Sets + +A **motion set** is a group of related movement styles (e.g., locomotion, gestures, or crouching). Selecting a mode within a set makes the robot behave in that style. Each set contains up to eight selectable modes. + +Cycle through motion sets with **`N`** (next) / **`P`** (previous). Within each set, press **`1`**–**`8`** to select a mode. For more details on the underlying planner model, mode indices, and input/output specifications, see the [Kinematic Planner ONNX Model Reference](../references/planner_onnx.md). + +### Set 0 — Locomotion (Standing) + +| Key | Mode | Speed Range | +|-----|------|-------------| +| **1** | Slow Walk | 0.2–0.8 m/s | +| **2** | Walk | — | +| **3** | Run | 1.5–3.0 m/s | +| **4** | Happy | — | +| **5** | Stealth | — | +| **6** | Injured | — | + +```{tip} +For lateral (side-stepping) movement using **`,`** / **`.`**, we recommend keeping the target velocity at around **0.4 m/s**. Higher velocities during strafing can cause the robot's feet to collide due to the cross-legged foot placement required for lateral steps. +``` + +
+
+ +
Happy styled walking.
+
+
+ +
Stealth styled walking.
+
+
+ +
Injured styled walking.
+
+
+ +
Running locomotion mode.
+
+
+ +### Set 1 — Squat / Ground + +Height adjustable with **`-`** / **`=`** (0.2–0.8 m). Initial height defaults to 0.8 m when entering this set. + +| Key | Mode | Speed Range | +|-----|------|-------------| +| **1** | Squat | static | +| **2** | Kneel (Two Legs) | static | +| **3** | Kneel (One Leg) | static | +| **4** | Hand Crawling | 0.4–1.0 m/s | +| **5** | Elbow Crawling | 0.7–1.0 m/s | + +
+
+ +
Kneeling mode with variable height control.
+
+
+ +
Hand crawling locomotion.
+
+
+ +
Elbow crawling locomotion.
+
+
+ +### Set 2 — Boxing + +| Key | Mode | Speed Range | +|-----|------|-------------| +| **1** | Idle Boxing | static | +| **2** | Walk Boxing | 0.7–1.5 m/s | +| **3** | Left Jab | 0.7–1.5 m/s | +| **4** | Right Jab | 0.7–1.5 m/s | +| **5** | Random Punches | 0.7–1.5 m/s | +| **6** | Left Hook | 0.7–1.5 m/s | +| **7** | Right Hook | 0.7–1.5 m/s | + +
+ +
Boxing mode demo.
+
+ +### Set 3 — Additional Styled Walking + +| Key | Mode | +|-----|------| +| **1** | Careful | +| **2** | Object Carrying | +| **3** | Crouch | +| **4** | Happy Dance | +| **5** | Zombie | +| **6** | Point | +| **7** | Scared | + +## Movement Momentum System + +The planner usage in keyboard mode uses a momentum-based movement system: +- Pressing a direction key (**W/S/A/D/,/.**) sets momentum to **1.0** (full speed). +- Each frame without a direction key press, momentum decays multiplicatively (×0.999). +- When momentum drops below **0.1**, the robot transitions to idle (for the Locomotion set) or holds the current static pose (for Squat and Boxing sets). +- Emergency stop (**R/`/~**) instantly resets momentum to zero. + +This means you don't need to hold a key down — a single press starts movement, and the robot coasts to a stop naturally. diff --git a/GR00T-WholeBodyControl/docs/source/tutorials/live_camera_teleop.md b/GR00T-WholeBodyControl/docs/source/tutorials/live_camera_teleop.md new file mode 100644 index 0000000000000000000000000000000000000000..f58096361ff63601b61c3652b7950fa98cff8224 --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/tutorials/live_camera_teleop.md @@ -0,0 +1,423 @@ +# Live Camera Teleoperation with GEM-X + +Teleoperate the Unitree G1 from a **single RGB camera** — no motion-capture suit, no VR headset, no body trackers. [GEM-X](https://github.com/NVlabs/GEM-X) recovers your full-body 3D motion from the webcam image, this example converts that motion to SMPL and streams it over SONIC's ZMQ pose protocol (`--input-type zmq`), and the SONIC policy tracks it while keeping the robot balanced. + +
+ +
Live webcam teleoperation on a real G1 — the operator (left) is tracked by a single camera, with no suit, gloves, or headset. The robot is on a safety gantry, as recommended for a first hardware session.
+
+ +```{admonition} Prerequisites +:class: note +Complete the [Quick Start](../getting_started/quickstart.md) so the sim2sim loop runs. This tutorial drives the same interface described in [Streaming Motion Tracking](zmq.md) — read that page first if you have not used `--input-type zmq` before, since the keyboard controls and stream protocol are shared. +``` + +```{admonition} GEM-X is an external dependency +:class: important +GEM-X is **not** bundled with this repository and is not installed by any of the SONIC install scripts. You install it separately and point this example at it with `--gemx-root` (or `$GEMX_ROOT`). GEM-X is Apache-2.0 licensed. +``` + +--- + +## How It Works + +```text + Webcam ──RGB──► GEM-X rolling window ──SOMA──► soma_to_smpl.py + (1 camera) YOLOX → ViTPose (2D) 77 joints SOMA → SMPL, 24 joints + → diffusion denoiser root-local, Z-up + → SOMA decoder │ + │ ZMQ Protocol v3 + │ topic "pose" + │ tcp://:5556 + ▼ + SONIC C++ deployment + smpl encoder → WBC policy + │ + ▼ + Unitree G1 (sim or real) +``` + +| Stage | What happens | Output | +|---|---|---| +| Capture | OpenCV reads one frame from the camera | RGB image | +| Detect + 2D pose | YOLOX detection, then ViTPose keypoints (ONNX) | 77 2D keypoints | +| Lift to 3D | GEM-X diffusion denoiser runs over a rolling window of buffered frames | SOMA latent | +| Decode | GEM-X decoder produces body parameters, and the gravity-aligned root orientation is fused in | SOMA body params (77 joints) | +| Convert | `soma_to_smpl.py` maps SOMA → SMPL, rotates Y-up → Z-up, removes the root rotation | `smpl_joints` (24×3), `body_quat` | +| Stream | ZMQ `PUB` socket sends a Protocol v3 message per frame | wire message on topic `pose` | +| Track | SONIC's `smpl` encoder (mode 2) turns SMPL into latent commands the policy tracks | robot joint targets | + +### Why stream SMPL instead of joint angles + +SONIC ships a learned `smpl` encoder that was trained on human SMPL motion, so the human-to-robot mapping already lives **inside the policy**. Streaming SMPL hands that mapping to the encoder and keeps the online loop free of any per-frame retargeting or inverse-kinematics solve. + +The alternative — converting SOMA to G1 joint angles offline and streaming Protocol v1 — requires a retargeter in the loop, which is the expensive part. The conversion in `soma_to_smpl.py` mirrors `gear_sonic/scripts/pico_manager_thread_server.py:process_smpl_joints` exactly, so the streamed data is in-distribution for the same encoder the PICO path uses. + +### Files + +Everything lives in `gear_sonic/examples/live_camera_teleop/`: + +| File | Role | +|---|---| +| `webcam_stream.py` | Live loop: capture → GEM-X → convert → publish. Also the camera test tool. | +| `soma_to_smpl.py` | SOMA → SMPL conversion and the Protocol v3 ZMQ publisher. Imported, not run directly. | +| `soma_pt_to_sonic_v3.py` | Replays a saved GEM-X result (`hpe_results.pt`) — camera-free verification. | +| `README.md` | Short reference version of this tutorial. | + +--- + +## Prerequisites + +1. **SONIC deployment built and runnable** — see [Installation (Deployment)](../getting_started/installation_deploy.md) and [Quick Start](../getting_started/quickstart.md), which also covers [downloading the released checkpoint](../getting_started/download_models.md) whose `smpl` encoder this path uses. +2. **GEM-X installed** — cloned, its virtual environment built, and its checkpoint available. It is a separate repository that no SONIC install script sets up for you; Step 1 below covers it. +3. **A camera** — any UVC USB webcam or laptop camera. A video file works as a stand-in for everything except the live feel. + +--- + +## Step 1 — Install GEM-X and Set Up + +Follow the [GEM-X installation guide](https://github.com/NVlabs/GEM-X/blob/main/docs/INSTALL.md). The short version: + +```bash +git clone --recursive https://github.com/NVlabs/GEM-X.git +cd GEM-X + +pip install uv && uv venv .venv --python 3.12 && source .venv/bin/activate +uv pip install torch torchvision --index-url https://download.pytorch.org/whl/cu126 +uv pip install -e third_party/soma && (cd third_party/soma && git lfs pull) +bash scripts/install_env.sh +``` + +Then two things this example needs on top of a stock GEM-X install: + +```bash +# 1. SOMA body-model assets must be reachable at inputs/soma_assets +mkdir -p inputs +ln -sfn "$PWD/third_party/soma/assets" inputs/soma_assets + +# 2. Extra Python deps for the ZMQ bridge +uv pip install pyzmq scipy +``` + +```{admonition} The soma_assets link is not optional +:class: warning +`webcam_stream.py` constructs `SomaLayer(data_root="/inputs/soma_assets")`. Without that path the run fails when the converter starts up, *after* the camera and the denoiser have already initialized — which looks like a late, confusing crash. Create the link before the first live run. + +Use an **absolute** target as shown. A relative target would be resolved against `inputs/`, not the repo root, and silently produce a dangling link. +``` + +GEM-X downloads its checkpoint from Hugging Face on first use, so the first run is slower than later ones. + +Finally, point the example at both repositories. The scripts live in the SONIC repo but import `gem` from GEM-X, so they run in **GEM-X's virtual environment** and locate SONIC by path: + +```bash +cd /path/to/GEM-X +source .venv/bin/activate + +export GEMX_ROOT=$PWD +export SONIC_ROOT=/path/to/GR00T-WholeBodyControl +``` + +```{admonition} Why SONIC_ROOT is required +:class: note +The conversion in `soma_to_smpl.py` reuses SONIC's own rotation helpers (`gear_sonic.isaac_utils.rotations`) so the SMPL convention matches the deployment bit for bit. Python puts the *script's* directory on `sys.path`, not the repo root, so `gear_sonic` is not importable from the GEM-X environment unless you set `$SONIC_ROOT` (or pass `--sonic-root`). Those helpers only need `torch` and `numpy` — you do **not** need to install `gear_sonic` into the GEM-X environment. +``` + +--- + +## Step 2 — Verify the Input Camera + +Do this before involving the robot. `webcam_stream.py` doubles as the camera test tool: `--kp-only` runs detection and 2D keypoints but skips the 3D denoiser, so it starts fast and needs no checkpoint. + +### Find the device + +```bash +ls /dev/video* +v4l2-ctl --list-devices # sudo apt install v4l-utils +v4l2-ctl -d /dev/video0 --list-formats-ext # modes the camera actually supports +``` + +The index in `/dev/videoN` is what you pass to `--source` (`/dev/video0` → `--source 0`). Many USB cameras expose several nodes for one physical device; the lowest-numbered one is usually the capture node. + +### Preview the tracking + +```bash +# Live overlay window, press q to quit +python "$SONIC_ROOT/gear_sonic/examples/live_camera_teleop/webcam_stream.py" \ + --source 0 --kp-only --show +``` + +You should see green keypoints tracking your body and a steady frame rate on the status line. Fix any missing or jumping keypoints here — the controller can only track what the estimator sees. + +### Request a capture mode + +A camera's default mode is often low-resolution or low-fps, which caps the teleop rate. Request one of the modes reported by `--list-formats-ext`: + +```bash +... --source 0 --resolution 1280x720 --cap-fps 30 +``` + +Cameras silently ignore unsupported settings, so the script logs the mode it actually negotiated. Check that line rather than assuming the request applied. + +(framing)= +### How to Position the Camera and the Operator + +Monocular estimation only knows what it can see, and the lower body is what the controller balances on: + +- Keep the **full body in frame, including the feet.** Cropped legs make the lower body unreliable. +- Stand roughly **2–3 m back**, camera near torso height, lens roughly level. +- **Even and front facing** light. Avoid strong backlight. +- **One person in frame** — the largest detection is the one tracked. +- **Face the same direction as the robot.** Orientation is streamed in the robot's own frame, so operator and robot should be aligned. + +--- + +## Step 3 — Dry Run Without a Camera + +`soma_pt_to_sonic_v3.py` replays a saved GEM-X result, which isolates the conversion and the wire format from anything camera-related. Produce an `hpe_results.pt` by running any GEM-X demo on a video, then: + +```bash +# Convert frame 0 and print the result — no ZMQ, no robot +python "$SONIC_ROOT/gear_sonic/examples/live_camera_teleop/soma_pt_to_sonic_v3.py" \ + --pt /path/to/hpe_results.pt --dry-run +``` + +Once that prints a sane 24-joint skeleton, replay it as a stream against the simulator (start Terminals 1 and 2 from the next section first): + +```bash +python "$SONIC_ROOT/gear_sonic/examples/live_camera_teleop/soma_pt_to_sonic_v3.py" \ + --pt /path/to/hpe_results.pt --fps 30 --loop +``` + +This is the fastest way to confirm the SONIC side is wired up correctly, and it is reproducible — the same input produces the same motion every time. + +--- + +## Step 4 — Teleop in Simulation + +Run **three terminals**. + +### Terminal 1 — MuJoCo simulator + +From the **repo root**: + +```bash +source .venv_sim/bin/activate +python gear_sonic/scripts/run_sim_loop.py +``` + +### Terminal 2 — C++ deployment + +From `gear_sonic_deploy/`: + +```bash +cd gear_sonic_deploy +source scripts/setup_env.sh +./deploy.sh --input-type zmq --zmq-host localhost sim +# Wait until you see "Init done" +``` + +```{admonition} Do not add --zmq-port or --zmq-topic here +:class: warning +`deploy.sh` forwards only `--input-type` and `--zmq-host`. The port and topic stay at the deployment defaults, `5556` and `pose` — exactly what this example publishes on, so no extra flags are needed. + +Unrecognized flags are not rejected; `deploy.sh` treats any unknown argument as the positional interface argument, so passing `--zmq-port 5556` can quietly override your `sim` / `real` selection depending on argument order. +``` + +(remote-streamer)= +```{admonition} Running GEM-X on another machine +:class: note +The example **binds** its ZMQ `PUB` socket (`tcp://*:5556`) and the deployment **connects** to it. So `--zmq-host` is the IP of the machine running `webcam_stream.py`, not the robot. Keep it on `localhost` when both run on the same workstation; pass the workstation's IP when the deployment runs onboard the robot. +``` + +### Terminal 3 — GEM-X Webcam Bridge + +From the **GEM-X repo**, with its environment active and both roots exported (Step 1): + +```bash +python "$SONIC_ROOT/gear_sonic/examples/live_camera_teleop/webcam_stream.py" \ + --source 0 --stream-sonic --window 30 --smooth 0.8 +``` + +Expect output along these lines: + +```text +[webcam] backends: vitpose=..., denoiser=... +[bridge] Using SONIC's pack_pose_message (exact wire format). +[webcam] streaming SMPL v3 to SONIC on tcp://*:5556 +[webcam] frame 42 | 12.3 fps | soma=yes +``` + +Two lines are worth reading carefully: + +- **`Using SONIC's pack_pose_message`** confirms `$SONIC_ROOT` resolved. If you instead see `gear_sonic not importable ... using built-in fallback packer`, the wire format is still byte-identical, but the conversion needs those same helpers — so fix `$SONIC_ROOT` rather than relying on the fallback. +- **`soma=yes`** means 3D output is flowing. The first couple of frames report `warmup/kp-only` while the rolling window fills. + +### Your first session + +With all three terminals running, drive the deployment from Terminal 2: + +1. Press **`]`** to start the control system. +2. In the MuJoCo window, press **`9`** to drop the robot to the ground. +3. Stand in front of the camera in a **relaxed, upright pose** with your whole body in frame. Check Terminal 3 shows `soma=yes`. +4. Back in Terminal 2, press **`ENTER`** to enable ZMQ streaming. The terminal prints `ZMQ STREAMING MODE: ENABLED` and the robot begins tracking you. +5. Start with **slow arm motions only.** Confirm the robot mirrors you in the expected direction before moving your legs or torso. +6. Press **`ENTER`** again to return to reference-motion mode, and **`O`** to stop and exit. + +```{admonition} Align yourself before enabling the stream +:class: danger +The robot snaps to whatever pose is streaming the moment you press **`ENTER`**. A large mismatch between the robot's current pose and yours produces sudden, aggressive motion — the same hazard described for POSE mode in the [PICO VR tutorial](vr_wholebody_teleop.md). Stand relaxed and upright, and verify tracking is stable, before enabling the stream. +``` + +--- + +## Step 5 — Teleop on the Real Robot + +```{admonition} Safety Warning +:class: danger +Only proceed once you can run a smooth session in simulation and are comfortable with the emergency stop. **Terminate `run_sim_loop.py` first** — a simulator and a real robot running at once will conflict. + +Bring the robot up on a gantry for the first real session, and keep a safety operator on **`O`** and the hardware E-stop. Review the [Whole-body Teleoperation Guide](../user_guide/teleoperation.md) before you start. +``` + +```{admonition} Expect the Robot to Walk Forward +:class: warning +Reaching your arms out in front of you, or making large, fast arm motions, shifts the streamed reference enough that the policy steps forward to keep its balance. On hardware that means the robot leaves the spot it started on, without any locomotion being commanded — so keep the space in front of it clear, do not stand directly in its path, and start with small arm motions close to your body. Work on this is in progress. +``` + +The real-robot workflow uses **two terminals** (no MuJoCo). + +### Terminal 1 — C++ deployment + +From `gear_sonic_deploy/`: + +```bash +cd gear_sonic_deploy +source scripts/setup_env.sh + +# 'real' auto-detects the robot network interface (192.168.123.x). +# If GEM-X runs on this workstation, localhost is correct: +./deploy.sh --input-type zmq --zmq-host localhost real + +# If the deployment runs onboard and GEM-X is on a workstation, +# point it at the workstation: +# ./deploy.sh --input-type zmq --zmq-host real + +# Wait until you see "Init done" +``` + +### Terminal 2 — GEM-X Webcam Bridge + +Identical to the simulation case: + +```bash +cd /path/to/GEM-X && source .venv/bin/activate +export GEMX_ROOT=$PWD SONIC_ROOT=/path/to/GR00T-WholeBodyControl + +python "$SONIC_ROOT/gear_sonic/examples/live_camera_teleop/webcam_stream.py" \ + --source 0 --stream-sonic --window 30 --smooth 0.8 +``` + +Then follow the same sequence: **`]`** to start, **`ENTER`** to enable streaming once tracking is stable, **`O`** to stop. + +--- + +## Controls Reference + +All keys are pressed in the **C++ deployment terminal** and are shared with [Streaming Motion Tracking](zmq.md). + +| Key | Action | +|---|---| +| **`]`** | Start the control system | +| **`ENTER`** | Toggle ZMQ streaming mode (camera tracking) on / off | +| **`Q`** / **`E`** | Adjust heading left / right (±0.1 rad per press) | +| **`I`** | Reinitialize base quaternion and reset heading to zero | +| **`T`** / **`N`** / **`P`** / **`R`** | Reference-motion playback (streaming mode off) | +| **`O`** | **Emergency stop** — halt control and exit | + +--- + +## Command-Line Reference + +### `webcam_stream.py` + +| Option | Default | Description | +|---|---|---| +| `--source` | `0` | Camera index (`ls /dev/video*`) or a video file path | +| `--stream-sonic` | off | Publish the Protocol v3 SMPL stream. Without it, nothing is sent to SONIC. | +| `--window` | `120` | Rolling-window length in frames. Dominates the frame rate — see [Tuning](#tuning). | +| `--smooth` | `0.75` | Temporal smoothing weight on the streamed reference (`0` = off) | +| `--port` | `5556` | ZMQ `PUB` port to bind. Matches the deployment default; change both or neither. | +| `--resolution` | camera default | Requested capture resolution, e.g. `1280x720` | +| `--cap-fps` | camera default | Requested capture frame rate | +| `--kp-only` | off | 2D keypoints only — skips the denoiser. Camera checks. | +| `--show` / `--save` | off | Live preview window / write an overlay mp4 | +| `--max-frames` | `0` | Stop after N frames (`0` = run until interrupted) | +| `--gemx-root` | `$GEMX_ROOT` | GEM-X repo root | +| `--sonic-root` | `$SONIC_ROOT` | SONIC repo root (see Step 1) | + +### `soma_pt_to_sonic_v3.py` + +| Option | Default | Description | +|---|---|---| +| `--pt` | *required* | Path to a saved GEM-X `hpe_results.pt` | +| `--fps` | `30` | Replay rate | +| `--loop` | off | Replay continuously | +| `--dry-run` | off | Convert frame 0, print it, and exit — no ZMQ | +| `--smooth` | `0.0` | Temporal smoothing (off by default, unlike the live path) | +| `--port` / `--max-frames` / `--gemx-root` / `--sonic-root` | — | As above | + +--- + +(tuning)= +## Tuning: Frame Rate vs. Stability + +The denoiser runs over the whole buffered window each frame, so cost grows with `--window` until the buffer is full. That makes `--window` the main throughput knob. + +| Symptom | Try | +|---|---| +| Frame rate too low | Lower `--window` (e.g. `30` → `20`); request a lighter capture mode with `--resolution` / `--cap-fps` | +| Robot jitters or takes small stepping motions | Raise `--smooth` toward `0.85`; improve lighting and [framing](#framing) | +| Robot feels laggy or "behind" you | Lower `--smooth`, lower `--window`, and move more deliberately | +| Legs or feet look wrong | Almost always framing — get the feet fully in frame and step further back | +| Robot drifts in heading | Press **`I`** to reinitialize the base quaternion, then nudge with **`Q`** / **`E`** | + +Start at `--window 30 --smooth 0.8` and adjust one knob at a time. + +--- + +## Troubleshooting + +| Symptom | Cause / fix | +|---|---| +| `Could not open video source: 0` | Wrong index (`ls /dev/video*`), or the user is not in the `video` group (`sudo usermod -aG video $USER`, then re-login) | +| Opens, but frames fail or hang | Another process holds the device (browser tab, earlier run). Check with `sudo fuser /dev/video0` | +| `ModuleNotFoundError: No module named 'gear_sonic'` | `$SONIC_ROOT` is unset or wrong — see Step 1 | +| `ModuleNotFoundError: No module named 'gem'` | Running in the wrong environment. Activate GEM-X's `.venv`, not a SONIC one. | +| Crash when the converter starts, referencing SOMA assets | The `inputs/soma_assets` link is missing or dangling — see Step 1 | +| `KeyError` about `identity_coeffs` / `scale_params` | The SOMA decode is incomplete; those parameters have no safe zero default, so the bridge refuses to stream a collapsed skeleton rather than sending garbage to the controller | +| Robot never moves after **`ENTER`** | No messages arriving. Check Terminal 3 prints `soma=yes`, that `--zmq-host` points at the machine running GEM-X, and that port `5556` is reachable | +| Very low frame rate | Camera negotiated a slow mode, or `--window` is too large — see [Tuning](#tuning) | +| `--show` fails on a headless box | No display; use `--save` instead | +| No keypoints detected | Body not fully in frame, too dark, or too far away | + +--- + +## Limitations + +- **No commanded root translation.** The stream carries root-local pose plus heading, not an explicit root translation, so walking the robot around by walking yourself is not supported. Combine with the [kinematic planner](keyboard.md) for locomotion and use the camera for upper-body motion. +- **Wrists and fingers are not tracked.** `smpl_pose` and the 6 wrist joints are streamed as zeros. GEM-X estimates hands, so wiring them through is a natural extension. +- **The robot walks forward to keep its balance.** Some arm motions — reaching out in front of you, or large, fast swings — shift the streamed reference far enough that the policy takes steps to stay upright. The robot then drifts from where it started even though no locomotion was commanded. Work on this is in progress. +- **Monocular lower body is the weakest signal.** Feet and depth are inherently less certain from one camera than from trackers, and that is exactly what the controller balances on. Framing matters more here than any parameter. +- **Single subject, static camera.** The largest detection is tracked, and the loop assumes a fixed camera. + +--- + +## Next Steps + +- [Streaming Motion Tracking](zmq.md) — the underlying ZMQ interface and full protocol reference +- [PICO VR Whole-body Teleop](vr_wholebody_teleop.md) — tracker-based teleoperation, for comparison +- [Whole-body Teleoperation Guide](../user_guide/teleoperation.md) — movement patterns and operating practices +- [Data Collection for VLA](data_collection.md) — recording teleoperation sessions as training data diff --git a/GR00T-WholeBodyControl/docs/source/tutorials/manager.md b/GR00T-WholeBodyControl/docs/source/tutorials/manager.md new file mode 100644 index 0000000000000000000000000000000000000000..9808046e0747f1edcc6a632db868e2120a45d51f --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/tutorials/manager.md @@ -0,0 +1,119 @@ +# Interface Manager (All-In-One) + +Dynamically switch between keyboard, gamepad, and ZMQ input interfaces at runtime using hotkeys (`--input-type manager`). The manager owns all interfaces simultaneously and delegates to the currently active one. You can switch at any time without restarting the program. + +```{admonition} Prerequisites +:class: note +Complete the [Quick Start](../getting_started/quickstart.md) to have the sim2sim loop running. +``` + +```{admonition} Emergency Stop +:class: danger +Press **`O`** at any time to immediately stop control and exit — this works regardless of which interface is active, including when the gamepad is selected. Always keep a hand near the keyboard ready to press **`O`**. +``` + +## Launch + +**Sim2Sim (MuJoCo):** + +```bash +# Terminal 1 — MuJoCo simulator (from repo root) +source .venv_sim/bin/activate +python gear_sonic/scripts/run_sim_loop.py + +# Terminal 2 — C++ deployment (from gear_sonic_deploy/) +bash deploy.sh --input-type manager sim +``` + +**Real Robot:** + +```bash +# From gear_sonic_deploy/ +bash deploy.sh --input-type manager real +``` + +If you plan to use the ZMQ interface, add ZMQ flags: + +```bash +bash deploy.sh --input-type manager \ + --zmq-host \ + --zmq-port 5556 \ + --zmq-topic pose \ + sim +``` + +## Step-by-Step + +1. The manager starts in **Keyboard** mode by default. +2. Press **`]`** to start the control system (keyboard mode). Use the robot as described in the [Keyboard tutorial](keyboard.md). +3. To switch to gamepad, press **`Shift+2`** (types `@`). The terminal prints `Switched to: GAMEPAD (safety reset triggered)`. The robot returns to reference motion at frame 0 and the planner is disabled. +4. Use the gamepad controls as described in the [Gamepad tutorial](gamepad.md). +5. To switch to ZMQ streaming, press **`Shift+3`** (types `#`). A safety reset is triggered and the terminal prints `Switched to: ZMQ`. Use the ZMQ controls as described in the [ZMQ tutorial](zmq.md). +6. To switch back to keyboard at any time, press **`Shift+1`** (types `!`). +7. Press **`O`** to stop control and exit from any interface. + +## Switching Interfaces + +| Hotkey | Interface | Notes | +|--------|-----------|-------| +| **Shift+1** (`!`) | Keyboard | Default. Full keyboard controls (Normal + Planner modes). See [Keyboard tutorial](keyboard.md). | +| **Shift+2** (`@`) | Gamepad | Unitree wireless gamepad. See [Gamepad tutorial](gamepad.md). | +| **Shift+3** (`#`) | ZMQ | ZMQ streaming (requires `--zmq-host` etc.). See [ZMQ tutorial](zmq.md). | + +```{tip} +A **ROS2** interface is also available via **Shift+4** (`$`) when built with ROS2 support. It requires the planner to be loaded and falls back to Keyboard otherwise. The ROS2 interface is provided as a reference implementation for building custom ROS2-based control pipelines and may not receive the same level of updates as the other interfaces. +``` + +### What Happens When You Switch + +Each switch triggers a **safety reset** on **all** managed interfaces (not just the one you're switching to). This: + +- Disables the planner and returns to reference motion at frame 0 +- Resets heading and movement states +- Disables ZMQ streaming (if it was active) +- Prevents control discontinuities from stale state in the previous interface + +The safety reset ensures you always start from a clean state after switching, regardless of what the previous interface was doing. + +```{note} +All four interfaces are created at startup and stay alive across switches. Their internal state (e.g., gamepad mode, ZMQ connection) is preserved — only the planner/motion state is reset for safety. This means you don't need to re-establish the ZMQ connection or re-pair the gamepad when switching back. +``` + +## Global Controls + +These controls work at the manager level, **regardless of which interface is active**. They are intercepted by the manager before being passed to the active interface. + +### Emergency Stop + +| Key | Action | +|-----|--------| +| **O** / **o** | Immediate emergency stop — works even when gamepad or ZMQ is the active interface | +| **F** / **f** | Report motor temperatures (TTS voice alert) | + +### Compliance Controls + +These adjust hand compliance and grasp parameters globally. They are especially useful during teleoperation (ZMQ / ROS2) where the robot's hands interact with objects. + +| Key | Action | +|-----|--------| +| **G** / **H** | Increase / Decrease left hand compliance by 0.1 | +| **B** / **V** | Increase / Decrease right hand compliance by 0.1 | +| **X** / **C** | Increase / Decrease max hand close ratio by 0.1 | + +```{note} +Compliance controls are global — they affect the robot's hands no matter which interface is active. This lets you adjust compliance from the keyboard even while the gamepad or ZMQ is controlling the robot's movement. +``` + +## Interface-Specific Controls + +Once an interface is active, all its normal controls work as documented in its own tutorial: + +- **Keyboard** (`!`): All keys from the [Keyboard tutorial](keyboard.md) — T/R/P/N for motions, ENTER for planner, WASD for movement, etc. +- **Gamepad** (`@`): All buttons from the [Gamepad tutorial](gamepad.md) — Start, A/B, L1/R1, analog sticks, etc. +- **ZMQ** (`#`): ENTER to toggle streaming, T/P/N/R for reference motions when not streaming. See the [ZMQ tutorial](zmq.md). + +The only exceptions are: +- **`O`** is always intercepted by the manager for emergency stop (not passed to the active interface). +- **`G/H/B/V/X/C`** are always intercepted for compliance control. +- **`!/@ /#/$`** are always intercepted for interface switching. + diff --git a/GR00T-WholeBodyControl/docs/source/tutorials/vla_inference.md b/GR00T-WholeBodyControl/docs/source/tutorials/vla_inference.md new file mode 100644 index 0000000000000000000000000000000000000000..2a07e97fe48c970faf3900809fa0a16637654095 --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/tutorials/vla_inference.md @@ -0,0 +1,386 @@ +# VLA Inference + +This guide covers running a trained Isaac-GR00T VLA policy on the Unitree G1 robot +using the Sonic whole-body control stack. + +## Overview + +The inference pipeline consists of: + +1. **Isaac-GR00T PolicyServer** — loads the VLA model and serves actions over ZMQ +2. **VLA inference client** (`run_vla_inference.py`) — reads camera + robot state, + queries the PolicyServer, and publishes actions to the C++ control loop +3. **C++ deploy** (`gear_sonic_deploy`) — executes whole-body control on the robot +4. **Camera server** — provides camera images over ZMQ (runs as a systemd service) +5. **Data exporter** (optional) — records episodes during inference + +``` +┌──────────────────────┐ +│ Isaac-GR00T │ +│ PolicyServer │ +│ (GPU machine) │ +└──────┬───────────────┘ + │ ZMQ REQ/REP + ▼ +┌─────────────────────┐ ZMQ TCP ┌──────────────────────┐ +│ VLA Inference │ ◄─────────── │ Camera Server │ +│ (run_vla_inference)│ │ (on robot) │ +└────┬───────────┬────┘ └──────────────────────┘ + │ │ + │ ZMQ PUB │ ZMQ SUB + │ (actions) │ (state) + ▼ ▼ +┌─────────────────────┐ +│ C++ Deploy │ +│ (gear_sonic_deploy)│ +└─────────────────────┘ +``` + +## Prerequisites + +### 1. Isaac-GR00T PolicyServer + +The PolicyServer runs on a machine with a GPU. It loads your finetuned VLA model +and serves inference over ZMQ. + +Install [Isaac-GR00T](https://github.com/NVIDIA/Isaac-GR00T) and start the server: + +```bash +# On the GPU machine (from the Isaac-GR00T repo) +uv run python gr00t/eval/run_gr00t_server.py \ + --model-path /path/to/your/finetuned_model \ + --embodiment-tag UNITREE_G1_SONIC \ + --device cuda:0 \ + --port 5550 +``` + +### 2. Inference Environment + +On the inference machine (can be the same as the PolicyServer or a separate PC): + +```bash +bash install_scripts/install_inference.sh +``` + +This creates `.venv_inference` with the Isaac-GR00T PolicyClient and all +inference dependencies. + +### 3. Camera Server + +The camera server should be running as a systemd service on the robot. +See [Data Collection](data_collection.md) for camera server setup. + +### 4. C++ Deploy + +The `gear_sonic_deploy` binary must be built. See the main README. + +### SONIC v1.1 Checkpoint + +Use the `sonic_v1_1/` checkpoint when the VLA policy was trained against +the robot-heading-normalized SONIC controller. It uses a 10-frame SMPL/wrist +reference horizon and was trained with wrist-pose augmentation. It is not the +low-latency checkpoint. + +```bash +python download_from_hf.py --sonic-v1-1 +``` + +Launch the matching C++ controller: + +```bash +cd gear_sonic_deploy +./deploy.sh \ + --cp policy/sonic_v1_1/model \ + --obs-config policy/sonic_v1_1/observation_config.yaml \ + --input-type zmq_manager \ + real +``` + +Or pass the same model pair to the Python launcher: + +```bash +python gear_sonic/scripts/launch_inference.py \ + --deploy-checkpoint policy/sonic_v1_1/model \ + --deploy-obs-config policy/sonic_v1_1/observation_config.yaml \ + --camera-host 192.168.123.164 \ + --prompt "pick up the cup" +``` + +### Low-Latency Teleoperation Checkpoint + +The `low_latency/` checkpoint is configured for responsive whole-body +teleoperation. Its SMPL encoder uses 4 future reference frames at 50 Hz +(approximately 80 ms of reference lookahead), compared with 10 frames +(approximately 200 ms) in the default release. This is reference lookahead, +not total end-to-end system latency. + +Download the deployment files from Hugging Face: + +```bash +python download_from_hf.py --low-latency +``` + +Then launch `gear_sonic_deploy` with the low-latency model prefix and matching +observation config: + +**C++ deploy:** + +```bash +cd gear_sonic_deploy +./deploy.sh \ + --cp policy/low_latency/model \ + --obs-config policy/low_latency/observation_config.yaml \ + --input-type zmq_manager \ + real +``` + +For simulation, replace `real` with `sim`. The `--cp` value is a model prefix: +`deploy.sh` appends `_encoder.onnx` and `_decoder.onnx` internally. + +**Python launcher:** + +```bash +python gear_sonic/scripts/launch_inference.py \ + --deploy-checkpoint policy/low_latency/model \ + --deploy-obs-config policy/low_latency/observation_config.yaml \ + --camera-host 192.168.123.164 \ + --prompt "pick up the cup" +``` + +The Python launcher starts the same C++ deploy command in a tmux pane, then runs +the Python VLA inference client, keyboard publisher, and optional data exporter. + +## Action Space + +The Sonic embodiment (`unitree_g1_sonic`) uses a 78-dimensional action +space: 64-dim motion token + 7-dim left hand joints + 7-dim right hand joints. + +## Quick Start — tmux Launcher + +The easiest way to run inference is with the all-in-one tmux launcher: + +```bash +# Real robot +python gear_sonic/scripts/launch_inference.py \ + --prompt "pick up the apple" \ + --camera-host 192.168.123.164 + +# Simulation +python gear_sonic/scripts/launch_inference.py --sim \ + --prompt "pick up the apple" + +# Without data recording +python gear_sonic/scripts/launch_inference.py \ + --no-data-exporter \ + --prompt "pick up the apple" +``` + +The launcher creates a tmux session with four panes: + +| Pane | Component | Description | +|------|-----------|-------------| +| 0 (top-left) | C++ Deploy | Whole-body controller | +| 1 (bottom-left) | Keyboard Publisher | Type keyboard commands here | +| 2 (top-right) | VLA Inference | Policy client + action loop | +| 3 (bottom-right) | Data Exporter | Records episodes (optional) | + +### Keyboard Controls + +Type these keys in the **Keyboard Publisher** pane (pane 1): + +| Key | Action | +|-----|--------| +| `k` | Start / stop the C++ control loop | +| `i` | Blend smoothly to initial pose and switch to POSE mode | +| `p` | Pause / resume policy inference | +| `[` | Toggle left hand open/closed (initial pose) | +| `]` | Toggle right hand open/closed (initial pose) | +| `t ` | Change the inference prompt (e.g., `t pick up the cup`) | +| `c` | Start recording an episode (data exporter) | +| `s` | Stop recording — success (data exporter) | +| `f` | Stop recording — failure / discard (data exporter) | + +### Typical Workflow + +1. Wait for all panes to initialize +2. Click on **pane 0** (C++ Deploy) and press Enter to confirm deployment +3. Switch to **pane 1** (Keyboard Publisher) +4. Press `k` to start the C++ control loop (starts in PLANNER mode) +5. Press `i` to blend to the initial pose (switches to POSE mode) + > The robot smoothly interpolates to the initial pose over 1 second. If your + > task starts from a different pose than the default, see + > [Customizing the Initial Pose](#customizing-the-initial-pose) below. +6. Press `p` to unpause the inference loop +7. The robot will begin executing VLA-predicted actions +8. Press `p` to pause, `k` to stop the control loop when done + +## Manual Setup (Without tmux) + +If you prefer to run each component in separate terminals: + +### Terminal 1 — Isaac-GR00T PolicyServer (GPU machine) + +```bash +# From the Isaac-GR00T repo +uv run python gr00t/eval/run_gr00t_server.py \ + --model-path /path/to/your/finetuned_model \ + --embodiment-tag UNITREE_G1_SONIC \ + --device cuda:0 \ + --port 5550 +``` + +### Terminal 2 — C++ Deploy + +```bash +cd gear_sonic_deploy +./deploy.sh --input-type zmq_manager real +``` + +Low-latency variant: + +```bash +python gear_sonic/scripts/launch_inference.py \ + --deploy-checkpoint policy/low_latency/model \ + --deploy-obs-config policy/low_latency/observation_config.yaml \ + --camera-host 192.168.123.164 \ + --prompt "pick up the apple" +``` + +Manual C++ deploy equivalent: + +```bash +cd gear_sonic_deploy +./deploy.sh \ + --cp policy/low_latency/model \ + --obs-config policy/low_latency/observation_config.yaml \ + --input-type zmq_manager \ + real +``` + +### Terminal 3 — VLA Inference + +```bash +source .venv_inference/bin/activate +python gear_sonic/scripts/run_vla_inference.py \ + --host \ + --port 5550 \ + --embodiment-tag unitree_g1_sonic \ + --prompt "pick up the apple" \ + --camera-host 192.168.123.164 +``` + +### Terminal 4 — Data Exporter (optional) + +```bash +source .venv_data_collection/bin/activate +python gear_sonic/scripts/run_data_exporter.py \ + --task-prompt "pick up the apple" \ + --camera-host 192.168.123.164 +``` + +## Configuration Reference + +### VLA Inference (`run_vla_inference.py`) + +| Flag | Default | Description | +|------|---------|-------------| +| `--host` | `localhost` | PolicyServer host | +| `--port` | `5550` | PolicyServer port | +| `--embodiment-tag` | `unitree_g1_sonic` | Embodiment tag | +| `--prompt` | `demo` | Language prompt | +| `--action-publish-rate` | `50` | Action publish rate (Hz) | +| `--action-horizon` | `40` | Actions per inference chunk | +| `--rate` | `2.5` | Inference rate (Hz) | +| `--camera-host` | `localhost` | Camera server host | +| `--camera-port` | `5555` | Camera server port | +| `--initial-pose-blend-duration` | `1.0` | Seconds to blend to initial pose (0 = instant snap) | +| `--verbose-timing` | `false` | Always print loop timing | + +### tmux Launcher (`launch_inference.py`) + +The launcher exposes all the above flags plus deploy and data exporter options. +Run `python gear_sonic/scripts/launch_inference.py --help` for the full list. + +## Remote PolicyServer + +When running the PolicyServer on a separate GPU machine: + +```bash +# On the inference machine, point to the remote server +python gear_sonic/scripts/launch_inference.py \ + --policy-host \ + --policy-port 5550 \ + --camera-host 192.168.123.164 \ + --prompt "pick up the apple" +``` + +Make sure port 5550 (or your chosen port) is accessible between the two machines. + +## Latency Compensation + +The inference loop automatically compensates for network and compute latency. +When a new action chunk arrives, the system calculates how many actions in the +chunk are already "stale" based on the time elapsed since inference started, +and skips to the appropriate action index. This is controlled by `--action-publish-rate` +and `--action-horizon`. + +## Customizing the Initial Pose + +When you press `i`, the inference client blends the robot smoothly from its +current configuration to a predefined **initial pose** encoded as a 64-dim +latent motion token. This pose should match the starting configuration your +demonstrations typically begin from. + +### When to Change the Initial Pose + +You should update the initial motion token if: + +- Your collected demonstrations start from a pose far from the default + (e.g., arms raised, holding an object, or a different standing stance) +- You switch to a different SONIC checkpoint (each checkpoint has its own + latent space — the same token produces different poses across checkpoints) +- The robot is snapping to a dangerous or unstable configuration on `i` press + +### Where to Change It + +Edit `gear_sonic/utils/inference/initial_poses.py`: + +```python +LATENT_INITIAL_MOTION_TOKEN = np.array( + [ + # Replace with your 64-dim token + ... + ], + dtype=np.float32, +) +``` + +### How to Find a Good Token + +1. **From data collection:** Look at the first action frame of a good demonstration + episode. The `action.motion_token` column in the parquet file at `frame_index=0` + gives you the latent token for that pose. + +2. **From the C++ deploy:** Put the robot in the desired starting pose via teleop, + then read the most recent latent token published on the ZMQ action channel. + +### Blend Duration + +The blend duration controls how quickly the robot transitions to the initial pose: + +```bash +# Default: 1 second smooth blend +python gear_sonic/scripts/run_vla_inference.py --initial-pose-blend-duration 1.0 + +# Faster blend (0.5 seconds) +python gear_sonic/scripts/run_vla_inference.py --initial-pose-blend-duration 0.5 + +# Instant snap (no interpolation, legacy behavior) +python gear_sonic/scripts/run_vla_inference.py --initial-pose-blend-duration 0 +``` + +```{warning} +Setting `--initial-pose-blend-duration` too low (or to 0) can cause jerky motion, +especially if the robot's current pose is far from the initial pose. The default +1-second blend is safe for most configurations. +``` diff --git a/GR00T-WholeBodyControl/docs/source/tutorials/vla_workflow.md b/GR00T-WholeBodyControl/docs/source/tutorials/vla_workflow.md new file mode 100644 index 0000000000000000000000000000000000000000..450efa0de2e929a0344b1b33c435e9f7604842a7 --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/tutorials/vla_workflow.md @@ -0,0 +1,208 @@ +# VLA Workflow: Collect, Fine-tune, Deploy + +This tutorial walks through the end-to-end workflow for training and deploying a +VLA policy on the Unitree G1 with SONIC whole-body control: + +1. **Collect** teleop demonstrations using the SONIC stack +2. **Fine-tune** the Isaac-GR00T N1.7 model on your collected data +3. **Deploy** the finetuned policy for autonomous inference + +```text ++-----------------+ +-----------------+ +-----------------+ +| 1. Collect | | 2. Fine-tune | | 3. Deploy | +| VR teleop + | --> | Isaac-GR00T | --> | PolicyServer + | +| data export | | N1.7 | | SONIC | ++-----------------+ +-----------------+ +-----------------+ +``` + +For examples of whole-body manipulation tasks accomplished with this workflow, see the +[VLA result videos on the GEAR-SONIC project page](https://nvlabs.github.io/GEAR-SONIC/#connection-to-vla-foundation-model). + +## How It Works: SONIC Latent Actions + +Instead of predicting raw joint angles, the VLA predicts **SONIC latent motion +tokens** — a compact 64-dimensional representation learned by the SONIC whole-body +controller. SONIC then decodes these latents into full-body joint commands at 50 Hz. + +```text ++-------------+ latent tokens +-------------+ joint commands +-------+ +| VLA Model | ----------------> | SONIC | -----------------> | Robot | +| 2.5 Hz | 64-dim x 40 | Decoder | 50 Hz | | +| | | C++ | | | ++-------------+ +-------------+ +-------+ +``` + +This means the VLA only needs to reason about *what* to do — SONIC handles the +*how*: balance, locomotion, and smooth whole-body coordination +all come for free from the pretrained controller. The result is a system that can +walk, reach, grasp, and manipulate simultaneously. + +The full action space per inference step is 78-dimensional: 64-dim motion token + +7-dim left hand joints + 7-dim right hand joints. + +## Step 1: Data Collection + +Collect teleop demonstrations using VR whole-body teleoperation. The data exporter +records robot state, camera images, and teleop actions as a LeRobot dataset. + +See the [Data Collection tutorial](data_collection.md) for full setup instructions +(camera server, VR teleop, recording controls). + +**Quick start:** + +```bash +python gear_sonic/scripts/launch_data_collection.py \ + --camera-host 192.168.123.164 \ + --task-prompt "pick up the soda can and place it in the bin" +``` + +**Output:** A LeRobot v2.1 dataset directory, e.g.: + +```text +outputs/2026-04-03-14-30-00-G1-robot01/ ++-- data/ +| +-- train-00000.parquet ++-- videos/ +| +-- observation.images.ego_view/ +| +-- episode_000000.mp4 ++-- meta/ + +-- info.json + +-- modality.json + +-- episodes.jsonl + +-- tasks.jsonl +``` + +```{tip} +Collect at least 50–100 demonstrations of the target task for reliable fine-tuning. +Use `--dataset-name` to append multiple sessions into the same dataset, or merge +sessions afterwards with `process_dataset.py`. +``` + +### Post-Process Before Fine-tuning + +After collection, run the processing script to remove discarded episodes +(flagged via `x` key during recording) and clean stale SMPL frames: + +```bash +source .venv_data_collection/bin/activate +python gear_sonic/scripts/process_dataset.py \ + --dataset-path outputs/2026-04-03-14-30-00-G1-robot01 \ + --output-path outputs/my_task_cleaned +``` + +This ensures only successful demonstrations are used for fine-tuning. + +## Step 2: Fine-tuning with Isaac-GR00T + +Fine-tune the GR00T N1.7 base model on your collected dataset using the +[Isaac-GR00T](https://github.com/NVIDIA/Isaac-GR00T) training pipeline. + +### Prerequisites + +- Clone and install [Isaac-GR00T](https://github.com/NVIDIA/Isaac-GR00T): + ```bash + git clone https://github.com/NVIDIA/Isaac-GR00T.git + cd Isaac-GR00T + uv sync --all-extras + ``` +- Multi-GPU machine (4+ GPUs recommended) +- The collected dataset accessible from the training machine + +### Launch Fine-tuning + +```bash +export NUM_GPUS=4 +uv run python \ + gr00t/experiment/launch_finetune.py \ + --base-model-path nvidia/GR00T-N1.7-3B \ + --dataset-path /path/to/your/collected_dataset \ + --embodiment-tag UNITREE_G1_SONIC \ + --modality-config-path gr00t/configs/data/embodiment_configs.py \ + --num-gpus $NUM_GPUS \ + --output-dir /path/to/output \ + --save-total-limit 5 \ + --save-steps 5000 \ + --max-steps 20000 \ + --use-wandb \ + --global-batch-size 32 \ + --color-jitter-params brightness 0.3 contrast 0.4 saturation 0.5 hue 0.08 \ + --dataloader-num-workers 4 +``` + +### Key Parameters + +| Flag | Description | +|------|-------------| +| `--base-model-path` | HuggingFace model ID or local path to pretrained weights | +| `--dataset-path` | Path to the LeRobot dataset from Step 1 | +| `--embodiment-tag` | Must match the dataset's embodiment (`UNITREE_G1_SONIC`) | +| `--modality-config-path` | Python file defining the modality configuration | +| `--num-gpus` | Number of GPUs for distributed training | +| `--max-steps` | Total training steps (20k is a good starting point) | +| `--global-batch-size` | Total batch size across all GPUs | +| `--save-steps` | Checkpoint save interval | +| `--use-wandb` | Enable Weights & Biases logging | + +### Monitoring + +With `--use-wandb`, training metrics (loss, learning rate, etc.) are logged to your +W&B project. Monitor the training loss curve — it should decrease steadily and +plateau before `--max-steps`. + +### Output + +Checkpoints are saved to `--output-dir`: + +```text +/path/to/output/ ++-- checkpoint-5000/ ++-- checkpoint-10000/ ++-- checkpoint-15000/ ++-- checkpoint-20000/ ++-- config.json ++-- processor_config.json +``` + +Use the final checkpoint (or the best-performing one based on your evaluation) for +deployment in Step 3. + +## Step 3: Deploy for Inference + +Deploy the finetuned model using the Isaac-GR00T PolicyServer and the SONIC +inference stack. See the [VLA Inference tutorial](vla_inference.md) for full +details on the inference pipeline, keyboard controls, and configuration. + +### Start the PolicyServer + +On the GPU machine, from the Isaac-GR00T repository: + +```bash +uv run python gr00t/eval/run_gr00t_server.py \ + --model-path /path/to/output/checkpoint-20000 \ + --embodiment-tag UNITREE_G1_SONIC \ + --device cuda:0 \ + --port 5550 +``` + +### Run Inference + +On the inference machine (from the GR00T-WholeBodyControl repository): + +```bash +python gear_sonic/scripts/launch_inference.py \ + --policy-host \ + --policy-port 5550 \ + --camera-host 192.168.123.164 \ + --prompt "pick up the soda can and place it in the bin" +``` + +## Summary + +| Step | Where | Key Command | +|------|-------|-------------| +| Collect | GR00T-WholeBodyControl | `launch_data_collection.py` | +| Fine-tune | Isaac-GR00T | `launch_finetune.py` | +| Deploy | Both repos | `run_gr00t_server.py` + `launch_inference.py` | + +For iterating on a task, repeat Steps 1–3: collect more data, fine-tune again +(or continue from an existing checkpoint), and redeploy. diff --git a/GR00T-WholeBodyControl/docs/source/tutorials/vr_wholebody_teleop.md b/GR00T-WholeBodyControl/docs/source/tutorials/vr_wholebody_teleop.md new file mode 100644 index 0000000000000000000000000000000000000000..50a9a0f1444c5d298066ab9d0d54398a2acd46b5 --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/tutorials/vr_wholebody_teleop.md @@ -0,0 +1,368 @@ +# PICO VR Whole-body Teleop + +Full whole-body teleoperation using PICO VR headset and controllers. To teleop, use the option `--input-type zmq_manager` during deployment. The `zmq_manager` input type switches between a **planner mode** (locomotion commands via ZMQ) and a **streamed motion mode** (full-body SMPL poses from PICO). + +## SONIC Low Latency + +The sequence below shows 3-point VR teleoperation with SONIC Low Latency, +including whole-body tracking and a successful ground pickup. + +```{image} ../_static/sonic_low_latency_demo.gif +:alt: SONIC Low Latency whole-body teleoperation and ground pickup +:width: 640px +:align: center +``` + +```{admonition} Isaac Teleop / CloudXR Scope +:class: note +The same `zmq_manager` workflow can also drive the headset through Isaac Teleop / CloudXR by launching `gear_sonic/scripts/pico_manager_thread_server.py --input-source isaac-teleop`. The streamer hosts the CloudXR runtime in-process via `isaacteleop[cloudxr]` — no separate publisher container required. That path is currently supported only for **G1 with a Thor backpack**; a regular G1 setup is not supported yet. +``` + +```{admonition} Safety Warning +:class: danger +Whole-body teleoperation involves fast, agile motions. **Always** maintain a clear safety zone and keep a safety operator at the keyboard ready to trigger an emergency stop (**`O`** in the C++ terminal, or **A+B+X+Y** on the PICO controllers). + +You **must wear tight-fitting pants or leggings** to guarantee line-of-sight for the foot trackers — loose or baggy clothing can make tracking fail unpredictably and may result in dangerous motion. +``` + +```{video} ../_static/teleop/teleop_session_overview.mp4 +:width: 100% +``` +*Video: End-to-end teleoperation walkthrough — PICO calibration, policy engagement, and the robot balancing independently. See the [Teleoperation Guide](../user_guide/teleoperation.md) for detailed best practices.* + +## Prerequisites + +1. **Completed the [Quick Start](../getting_started/quickstart.md)** — you can run the sim2sim loop (includes [installing the deployment](../getting_started/installation_deploy.md) and [downloading model checkpoints](../getting_started/download_models.md)). +2. **Completed the [VR Teleop Setup](../getting_started/vr_teleop_setup.md)** — `.venv_teleop` is ready. For the default path, PICO hardware is installed, calibrated, and connected. For Isaac Teleop / CloudXR, the `isaacteleop[cloudxr]` package is also installed (handled by `install_pico.sh`) and the headset connects to the in-process CloudXR runtime — see [Isaac Teleop Setup](isaac_teleop_publisher_setup.md). + +--- + +## Step-by-Step: Teleop in SIM + +Run **three terminals** to teleoperate the simulated robot. + +### Terminal 1 — Launch virtual robot in MuJoCo Simulator + +From the **repo root**: + +```bash +# bash install_scripts/install_pico.sh + +source .venv_teleop/bin/activate +python gear_sonic/scripts/run_sim_loop.py +``` + +### Terminal 2 — C++ Deployment + +From `gear_sonic_deploy/`: + +```bash +cd gear_sonic_deploy +source scripts/setup_env.sh +./deploy.sh --input-type zmq_manager sim +# Wait until you see "Init done" +``` + +**Isaac Teleop / CloudXR alternative** (**G1 + Thor backpack only**) — run the C++ deployment from the project's ROS2 docker container instead of bare metal: + +```bash +cd gear_sonic_deploy +export TensorRT_ROOT=$HOME/TensorRT # only if not already in ~/.bashrc +./docker/run-ros2-dev.sh + +# inside the container (setup_env.sh is sourced automatically): +just build # first run only +./deploy.sh --input-type zmq_manager sim +# Wait until you see "Init done" +``` + +See [Installation (Deployment) → Docker (ROS2 Development Environment)](../getting_started/installation_deploy.md) for details on `run-ros2-dev.sh` and `TensorRT_ROOT`. + +```{note} +The `--zmq-host` flag defaults to `localhost`, which is correct when both C++ deployment scripts and teleop scripts (Terminal 3) run on the same machine. If the teleop script runs on a different machine, pass `--zmq-host `. +``` + +### Terminal 3 — PICO Teleop Streamer + +From the **repo root**: + +```bash +source .venv_teleop/bin/activate + +# With full visualization (recommended for first run): +python gear_sonic/scripts/pico_manager_thread_server.py --manager \ + --vis_vr3pt --vis_smpl + +# Without visualization (for headless / onboard practice): +# python gear_sonic/scripts/pico_manager_thread_server.py --manager +``` + +**Isaac Teleop / CloudXR alternative** (**G1 + Thor backpack only**) — connects the headset over CloudXR (no XRoboToolKit PC service required); the streamer launches the CloudXR runtime in-process via `isaacteleop[cloudxr]`: + +```bash +source .venv_teleop/bin/activate + +python gear_sonic/scripts/pico_manager_thread_server.py --manager \ + --input-source isaac-teleop + +# If running offboard with a display, add visualization: +# --vis_vr3pt --vis_smpl +``` + +When you turn on the visualization, wait for a window to pop up showing a Unitree G1 mesh with all joints at the default angles. If no window shows up on the default PICO path, double-check the PICO's XRoboToolKit IP configuration in the [VR Teleop Setup](../getting_started/vr_teleop_setup.md). If you are using Isaac Teleop instead, verify the headset is connected to the in-process CloudXR runtime — see [Isaac Teleop Setup](isaac_teleop_publisher_setup.md) for connection steps. + +### Your First Teleop Session + +1. **Assume the calibration pose** — stand upright, feet together, upper arms at your sides, forearms bent 90° forward (L-shape at each elbow), palms inward. See [Calibration Pose](#calibration-pose) for details. +2. Press **A + B + X + Y** simultaneously to engage the control policy and run the initial full calibration (`CALIB_FULL`). +3. Align your arms with the robot's current pose, then press **A + X** to enter full-body SMPL teleop (**POSE** mode). Move your arms and legs — the robot follows. +4. Press **A + X** again to fall back to **PLANNER** (idle) mode. +5. Press **A + B + X + Y** again to stop the robot. + +
+ +
Basic whole-body teleop workflow: calibration pose → engage → POSE mode → PLANNER idle → stop.
+
+ +--- + +(pico-controls)= + +## Complete PICO Controls + +### Modes & Calibration + +The system has **4 operating modes** and **2 calibration types**. + +**Modes:** + +| Mode | Encoder | Description | +|---|---|---| +| **OFF** | -- | Policy not running. Stand in [calibration pose](#calibration-pose), then press **A+B+X+Y** to start policy. | +| **POSE** | SMPL | Whole-body teleop — streaming the SMPL pose from PICO to the C++ deployment side. Your motion will directly map to the robot .| +| **PLANNER** | G1 | Locomotion planner active; upper body controller by planner. Joysticks control direction and heading in walking and running modes. | +| **PLANNER_FROZEN_UPPER** | G1 | Planner locomotion; upper body frozen at last POSE snapshot. | +| **VR_3PT** | TELEOP | Planner locomotion; upper body follows VR 3-point tracking (head + 2 hands). Depends on non-IK-based VR 3-point calibration. | + +**Calibration types** (non-IK workflow for minimal latency): + +| Type | What Is Calibrated | When Triggered | +|---|---|---| +| **CALIB_FULL** | Head + both wrists against **all-zero** reference pose. | Once on first **A+B+X+Y** (startup). | +| **CALIB** | Both wrists only against the **current robot pose**. | Each switch into VR_3PT via **Left Stick Click**. | + +### State Machine + +There are 4 modes and 2 control chains. Each chain forms a triangle: **A+X** (or **B+Y**) returns to POSE from *both* the planner node and its VR_3PT sub-mode. + +```text + ┌──────────────────────────────────────┐ + │ A+B+X+Y (any mode) ──► OFF │ + └──────────────────────────────────────┘ + + Startup: + OFF ──(A+B+X+Y)──► PLANNER ──(A+X)──► POSE + CALIB_FULL + + Chain 1 — G1 encoder listens to planner-generated full-body motion: PLANNER + + PLANNER ─── L-Stick (+CALIB) ──► VR_3PT + ▲ │ ◄─────── L-Stick ───────── │ + │ │ │ + │A+X │A+X A+X │ + │ ▼ ▼ + └─ POSE ◄──────────────────────────┘ + + Chain 2 — G1 encoder listens to planner-generated lower-body motion: PLANNER_FROZEN_UPPER + + PLANNER_FROZEN_UPPER ── L-Stick (+CALIB) ──► VR_3PT + ▲ │ ◄──────── L-Stick ────────── │ + │ │ │ + │B+Y │B+Y B+Y │ + │ ▼ ▼ + └── POSE ◄───────────────────────────────┘ +``` + +```{admonition} DANGER — Mode-Switching Safety +:class: danger +**Before switching into POSE or VR_3PT**, always align your body with the robot's current pose first!! + +- **POSE:** The robot instantly snaps to your physical pose. A large mismatch causes sudden, aggressive motion. +- **VR_3PT:** A misaligned calibration produces erratic, dangerous motion as soon as you move your arms. Check more info at section [per-switch-calib](#per-switch-calib) +``` + +(calibration-pose)= + +### VR_3PT Calibration Hint + +The `VR_3PT` mode depends on accurate calibration. Two calibration events occur: + +#### One-time `CALIB_FULL` (head + wrists) + +Before pressing **A + B + X + Y** for the first time, you **must** stand in the robot's **all-zero reference pose**. The system captures your PICO body-tracking frame as the zero-reference for all subsequent motion mapping. + +**The reference pose:** +1. **Stand upright**, feet together, looking straight forward. +2. **Upper arms** hang straight down, close to your torso. +3. **Forearms** bent 90° forward (L-shape at each elbow), palms facing inward. + +```{tip} +Launch the teleop script with `--vis_vr3pt` to see the robot's reference pose in a visualization window. Match your body to it before pressing the start combo. +``` + +(per-switch-calib)= +#### Per-switch `CALIB` (wrists only) + +Each time you enter `VR_3PT` via **Left Stick Click**, the system re-calibrates both wrists against the robot's **current** pose. Always align your arms with the robot before clicking. + +Below is an example of **bad calibration practice** — transitioning into VR_3PT without aligning your arms to the robot's current pose. The robot may not jump immediately, but will exhibit erratic and dangerous motion as soon as you move. + +
+ +
Bad practice: entering VR_3PT without arm alignment causes erratic, unsafe motion.
+
+ + + + +```{admonition} DANGER — Mode-Switching Safety +:class: danger +**Before switching into POSE or VR_3PT**, always align your body with the robot's current pose first. + +**Recovery from bad VR_3PT calibration:** +1. Freeze the upper body — switch back via **Left Stick Click**. +2. Re-align your arms with the robot's current (possibly distorted) pose. +3. Switch to **POSE** mode (**A+X**) to reset. +``` + +Below is the **recovery procedure** — if you accidentally enter a badly calibrated VR_3PT state, freeze the upper body (Left Stick Click back), then switch to POSE mode (A+X) to reset safely. + +
+ +
Recovery from bad VR_3PT calibration: freeze upper body → re-align → switch to POSE mode.
+
+ +### Quick-Start Cheatsheet + +| Action | Button | Notes | +|---|---|---| +| **Start / Stop policy** | **A+B+X+Y** | First press: engage + CALIB_FULL. Again: emergency stop → OFF. | +| **Toggle POSE** | **A+X** | Switches between PLANNER ↔ POSE. OR from VR_3PT (entered via PLANNER) → POSE. | +| **Toggle PLANNER_FROZEN_UPPER** | **B+Y** | Switches between POSE ↔ PLANNER_FROZEN_UPPER. OR from VR_3PT (entered via PLANNER_FROZEN_UPPER) → POSE. | +| **Toggle VR_3PT** | **Left Stick Click** | From any Planner mode → VR_3PT (triggers CALIB). Click again to return. | +| **Hand grasp** | **Trigger** (per hand) | Controls the corresponding hand's grasp. | + +### Joystick Controls (Planner Modes) + +Active in **PLANNER**, **PLANNER_FROZEN_UPPER**, and **VR_3PT**: + +| Input | Function | +|---|---| +| **Left Stick** | Move direction (forward / backward / strafe) | +| **Right Stick (horizontal)** | Yaw / heading (continuous accumulation) | +| **A + B** | Next locomotion mode | +| **X + Y** | Previous locomotion mode | + +**Locomotion modes** (cycled via A+B / X+Y): + +| ID | Mode | +|---|---| +| 0 | Idle [DEFAULT] | +| 1 | Slow Walk | +| 2 | Walk | +| 3 | Run | +| 4 | Squat | +| 5 | Kneel (two legs) | +| 6 | Kneel | +| 7 | Lying face-down | +| 8 | Crawling | +| 9–16 | Boxing variants (idle, walk, punches, hooks) | +| 17 | Forward Jump | +| 18 | Stealth Walk | +| 19 | Injured Walk | + +--- + +## Emergency Stop + +| Method | Action | +|---|---| +| **PICO controllers** | Press **A+B+X+Y** simultaneously → OFF | +| **Keyboard** (C++ terminal) | Press **`O`** for immediate stop | + +--- + +## Step-by-Step: Teleop on Real Robot + +```{admonition} Safety Warning +:class: danger +Only proceed once you can smoothly control the robot in simulation and are comfortable with emergency stops. **Terminate any running `run_sim_loop.py` process first** — simultaneous sim and real instances will conflict. +``` + +The real-robot workflow uses **two terminals** (no MuJoCo simulator). + +### Terminal 1 — C++ Deployment (Real Robot) + +From `gear_sonic_deploy/`: + +```bash +cd gear_sonic_deploy +source scripts/setup_env.sh + +# 'real' auto-detects the robot network interface (192.168.123.x). +# If auto-detection fails, pass the G1's IP directly: +# ./deploy.sh --input-type zmq_manager +./deploy.sh --input-type zmq_manager real + +# Wait until you see "Init done" +``` + +**Isaac Teleop / CloudXR alternative** (**G1 + Thor backpack only**) — run the C++ deployment from the project's ROS2 docker container instead of bare metal: + +```bash +cd gear_sonic_deploy +export TensorRT_ROOT=$HOME/TensorRT # only if not already in ~/.bashrc +./docker/run-ros2-dev.sh + +# inside the container (setup_env.sh is sourced automatically): +just build # first run only +./deploy.sh --input-type zmq_manager real +# Wait until you see "Init done" +``` + +```{note} +If the teleop script (Terminal 2) runs on a different machine, add `--zmq-host ` so the C++ side knows where the ZMQ publisher is. +``` + +### Terminal 2 — PICO Teleop Streamer + +From the **repo root**: + +```bash +# bash install_scripts/install_pico.sh + +source .venv_teleop/bin/activate +python gear_sonic/scripts/pico_manager_thread_server.py --manager + +# If running offboard with a display, add visualization: +# --vis_vr3pt --vis_smpl +``` + +**Isaac Teleop / CloudXR alternative** (**G1 + Thor backpack only**) — in-process CloudXR runtime via `isaacteleop[cloudxr]`, no XRoboToolKit PC service required: + +```bash +source .venv_teleop/bin/activate +python gear_sonic/scripts/pico_manager_thread_server.py --manager --input-source isaac-teleop +``` + +```{note} +Update the IP in the PICO's XRoboToolKit app to match this machine before starting the default PICO path. For Isaac Teleop, make sure the headset is connected to the in-process CloudXR runtime — see [Isaac Teleop Setup](isaac_teleop_publisher_setup.md). +``` + +Follow the same start sequence: calibration pose → **A+B+X+Y** → **A+X** for POSE mode. See [Complete PICO Controls](#pico-controls) for all available commands. diff --git a/GR00T-WholeBodyControl/docs/source/tutorials/zmq.md b/GR00T-WholeBodyControl/docs/source/tutorials/zmq.md new file mode 100644 index 0000000000000000000000000000000000000000..4749e1f75369493543ea7735d0255c952ab0d0e3 --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/tutorials/zmq.md @@ -0,0 +1,350 @@ +# Streaming Motion Tracking + +Stream motion data to the robot over ZMQ for reference motion tracking. This interface supports streaming either **SMPL-based poses** (e.g., from PICO) or **G1 whole-body joint positions** (qpos) from any external source (`--input-type zmq`). + +```{admonition} Prerequisites +:class: note +Complete the [Quick Start](../getting_started/quickstart.md) to have the sim2sim loop running. +``` + +```{admonition} Emergency Stop +:class: danger +Press **`O`** at any time to immediately stop control and exit. Always keep a hand near the keyboard ready to press **`O`**. +``` + +## Launch + +**Sim2Sim (MuJoCo):** + +```bash +# Terminal 1 — MuJoCo simulator (from repo root) +source .venv_sim/bin/activate +python gear_sonic/scripts/run_sim_loop.py + +# Terminal 2 — C++ deployment (from gear_sonic_deploy/) +bash deploy.sh --input-type zmq \ + --zmq-host \ + --zmq-port 5556 \ + --zmq-topic pose \ + sim +``` + +**Real Robot:** + +```bash +# From gear_sonic_deploy/ +bash deploy.sh --input-type zmq \ + --zmq-host \ + --zmq-port 5556 \ + --zmq-topic pose \ + real +``` + +## Step-by-Step + +1. Press **`]`** to start the control system. +2. By default you are in **reference motion mode** — use **`T`** to play motions, **`N`** / **`P`** to switch, **`R`** to restart (same as the [keyboard interface](keyboard.md)). +3. Press **`ENTER`** to toggle into **ZMQ streaming mode**. The terminal will print `ZMQ STREAMING MODE: ENABLED`. +4. The policy now tracks motion frames arriving from the ZMQ publisher in real time. Playback starts automatically. +5. Press **`ENTER`** again to switch back to reference motions. The terminal will print `ZMQ STREAMING MODE: DISABLED`, and the encode mode resets to `0` (joint-based). +6. Use **`Q`** / **`E`** to adjust the heading (±0.1 rad per press) in either mode. +7. Press **`I`** to reinitialize the base quaternion and reset the heading to zero. +8. When done, press **`O`** to stop control and exit. + +```{note} +**No planner support** — this interface uses pre-loaded and ZMQ-streamed reference motions only. For planner + ZMQ control (e.g., PICO VR teleoperation), use `--input-type zmq_manager` instead. See the [VR Whole-Body Teleop tutorial](vr_wholebody_teleop.md). +``` + +```{tip} +**Build your own streaming source.** The ZMQ stream protocol documented below is self-contained — any publisher that sends messages in this format can drive the robot. You can write your own motion capture retargeting pipeline, simulator bridge, or any other source that produces the required fields. No PICO hardware is needed. +``` + +## Using with PICO VR Teleop + +You can use `--input-type zmq` with the PICO teleop streamer for a simple, streaming-only whole-body teleoperation setup. In this mode, the PICO streams full-body SMPL poses over ZMQ and the deployment side tracks them directly — no locomotion planner, no PICO-button mode switching. All control is done from the keyboard. + +### Prerequisites + +1. **Completed the [Quick Start](../getting_started/quickstart.md)** — you can run the sim2sim loop. +2. **PICO VR hardware is set up** — headset and controllers are connected, body tracking is working, and `.venv_teleop` is installed. See the [VR Teleop Setup](../getting_started/vr_teleop_setup.md) for installation and calibration. + +### Launch (Sim2Sim) + +Run three terminals: + +**Terminal 1 — MuJoCo simulator** (from repo root): + +```bash +source .venv_sim/bin/activate +python gear_sonic/scripts/run_sim_loop.py +``` + +**Terminal 2 — C++ deployment** (from `gear_sonic_deploy/`): + +```bash +bash deploy.sh --input-type zmq \ + --zmq-host localhost \ + --zmq-port 5556 \ + --zmq-topic pose \ + sim +``` + +**Terminal 3 — PICO teleop streamer** (from repo root): + +```bash +source .venv_teleop/bin/activate + +# With visualization (recommended for first run): +python gear_sonic/scripts/pico_manager_thread_server.py \ + --manager --vis_smpl --vis_vr3pt + +# Without visualization (headless): +# python gear_sonic/scripts/pico_manager_thread_server.py --manager +``` + +### Launch (Real Robot) + +Run two terminals (no MuJoCo): + +**Terminal 1 — C++ deployment** (from `gear_sonic_deploy/`): + +```bash +bash deploy.sh --input-type zmq \ + --zmq-host \ + --zmq-port 5556 \ + --zmq-topic pose \ + real +``` + +Replace `` with `localhost` if the PICO streamer runs on the same machine, or the IP of the machine running Terminal 2. + +**Terminal 2 — PICO teleop streamer** (from repo root): + +```bash +source .venv_teleop/bin/activate +python gear_sonic/scripts/pico_manager_thread_server.py --manager +``` + +### Step-by-Step + +1. **Calibration pose**: Stand upright, feet together, upper arms at your sides, forearms bent 90° forward (L-shape at each elbow), palms facing inward. +2. On the PICO controllers, press **A + B + X + Y** simultaneously to initialize and calibrate the body tracking. +3. Press **A + X** on the PICO controllers to start streaming poses. +4. In Terminal 2 (C++ deployment), press **`]`** to start the control system. +5. In the MuJoCo window (sim only), press **`9`** to drop the robot to the ground. +6. Back in Terminal 2, press **`ENTER`** to enable ZMQ streaming. The terminal prints `ZMQ STREAMING MODE: ENABLED`. The robot begins tracking your PICO poses in real time. +7. Move your body — the robot mirrors your motions. Use the **Trigger** button on each PICO controller to close the corresponding robot hand. +8. To **pause** streaming (e.g., to reposition yourself), press **`ENTER`** again. The terminal prints `ZMQ STREAMING MODE: DISABLED`. The robot holds its last pose and stops tracking. You can move freely without affecting the robot. +9. To **resume**, press **`ENTER`** once more. The robot will snap to your current pose — **move back close to the robot's current pose before resuming** to avoid sudden jumps. +10. When done, press **`O`** to stop control and exit. + +```{admonition} DANGER — Resuming from Pause +:class: danger +When you press **`ENTER`** to resume streaming after a pause, the robot will immediately try to reach your current physical pose. If your body is in a very different position from the robot, the robot may perform sudden, aggressive motions. **Always move back close to the robot's current pose before pressing `ENTER` to resume.** +``` + +### PICO Buttons in ZMQ Mode + +In `--input-type zmq` mode, the C++ deployment side does **not** process PICO controller button combos directly. However, the buttons still affect the **Python streamer**, which controls what data gets published on the `pose` ZMQ topic. Since the deployment side tracks whatever arrives (or stops arriving) on that topic, several buttons still have an indirect effect on the robot. + +| PICO Button | Effect | +|-------------|--------| +| **A + B + X + Y** | Calibrate body tracking in the streamer. Press once to initialize; press again to stop streaming (emergency stop on the streamer side). | +| **A + X** | Toggle Pose mode in the streamer — starts or stops publishing pose data. When stopped, the robot holds its last pose. **Works as pause/resume.** | +| **Menu (hold)** | Pauses pose streaming in the streamer while held. The robot holds its last pose until you release. **Works as pause.** Move back close to the robot's current pose before releasing. | +| **Trigger** | Hand grasp — processed by the streamer and sent as `left_hand_joints` / `right_hand_joints` in the stream. | +| **B + Y** | Toggle Pose mode in the streamer (same effect as A+X) — starts or stops publishing pose data. **Works as pause/resume.** | + +All mode control on the deployment side is done from the keyboard: + +| Key | Action | +|-----|--------| +| **`]`** | Start control system | +| **`ENTER`** | Toggle streaming on/off (pause/resume) | +| **`O`** | Emergency stop — stop control and exit | +| **`I`** | Reinitialize base quaternion and reset heading | +| **`Q`** / **`E`** | Adjust heading (±0.1 rad) | +| **`F`** | Report motor temperatures (TTS voice alert) | + +```{note} +For the full PICO VR experience with planner support, locomotion modes, and PICO-controller-based mode switching, use `--input-type zmq_manager` instead. See the [VR Whole-Body Teleop tutorial](vr_wholebody_teleop.md). +``` + +## Controls + +| Key | Action | +|-----|--------| +| **]** | Start control system | +| **O** | Stop control and exit (emergency stop) | +| **ENTER** | Toggle between reference motions and ZMQ streaming | +| **I** | Reinitialize base quaternion and reset heading | +| **Q** / **E** | Adjust delta heading left / right (±0.1 rad) | +| **F** | Report motor temperatures (TTS voice alert) | + +*Reference motion mode only (streaming off):* + +| Key | Action | +|-----|--------| +| **T** | Play current motion to completion | +| **R** | Restart current motion from beginning (pause at frame 0) | +| **P** / **N** | Previous / Next motion sequence | + +## Stream Protocol Versions + +The encode mode is determined automatically by the ZMQ stream protocol version. **SONIC uses Protocol v1, v3, and v4.** Protocol v2 is available for custom applications. + +### Encode Mode Logic + +The encode mode only takes effect when the policy model has an **encoder** configured and loaded. At startup, each motion's encode mode is initialized based on encoder availability: + +| `encode_mode` | Meaning | +|----------------|---------| +| `-2` | No encoder / token state configured in the model — encode mode has no effect | +| `-1` | Encoder config exists (token state dimension > 0) but no encoder model file provided | +| `0` | Encoder loaded, joint-based mode (default) | +| `1` | Encoder loaded, teleop / 3 points upper-body mode | +| `2` | Encoder loaded, SMPL-based mode | + +When ZMQ streaming is active, the protocol version sets the encode mode on the streamed motion: v1 → `0`, v2/v3 → `2`. Protocol v4 bypasses the encoder entirely — it streams pre-computed tokens directly into the policy. This only affects inference if the model actually has an encoder (`encode_mode >= 0`). If no encoder is configured (`-2`), the value is set but has no effect on the inference pipeline. + +When switching back to reference motions (pressing **ENTER** to disable streaming), the encode mode resets to `0` (if the motion has an encoder, i.e. `encode_mode >= 0`). + +### Common Fields (All Versions) + +All versions require two common fields: + +| Field | Shape | Dtype | Description | +|-------|-------|-------|-------------| +| `body_quat` | `[N, 4]` or `[N, num_bodies, 4]` | `f32` / `f64` | Body quaternion(s) per frame (w, x, y, z) | +| `frame_index` | `[N]` | `i32` / `i64` | Monotonically increasing frame indices for alignment | + +```{warning} +Changing the protocol version mid-session is not allowed. If the publisher switches protocol versions while streaming, the interface will automatically disable ZMQ mode and return to reference motions for safety. + +Error message: `Protocol version changed from X to Y during active ZMQ session!` +``` + +### Protocol v1 — Joint-Based (Encode Mode 0) + +Streams raw G1 joint positions and velocities. Use this when your source provides direct qpos/qvel data (e.g., from another simulator or motion capture retargeting pipeline). + +**Required fields:** + +| Field | Shape | Dtype | Description | +|-------|-------|-------|-------------| +| `joint_pos` | `[N, 29]` | `f32` / `f64` | Joint positions in IsaacLab order (all 29 joints) | +| `joint_vel` | `[N, 29]` | `f32` / `f64` | Joint velocities in IsaacLab order (all 29 joints) | + +- `N` = number of frames per message (batch size). +- All 29 joint values must be provided and meaningful. +- Frame counts of `joint_pos` and `joint_vel` must match. + +**Common errors:** +- `Version 1 missing required fields (joint_pos, joint_vel)` — one or both fields are absent. +- `Frame count mismatch between joint_pos and joint_vel` — the `N` dimension differs. + +### Protocol v2 — SMPL-Based (Encode Mode 2) + +Streams SMPL body model data. This protocol is **not used by SONIC's built-in pipelines** — it is available for your own custom applications that produce SMPL representations, for example a plicy only observe the SMPL. + +**Required fields:** + +| Field | Shape | Dtype | Description | +|-------|-------|-------|-------------| +| `smpl_joints` | `[N, 24, 3]` | `f32` / `f64` | SMPL joint positions (24 joints × xyz) | +| `smpl_pose` | `[N, 21, 3]` | `f32` / `f64` | SMPL joint rotations in axis-angle (21 body poses × xyz) | + +- `joint_pos` and `joint_vel` are **optional** in v2. + +**Common errors:** +- `Version 2 missing required field 'smpl_joints'` or `'smpl_pose'` — required SMPL fields are absent. + +### Protocol v3 — Joint + SMPL Combined (Encode Mode 2) + +Combines both joint-level and SMPL data. This is what SONIC uses for whole-body teleoperation (e.g., PICO VR). + +**Required fields:** + +| Field | Shape | Dtype | Description | +|-------|-------|-------|-------------| +| `joint_pos` | `[N, 29]` | `f32` / `f64` | Joint positions in IsaacLab order | +| `joint_vel` | `[N, 29]` | `f32` / `f64` | Joint velocities in IsaacLab order | +| `smpl_joints` | `[N, 24, 3]` | `f32` / `f64` | SMPL joint positions (24 joints × xyz) | +| `smpl_pose` | `[N, 21, 3]` | `f32` / `f64` | SMPL joint rotations in axis-angle (21 body poses × xyz) | + +```{important} +In Protocol v3, **only the 6 wrist joints need meaningful values** in `joint_pos` — the remaining 23 joints can be zero. The wrist joint indices (in IsaacLab order) are: **[23, 24, 25, 26, 27, 28]** (3 joints per wrist × 2 wrists). The `joint_vel` values for non-wrist joints can also be zero. + +The SMPL fields (`smpl_joints`, `smpl_pose`) carry the primary motion data in v3; the wrist joints in `joint_pos` provide fine-grained wrist control that SMPL alone cannot capture. +``` + +- Frame counts across all four fields must be consistent. + +**Common errors:** +- `Version 3 missing required field 'joint_pos'` or `'joint_vel'` — joint fields are absent (unlike v2, they are required in v3). +- `Version 3 frame count mismatch between smpl_joints (X) and joint_pos (Y)` — the `N` dimension differs across fields. + +### Protocol v4 — Token-Only Streaming (Direct Latent Actions) + +Streams pre-computed motion tokens directly to the policy, bypassing the encoder entirely. Use this when your source produces encoded latent actions (e.g., from a separate encoder running on a different machine, or a generative model that outputs tokens directly). + +Unlike v1–v3, Protocol v4 does **not** carry motion frames — the reference motion on the robot side is left unchanged. The tokens are injected directly into the `token_state` observation slot of the decoder policy. + +**Required fields:** + +| Field | Shape | Dtype | Description | +|-------|-------|-------|-------------| +| `token_state` | `[D]` | `f32` / `f64` | Motion token array (dimension must match the encoder `dimension` in the observation config) | + +**Optional fields:** + +| Field | Shape | Dtype | Description | +|-------|-------|-------|-------------| +| `frame_index` | `[1]` | `i32` / `i64` | Frame index (for logging only, does not affect playback) | +| `left_hand_joints` | `[7]` or `[1, 7]` | `f32` / `f64` | Left hand 7-DOF Dex3 joint positions | +| `right_hand_joints` | `[7]` or `[1, 7]` | `f32` / `f64` | Right hand 7-DOF Dex3 joint positions | +| `body_quat_w` | `[4]` or `[1, 4]` | `f32` / `f64` | Body quaternion (w,x,y,z) for heading updates | + +- The `token_state` dimension is validated against the encoder configuration. A mismatch is logged as a warning. +- Hand joints, when provided, are applied to the robot directly (same as v1–v3 optional hand fields). +- `body_quat_w` can be used to update the heading reference during token streaming. + +**Common errors:** +- `Version 4 missing required field 'token_state'` — the `token_state` field is absent from the message. +- `Protocol version 4 with motion data is impossible!` — v4 message produced a motion sequence (should never happen; indicates a decoder bug). +- `Protocol version 4 with empty token data!` — `token_state` field was present but contained no data. + +```{warning} +Protocol v4 requires the policy to have an encoder configuration with `token_state` in its observations. If the model has no encoder (`encode_mode == -2`), the tokens will be received but have no effect. +``` + +### Protocol Summary + +| Protocol | Encode Mode | Used by SONIC | Required Fields | +|----------|-------------|---------------|-----------------| +| v1 | `0` (joint-based) | ✅ Yes | `joint_pos`, `joint_vel` | +| v2 | `2` (SMPL-based) | ❌ Custom only | `smpl_joints`, `smpl_pose` | +| v3 | `2` (SMPL-based) | ✅ Yes | `joint_pos`, `joint_vel`, `smpl_joints`, `smpl_pose` | +| v4 | N/A (bypasses encoder) | ✅ Yes | `token_state` | + +## Optional Stream Fields + +The following optional fields can be included in any protocol version: + +| Field | Shape | Dtype | Description | +|-------|-------|-------|-------------| +| `left_hand_joints` | `[7]` or `[1, 7]` | `f32` / `f64` | Left hand 7-DOF Dex3 joint positions | +| `right_hand_joints` | `[7]` or `[1, 7]` | `f32` / `f64` | Right hand 7-DOF Dex3 joint positions | +| `vr_position` | `[9]` or `[3, 3]` | `f32` / `f64` | VR 3-point tracking positions: left wrist, right wrist, head (xyz × 3) | +| `vr_orientation` | `[12]` or `[3, 4]` | `f32` / `f64` | VR 3-point orientations: left, right, head quaternions (wxyz × 3) | +| `catch_up` | scalar | `bool` / `u8` / `i32` | If `true` (default), resets playback when a large frame gap is detected | +| `heading_increment` | scalar | `f32` / `f64` | Incremental heading adjustment applied per message | + +## Configuration + +| Flag | Default | Description | +|------|---------|-------------| +| `--zmq-host` | `localhost` | ZMQ publisher host | +| `--zmq-port` | `5556` | ZMQ publisher port | +| `--zmq-topic` | `pose` | ZMQ topic prefix | +| `--zmq-conflate` | off | Keep only the latest message (drop stale frames) | diff --git a/GR00T-WholeBodyControl/docs/source/user_guide/configuration.md b/GR00T-WholeBodyControl/docs/source/user_guide/configuration.md new file mode 100644 index 0000000000000000000000000000000000000000..05dffe77f52e109828a924d278f62a230eb267f8 --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/user_guide/configuration.md @@ -0,0 +1,254 @@ +# Configuration Guide + +SONIC uses [Hydra](https://hydra.cc/) for hierarchical configuration. This guide +explains the config structure and the most important parameters to tune. + +## Config Hierarchy + +When you run a training command like: + +```bash +python gear_sonic/train_agent_trl.py +exp=manager/universal_token/all_modes/sonic_release +``` + +Hydra composes the final config from a chain of YAML files: + +``` +gear_sonic/config/ +├── base.yaml # Global defaults (seed, num_envs, paths) +├── base/ +│ ├── hydra.yaml # Hydra output directory settings +│ └── structure.yaml # Resolved experiment directory structure +├── algo/ +│ └── ppo_im_phc.yaml # PPO hyperparameters +├── manager_env/ +│ ├── base_env.yaml # Environment defaults (sim_dt, decimation, episode length) +│ ├── actions/tracking/base.yaml +│ ├── commands/tracking/base.yaml +│ │ └── terms/motion.yaml # Motion library, body names, future frames +│ ├── rewards/tracking/ +│ │ └── base_5point_local_feet_acc.yaml # Reward composition +│ │ └── terms/*.yaml # Individual reward terms with weights +│ ├── terminations/tracking/ +│ │ └── base_adaptive_strict_ori_foot_xyz.yaml # Termination composition +│ │ └── terms/*.yaml # Individual termination conditions +│ ├── events/tracking/ +│ │ └── level0_4.yaml # Domain randomization events +│ └── observations/ +│ ├── tokenizer/ # Encoder input observations +│ ├── policy/ # Policy (actor) observations +│ └── critic/ # Critic observations +├── actor_critic/ +│ └── universal_token/ # Network architecture (encoders, decoders, quantizer) +├── aux_losses/ +│ └── universal_token/ # Auxiliary loss terms +├── trainer/ +│ └── trl_ppo_aux.yaml # Trainer config (PPO with aux losses) +├── callbacks/ # Training callbacks (save, eval, W&B, resample) +└── exp/manager/universal_token/all_modes/ + ├── sonic_release.yaml # Original release experiment config + └── sonic_v1_1.yaml # SONIC v1.1 experiment config +``` + +The experiment config (`sonic_release.yaml`) sits at the top and overrides +specific values from the base configs. You can further override any value +from the command line with `++key=value`. + +## Overriding Config Values + +Hydra uses `++` prefix to force-override values (even nested ones): + +```bash +# Override a top-level value +python gear_sonic/train_agent_trl.py +exp=... num_envs=16 + +# Override a nested value (use dots for nesting) +python gear_sonic/train_agent_trl.py +exp=... \ + ++manager_env.commands.motion.motion_lib_cfg.motion_file=/path/to/data + +# Override a reward weight +python gear_sonic/train_agent_trl.py +exp=... \ + ++manager_env.rewards.tracking_anchor_pos.weight=1.0 +``` + +## Top Parameters to Tune + +### Training scale + +| Parameter | Default | Location | Description | +|-----------|---------|----------|-------------| +| `num_envs` | 4096 | `base.yaml` | Number of parallel environments. Reduce for debugging (`16`), increase for throughput. | +| `headless` | True | `base.yaml` | Set `False` to open the Isaac Lab viewer for visual debugging. | +| `seed` | 0 | `base.yaml` | Random seed for reproducibility. | + +### PPO hyperparameters + +| Parameter | Default | Location | Description | +|-----------|---------|----------|-------------| +| `algo.config.actor_learning_rate` | 2e-5 | `ppo_im_phc.yaml` | Actor learning rate. Lower for finetuning, higher for training from scratch. | +| `algo.config.critic_learning_rate` | 1e-3 | `ppo_im_phc.yaml` | Critic learning rate. Usually 10-100x the actor LR. | +| `algo.config.num_learning_epochs` | 5 | `ppo_im_phc.yaml` | PPO epochs per batch of experience. | +| `algo.config.num_mini_batches` | 4 | `ppo_im_phc.yaml` | Mini-batches per PPO epoch. | +| `algo.config.num_steps_per_env` | 24 | `sonic_release.yaml` | Rollout length (steps per env before PPO update). | +| `algo.config.gamma` | 0.99 | `ppo_im_phc.yaml` | Discount factor. | +| `algo.config.lam` | 0.95 | `ppo_im_phc.yaml` | GAE lambda. | +| `algo.config.clip_param` | 0.2 | `ppo_im_phc.yaml` | PPO clip parameter. | +| `algo.config.entropy_coef` | 0.01 | `ppo_im_phc.yaml` | Entropy bonus coefficient. | +| `algo.config.desired_kl` | 0.01 | `ppo_im_phc.yaml` | Target KL for adaptive learning rate schedule. | +| `algo.config.num_learning_iterations` | 100000 | `ppo_im_phc.yaml` | Total training iterations. | + +### Simulation + +| Parameter | Default | Location | Description | +|-----------|---------|----------|-------------| +| `manager_env.config.sim_dt` | 0.005 | `base_env.yaml` | Physics timestep (200 Hz). Smaller = more stable but slower. | +| `manager_env.config.decimation` | 4 | `base_env.yaml` | Policy runs every `decimation` sim steps (50 Hz policy at 200 Hz sim). | +| `manager_env.config.episode_length_s` | 10.0 | `base_env.yaml` | Episode length in seconds before timeout reset. | +| `manager_env.config.terrain_type` | trimesh | `sonic_release.yaml` | `plane` for flat ground, `trimesh` for rough terrain. | +| `manager_env.config.robot.type` | g1_model_12_dex | `sonic_release.yaml` | Robot type (must match `robot_mapping` in code). | + +### Motion data + +| Parameter | Default | Location | Description | +|-----------|---------|----------|-------------| +| `manager_env.commands.motion.motion_lib_cfg.motion_file` | — | `sonic_release.yaml` | Path to retargeted robot motion PKLs. | +| `manager_env.commands.motion.motion_lib_cfg.smpl_motion_file` | — | `sonic_release.yaml` | Path to SMPL motion PKLs (or `dummy`). | +| `manager_env.commands.motion.motion_lib_cfg.soma_motion_file` | — | `sonic_bones_seed.yaml` | Path to SOMA motion PKLs (4-encoder config only). | +| `manager_env.commands.motion.motion_lib_cfg.smpl_y_up` | true | `sonic_release.yaml` | Set `true` if SMPL data uses y-up coordinates. | +| `manager_env.commands.motion.motion_lib_cfg.target_fps` | 50 | `motion.yaml` | Target FPS for motion resampling. | +| `manager_env.commands.motion.motion_lib_cfg.asset.assetFileName` | g1_29dof_rev_1_0.xml | `motion.yaml` | MJCF file for motion library FK. Change for different robots. | + +### Motion command + +| Parameter | Default | Location | Description | +|-----------|---------|----------|-------------| +| `manager_env.commands.motion.num_future_frames` | 10 | `sonic_release.yaml` | Number of future reference frames provided to the policy. | +| `manager_env.commands.motion.dt_future_ref_frames` | 0.1 | `sonic_release.yaml` | Time spacing between future frames (seconds). | +| `manager_env.commands.motion.cat_upper_body_poses` | true | `sonic_release.yaml` | Augment lower-body motions with upper-body from different clips. | +| `manager_env.commands.motion.cat_upper_body_poses_prob` | 0.5 | `sonic_release.yaml` | Probability of upper-body augmentation per episode. | +| `manager_env.commands.motion.freeze_frame_aug` | true | `sonic_release.yaml` | Augment with frozen (static) reference frames. | + +### Observation history + +| Parameter | Default | Location | Description | +|-----------|---------|----------|-------------| +| `actor_prop_history_length` | 10 | `sonic_release.yaml` | Number of past proprioception frames stacked for actor. | +| `actor_actions_history_length` | 10 | `sonic_release.yaml` | Number of past actions stacked for actor. | +| `critic_prop_history_length` | 10 | `sonic_release.yaml` | Same, for critic. | +| `critic_actions_history_length` | 10 | `sonic_release.yaml` | Same, for critic. | + +### Reward weights + +All reward terms have a `weight` parameter. Positive weights encourage the behavior, +negative weights penalize it. The default weights for `base_5point_local_feet_acc`: + +| Reward term | Weight | Description | +|-------------|--------|-------------| +| `tracking_anchor_pos` | 0.5 | Root position tracking | +| `tracking_anchor_ori` | 0.5 | Root orientation tracking | +| `tracking_relative_body_pos` | 1.0 | Body position tracking (anchor-relative) | +| `tracking_relative_body_ori` | 1.0 | Body orientation tracking (anchor-relative) | +| `tracking_body_linvel` | 1.0 | Body linear velocity tracking | +| `tracking_body_angvel` | 1.0 | Body angular velocity tracking | +| `tracking_vr_5point_local` | 2.0 | 5-point (wrists + head + feet) local tracking | +| `action_rate_l2` | -0.1 | Smooth actions (penalize jerk) | +| `joint_limit` | -10.0 | Stay within joint limits | +| `undesired_contacts` | -0.1 | Penalize non-foot ground contacts | +| `anti_shake_ang_vel` | -0.005 | Penalize wrist/head jitter | +| `feet_acc` | -2.5e-6 | Penalize foot acceleration (smooth stepping) | + +Each reward term also has a `std` parameter controlling the Gaussian kernel +sharpness. Smaller `std` = stricter tracking (reward drops faster with error). + +Override example: +```bash +++manager_env.rewards.tracking_anchor_pos.weight=2.0 +++manager_env.rewards.tracking_anchor_pos.params.std=0.1 +``` + +### Termination thresholds + +Terminations end episodes early when tracking error exceeds a threshold. The +adaptive variants use a curriculum that tightens thresholds over training: + +| Termination | Threshold | Description | +|-------------|-----------|-------------| +| `anchor_pos` | 0.15 m | Root position deviation | +| `anchor_ori_full` | 0.2 rad | Root orientation deviation | +| `ee_body_pos` | 0.15 m | End-effector position deviation | +| `foot_pos_xyz` | 0.2 m | Foot position deviation | +| `motion_time_out` | — | Episode ends when motion clip finishes | + +Looser thresholds (larger values) make training easier initially. The adaptive +terminations automatically tighten as the policy improves. + +### Adaptive motion sampling + +The motion library supports adaptive sampling — motions the policy fails on are +sampled more frequently: + +| Parameter | Default | Description | +|-----------|---------|-------------| +| `adaptive_sampling.enable` | true | Enable adaptive sampling. | +| `adaptive_sampling.bin_size` | 50 | Window size for failure rate tracking. | +| `adaptive_sampling.adp_samp_failure_rate_max_over_mean` | 200 | Max/mean failure rate ratio cap. Prevents one hard motion from dominating. | + +### Saving and logging + +| Parameter | Default | Location | Description | +|-----------|---------|----------|-------------| +| `algo.config.save_interval` | 500 | `ppo_im_phc.yaml` | Save checkpoint every N iterations. | +| `algo.config.eval_frequency` | 500 | `ppo_im_phc.yaml` | Run evaluation every N iterations. | +| `use_wandb` | false | `base.yaml` | Enable Weights & Biases logging. | +| `base_dir` | logs_rl | `base.yaml` | Root directory for training outputs. | + +## Experiment Configs + +| Config | Encoders | Use case | +|--------|----------|----------| +| `sonic_release` | G1, teleop, SMPL | Default — matches the released checkpoint | +| `sonic_v1_1` | G1, teleop, SMPL | SONIC v1.1 with heading-normalized targets and wrist-pose augmentation | +| `sonic_bones_seed` | G1, teleop, SMPL, SOMA | Extended training with SOMA skeleton encoder | +| `sonic_h2` | G1, teleop, SMPL | H2 robot (31 DOF) | + +## Common Recipes + +### Debug a training run visually + +```bash +python gear_sonic/train_agent_trl.py +exp=... \ + num_envs=4 headless=False \ + algo.config.num_learning_iterations=10 +``` + +### Finetune with lower learning rate + +```bash +python gear_sonic/train_agent_trl.py +exp=... \ + +checkpoint=sonic_release/last.pt \ + ++algo.config.actor_learning_rate=5e-6 \ + ++algo.config.desired_kl=0.005 +``` + +### Train on flat ground only + +```bash +python gear_sonic/train_agent_trl.py +exp=... \ + ++manager_env.config.terrain_type=plane +``` + +### Relax termination thresholds for hard motions + +```bash +python gear_sonic/train_agent_trl.py +exp=... \ + ++manager_env.terminations.anchor_pos.params.threshold=0.3 \ + ++manager_env.terminations.ee_body_pos.params.threshold=0.3 +``` + +### Increase tracking precision + +```bash +python gear_sonic/train_agent_trl.py +exp=... \ + ++manager_env.rewards.tracking_relative_body_pos.params.std=0.1 \ + ++manager_env.rewards.tracking_anchor_pos.params.std=0.1 +``` diff --git a/GR00T-WholeBodyControl/docs/source/user_guide/new_embodiments.md b/GR00T-WholeBodyControl/docs/source/user_guide/new_embodiments.md new file mode 100644 index 0000000000000000000000000000000000000000..99b3c9bb75c34ede1ef6c08031a482712b6cf066 --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/user_guide/new_embodiments.md @@ -0,0 +1,471 @@ +# Training on New Embodiments + +SONIC's training pipeline is designed around the Unitree G1 (29 DOF) but can be +extended to other humanoid robots. This guide walks through every file you need +to touch, using the Unitree H2 (31 DOF) as a concrete example. + +## What You Need + +To train SONIC on a new robot, you need: + +1. **Robot model files** — URDF or USD (for Isaac Lab) and MJCF/XML (for the motion library) +2. **Retargeted motion data** — Human motions retargeted to your robot's skeleton (PKL format) +3. **Robot configuration** — Joint/body definitions, actuator parameters, action scales +4. **Experiment config** — Hydra YAML connecting everything together + +## Files You Need to Add or Modify + +Here is every file that needs attention, in the order you should work through them: + +| File | Action | Purpose | +|------|--------|---------| +| `gear_sonic/data/assets/robot_description/urdf//` | **Add** | URDF + mesh files for Isaac Lab simulation | +| `gear_sonic/data/assets/robot_description/mjcf/.xml` | **Add** | MuJoCo XML for motion library forward kinematics | +| `gear_sonic/envs/manager_env/robots/.py` | **Add** | Robot config: joints, actuators, mappings, action scales | +| `gear_sonic/envs/manager_env/robots/__init__.py` | **Modify** | Import your new robot module | +| `gear_sonic/envs/manager_env/modular_tracking_env_cfg.py` | **Modify** | Add robot to `robot_mapping` dict (~line 998) | +| `gear_sonic/trl/utils/order_converter.py` | **Modify** | Add converter class for joint/body reordering | +| `gear_sonic/config/exp/manager/universal_token/all_modes/sonic_.yaml` | **Add** | Experiment config | +| Config YAMLs (terminations, rewards, commands) | **Check** | Body names must exist on your robot | + +## Step 1: Robot Model Files + +Place your URDF and meshes under `gear_sonic/data/assets/robot_description/`: + +``` +gear_sonic/data/assets/robot_description/ +├── urdf/h2/ +│ ├── h2.urdf +│ └── meshes/ # STL/OBJ mesh files +└── mjcf/ + └── h2.xml # MuJoCo XML +``` + +The **URDF** is loaded by Isaac Lab for physics simulation. The **MJCF** is used +by the motion library to compute forward kinematics on reference motion data. +Both must represent the same robot with consistent joint names and tree structure. + +Make sure your URDF mesh paths are correct (relative paths like `meshes/pelvis.stl` +work best). If your URDF uses `package://` paths, update them to match the +directory layout. + +## Step 2: Robot Configuration + +Create `gear_sonic/envs/manager_env/robots/.py`. This is the most +important file — it defines how your robot integrates with the training pipeline. + +### Joint and body ordering + +Isaac Lab and MuJoCo traverse the kinematic tree in different orders. You must +define bidirectional index mappings. Get these by loading your URDF in Isaac Lab +and your MJCF in MuJoCo, printing the joint/body lists, and computing the +reorder indices. + +```python +# All bodies in IsaacLab traversal order (including root "pelvis") +H2_ISAACLAB_JOINTS = [ + "pelvis", + "left_hip_pitch_link", + "right_hip_pitch_link", + # ... all 32 bodies for H2 +] + +# Index arrays: position i in the output = position mapping[i] in the input +H2_ISAACLAB_TO_MUJOCO_DOF = [...] # len = num_dof (31 for H2) +H2_MUJOCO_TO_ISAACLAB_DOF = [...] +H2_ISAACLAB_TO_MUJOCO_BODY = [...] # len = num_bodies (32 for H2) +H2_MUJOCO_TO_ISAACLAB_BODY = [...] + +H2_ISAACLAB_TO_MUJOCO_MAPPING = { + "isaaclab_joints": H2_ISAACLAB_JOINTS, + "isaaclab_to_mujoco_dof": H2_ISAACLAB_TO_MUJOCO_DOF, + "mujoco_to_isaaclab_dof": H2_MUJOCO_TO_ISAACLAB_DOF, + "isaaclab_to_mujoco_body": H2_ISAACLAB_TO_MUJOCO_BODY, + "mujoco_to_isaaclab_body": H2_MUJOCO_TO_ISAACLAB_BODY, +} +``` + +**Getting the mappings right is critical.** If they are wrong, the policy will +receive scrambled observations and produce scrambled actions. Verify by loading a +known pose in both simulators and checking that joint values match after reordering. + +### Actuator parameters (KP/KD tuning) + +The actuator stiffness (KP) and damping (KD) are critical for sim-to-real +transfer and training stability. SONIC uses implicit PD actuators in Isaac Lab. + +```python +# Derive from motor specs — these need tuning for your robot +NATURAL_FREQ = 10 * 2.0 * 3.1415926535 # 10Hz natural frequency +DAMPING_RATIO = 2.0 # Overdamped for stability + +# Per-motor stiffness: KP = armature * omega^2 +STIFFNESS_5020 = ARMATURE_5020 * NATURAL_FREQ**2 +# Per-motor damping: KD = 2 * zeta * armature * omega +DAMPING_5020 = 2.0 * DAMPING_RATIO * ARMATURE_5020 * NATURAL_FREQ +``` + +**Tuning guidance:** + +- Start with the real motor's **armature** (rotor inertia) from the datasheet. +- The **natural frequency** controls responsiveness. 10 Hz is a good starting point + for humanoids. Increase for stiffer/faster tracking, decrease for compliance. +- The **damping ratio** should be >= 1.0 (critically damped or overdamped) to avoid + oscillation. 2.0 works well for SONIC. +- **Different joint groups need different gains.** Hip/knee motors are much stronger + than wrist motors. Group joints by motor type (see G1/H2 configs for examples). +- If training is unstable (robot explodes or falls immediately), your KP/KD values + are likely wrong. Try reducing KP or increasing KD. +- The **effort limits** (max torque) per joint should match the real motor specs. + +### Articulation config + +```python +H2_CFG = ArticulationCfg( + spawn=sim_utils.UrdfFileCfg( + asset_path="gear_sonic/data/assets/robot_description/urdf/h2/h2.urdf", + fix_base=False, + replace_cylinders_with_capsules=True, + activate_contact_sensors=True, + ... + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 1.04), # Standing height — must match your robot + joint_pos={ + ".*_knee_joint": -0.363, # Slight knee bend for stability + # ... default standing pose for all joints + }, + ), + actuators={ + "legs": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_.*", ".*_knee_.*"], + effort_limit={...}, # Max torque per joint (Nm) + stiffness={...}, # KP values + damping={...}, # KD values + armature={...}, # Rotor inertia + ), + # ... one group per motor type (arms, waist, feet, etc.) + }, +) +``` + +**Important init_state notes:** + +- `pos` z-value is the spawn height. Set this so the robot starts standing with + feet slightly above ground. Too low = feet clip through ground on first frame. +- `joint_pos` should be a stable standing pose. Get this from your robot's real + default calibration pose or a MuJoCo keyframe. + +### Action scale + +Action scale maps normalized policy outputs to joint position targets. Compute +from effort limit and stiffness: + +```python +H2_ACTION_SCALE = {} +for joint_name in joint_names: + H2_ACTION_SCALE[joint_name] = effort_limit[joint_name] / stiffness[joint_name] +``` + +Larger action scale = larger joint movements per policy output. If the robot +moves too aggressively, reduce the action scale. + +### Register in __init__.py + +Add your module to `gear_sonic/envs/manager_env/robots/__init__.py` so it's +importable. + +### Register in modular_tracking_env_cfg.py + +Add your robot to the `robot_mapping` dict (around line 998): + +```python +from gear_sonic.envs.manager_env.robots import g1, h2 # Add your import + +robot_mapping = { + "g1_model_12_dex": {...}, + "h2": { + "robot_cfg": h2.H2_CFG, + "action_scale": h2.H2_ACTION_SCALE, + "isaaclab_to_mujoco_mapping": h2.H2_ISAACLAB_TO_MUJOCO_MAPPING, + }, +} +``` + +The string key (e.g., `"h2"`) is what you'll use as `robot.type` in the +experiment config. + +## Step 3: Order Converter + +In `gear_sonic/trl/utils/order_converter.py`, add a converter class. This is used +by the evaluation and export pipeline: + +```python +class H2Converter(IsaacLabMuJoCoConverter): + def __init__(self): + from gear_sonic.envs.manager_env.robots.h2 import ( + H2_ISAACLAB_JOINTS, H2_ISAACLAB_TO_MUJOCO_BODY, + H2_ISAACLAB_TO_MUJOCO_DOF, H2_MUJOCO_TO_ISAACLAB_BODY, + H2_MUJOCO_TO_ISAACLAB_DOF, + ) + self.JOINT_NAMES = H2_ISAACLAB_JOINTS + self.DOF_MAPPINGS = { + ("isaaclab", "mujoco"): H2_ISAACLAB_TO_MUJOCO_DOF, + ("mujoco", "isaaclab"): H2_MUJOCO_TO_ISAACLAB_DOF, + } + self.BODY_MAPPINGS = { + ("isaaclab", "mujoco"): H2_ISAACLAB_TO_MUJOCO_BODY, + ("mujoco", "isaaclab"): H2_MUJOCO_TO_ISAACLAB_BODY, + } + + # Bodies used for VR tracking and foot contact — update for your robot + VR_3POINTS_BODY_NAMES = ["torso_link", "left_wrist_pitch_link", "right_wrist_pitch_link"] + FOOT_BODY_NAMES = ["left_ankle_roll_link", "right_ankle_roll_link"] +``` + +Use lazy imports (inside `__init__`) to avoid circular dependencies. + +## Step 4: Body Name Compatibility + +This is a common source of errors. The training configs reference specific body +names that must exist on your robot. Check **all** of these: + +### Command config (`config/manager_env/commands/terms/motion.yaml`) + +```yaml +anchor_body: "pelvis" # Root body +vr_3point_body: ["left_wrist_yaw_link", "right_wrist_yaw_link", "torso_link"] +reward_point_body: ["pelvis", "left_wrist_yaw_link", "right_wrist_yaw_link", + "left_ankle_roll_link", "right_ankle_roll_link"] +body_names: [ # 14 tracked bodies + "pelvis", "left_hip_roll_link", "left_knee_link", "left_ankle_roll_link", + "right_hip_roll_link", "right_knee_link", "right_ankle_roll_link", + "torso_link", "left_shoulder_roll_link", "left_elbow_link", + "left_wrist_yaw_link", "right_shoulder_roll_link", "right_elbow_link", + "right_wrist_yaw_link", +] +``` + +### Termination configs (`config/manager_env/terminations/terms/`) + +- `ee_body_pos_adaptive.yaml`: references `left_ankle_roll_link`, `right_ankle_roll_link`, + `left_wrist_yaw_link`, `right_wrist_yaw_link` +- `foot_pos_xyz.yaml`: references `left_ankle_roll_link`, `right_ankle_roll_link` + +### Reward configs (`config/manager_env/rewards/terms/`) + +- `undesired_contacts.yaml`: regex pattern excluding specific bodies from contact + penalty — references ankle and wrist link names +- `anti_shake_ang_vel.yaml`: references `left_wrist_yaw_link`, `right_wrist_yaw_link`, + `head_link` + +### What to do if names differ + +If your robot uses different names for equivalent bodies (e.g., H2 has +`head_pitch_link` instead of G1's `head_link`), you have two options: + +1. **Override in experiment config** (recommended): Add overrides in your + `sonic_.yaml` for the specific fields that differ. + +2. **Create robot-specific config variants**: Copy the affected term YAML files + and create robot-specific versions (e.g., `anti_shake_ang_vel_h2.yaml`). + +For H2, most G1 body names happen to exist (both are Unitree humanoids), but +`head_link` does not — H2 has `head_yaw_link` instead. Override in the +experiment config: + +```yaml +manager_env: + rewards: + anti_shake_ang_vel: + params: + body_names: ["left_wrist_yaw_link", "right_wrist_yaw_link", "head_yaw_link"] +``` + +**Tip:** Run training with `num_envs=1` first. If a body name doesn't exist, Isaac +Lab will raise a clear error telling you which name failed. Fix it and retry. + +## Step 5: Motion Data + +SONIC expects retargeted motion data as PKL files (joblib format). Each file +contains a dict keyed by motion name: + +```python +{ + "motion_name": { + "root_trans_offset": np.ndarray, # (T, 3) — root translation + "pose_aa": np.ndarray, # (T, num_bodies, 3) — axis-angle per body + "dof": np.ndarray, # (T, num_dof) — joint positions in MuJoCo order + "root_rot": np.ndarray, # (T, 4) — root quaternion (wxyz) + "smpl_joints": np.ndarray, # (T, 24, 3) — SMPL joint positions (optional) + "fps": int, # Frame rate (typically 30) + } +} +``` + +**Important data format notes:** + +- `num_bodies` and `num_dof` must match your robot (e.g., 32 bodies / 31 DOF for H2). +- `dof` values must be in **MuJoCo joint order**, not IsaacLab order. +- `pose_aa` must be in **MuJoCo body order**. +- Mirrored variants (filename ending in `_M.pkl`) double your effective dataset + size and improve symmetry. +- The `smpl_joints` field is used by the SMPL encoder. Set it to zeros if you + don't have SMPL data. + +The motion library loads PKL files **recursively** from a directory: + +``` +data/h2_motions/ +├── session_01/ +│ ├── walk_forward_001.pkl +│ └── walk_forward_001_M.pkl +└── session_02/ + └── ... +``` + +### Source motion data + +The recommended source is [Bones-SEED](https://huggingface.co/datasets/bones-studio/seed) +— a large-scale human motion dataset (142K+ motions, ~288 hours) that provides: + +- **Raw BVH files** — full-body human motion capture +- **G1 retargeted CSVs** — already retargeted to the Unitree G1 (29 DOF) + +For a new robot, you need to **retarget** the raw human motions to your robot's +skeleton. This is the most labor-intensive step. + +### Retargeting options + +1. **[SOMA Retargeter](https://github.com/NVIDIA/soma-retargeter)** (recommended) — + NVIDIA's BVH-to-humanoid motion retargeting library built with Newton and + NVIDIA Warp. Supports any humanoid robot via JSON configuration. Includes a + viewer for inspecting source and retargeted motions side by side. This is + the same tool used to produce the Bones-SEED G1 retargeted data. + +2. **[GMR](https://github.com/YanjieZe/GMR)** (General Motion Retargeting) — + retargets human motions to arbitrary humanoid robots in real time on CPU. + Supports any URDF. A lighter-weight alternative. + +3. **This repo's data processing** (`gear_sonic/data_process/`) — converts + retargeted CSVs/BVHs into the PKL format SONIC expects. Use this as the + final step after retargeting: + + ```bash + # Convert retargeted CSVs to motion library PKLs + python gear_sonic/data_process/convert_soma_csv_to_motion_lib.py \ + --input /path/to/retargeted_csvs/ \ + --output data/my_robot_motions/robot \ + --fps 30 --fps_source 120 --individual --num_workers 16 + + # Filter out motions that are physically impossible for your robot + python gear_sonic/data_process/filter_and_copy_bones_data.py \ + --source data/my_robot_motions/robot \ + --dest data/my_robot_motions/robot_filtered + ``` + +### SMPL data (optional but recommended) + +The SMPL encoder gives the policy an additional human-skeleton input signal. +You need SMPL retargeted data matching the same motion keys as your robot data. + +- For Bones-SEED motions, pre-computed SMPL data is available on + [Hugging Face](https://huggingface.co/nvidia/GEAR-SONIC): + `python download_from_hf.py --training` +- If you use custom motions, extract SMPL joints from the BVH files: + + ```bash + python gear_sonic/data_process/extract_soma_joints_from_bvh.py \ + --input /path/to/bvh_files/ \ + --output data/my_robot_motions/soma \ + --fps 30 --num_workers 16 + ``` + +- If you don't have SMPL data, set `smpl_motion_file: dummy` in the config. + The training pipeline will generate minimal placeholder SMPL data from the + robot motions. This works but produces weaker SMPL encoder performance. + +## Step 6: Experiment Config + +Create `gear_sonic/config/exp/manager/universal_token/all_modes/sonic_.yaml`. +Start by copying `sonic_release.yaml` and modify: + +```yaml +# @package _global_ +defaults: + - /algo: ppo_im_phc + - /manager_env: base_env + # ... same defaults as sonic_release.yaml + +project_name: TRL_H2_Track # Change project name + +manager_env: + config: + robot: + type: h2 # Must match robot_mapping key + commands: + motion: + motion_lib_cfg: + motion_file: null # Provide on command line + asset: + assetFileName: "h2.xml" # Your MJCF filename +``` + +**Fields to review and potentially override:** + +- `robot.type` — must match the key in `robot_mapping` +- `motion_lib_cfg.asset.assetFileName` — your MJCF file +- `reward_point_body` / `reward_point_body_offset` — key bodies for reward computation +- `vr_3point_body` / `vr_3point_body_offset` — if doing VR teleoperation +- `upper_body_augment_prefixes` — remove if your motion data uses different naming +- Body names in reward/termination overrides — see Step 4 + +## Step 7: Train + +```bash +python gear_sonic/train_agent_trl.py \ + +exp=manager/universal_token/all_modes/sonic_h2 \ + num_envs=16 headless=False \ + ++manager_env.commands.motion.motion_lib_cfg.motion_file= +``` + +Start with `num_envs=16 headless=False` to visually verify the robot loads and +motions play correctly. Then scale up to `num_envs=4096 headless=True` for +full training. + +## Example: H2 (included) + +The codebase includes full H2 support as a reference: + +| Component | File | +|-----------|------| +| Robot config | `gear_sonic/envs/manager_env/robots/h2.py` | +| URDF + meshes | `gear_sonic/data/assets/robot_description/urdf/h2/` | +| MJCF | `gear_sonic/data/assets/robot_description/mjcf/h2.xml` | +| Experiment config | `gear_sonic/config/exp/manager/universal_token/all_modes/sonic_h2.yaml` | +| Order converter | `gear_sonic/trl/utils/order_converter.py` (`H2Converter`) | +| Robot mapping | `gear_sonic/envs/manager_env/modular_tracking_env_cfg.py` | + +## Checklist + +When adding a new robot, verify each of these: + +- [ ] URDF + meshes in `gear_sonic/data/assets/robot_description/urdf//` +- [ ] MJCF in `gear_sonic/data/assets/robot_description/mjcf/.xml` +- [ ] Robot config in `gear_sonic/envs/manager_env/robots/.py`: + - [ ] Joint/body name lists + - [ ] IsaacLab ↔ MuJoCo index mappings (verified correct!) + - [ ] `ArticulationCfg` with tuned KP/KD/effort for each motor group + - [ ] Correct init_state (standing height + default joint angles) + - [ ] Action scale dict +- [ ] Robot imported in `robots/__init__.py` +- [ ] Robot added to `robot_mapping` in `modular_tracking_env_cfg.py` +- [ ] Order converter class in `order_converter.py` +- [ ] Experiment config YAML with correct `robot.type` and `assetFileName` +- [ ] All body names in config YAMLs exist on your robot (check with `num_envs=1`) +- [ ] Human motion source data (e.g., [Bones-SEED](https://huggingface.co/datasets/bones-studio/seed) BVH/CSV files) +- [ ] Motions retargeted to your robot's skeleton (e.g., via [SOMA Retargeter](https://github.com/NVIDIA/soma-retargeter)) +- [ ] Retargeted data converted to PKL format (MuJoCo joint/body order) +- [ ] Motions filtered for physical feasibility (`filter_and_copy_bones_data.py`) +- [ ] Mirrored motion variants (`_M.pkl`) for symmetric training +- [ ] SMPL data matching the same motion keys (or `smpl_motion_file: dummy`) diff --git a/GR00T-WholeBodyControl/docs/source/user_guide/teleoperation.md b/GR00T-WholeBodyControl/docs/source/user_guide/teleoperation.md new file mode 100644 index 0000000000000000000000000000000000000000..18ca824e3b472076f0856e9b6eb39b6cdadb15d6 --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/user_guide/teleoperation.md @@ -0,0 +1,245 @@ +# Whole-body Teleoperation Guide + +This guide covers best practices during whole-body teleoperation. + +```{admonition} Prerequisites +:class: note +Complete the [Quick Start](../getting_started/quickstart), [PICO Setup](../getting_started/vr_teleop_setup), and [Teleop Setup](../tutorials/vr_wholebody_teleop) +``` + +## Overview + +Whole-body teleoperation is very very hard to get right and there are a lot of moving parts. This guide will walk you through the details that are required during whole-body teleoperation. During whole-body teleoperation, the GEAR-SONIC policy will try to copy your motion **as much as possible**, including your foot movements! Thus, it can be quite **demanding and dangerous** if proper precautions are not taken. + +```{admonition} Safety Warning +:class: danger +The robot will track your full-body movements in real-time. Always maintain a clear 3-meter safety zone around the robot, keep a safety operator at the keyboard ready to press **`O`** for emergency stop and always be prepared to emergecy stop on the PICO controller on your own. Practice extensively in simulation before attempting on real hardware! +``` + +## Sample Teleoperation Session + +A typical whole-body teleoperation session follows this workflow: + + +```{video} ../_static/teleop/teleop_session_overview.mp4 +:width: 100% +``` +*Video: Full startup sequence — calibrating the PICO headset, engaging the policy, and the robot starting to balance independently on the gantry.* + +**Terminal 1 — MuJoCo Simulator** (or skip for real robot): +```bash +source .venv_teleop/bin/activate +python gear_sonic/scripts/run_sim_loop.py +``` + +**Terminal 2 — C++ Deployment**: +```bash +cd gear_sonic_deploy +# For simulation: +bash deploy.sh --input-type zmq_manager sim +# For real robot: +# bash deploy.sh --input-type zmq_manager real +``` + +**Terminal 3 — PICO Teleop Streamer**: +```bash +source .venv_teleop/bin/activate +python gear_sonic/scripts/pico_manager_thread_server.py --manager +``` + +**Operator Actions**: +1. **Put on PICO headset and controllers** — Ensure foot trackers are securely attached. +2. **Stand in calibration pose** — Upright, feet together, arms in down. Recalibrate often!!! +3. **Make robot stand loose but standing** - Put the G1 somehow slack on gantry (the policy will start and start balancing on its own). +4. **Press A+B+X+Y** on controllers — Initializes the policy and calibrates (enters Planner mode) +5. **Press A+X** — Switches to Pose mode (whole-body teleoperation active) +6. **Teleoperate** — Your movements are now mirrored by the robot +7. **Press A+B+X+Y** when done — Emergency stop and exit. Policy will stop!!! + +## Clothing Requirements + +**Critical:** You **must** wear **tight-fitting pants or leggings** during teleoperation. + +**Why this matters:** +- The PICO foot trackers use visual tracking and need a clear view of your leg/foot movements +- Loose, baggy, or flowing clothing (sweatpants, wide-leg pants, long skirts) will occlude the trackers +- Even brief tracking loss causes the robot to receive incorrect foot positions, leading to stumbling or dangerous motions + +**Recommended:** +- ✅ Athletic leggings or compression tights +- ✅ Fitted jeans or slim-fit pants + +**Not recommended:** +- ❌ Baggy sweatpants or cargo pants +- ❌ Wide-leg or flared pants +- ❌ Long dresses or skirts +- ❌ Any loose or flowing fabric around the legs + +**Upper body:** Normal fitted clothing is fine. Avoid very baggy sleeves that might interfere with controller tracking. + +**❗️❗️❗️Recalibrate Often** The tracking quality of PICO may get worse overtime. When seeing performance drop, always recalibrate! Also, when the PICO controller loses track, it may get stuck in a sitting/weird pose. Always make sure your tracked motion is correct (by viewing the PICO avatar) before starting the teleoperation policy! + +## WiFi Delays + +```{video} ../_static/teleop/teleop_natural_movement.mp4 +:width: 100% +``` +*Video: Demonstrating how natural vs. hesitant walking affects robot stability — stumbling caused by WiFi delays or unnatural movement.* + +**Network latency should be kept as low as possible.** We provide tools to detect delays. Network delays can significantly affect the GEAR-SONIC policy's performance, as the flow of movement for the robot will be interrupted mid-movement (say mid-stride during walking) and the robot can stumble or lose balance. + +**Best practices:** +- **Use Private WiFi Routers** Public and school wifis can easily have large delays. +- **Minimize WiFi hops** — Ideally the PICO and deployment machine are on the same local network +- **Check latency** — Monitor ZMQ message delays in the terminal output + + +**Expected latency:** +- **Good:** < 10ms (wired or strong local WiFi) +- **Acceptable:** 10-30ms (may notice slight lag) +- **Poor:** > 30ms (robot will struggle to track smooth motions, increased stumbling risk) + +**Checking network performance:** +The deployment terminal will show warnings if message delays exceed thresholds. Watch for messages like: +``` +WARNING: High ZMQ latency detected: 45ms +``` + +If you see persistent high latency warnings, improve your network setup before continuing. + +## Movement Patterns + +```{video} ../_static/teleop/teleop_walking.mp4 +:width: 100% +``` +*Video: Walking demo — forward, backward, running, and sideways movement with natural gait.* + +**Try to be as natural as possible.** The GEAR-SONIC policy is trained on natural human motion, so moving naturally gives the best results. Hesitating when moving actually lead to more stumbling! + +**Good movement practices:** + +1. **Walk naturally** — Use your normal gait with natural arm swing. Don't exaggerate or try to "help" the robot. + +2. **Transfer weight smoothly** — When stepping, shift your weight from one foot to the other just as you normally would. Be confident! + +3. **Use moderate speeds** — The robot can track fast motions, but start with walking at a comfortable pace. You can speed up/starts running once you're comfortable. + +**Common mistakes:** + +- ❌ **Overtly slow movements** — Don't slow down too much! +- ❌ **Trying to match the robot's movement** — Don't try to match the robot movement, as the robot will then try to match your movement and starts stumbling. Be natural! + +**Advanced movements:** + +```{video} ../_static/teleop/teleop_advanced.mp4 +:width: 100% +``` +*Video: Advanced movements — kneeling, dynamic leg motions, and challenging poses.* + +Once comfortable with basic walking: +- **Turning** — Turn naturally by rotating your torso and stepping in the new direction +- **Reaching** — Extend your arms smoothly to grab objects +- **Squatting** — Bend your knees and lower your body naturally +- **Sidestepping** — Step sideways with natural weight transfer + +## Calibration Best Practices + +The initial calibration is **critical** to successful teleoperation. + +**The calibration pose:** +1. **Stand upright** — Look straight ahead (not down at controllers) +2. **Feet together** — Foot parallel with no gaps. +3. **Upper arms down** — Hang straight down beside your torso + + +**Tips:** +- Hold the pose steady for 1-2 seconds after pressing A+B+X+Y +- If the robot seems offset throughout the session, recalibrate (stop with A+B+X+Y, then restart) + + +## Mode Switching Safety + +When switching between modes, **always match the robot's current pose first**. + +**Dangerous scenario:** +1. Robot is in Planner mode standing upright +2. You're crouching or reaching in a different pose +3. You press **A+X** to switch to Pose mode +4. **Robot violently tries to match your crouched pose** ⚠️ + +**Safe procedure:** +1. Before pressing **A+X**, look at the robot (or visualization) +2. Move your body to approximately match the robot's current pose +3. Then press **A+X** — transition will be smooth + +**Pause feature (Menu button):** + +```{video} ../_static/teleop/teleop_pause.mp4 +:width: 100% +``` +*Video: Using the Menu button to pause and resume pose streaming during teleoperation.* + +- Holding **Menu** pauses pose streaming +- **Before releasing Menu**, move your body back to match the robot's current pose +- Releasing Menu while in a very different pose causes sudden dangerous motions + +## Troubleshooting + +### Robot is not tracking my movements + +**Possible causes:** +- Foot trackers are not securely attached or have low battery +- Loose clothing is occluding the trackers +- Poor lighting conditions +- XRoboToolKit not running on PICO or configured incorrectly + +**Solutions:** +1. Check foot tracker placement and battery level +2. Verify you're wearing tight-fitting pants +3. Improve lighting (avoid very bright or very dark areas) +4. Restart XRoboToolKit on the PICO headset +5. Recalibrate + +### Robot makes sudden aggressive motions + +**Possible causes:** +- Switched modes while poses didn't match +- Tracking glitch or foot tracker occlusion +- Network packet loss causing delayed frames + +**Solutions:** +1. Recalibrate carefully before resuming +2. Always match robot's pose before switching modes +3. Check network latency and improve WiFi/wired connection + +### Tracking is jittery or stumbles + +**Possible causes:** +- Wireless delays +- IMU drift +- Joint encoder drift + + +**Solutions:** +1. Reduce WiFi interference (move away from other wireless devices) +2. Reclibrate robot + + +## Emergency Procedures + +### Emergency stop methods + +**Keyboard (deployment terminal):** +- Press **`O`** for immediate stop + +**PICO controllers:** +- Press **A + B + X + Y** simultaneously + +Both methods immediately halt the policy and exit control mode. + + +## Next Steps + +- **Understand input interfaces** — See tutorials for [Keyboard](../tutorials/keyboard.md), [Gamepad](../tutorials/gamepad.md), [ZMQ](../tutorials/zmq.md), [Manager](../tutorials/manager.md) +- **Learn about deployment** — See [Deployment Code & Program Flow](../references/deployment_code) +- **General troubleshooting** — See [Troubleshooting Guide](troubleshooting) \ No newline at end of file diff --git a/GR00T-WholeBodyControl/docs/source/user_guide/training.md b/GR00T-WholeBodyControl/docs/source/user_guide/training.md new file mode 100644 index 0000000000000000000000000000000000000000..200803d409251d5e9865bf28e6d2388716bae32b --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/user_guide/training.md @@ -0,0 +1,339 @@ +# Training Guide + +This guide covers data processing, training, evaluation, and ONNX export for +SONIC whole-body controllers. + +## Overview + +SONIC uses a universal-token architecture to control a humanoid robot (Unitree +G1, 29 DOF) by imitating human motion capture data. Multiple parallel encoders +accept different motion input formats: + +- **G1**: Robot joint trajectories +- **Teleop**: VR 3-point tracking targets (head + two wrists) +- **SMPL**: Parametric human body model joint positions +- **SOMA**: BVH-derived skeleton joint positions (optional 4th encoder) + +All encoders project into a shared latent token space via FSQ (Finite Scalar +Quantization), and a single decoder produces joint actions regardless of input +modality. Training uses PPO with auxiliary losses in Isaac Lab simulation. + +| Config | Encoders | Use case | +|--------|----------|----------| +| `sonic_release` | G1, teleop, SMPL | **Default** — matches the released checkpoint | +| `sonic_v1_1` | G1, teleop, SMPL | SONIC v1.1 with heading-normalized targets and wrist-pose augmentation | +| `sonic_bones_seed` | G1, teleop, SMPL, SOMA | Extended training with SOMA skeleton encoder | + +Use `sonic_release` for finetuning and evaluation. The `sonic_bones_seed` +config adds a fourth SOMA encoder (see [Training with SOMA](#training-with-soma-encoder)). +Use `sonic_v1_1` with `sonic_v1_1/last.pt`. + +## Data Processing + +### Step 1: Convert motion data + +SONIC requires motion data in **motion_lib PKL format**. Convert Bones-SEED +CSV files: + +```bash +python gear_sonic/data_process/convert_soma_csv_to_motion_lib.py \ + --input /path/to/bones_seed/g1/csv/ \ + --output data/motion_lib_bones_seed/robot \ + --fps 30 \ + --fps_source 120 \ + --individual \ + --num_workers 16 +``` + +### Step 2: Filter motions + +Remove motions the G1 robot cannot perform (furniture interaction, vehicles, +acrobatics, elevated surfaces): + +```bash +python gear_sonic/data_process/filter_and_copy_bones_data.py \ + --source data/motion_lib_bones_seed/robot \ + --dest data/motion_lib_bones_seed/robot_filtered \ + --workers 16 +``` + +This removes ~8.7% of motions (~130K of 142K remain). Use `--dry-run` to +preview, or `--add-keywords` to add custom filters. + +### Data layout + +Place processed data at the repo root: + +``` +/ +├── data/motion_lib_bones_seed/ +│ ├── robot/ # Full motion library (142K PKLs) +│ └── robot_filtered/ # Filtered subset (~130K PKLs) +└── gear_sonic/ +``` + +## Training + +### Basic command + +```bash +python gear_sonic/train_agent_trl.py \ + +exp=manager/universal_token/all_modes/sonic_release \ + num_envs=4096 headless=True \ + ++manager_env.commands.motion.motion_lib_cfg.motion_file= \ + ++manager_env.commands.motion.motion_lib_cfg.smpl_motion_file= +``` + +For example, using the sample data from Hugging Face: + +```bash +python gear_sonic/train_agent_trl.py \ + +exp=manager/universal_token/all_modes/sonic_release \ + num_envs=16 headless=True \ + ++manager_env.commands.motion.motion_lib_cfg.motion_file=sample_data/robot_filtered \ + ++manager_env.commands.motion.motion_lib_cfg.smpl_motion_file=sample_data/smpl_filtered +``` + +Or using the full dataset: + +```bash +python gear_sonic/train_agent_trl.py \ + +exp=manager/universal_token/all_modes/sonic_release \ + num_envs=4096 headless=True \ + ++manager_env.commands.motion.motion_lib_cfg.motion_file=data/motion_lib_bones_seed/robot_filtered \ + ++manager_env.commands.motion.motion_lib_cfg.smpl_motion_file=data/smpl_filtered +``` + +### Finetuning from the released checkpoint + +```bash +python gear_sonic/train_agent_trl.py \ + +exp=manager/universal_token/all_modes/sonic_release \ + +checkpoint=sonic_release/last.pt \ + num_envs=4096 headless=True \ + ++manager_env.commands.motion.motion_lib_cfg.motion_file= \ + ++manager_env.commands.motion.motion_lib_cfg.smpl_motion_file= +``` + +### Multi-GPU and multi-node training + +We recommend training with **64+ GPUs** for reasonable convergence times. +Single-node (8 GPU) training works but is significantly slower. + +```bash +# Single node (8 GPUs) +accelerate launch --num_processes=8 gear_sonic/train_agent_trl.py \ + +exp=manager/universal_token/all_modes/sonic_release \ + num_envs=4096 headless=True + +# Multi-node — use accelerate config for distributed setup +accelerate launch \ + --multi_gpu \ + --num_machines=8 \ + --num_processes=64 \ + --machine_rank=$MACHINE_RANK \ + --main_process_ip=$MASTER_ADDR \ + --main_process_port=$MASTER_PORT \ + gear_sonic/train_agent_trl.py \ + +exp=manager/universal_token/all_modes/sonic_release \ + num_envs=4096 headless=True +``` + +For multi-node setup, see the +[Accelerate distributed training guide](https://huggingface.co/docs/accelerate/usage_guides/deepspeed) +and +[multi-node launcher docs](https://huggingface.co/docs/accelerate/package_reference/cli#accelerate-launch). + +### W&B logging + +Enabled by default. Key overrides: + +```bash +WANDB_MODE=offline python gear_sonic/train_agent_trl.py ... # offline mode + wandb.wandb_project=my_project wandb.wandb_entity=my_team # custom project + use_wandb=false # disable entirely +``` + +### Local debug run + +```bash +python gear_sonic/train_agent_trl.py \ + +exp=manager/universal_token/all_modes/sonic_release \ + num_envs=16 headless=False \ + ++algo.config.num_learning_iterations=100 +``` + +## Monitoring + +### Key metrics + +| Metric | Good range | Description | +|--------|-----------|-------------| +| `rewards/total` | 3.0+ | Total reward | +| `rewards/anchor_pos_err` | < 0.15 | Root position tracking error (m) | +| `rewards/body_pos_err` | < 0.10 | Body position tracking error (m) | +| `throughput/fps` | ~4000+ | Training throughput | + +### Checkpoints + +Saved every 2000 steps to: + +``` +logs_rl/TRL_G1_Track/-/ +├── model_step_002000.pt +├── config.yaml +└── ... +``` + +## Evaluation + +### Visualize reference motions + +Replay motions to verify data quality before training: + +```bash +python gear_sonic/train_agent_trl.py \ + +exp=manager/universal_token/all_modes/sonic_release \ + ++replay=True num_envs=4 headless=False +``` + +### Evaluate a checkpoint + +Two eval modes: **metrics** (success rate, MPJPE) and **render** (video output). + +For the released checkpoint, you must override motion paths since its +`config.yaml` has internal training paths. For your own checkpoints trained +with `sonic_release`, omit the motion overrides. + +```bash +# --- Metrics --- +python gear_sonic/eval_agent_trl.py \ + +checkpoint= \ + +headless=True \ + ++eval_callbacks=im_eval \ + ++run_eval_loop=False \ + ++num_envs=128 \ + "+manager_env/terminations=tracking/eval" \ + "++manager_env.commands.motion.motion_lib_cfg.max_unique_motions=512" +``` + +```bash +# --- Render videos --- +python gear_sonic/eval_agent_trl.py \ + +checkpoint= \ + +headless=True \ + ++eval_callbacks=im_eval \ + ++run_eval_loop=False \ + ++num_envs=8 \ + ++manager_env.config.render_results=True \ + "++manager_env.config.save_rendering_dir=/tmp/renders" \ + ++manager_env.config.env_spacing=10.0 \ + "~manager_env/recorders=empty" "+manager_env/recorders=render" +``` + +For the **released checkpoint only**, append this override to either command +(its embedded config has internal training paths): + +```bash + "++manager_env.commands.motion.motion_lib_cfg.motion_file=data/motion_lib_bones_seed/robot_filtered" +``` + +Videos are saved as `000000.mp4`, `000001.mp4`, etc. in `save_rendering_dir`. + +### Expected eval metrics + +*Training rewards* (W&B `Episode_Reward/`): + +| Metric | Converged | Description | +|--------|-----------|-------------| +| `tracking_vr_5point_local` | > 0.80 | 5-point tracking quality | +| `tracking_relative_body_pos` | > 0.44 | Upper-body position tracking | +| `tracking_anchor_pos` | > 0.14 | Root position tracking | +| `time_out` | > 0.90 | Episode completion rate | + +*Eval metrics* (from `eval_agent_trl.py`): + +| Metric | Converged | Description | +|--------|-----------|-------------| +| `success_rate` | > 0.97 | Motions tracked without early termination | +| `mpjpe_l` | < 30 mm | Local per-joint position error | +| `mpjpe_g` | < 200 mm | Global per-joint position error | + +A well-converged policy reaches >0.98 success rate and <29 mm mpjpe_l after +100K iterations. + +## ONNX Export + +Export a trained checkpoint to ONNX for C++ deployment: + +```bash +python gear_sonic/eval_agent_trl.py \ + +checkpoint= \ + +headless=True ++num_envs=1 \ + +export_onnx_only=true +``` + +For the released checkpoint, append the motion path overrides shown in the +eval section above. + +Output (in `exported/` next to the checkpoint): + +| File | Description | +|------|-------------| +| `*_smpl.onnx` | SMPL encoder + decoder (pose estimation input) | +| `*_g1.onnx` | G1 encoder + decoder (robot joint input) | +| `*_teleop.onnx` | Teleop encoder + decoder (VR tracking input) | +| `*_encoder.onnx` | All encoders combined | +| `*_decoder.onnx` | Decoder only | + +Use the encoder+decoder pair matching your input modality. See +[deployment code reference](../references/deployment_code.md) for C++ details. + +## Training with SOMA encoder + +The `sonic_bones_seed` config adds a fourth SOMA encoder for BVH-derived +skeleton joint positions. + +### SOMA data preparation + +```bash +# Extract SOMA joints from BVH +python gear_sonic/data_process/extract_soma_joints_from_bvh.py \ + --input /path/to/bones_seed/bvh/ \ + --output data/motion_lib_bones_seed/soma \ + --fps 30 --num_workers 16 --skip_existing + +# Filter to match robot data +python gear_sonic/data_process/filter_and_copy_bones_data.py \ + --source data/motion_lib_bones_seed/soma \ + --dest data/motion_lib_bones_seed/soma_filtered \ + --workers 16 +``` + +### Training + +Use multi-node training (64+ GPUs recommended): + +```bash +accelerate launch \ + --multi_gpu --num_machines=8 --num_processes=64 \ + --machine_rank=$MACHINE_RANK \ + --main_process_ip=$MASTER_ADDR \ + --main_process_port=$MASTER_PORT \ + gear_sonic/train_agent_trl.py \ + +exp=manager/universal_token/all_modes/sonic_bones_seed \ + num_envs=4096 headless=True \ + ++manager_env.commands.motion.motion_lib_cfg.motion_file=data/motion_lib_bones_seed/robot_filtered \ + ++manager_env.commands.motion.motion_lib_cfg.smpl_motion_file=data/smpl_filtered \ + ++manager_env.commands.motion.motion_lib_cfg.soma_motion_file=data/motion_lib_bones_seed/soma_filtered +``` + +Data layout for 4-encoder training: + +``` +data/ +├── motion_lib_bones_seed/ +│ ├── robot_filtered/ # ~130K PKLs (G1 retargeted) +│ └── soma_filtered/ # ~130K PKLs (SOMA skeleton) +└── smpl_filtered/ # ~131K PKLs (SMPL human) +``` diff --git a/GR00T-WholeBodyControl/docs/source/user_guide/training_data.md b/GR00T-WholeBodyControl/docs/source/user_guide/training_data.md new file mode 100644 index 0000000000000000000000000000000000000000..b51eb3c464a1bbbf5060bcd4aed1f187ded7c345 --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/user_guide/training_data.md @@ -0,0 +1,69 @@ +# Training Data + +## BONES-SEED + +[BONES-SEED](https://huggingface.co/datasets/bones-studio/seed) (Skeletal Everyday Embodiment Dataset) is an open dataset of **142,220 annotated human motion animations** for humanoid robotics, created by [Bones Studio](https://bones.studio/datasets). It provides motion capture data in SOMA and Unitree G1 formats with natural language descriptions, temporal segmentation labels, and detailed skeletal metadata. + +| | | +|---|---| +| **Total motions** | 142,220 (71,132 original + 71,088 mirrored) | +| **Total duration** | ~288 hours (@ 120 fps) | +| **Performers** | 522 actors (253 F / 269 M) | +| **Age range** | 17–71 years | +| **Height range** | 145–199 cm | +| **Weight range** | 38–145 kg | +| **Output formats** | SOMA Uniform · SOMA Proportional · Unitree G1 MuJoCo-compatible | +| **Annotations** | Up to 6 NL descriptions per motion + temporal segmentation + skeletal metadata | + +### Relevance to SONIC + +BONES-SEED a large subset of SONIC training data: + +- **Unitree G1 joint trajectories** — retargeted for MuJoCo, directly usable for motion tracking training +- **Broad motion coverage** — locomotion, manipulation, dance, sports, communication, and everyday activities across 8 categories and 20 sub-categories +- **Rich language annotations** — up to 6 natural language descriptions per motion, enabling language-conditioned policy learning +- **Temporal segmentation** — per-motion phase labels with timestamps for structured skill decomposition +- **Performer diversity** — 522 actors spanning a wide range of body types, ages, and movement styles + +### Motion Categories + +| Package | Motions | Description | +|---------------|---------|-------------------------------------------------------------------------| +| Locomotion | 74,488 | Walking, jogging, jumping, climbing, crawling, turning, and transitions | +| Communication | 21,493 | Gestures, pointing, looking, and communicative body language | +| Interactions | 14,643 | Object manipulation, pick-and-place, carrying, and tool use | +| Dances | 11,006 | Full-body dance performances across multiple styles | +| Gaming | 8,700 | Game-inspired actions and dynamic movements | +| Everyday | 5,816 | Household tasks, consuming, sitting, reading, and daily activities | +| Sport | 3,993 | Athletic movements and sports-specific actions | +| Other | 2,081 | Stunts, martial arts, and edge-case motions | + +### Data Formats + +Every motion is available in three formats: + +- **SOMA Proportional (BVH)** — per-actor skeleton preserving original body proportions +- **SOMA Uniform (BVH)** — standardized skeleton shared across all motions for batch processing +- **Unitree G1 (CSV)** — joint-angle trajectories retargeted to the Unitree G1 humanoid + +### Download + +```bash +# Using the Hugging Face CLI +pip install huggingface_hub +huggingface-cli download bones-studio/seed --repo-type dataset --local-dir ./bones-seed +``` + +```python +# Using Python +from huggingface_hub import snapshot_download + +snapshot_download( + repo_id="bones-studio/seed", + repo_type="dataset", + local_dir="./bones-seed" +) +``` + +After downloading, extract the motion archives: + diff --git a/GR00T-WholeBodyControl/docs/source/user_guide/troubleshooting.md b/GR00T-WholeBodyControl/docs/source/user_guide/troubleshooting.md new file mode 100644 index 0000000000000000000000000000000000000000..a039b64c37fb265b573d74108df5b04838745df1 --- /dev/null +++ b/GR00T-WholeBodyControl/docs/source/user_guide/troubleshooting.md @@ -0,0 +1,296 @@ +# Troubleshooting + +Common issues and solutions. If your problem isn't listed here, check the +[GitHub issues](https://github.com/NVlabs/GR00T-WholeBodyControl/issues) page. + +--- + +## 1. `ModuleNotFoundError: No module named 'isaaclab'` + +**Symptom:** Training or eval script exits immediately with an import error. + +**Cause:** Isaac Lab is not installed, or you're running in the wrong Python +environment. Isaac Lab is not a pip dependency — it must be installed separately. + +**Fix:** + +1. Install Isaac Lab following the + [official guide](https://isaac-sim.github.io/IsaacLab/main/source/setup/installation/index.html). +2. Make sure you activate the correct conda/venv environment before running: + ```bash + conda activate env_isaaclab # or whatever you named it + python -c "import isaaclab; print(isaaclab.__version__)" + ``` + +--- + +## 2. Mesh files are tiny text files (Git LFS not installed) + +**Symptom:** Simulation crashes or renders an invisible/broken robot. Mesh files +(`.stl`, `.STL`) are ~130 bytes and contain text like `version https://git-lfs.github.com/spec/v1`. + +**Cause:** The repo was cloned without Git LFS. Large files (meshes, ONNX models) +are stored via Git LFS and need to be fetched separately. + +**Fix:** + +```bash +sudo apt install git-lfs +git lfs install +git lfs pull +``` + +Verify: `ls -la gear_sonic/data/assets/robot_description/urdf/g1/main.urdf` should +be ~60KB+, not ~130 bytes. + +--- + +## 3. `RuntimeError: size mismatch` when loading a checkpoint + +**Symptom:** Training or eval crashes with errors like: +``` +size mismatch for actor_module.decoders.g1_dyn.module.0.weight: + copying a param with shape torch.Size([2048, 994]) from checkpoint, + the shape in current model is torch.Size([4096, 994]) +``` + +**Cause:** The experiment config defines a different network architecture than +what the checkpoint was trained with. Common when the config overrides +`hidden_dims` to a different size. + +**Fix:** Make sure the experiment config matches the checkpoint's architecture. +Check the `config.yaml` saved alongside the checkpoint for the correct +`hidden_dims`, encoder/decoder settings, etc. The released `sonic_release` +checkpoint uses: + +```yaml +decoders: + g1_dyn: + params: + module_config_dict: + layer_config: + hidden_dims: [2048, 2048, 1024, 1024, 512, 512] +``` + +--- + +## 4. `trl` / `transformers` version conflict during pip install + +**Symptom:** `pip install -e "gear_sonic/[training]"` fails with a dependency +resolution error about incompatible `transformers` versions. + +**Cause:** `trl==0.28.0` requires `transformers>=4.56.2`. If you have an older +`transformers` pinned or installed, pip cannot resolve. + +**Fix:** + +```bash +pip install -e "gear_sonic/[training]" --upgrade +``` + +Or install in a fresh environment. If you need a specific `transformers` version +for another project, use a separate venv for SONIC training. + +--- + +## 5. TensorRT build fails (`TensorRT_ROOT` not set) + +**Symptom:** CMake error during C++ deployment build: +``` +Could not find a package configuration file provided by "TensorRT" +``` + +**Cause:** The `TensorRT_ROOT` environment variable is not set, or TensorRT is +not installed. + +**Fix:** + +1. Download the correct TensorRT version (TAR package, not DEB): + + | Platform | TensorRT Version | + |---|---| + | x86_64 (Desktop) | **10.13** (required) | + | Jetson / G1 onboard Orin | **10.7** (required; JetPack 6) | + +2. Extract and set the environment variable: + ```bash + export TensorRT_ROOT=$HOME/TensorRT + echo 'export TensorRT_ROOT=$HOME/TensorRT' >> ~/.bashrc + ``` + +--- + +## 6. Motion file path errors (`FileNotFoundError` or empty motion library) + +**Symptom:** Training crashes with `FileNotFoundError` on a motion path, or starts +but logs `0 motions loaded`. + +**Cause:** The experiment config has placeholder paths (e.g., +`data/motion_lib_bones_seed/robot_filtered`) that don't exist on your machine. +Motion data paths must be provided on the command line. + +**Fix:** Always pass motion data paths explicitly: + +```bash +python gear_sonic/train_agent_trl.py \ + +exp=manager/universal_token/all_modes/sonic_release \ + ++manager_env.commands.motion.motion_lib_cfg.motion_file= \ + ++manager_env.commands.motion.motion_lib_cfg.smpl_motion_file= +``` + +For quick testing, download the sample data from HuggingFace: + +```bash +hf download nvidia/GEAR-SONIC --include "sample_data/*" --local-dir . +``` + +--- + +## 7. Body name errors (`RuntimeError: body 'xxx' not found`) + +**Symptom:** Isaac Lab crashes with an error about a body/joint name not found +in the robot's articulation. + +**Cause:** A config YAML references a body name that doesn't exist on your robot. +This commonly happens when using G1 configs with a different robot (e.g., H2). + +**Fix:** Check which body name failed and find where it's referenced: + +```bash +grep -rn "the_failing_body_name" gear_sonic/config/ +``` + +Override the body name in your experiment config, or check the +[Training on New Embodiments](new_embodiments.md) guide for the full list of +config files that reference body names. + +--- + +## 8. Robot explodes or falls immediately on first frame + +**Symptom:** The robot ragdolls, flies away, or collapses instantly when +simulation starts. + +**Cause:** Usually one of: + +- **Init state height is wrong** — the robot spawns inside the ground or too high. + Check `init_state.pos` in your robot config (the z-value is spawn height). +- **KP/KD values are wrong** — if stiffness (KP) is too low, joints have no + holding torque. If too high, the simulation becomes unstable. See + [Training on New Embodiments](new_embodiments.md) for tuning guidance. +- **Action scale is too large** — the policy outputs move joints too aggressively. + Reduce `action_scale` values. +- **Default joint angles are wrong** — the robot starts in an impossible pose. + Check `init_state.joint_pos` matches a stable standing configuration. + +**Debug:** Run with `num_envs=1 headless=False` and watch the first few frames. + +--- + +## 9. Robot behaves weirdly during deployment (wrong TensorRT version) + +**Symptom:** The robot stands but moves erratically, drifts, or produces +unnatural motions during C++ deployment — even though the same checkpoint works +correctly in Isaac Lab or MuJoCo simulation. + +**Cause:** You are using a different TensorRT version than required. TensorRT +version mismatches produce **silently wrong inference results** — the model runs +without errors but outputs incorrect actions. + +**Fix:** You **must** use the exact TensorRT versions: + +| Platform | Required Version | +|---|---| +| x86_64 (Desktop) | **TensorRT 10.13** | +| Jetson / G1 onboard Orin | **TensorRT 10.7** (JetPack 6) | + +Verify your version: + +```bash +echo $TensorRT_ROOT +ls $TensorRT_ROOT/lib/libnvinfer.so* +``` + +If the version is wrong, download the correct one from +[NVIDIA Developer](https://developer.nvidia.com/tensorrt/download/10x) and +rebuild the C++ deployment binary. + +--- + +## 10. `ChannelFactory create domain error` in MuJoCo sim + +**Symptom:** `run_sim_loop.py` crashes with: +``` +[ChannelFactory] create domain error. msg: Occurred upon initialisation +of a cyclonedds.domain.Domain +``` + +**Cause:** CycloneDDS domain initialization conflict. The SimulatorFactory +reinitializes a channel that was already created. + +**Fix:** This is a known issue ([#77](https://github.com/NVlabs/GR00T-WholeBodyControl/issues/77)). +Workaround: comment out the duplicate channel init in the simulator factory, +or ensure no other DDS process is using the same domain on your machine. + +--- + +## 11. SMPL tracking is unstable or drifts + +**Symptom:** The robot follows G1 motion tracking well but drifts or becomes +unstable when using SMPL encoder inputs. + +**Cause:** SMPL data may have mismatched coordinate conventions (y-up vs z-up), +incorrect joint ordering, or the SMPL-to-robot retargeting quality is poor. + +**Fix:** + +- Verify `smpl_y_up: true` is set in your config if your SMPL data uses y-up + coordinates. +- Check that the SMPL PKL files have the correct shape: `smpl_joints` should be + `(T, 24, 3)`. +- Try training with `smpl_motion_file: dummy` first to confirm the robot + encoder works before adding SMPL. + +--- + +## 12. MuJoCo viewer renders incorrectly in Docker + +**Symptom:** MuJoCo window is black, garbled, or shows rendering artifacts when +running inside Docker on a machine with an Intel display controller. + +**Cause:** GPU passthrough or display driver conflict between the Intel iGPU and +NVIDIA dGPU inside Docker. + +**Fix:** Force NVIDIA GPU rendering: + +```bash +export __NV_PRIME_RENDER_OFFLOAD=1 +export __GLX_VENDOR_LIBRARY_NAME=nvidia +``` + +Or run with `--gpus all -e DISPLAY=$DISPLAY` in your Docker run command. See +[#25](https://github.com/NVlabs/GR00T-WholeBodyControl/issues/25) for details. + +--- + +## 13. `deploy.sh` fails to bind ZMQ port 5557 on Orin + +**Symptom:** `deploy.sh` exits with a ZMQ bind error on port 5557. + +**Cause:** A Unitree system service (`iphone_server.service`) is already listening on port 5557. + +**Fix:** + +```bash +sudo systemctl stop iphone_server.service +``` + +Then re-run the deployment. The service restarts on the next boot; to keep it stopped across reboots use `sudo systemctl disable iphone_server.service`. + +--- + +## Still stuck? + +- Search [existing issues](https://github.com/NVlabs/GR00T-WholeBodyControl/issues) +- Open a [new issue](https://github.com/NVlabs/GR00T-WholeBodyControl/issues/new) + with your error message, Python version, and OS diff --git a/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/critics/mlp.yaml b/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/critics/mlp.yaml new file mode 100644 index 0000000000000000000000000000000000000000..23496b94b65753f010af4eafd0e51aa28d230911 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/critics/mlp.yaml @@ -0,0 +1,13 @@ +_target_: gear_sonic.trl.modules.actor_critic_modules.Critic +running_mean_std: True +backbone: + _target_: gear_sonic.trl.modules.base_module.BaseModule + process_output_dim: True + module_config_dict: + type: MLP + input_dim: [critic_obs] + output_dim: [1] + layer_config: + type: MLP + hidden_dims: [2048, 2048, 1024, 1024, 512, 512] + activation: SiLU diff --git a/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/decoders/g1_dyn_mlp.yaml b/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/decoders/g1_dyn_mlp.yaml new file mode 100644 index 0000000000000000000000000000000000000000..8d9f0e9204f10c9b648d3a046170fec5edc3df6c --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/decoders/g1_dyn_mlp.yaml @@ -0,0 +1,15 @@ +g1_dyn: + inputs: ["token_flattened", "proprioception"] + outputs: ["action"] + has_temporal_dim: False + params: + _target_: gear_sonic.trl.modules.base_module.BaseModule + input_dim: + output_dim: + num_input_temporal_dims: + num_output_temporal_dims: + module_config_dict: + layer_config: + type: MLP + hidden_dims: [2048, 2048, 1024, 1024, 512, 512] + activation: SiLU diff --git a/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/decoders/g1_kin_mf_mlp.yaml b/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/decoders/g1_kin_mf_mlp.yaml new file mode 100644 index 0000000000000000000000000000000000000000..ec9035b11f7044c04bba9facd1333e441d877f54 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/decoders/g1_kin_mf_mlp.yaml @@ -0,0 +1,15 @@ +g1_kin: + inputs: ["token"] + outputs: ["command_multi_future_nonflat", "motion_anchor_ori_b_mf_nonflat"] + has_temporal_dim: True + params: + _target_: gear_sonic.trl.modules.base_module.BaseModule + input_dim: + output_dim: + num_input_temporal_dims: ${algo.config.actor.backbone.max_num_tokens} + num_output_temporal_dims: ${manager_env.commands.motion.num_future_frames} + module_config_dict: + layer_config: + type: MLP + hidden_dims: [2048, 1024, 512, 512] + activation: SiLU diff --git a/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/encoders/g1_mf_mlp.yaml b/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/encoders/g1_mf_mlp.yaml new file mode 100644 index 0000000000000000000000000000000000000000..657d160a2f97454fe47d6b7f594b6dd1a54fea4a --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/encoders/g1_mf_mlp.yaml @@ -0,0 +1,13 @@ +g1: + inputs: ["command_multi_future_nonflat", "motion_anchor_ori_b_mf_nonflat"] + params: + _target_: gear_sonic.trl.modules.base_module.BaseModule + input_dim: + output_dim: + num_input_temporal_dims: ${manager_env.commands.motion.num_future_frames} + num_output_temporal_dims: ${algo.config.actor.backbone.max_num_tokens} + module_config_dict: + layer_config: + type: MLP + hidden_dims: [2048, 1024, 512, 512] + activation: SiLU diff --git a/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/encoders/smpl_mlp.yaml b/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/encoders/smpl_mlp.yaml new file mode 100644 index 0000000000000000000000000000000000000000..232a17a3d051c7bad21ba9b03c9831efc7585feb --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/encoders/smpl_mlp.yaml @@ -0,0 +1,13 @@ +smpl: + inputs: ["smpl_joints_multi_future_nonflat", "smpl_root_ori_b_multi_future"] + params: + _target_: gear_sonic.trl.modules.base_module.BaseModule + input_dim: + output_dim: + num_input_temporal_dims: ${manager_env.commands.motion.smpl_num_future_frames} + num_output_temporal_dims: ${algo.config.actor.backbone.max_num_tokens} + module_config_dict: + layer_config: + type: MLP + hidden_dims: [2048, 1024, 512, 512] + activation: SiLU diff --git a/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/encoders/soma_mlp.yaml b/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/encoders/soma_mlp.yaml new file mode 100644 index 0000000000000000000000000000000000000000..47a872dd26d33a3c38726b45d8ecba3ba83563b6 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/encoders/soma_mlp.yaml @@ -0,0 +1,18 @@ +soma: + inputs: + [ + "soma_joints_multi_future_local_nonflat", + "soma_root_ori_b_multi_future", + "joint_pos_multi_future_wrist_for_soma", + ] + params: + _target_: gear_sonic.trl.modules.base_module.BaseModule + input_dim: + output_dim: + num_input_temporal_dims: ${manager_env.commands.motion.smpl_num_future_frames} + num_output_temporal_dims: ${algo.config.actor.backbone.max_num_tokens} + module_config_dict: + layer_config: + type: MLP + hidden_dims: [2048, 1024, 512, 512] + activation: SiLU diff --git a/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/encoders/teleop_mlp.yaml b/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/encoders/teleop_mlp.yaml new file mode 100644 index 0000000000000000000000000000000000000000..ffa06a8eeea8c82a9f3bae731d7dd7cb040ade29 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/encoders/teleop_mlp.yaml @@ -0,0 +1,13 @@ +teleop: + inputs: ["command_multi_future_lower_body", "vr_3point_local_target", "vr_3point_local_orn_target", "motion_anchor_ori_b"] + params: + _target_: gear_sonic.trl.modules.base_module.BaseModule + input_dim: + output_dim: + num_input_temporal_dims: + num_output_temporal_dims: ${algo.config.actor.backbone.max_num_tokens} + module_config_dict: + layer_config: + type: MLP + hidden_dims: [2048, 1024, 512, 512] + activation: SiLU diff --git a/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/mlp.yaml b/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/mlp.yaml new file mode 100644 index 0000000000000000000000000000000000000000..dc761dc546faf8ec9dd15133be9ef5ccea7ceba5 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/mlp.yaml @@ -0,0 +1,31 @@ +# @package _global_ + +algo: + config: + actor: + _target_: gear_sonic.trl.modules.actor_critic_modules.Actor + running_mean_std: True + backbone: + _target_: gear_sonic.trl.modules.base_module.BaseModule + process_output_dim: True + module_config_dict: + input_dim: [actor_obs] + output_dim: [robot_action_dim] + layer_config: + type: MLP + hidden_dims: [2048, 2048, 1024, 1024, 512, 512] + activation: SiLU + critic: + _target_: gear_sonic.trl.modules.actor_critic_modules.Critic + running_mean_std: True + backbone: + _target_: gear_sonic.trl.modules.base_module.BaseModule + process_output_dim: True + module_config_dict: + type: MLP + input_dim: [critic_obs] + output_dim: [1] + layer_config: + type: MLP + hidden_dims: [2048, 2048, 1024, 1024, 512, 512] + activation: SiLU diff --git a/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/quantizers/fsq.yaml b/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/quantizers/fsq.yaml new file mode 100644 index 0000000000000000000000000000000000000000..5645ae867a1f8e0c7604c575a5764d63c2308090 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/quantizers/fsq.yaml @@ -0,0 +1 @@ +_target_: vector_quantize_pytorch.FSQ diff --git a/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/universal_token/all_mlp_v1.yaml b/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/universal_token/all_mlp_v1.yaml new file mode 100644 index 0000000000000000000000000000000000000000..67efc1e9e4f37412ffdb540605dca599164fb11d --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/universal_token/all_mlp_v1.yaml @@ -0,0 +1,63 @@ +# @package _global_ + +defaults: + - critics/mlp@algo.config.critic + - quantizers/fsq@algo.config.actor.backbone.quantizer + # encoders + - encoders/g1_mf_mlp@algo.config.actor.backbone.encoders + - encoders/teleop_mlp@algo.config.actor.backbone.encoders + - encoders/smpl_mlp@algo.config.actor.backbone.encoders + # decoders + - decoders/g1_dyn_mlp@algo.config.actor.backbone.decoders + - decoders/g1_kin_mf_mlp@algo.config.actor.backbone.decoders + +manager_env: + commands: + motion: + encoder_sample_probs: # unnormalized + g1: 1.0 + teleop: 1.0 + smpl: 1.0 + +algo: + config: + actor: + _target_: gear_sonic.trl.modules.actor_critic_modules.Actor + running_mean_std: false + input_obs_dict: true + has_aux_loss: true + backbone: + _target_: gear_sonic.trl.modules.universal_token_modules.UniversalTokenModule + num_future_frames: ${manager_env.commands.motion.num_future_frames} + proprioception_features: ["actor_obs"] + encoder_sample_probs: ${manager_env.commands.motion.encoder_sample_probs} + num_fsq_levels: 32 + fsq_level_list: 32 + max_num_tokens: 2 + encoders: + g1: + inputs: + [ + "command_multi_future_nonflat", + "motion_anchor_ori_b_mf_nonflat", + "command_z_multi_future_nonflat", + ] + teleop: + inputs: + [ + "command_multi_future_lower_body", + "vr_3point_local_target", + "vr_3point_local_orn_target", + "motion_anchor_ori_b", + "command_z", + ] + smpl: + inputs: ["smpl_joints_multi_future_nonflat", "smpl_root_ori_b_multi_future"] + decoders: + g1_kin: + outputs: + [ + "command_multi_future_nonflat", + "motion_anchor_ori_b_mf_nonflat", + "command_z_multi_future_nonflat", + ] diff --git a/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/universal_token/all_mlp_v1_soma.yaml b/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/universal_token/all_mlp_v1_soma.yaml new file mode 100644 index 0000000000000000000000000000000000000000..a82c314056589991b0680c639fa67e9d6b0fdccd --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/actor_critic/universal_token/all_mlp_v1_soma.yaml @@ -0,0 +1,72 @@ +# @package _global_ + +defaults: + - critics/mlp@algo.config.critic + - quantizers/fsq@algo.config.actor.backbone.quantizer + # encoders + - encoders/g1_mf_mlp@algo.config.actor.backbone.encoders + - encoders/teleop_mlp@algo.config.actor.backbone.encoders + - encoders/smpl_mlp@algo.config.actor.backbone.encoders + - encoders/soma_mlp@algo.config.actor.backbone.encoders + # decoders + - decoders/g1_dyn_mlp@algo.config.actor.backbone.decoders + - decoders/g1_kin_mf_mlp@algo.config.actor.backbone.decoders + +manager_env: + commands: + motion: + encoder_sample_probs: # unnormalized + g1: 1.0 + teleop: 1.0 + smpl: 1.0 + soma: 1.0 + +algo: + config: + actor: + _target_: gear_sonic.trl.modules.actor_critic_modules.Actor + running_mean_std: false + input_obs_dict: true + has_aux_loss: true + backbone: + _target_: gear_sonic.trl.modules.universal_token_modules.UniversalTokenModule + num_future_frames: ${manager_env.commands.motion.num_future_frames} + proprioception_features: ["actor_obs"] + encoder_sample_probs: ${manager_env.commands.motion.encoder_sample_probs} + num_fsq_levels: 32 + fsq_level_list: 32 + max_num_tokens: 2 + encoders: + g1: + inputs: + [ + "command_multi_future_nonflat", + "motion_anchor_ori_b_mf_nonflat", + "command_z_multi_future_nonflat", + ] + teleop: + inputs: + [ + "command_multi_future_lower_body", + "vr_3point_local_target", + "vr_3point_local_orn_target", + "motion_anchor_ori_b", + "command_z", + ] + smpl: + inputs: ["smpl_joints_multi_future_nonflat", "smpl_root_ori_b_multi_future"] + soma: + inputs: + [ + "soma_joints_multi_future_local_nonflat", + "soma_root_ori_b_multi_future", + "joint_pos_multi_future_wrist_for_soma", + ] + decoders: + g1_kin: + outputs: + [ + "command_multi_future_nonflat", + "motion_anchor_ori_b_mf_nonflat", + "command_z_multi_future_nonflat", + ] diff --git a/GR00T-WholeBodyControl/gear_sonic/config/algo/ppo_im_phc.yaml b/GR00T-WholeBodyControl/gear_sonic/config/algo/ppo_im_phc.yaml new file mode 100644 index 0000000000000000000000000000000000000000..b5432a3cb76533b55d8fc58e71fb292f7564086c --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/algo/ppo_im_phc.yaml @@ -0,0 +1,43 @@ +# @package _global_ + +defaults: + - trl@algo.trl: ppo + - /actor_critic: mlp + +algo: + _recursive_: False + config: + num_learning_epochs: 5 + num_mini_batches: 4 + clip_param: 0.2 + gamma: 0.99 + lam: 0.95 + value_loss_coef: 1.0 + entropy_coef: 0.01 + actor_learning_rate: 2e-5 # 5e-4 # 1.e-3 + critic_learning_rate: 1.e-3 # 5e-4 # 1.e-3 + max_grad_norm: 1.0 + use_clipped_value_loss: True + schedule: "adaptive" + desired_kl: 0.01 + adaptive_lr_min: 1e-5 + adaptive_lr_max: 2e-4 + + use_new_actor_critic: True + use_padding_mask: False + ppo_shuffle_every_epoch: True + sync_advantage_normalization: True + empty_cache_every_n_ppo_epoch: 3 + + num_steps_per_env: 32 + save_interval: 500 + eval_frequency: 500 + load_optimizer: True + + init_noise_std: 0.05 + + num_learning_iterations: 100000 + init_at_random_ep_len: True + + global_rank: 0 + world_size: 1 diff --git a/GR00T-WholeBodyControl/gear_sonic/config/algo/trl/ppo.yaml b/GR00T-WholeBodyControl/gear_sonic/config/algo/trl/ppo.yaml new file mode 100644 index 0000000000000000000000000000000000000000..9d14fc39578acfd4e3b22e8e012f5d9b98d9c85b --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/algo/trl/ppo.yaml @@ -0,0 +1,18 @@ +dataset_name: dummy +output_dir: './trl' +learning_rate: ${algo.config.actor_learning_rate} +num_total_batches: ${algo.config.num_learning_iterations} # same as num of total training steps +total_episodes: # num_total_batches * num_envs (batch_size) +num_ppo_epochs: ${algo.config.num_learning_epochs} +num_mini_batches: ${algo.config.num_mini_batches} +per_device_train_batch_size: +gradient_accumulation_steps: 1 +report_to: none +gamma: ${algo.config.gamma} +lam: ${algo.config.lam} +vf_coef: ${algo.config.value_loss_coef} +# entropy_coef: ${algo.config.entropy_coef} +max_grad_norm: ${algo.config.max_grad_norm} +lr_scheduler_type: constant +save_strategy: 'no' +disable_tqdm: True diff --git a/GR00T-WholeBodyControl/gear_sonic/config/aux_losses/terms/g1_recon.yaml b/GR00T-WholeBodyControl/gear_sonic/config/aux_losses/terms/g1_recon.yaml new file mode 100644 index 0000000000000000000000000000000000000000..135bc6be7834fd3daa26a6183816506e30e7a3b6 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/aux_losses/terms/g1_recon.yaml @@ -0,0 +1,3 @@ +g1_recon: + _target_: gear_sonic.trl.losses.token_losses.G1ReconLoss + loss_type: "mse" # Options: "mse", "l1", "huber" diff --git a/GR00T-WholeBodyControl/gear_sonic/config/aux_losses/terms/g1_smpl_latent.yaml b/GR00T-WholeBodyControl/gear_sonic/config/aux_losses/terms/g1_smpl_latent.yaml new file mode 100644 index 0000000000000000000000000000000000000000..b0bfd6e05966431d40da76b5498b85c92482d91b --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/aux_losses/terms/g1_smpl_latent.yaml @@ -0,0 +1,3 @@ +g1_smpl_latent: + _target_: gear_sonic.trl.losses.token_losses.G1SmplLatentLoss + loss_type: "mse" # Options: "mse", "l1", "huber", "cosine" diff --git a/GR00T-WholeBodyControl/gear_sonic/config/aux_losses/terms/g1_soma_latent.yaml b/GR00T-WholeBodyControl/gear_sonic/config/aux_losses/terms/g1_soma_latent.yaml new file mode 100644 index 0000000000000000000000000000000000000000..564de552d454f77f4fb30141e907f69a63eaffa9 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/aux_losses/terms/g1_soma_latent.yaml @@ -0,0 +1,3 @@ +g1_soma_latent: + _target_: gear_sonic.trl.losses.token_losses.G1SomaLatentLoss + loss_type: "mse" # Options: "mse", "l1", "huber", "cosine" diff --git a/GR00T-WholeBodyControl/gear_sonic/config/aux_losses/terms/g1_teleop_latent.yaml b/GR00T-WholeBodyControl/gear_sonic/config/aux_losses/terms/g1_teleop_latent.yaml new file mode 100644 index 0000000000000000000000000000000000000000..855f068f335a408971f75da64951296721d99519 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/aux_losses/terms/g1_teleop_latent.yaml @@ -0,0 +1,3 @@ +g1_teleop_latent: + _target_: gear_sonic.trl.losses.token_losses.G1TeleopLatentLoss + loss_type: "mse" # Options: "mse", "l1", "huber", "cosine" diff --git a/GR00T-WholeBodyControl/gear_sonic/config/aux_losses/terms/reencoded_smpl_g1_latent.yaml b/GR00T-WholeBodyControl/gear_sonic/config/aux_losses/terms/reencoded_smpl_g1_latent.yaml new file mode 100644 index 0000000000000000000000000000000000000000..1a7d63d53f36c4616bac33d30473815bc169fab6 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/aux_losses/terms/reencoded_smpl_g1_latent.yaml @@ -0,0 +1,3 @@ +reencoded_smpl_g1_latent: + _target_: gear_sonic.trl.losses.token_losses.ReencodedSmplG1LatentLoss + loss_type: "mse" # Options: "mse", "l1", "huber", "cosine" diff --git a/GR00T-WholeBodyControl/gear_sonic/config/aux_losses/terms/teleop_smpl_latent.yaml b/GR00T-WholeBodyControl/gear_sonic/config/aux_losses/terms/teleop_smpl_latent.yaml new file mode 100644 index 0000000000000000000000000000000000000000..3610bffab40725ae2bd3ec22cedeb283ca40f3e0 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/aux_losses/terms/teleop_smpl_latent.yaml @@ -0,0 +1,4 @@ +teleop_smpl_latent: + _target_: gear_sonic.trl.losses.token_losses.TeleopSmplLatentLoss + loss_type: "mse" # Options: "mse", "l1", "huber", "cosine" + detach_teleop_target: True # teleop is BRIDGE, train smpl to match teleop diff --git a/GR00T-WholeBodyControl/gear_sonic/config/aux_losses/universal_token/g1_recon_and_all_latent.yaml b/GR00T-WholeBodyControl/gear_sonic/config/aux_losses/universal_token/g1_recon_and_all_latent.yaml new file mode 100644 index 0000000000000000000000000000000000000000..9eec85d88b6de1c946129d3fca0151037472dcf0 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/aux_losses/universal_token/g1_recon_and_all_latent.yaml @@ -0,0 +1,16 @@ +# @package algo.config.actor.backbone + +defaults: + - terms/g1_recon@aux_loss_func + - terms/g1_smpl_latent@aux_loss_func + - terms/g1_teleop_latent@aux_loss_func + - terms/teleop_smpl_latent@aux_loss_func + - terms/reencoded_smpl_g1_latent@aux_loss_func + + +aux_loss_coef: + g1_recon: 0.01 + g1_smpl_latent: 1.0 + g1_teleop_latent: 1.0 + teleop_smpl_latent: 1.0 + reencoded_smpl_g1_latent: 1.0 diff --git a/GR00T-WholeBodyControl/gear_sonic/config/aux_losses/universal_token/g1_recon_and_all_latent_soma.yaml b/GR00T-WholeBodyControl/gear_sonic/config/aux_losses/universal_token/g1_recon_and_all_latent_soma.yaml new file mode 100644 index 0000000000000000000000000000000000000000..97cc61747cc160b681c1a7bad4859757814b8e71 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/aux_losses/universal_token/g1_recon_and_all_latent_soma.yaml @@ -0,0 +1,17 @@ +# @package algo.config.actor.backbone + +defaults: + - terms/g1_recon@aux_loss_func + - terms/g1_smpl_latent@aux_loss_func + - terms/g1_teleop_latent@aux_loss_func + - terms/teleop_smpl_latent@aux_loss_func + - terms/reencoded_smpl_g1_latent@aux_loss_func + - terms/g1_soma_latent@aux_loss_func + +aux_loss_coef: + g1_recon: 0.01 + g1_smpl_latent: 1.0 + g1_teleop_latent: 1.0 + teleop_smpl_latent: 1.0 + reencoded_smpl_g1_latent: 1.0 + g1_soma_latent: 1.0 diff --git a/GR00T-WholeBodyControl/gear_sonic/config/base/hydra.yaml b/GR00T-WholeBodyControl/gear_sonic/config/base/hydra.yaml new file mode 100644 index 0000000000000000000000000000000000000000..e4e6f7f401632afb2dbac7b6a475aaf4c8ffb407 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/base/hydra.yaml @@ -0,0 +1,13 @@ +# @package _global_ + +hydra: # So hydra will put your config info in the same dir as your checkpoints + run: + dir: ${save_dir} + sweep: + dir: ${save_dir} + job: + chdir: False + # job_logging: + # disable_existing_loggers: true + # hydra_logging: + # disable_existing_loggers: true diff --git a/GR00T-WholeBodyControl/gear_sonic/config/base/structure.yaml b/GR00T-WholeBodyControl/gear_sonic/config/base/structure.yaml new file mode 100644 index 0000000000000000000000000000000000000000..1284129df3ad11c51dfa03d3baf3228fb37a6113 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/base/structure.yaml @@ -0,0 +1,7 @@ +# @package _global_ + +algo: ??? + +env: ??? + +terrain: ??? # This is defined in the terrain configs diff --git a/GR00T-WholeBodyControl/gear_sonic/config/callbacks/im_eval.yaml b/GR00T-WholeBodyControl/gear_sonic/config/callbacks/im_eval.yaml new file mode 100644 index 0000000000000000000000000000000000000000..33cd247548227bd0e860fda63cb9667a17d598f5 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/callbacks/im_eval.yaml @@ -0,0 +1,5 @@ +im_eval: + _target_: gear_sonic.trl.callbacks.im_eval_callback.ImEvalCallback + eval_frequency: 1 + eval_only: true + output_dir: ${experiment_dir}/eval_metrics diff --git a/GR00T-WholeBodyControl/gear_sonic/config/callbacks/im_resample.yaml b/GR00T-WholeBodyControl/gear_sonic/config/callbacks/im_resample.yaml new file mode 100644 index 0000000000000000000000000000000000000000..5d48758f5516cab2a804e7d9dc159a70a1087201 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/callbacks/im_resample.yaml @@ -0,0 +1,4 @@ +im_resample: + _target_: gear_sonic.trl.callbacks.im_resample_callback.ImResampleCallback + motion_resample_frequency: 250 + # skip_resample_frequency: 500 diff --git a/GR00T-WholeBodyControl/gear_sonic/config/callbacks/model_save.yaml b/GR00T-WholeBodyControl/gear_sonic/config/callbacks/model_save.yaml new file mode 100644 index 0000000000000000000000000000000000000000..99bfa7436f50d05b9a42ac6dbd22b817b24e952a --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/callbacks/model_save.yaml @@ -0,0 +1,5 @@ +model_save: + _target_: gear_sonic.trl.callbacks.model_save_callback.ModelSaveCallback + save_dir: ${experiment_dir} + save_frequency: 2000 + max_disk_usage: 14.8 # TB diff --git a/GR00T-WholeBodyControl/gear_sonic/config/callbacks/read_eval.yaml b/GR00T-WholeBodyControl/gear_sonic/config/callbacks/read_eval.yaml new file mode 100644 index 0000000000000000000000000000000000000000..8ce8d085c3095b4abe29313ee5991ab0f1a9cce3 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/callbacks/read_eval.yaml @@ -0,0 +1,4 @@ +read_eval: + _target_: gear_sonic.trl.callbacks.read_eval_callback.ReadEvalCallback + eval_dir: ${experiment_dir}/eval + check_interval: 1 diff --git a/GR00T-WholeBodyControl/gear_sonic/config/callbacks/wandb.yaml b/GR00T-WholeBodyControl/gear_sonic/config/callbacks/wandb.yaml new file mode 100644 index 0000000000000000000000000000000000000000..a99fcd9722784799d7445d1e9c9e3096b948a378 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/callbacks/wandb.yaml @@ -0,0 +1,2 @@ +wandb: + _target_: gear_sonic.trl.callbacks.wandb_callback.WandbCallback diff --git a/GR00T-WholeBodyControl/gear_sonic/config/exp/manager/universal_token/all_modes/sonic_bones_seed.yaml b/GR00T-WholeBodyControl/gear_sonic/config/exp/manager/universal_token/all_modes/sonic_bones_seed.yaml new file mode 100644 index 0000000000000000000000000000000000000000..a3cbf24addd56a6f4772c26f37eab1aa89c2917a --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/exp/manager/universal_token/all_modes/sonic_bones_seed.yaml @@ -0,0 +1,119 @@ +# @package _global_ + +defaults: + - /algo: ppo_im_phc + - /manager_env: base_env + - /callbacks/read_eval + - /callbacks/im_resample + - /aux_losses: universal_token/g1_recon_and_all_latent_soma + - override /trainer: trl_ppo_aux + - override /actor_critic: universal_token/all_mlp_v1_soma + - override /manager_env/observations/tokenizer: unitoken_all_noz_soma + - override /manager_env/observations/policy: local_dir_hist + - override /manager_env/observations/critic: privileged_mf_hist + - override /manager_env/events: tracking/level0_4 + - override /manager_env/terminations: tracking/base_adaptive_strict_ori_foot_xyz + - override /manager_env/rewards: tracking/base_5point_local_feet_acc + +use_manager_env: true + +exp_base: ${hydra:runtime.choices.exp} +exp_var: test +experiment_name: ${exp_base}_${exp_var} +experiment_dir: ${base_dir}/${project_name}/${experiment_name}-${timestamp} + +num_envs: 4096 +project_name: TRL_G1_Track + +actor_prop_history_length: 10 +actor_actions_history_length: 10 + +critic_prop_history_length: 10 +critic_actions_history_length: 10 + +manager_env: + rewards: + feet_acc: + weight: -2.5e-6 + config: + robot: + type: g1_model_12_dex + terrain_type: trimesh + commands: + motion: + reward_point_body: ["torso_link", "left_wrist_yaw_link", "right_wrist_yaw_link"] + reward_point_body_offset: [[0.0, 0.0, 0.5], [0.0, -0.0, 0.0], [0.0, -0.0, 0.0]] + num_future_frames: 10 + dt_future_ref_frames: 0.1 + smpl_num_future_frames: 10 + smpl_dt_future_ref_frames: 0.02 + cat_upper_body_poses: true + cat_upper_body_poses_prob: 0.5 + + encoder_sample_probs: + g1: 1.0 + teleop: 1.0 + smpl: 1.0 + soma: 1.0 + freeze_frame_aug: true + teleop_sample_prob_when_smpl: 0.5 + + motion_lib_cfg: + adaptive_sampling: + adp_samp_failure_rate_max_over_mean: 200 + motion_file: data/motion_lib_bones_seed/robot_filtered + smpl_motion_file: dummy + smpl_y_up: true + soma_motion_file: data/motion_lib_bones_seed/soma_filtered + soma_y_up: true + +algo: + config: + empty_cache_every_n_ppo_epoch: -1 + num_steps_per_env: 24 + use_clampped_std: true + std_clamp_min: 0.001 + std_clamp_max: 0.5 + max_grad_norm: 0.1 + actor: + backbone: + reencode_smpl_g1_recon: true + encoders: + g1: + inputs: ["command_multi_future_nonflat", "motion_anchor_ori_b_mf_nonflat"] + teleop: + inputs: + [ + "command_multi_future_lower_body", + "vr_3point_local_target", + "vr_3point_local_orn_target", + "motion_anchor_ori_b", + ] + smpl: + inputs: + [ + "smpl_joints_multi_future_local_nonflat", + "smpl_root_ori_b_multi_future", + "joint_pos_multi_future_wrist_for_smpl", + ] + soma: + inputs: + [ + "soma_joints_multi_future_local_nonflat", + "soma_root_ori_b_multi_future", + "joint_pos_multi_future_wrist_for_soma", + ] + decoders: + g1_kin: + outputs: ["command_multi_future_nonflat", "motion_anchor_ori_b_mf_nonflat"] + g1_dyn: + params: + module_config_dict: + layer_config: + hidden_dims: [4096, 4096, 2048, 2048, 1024, 1024, 512, 512] + + critic: + backbone: + module_config_dict: + layer_config: + hidden_dims: [4096, 4096, 2048, 2048, 1024, 1024, 512, 512] diff --git a/GR00T-WholeBodyControl/gear_sonic/config/exp/manager/universal_token/all_modes/sonic_h2.yaml b/GR00T-WholeBodyControl/gear_sonic/config/exp/manager/universal_token/all_modes/sonic_h2.yaml new file mode 100644 index 0000000000000000000000000000000000000000..320b49b64814454d36c7089f43b4938ab3a652ea --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/exp/manager/universal_token/all_modes/sonic_h2.yaml @@ -0,0 +1,104 @@ +# @package _global_ +# SONIC H2 config — 3 encoders (G1/motion tracking, teleop, SMPL) for Unitree H2. +# Based on sonic_release but targeting the H2 robot (32 bodies, 31 DOF). + +defaults: + - /algo: ppo_im_phc + - /manager_env: base_env + - /callbacks/read_eval + - /callbacks/im_resample + - /aux_losses: universal_token/g1_recon_and_all_latent + - override /trainer: trl_ppo_aux + - override /actor_critic: universal_token/all_mlp_v1 + - override /manager_env/observations/tokenizer: unitoken_all_noz + - override /manager_env/observations/policy: local_dir_hist + - override /manager_env/observations/critic: privileged_mf_hist + - override /manager_env/events: tracking/level0_4 + - override /manager_env/terminations: tracking/base_adaptive_strict_ori_foot_xyz + - override /manager_env/rewards: tracking/base_5point_local_feet_acc + +use_manager_env: true + +exp_base: ${hydra:runtime.choices.exp} +exp_var: test +experiment_name: ${exp_base}_${exp_var} +experiment_dir: ${base_dir}/${project_name}/${experiment_name}-${timestamp} + +num_envs: 4096 +project_name: TRL_H2_Track + +actor_prop_history_length: 10 +actor_actions_history_length: 10 + +critic_prop_history_length: 10 +critic_actions_history_length: 10 + +manager_env: + rewards: + feet_acc: + weight: -2.5e-6 + config: + robot: + type: h2 + terrain_type: trimesh + commands: + motion: + reward_point_body: ["torso_link", "left_wrist_yaw_link", "right_wrist_yaw_link"] + reward_point_body_offset: [[0.0, 0.0, 0.5], [0.0, -0.0, 0.0], [0.0, -0.0, 0.0]] + num_future_frames: 10 + dt_future_ref_frames: 0.1 + smpl_num_future_frames: 10 + smpl_dt_future_ref_frames: 0.02 + cat_upper_body_poses: true + cat_upper_body_poses_prob: 0.5 + + freeze_frame_aug: true + teleop_sample_prob_when_smpl: 0.5 + + motion_lib_cfg: + adaptive_sampling: + adp_samp_failure_rate_max_over_mean: 200 + motion_file: null + smpl_motion_file: dummy + smpl_y_up: true + asset: + assetFileName: "h2.xml" + +algo: + config: + empty_cache_every_n_ppo_epoch: -1 + num_steps_per_env: 24 + use_clampped_std: true + std_clamp_min: 0.001 + std_clamp_max: 0.5 + max_grad_norm: 0.1 + actor: + backbone: + reencode_smpl_g1_recon: true + encoders: + g1: + inputs: ["command_multi_future_nonflat", "motion_anchor_ori_b_mf_nonflat"] + teleop: + inputs: + [ + "command_multi_future_lower_body", + "vr_3point_local_target", + "vr_3point_local_orn_target", + "motion_anchor_ori_b", + ] + smpl: + inputs: + [ + "smpl_joints_multi_future_local_nonflat", + "smpl_root_ori_b_multi_future", + "joint_pos_multi_future_wrist_for_smpl", + ] + decoders: + g1_kin: + outputs: ["command_multi_future_nonflat", "motion_anchor_ori_b_mf_nonflat"] + + critic: + backbone: + module_config_dict: + layer_config: + hidden_dims: [2048, 2048, 1024, 1024, 512, 512] diff --git a/GR00T-WholeBodyControl/gear_sonic/config/exp/manager/universal_token/all_modes/sonic_release.yaml b/GR00T-WholeBodyControl/gear_sonic/config/exp/manager/universal_token/all_modes/sonic_release.yaml new file mode 100644 index 0000000000000000000000000000000000000000..6c336cff534d3265b35ff093a296cee7e938f8ea --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/exp/manager/universal_token/all_modes/sonic_release.yaml @@ -0,0 +1,108 @@ +# @package _global_ +# SONIC release config — 3 encoders (G1, teleop, SMPL), no SOMA. +# Use this to finetune the released SONIC checkpoint. + +defaults: + - /algo: ppo_im_phc + - /manager_env: base_env + - /callbacks/read_eval + - /callbacks/im_resample + - /aux_losses: universal_token/g1_recon_and_all_latent + - override /trainer: trl_ppo_aux + - override /actor_critic: universal_token/all_mlp_v1 + - override /manager_env/observations/tokenizer: unitoken_all_noz + - override /manager_env/observations/policy: local_dir_hist + - override /manager_env/observations/critic: privileged_mf_hist + - override /manager_env/events: tracking/level0_4 + - override /manager_env/terminations: tracking/base_adaptive_strict_ori_foot_xyz + - override /manager_env/rewards: tracking/base_5point_local_feet_acc + +use_manager_env: true + +exp_base: ${hydra:runtime.choices.exp} +exp_var: test +experiment_name: ${exp_base}_${exp_var} +experiment_dir: ${base_dir}/${project_name}/${experiment_name}-${timestamp} + +num_envs: 4096 +project_name: TRL_G1_Track + +actor_prop_history_length: 10 +actor_actions_history_length: 10 + +critic_prop_history_length: 10 +critic_actions_history_length: 10 + +manager_env: + rewards: + feet_acc: + weight: -2.5e-6 + config: + robot: + type: g1_model_12_dex + terrain_type: trimesh + commands: + motion: + reward_point_body: ["torso_link", "left_wrist_yaw_link", "right_wrist_yaw_link"] + reward_point_body_offset: [[0.0, 0.0, 0.5], [0.0, -0.0, 0.0], [0.0, -0.0, 0.0]] + num_future_frames: 10 + dt_future_ref_frames: 0.1 + smpl_num_future_frames: 10 + smpl_dt_future_ref_frames: 0.02 + cat_upper_body_poses: true + cat_upper_body_poses_prob: 0.5 + + freeze_frame_aug: true + teleop_sample_prob_when_smpl: 0.5 + + motion_lib_cfg: + upper_body_augment_prefixes: + [ + "2025", + "sonic_mixed_walking_running", + "sonic_squating_different_height_switching", + "sonic_running", + "sonic_walking", + "sonic_squating_different_height_hold", + "sonic_walking_other_style", + "balance", + ] + adaptive_sampling: + adp_samp_failure_rate_max_over_mean: 200 + motion_file: data/motion_lib_bones_seed/robot_filtered + smpl_motion_file: data/bones_seed_smpl + smpl_y_up: true + +algo: + config: + empty_cache_every_n_ppo_epoch: -1 + num_steps_per_env: 24 + use_clampped_std: true + std_clamp_min: 0.001 + std_clamp_max: 0.5 + max_grad_norm: 0.1 + actor: + backbone: + reencode_smpl_g1_recon: true + encoders: + g1: + inputs: ["command_multi_future_nonflat", "motion_anchor_ori_b_mf_nonflat"] + teleop: + inputs: + [ + "command_multi_future_lower_body", + "vr_3point_local_target", + "vr_3point_local_orn_target", + "motion_anchor_ori_b", + ] + smpl: + inputs: + [ + "smpl_joints_multi_future_local_nonflat", + "smpl_root_ori_b_multi_future", + "joint_pos_multi_future_wrist_for_smpl", + ] + decoders: + g1_kin: + outputs: ["command_multi_future_nonflat", "motion_anchor_ori_b_mf_nonflat"] + \ No newline at end of file diff --git a/GR00T-WholeBodyControl/gear_sonic/config/exp/manager/universal_token/all_modes/sonic_v1_1.yaml b/GR00T-WholeBodyControl/gear_sonic/config/exp/manager/universal_token/all_modes/sonic_v1_1.yaml new file mode 100644 index 0000000000000000000000000000000000000000..8deab71057ce4a4edcd5ca261f798533498dbb47 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/exp/manager/universal_token/all_modes/sonic_v1_1.yaml @@ -0,0 +1,117 @@ +# @package _global_ +# SONIC v1.1: robot-heading-normalized orientation and wrist-pose augmentation +# for G1, teleop, and SMPL inputs. + +defaults: + - /algo: ppo_im_phc + - /manager_env: base_env + - /callbacks/read_eval + - /callbacks/im_resample + - /aux_losses: universal_token/g1_recon_and_all_latent + - override /trainer: trl_ppo_aux + - override /actor_critic: universal_token/all_mlp_v1 + - override /manager_env/observations/tokenizer: unitoken_all_noz_heading + - override /manager_env/observations/policy: local_dir_hist + - override /manager_env/observations/critic: privileged_mf_hist + - override /manager_env/events: tracking/level0_4 + - override /manager_env/terminations: tracking/base_adaptive_strict_ori_foot_xyz + - override /manager_env/rewards: tracking/local_feet_acc_energy_5pt + +use_manager_env: true + +exp_base: ${hydra:runtime.choices.exp} +exp_var: test +experiment_name: ${exp_base}_${exp_var} +experiment_dir: ${base_dir}/${project_name}/${experiment_name}-${timestamp} + +num_envs: 4096 +project_name: TRL_G1_Track + +actor_prop_history_length: 10 +actor_actions_history_length: 10 +critic_prop_history_length: 10 +critic_actions_history_length: 10 + +manager_env: + rewards: + feet_acc: + weight: -2.5e-6 + config: + robot: + type: g1_model_12_dex + terrain_type: trimesh + commands: + motion: + reward_point_body: ["torso_link", "left_wrist_yaw_link", "right_wrist_yaw_link"] + reward_point_body_offset: [[0.0, 0.0, 0.5], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] + num_future_frames: 10 + dt_future_ref_frames: 0.1 + smpl_num_future_frames: 10 + smpl_dt_future_ref_frames: 0.02 + cat_upper_body_poses: true + cat_upper_body_poses_prob: 0.5 + freeze_frame_aug: true + teleop_sample_prob_when_smpl: 0.5 + randomize_wrist_poses: true + + motion_lib_cfg: + upper_body_augment_prefixes: + [ + "2025", + "sonic_mixed_walking_running", + "sonic_squating_different_height_switching", + "sonic_running", + "sonic_walking", + "sonic_squating_different_height_hold", + "sonic_walking_other_style", + "balance", + ] + adaptive_sampling: + adp_samp_failure_rate_max_over_mean: 200 + motion_file: data/motion_lib_bones_seed/robot_filtered + smpl_motion_file: data/bones_seed_smpl + smpl_y_up: true + +algo: + config: + empty_cache_every_n_ppo_epoch: -1 + num_steps_per_env: 24 + use_clampped_std: true + std_clamp_min: 0.001 + std_clamp_max: 0.5 + max_grad_norm: 0.1 + actor: + backbone: + reencode_smpl_g1_recon: true + encoders: + g1: + inputs: ["command_multi_future_nonflat", "motion_anchor_ori_heading_mf_nonflat"] + teleop: + inputs: + [ + "command_multi_future_lower_body", + "vr_3point_local_target", + "vr_3point_local_orn_target", + "motion_anchor_ori_heading", + ] + smpl: + inputs: + [ + "smpl_joints_multi_future_local_nonflat", + "smpl_root_ori_heading_multi_future", + "joint_pos_multi_future_wrist_for_smpl", + ] + decoders: + g1_kin: + outputs: ["command_multi_future_nonflat", "motion_anchor_ori_heading_mf_nonflat"] + g1_dyn: + params: + module_config_dict: + layer_config: + hidden_dims: [4096, 4096, 2048, 2048, 1024, 1024, 512, 512] + + critic: + backbone: + module_config_dict: + layer_config: + hidden_dims: [4096, 4096, 2048, 2048, 1024, 1024, 512, 512] diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/actions/terms/joint_pos.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/actions/terms/joint_pos.yaml new file mode 100644 index 0000000000000000000000000000000000000000..4b8cd8abc4ce748fe7684401104656d60920a28f --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/actions/terms/joint_pos.yaml @@ -0,0 +1,5 @@ +joint_pos: + _target_: isaaclab.envs.mdp.actions.JointPositionActionCfg + asset_name: "robot" + joint_names: [".*"] + use_default_offset: true diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/actions/tracking/base.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/actions/tracking/base.yaml new file mode 100644 index 0000000000000000000000000000000000000000..1a553e5ffd78bf1ced6232c646dd53041a76b3ce --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/actions/tracking/base.yaml @@ -0,0 +1,6 @@ +# Standard action composition + +defaults: + - terms/joint_pos@_here_ + +_target_: gear_sonic.envs.manager_env.mdp.actions.ActionsCfg diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/base_env.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/base_env.yaml new file mode 100644 index 0000000000000000000000000000000000000000..b319d02b506ad5b0def528fee79755081e688d33 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/base_env.yaml @@ -0,0 +1,73 @@ +# @package manager_env +# Base tracking environment configuration +# This config provides defaults that can be overridden with specific combinations + +defaults: + - actions: tracking/base + - observations/policy: global + - observations/critic: privileged + - observations/tokenizer: + - observations/height_map: + - observations/policy_atm: + - observations/residual_action: + - rewards: tracking/base + - terminations: tracking/base + - commands: tracking/base + - events: tracking/base + - curriculum: empty + - recorders: empty + - _self_ + +# Environment configuration +_target_: gear_sonic.envs.manager_env.modular_tracking_env_cfg.ModularTrackingEnvCfg + +observations: + _target_: gear_sonic.envs.manager_env.mdp.observations.ObservationsCfg + +config: + num_envs: ${num_envs} + env_spacing: 2.0 + decimation: 4 + episode_length_s: 10.0 + sim_dt: 0.005 + viewer_eye: [4.5, 0.0, 6.0] + viewer_lookat: [0.0, 0.0, 2.0] + render_results: false + save_rendering_dir: ${experiment_dir}/renderings_training + experiment_dir: ${experiment_dir} + terrain_type: plane # "0: plane" or "1: trimesh" + + # Object mass override (kg). Set to override USD-defined mass. + # Example: ++manager_env.config.object_mass=0.1 + object_mass: null + + # Table position offset [x, y, z] added to table position from meta file + # Useful for adjusting table position relative to the robot + # Example: ++manager_env.config.table_offset=[0.0, 0.1, 0.0] + table_offset: null + + # Eval camera offset [X, Y, Z] relative to robot root position + # Default: [2, 2, 1] = 2m forward, 2m left, 1m up + eval_camera_offset: [2, 2, 1] + + action_transform_module_cfg: + action_transform_module_checkpoint: + meta_action_dim: + needs_policy_atm: true + + obs: + obs_dict: {} + obs_dims: {} + group_obs_dims: {} + group_obs_names: {} + + action_clip_value: 20.0 + robot: + type: "g1" + algo_obs_dim_dict: {} + rewards: + num_critics: 1 + + train_only_events: + - push_robot + - compliance_force_push # New name for force-based push (compliance training) diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/commands/terms/motion.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/commands/terms/motion.yaml new file mode 100644 index 0000000000000000000000000000000000000000..68b5b4a8f04ed51695324fd92183c11539ffa2b3 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/commands/terms/motion.yaml @@ -0,0 +1,67 @@ +motion: + _target_: gear_sonic.envs.manager_env.mdp.commands.TrackingCommandCfg + asset_name: "robot" + + motion_lib_cfg: + motion_file: ${robot.motion.motion_file} + smpl_motion_file: + asset: + assetRoot: "gear_sonic/data/assets/robot_description/mjcf/" + assetFileName: "g1_29dof_rev_1_0.xml" + urdfFileName: "" + extend_config: [] + target_fps: 50 + multi_thread: true + filter_motion_keys: + adaptive_sampling: + enable: true + bin_size: 50 + sequence_length_agnostic: true + init_num_failures: 1 + uniform_sampling_rate: 0.1 + pre_failure_sample_window: 200 + use_failure_rate_decay: false + decay_gamma: 0.8 + adp_samp_failure_rate_max_over_mean: 50.0 + + resampling_time_range: [1000000000.0, 1000000000.0] # [1.0e9, 1.0e9] + debug_vis: true + dt_future_ref_frames: 0.1 + num_future_frames: 5 + pose_range: + x: [-0.05, 0.05] + y: [-0.05, 0.05] + z: [-0.01, 0.01] + roll: [-0.1, 0.1] + pitch: [-0.1, 0.1] + yaw: [-0.2, 0.2] + velocity_range: + x: [-0.5, 0.5] + y: [-0.5, 0.5] + z: [-0.2, 0.2] + roll: [-0.52, 0.52] + pitch: [-0.52, 0.52] + yaw: [-0.78, 0.78] + joint_position_range: [-0.1, 0.1] + joint_velocity_range: [-0, 0] # default is turned off. + anchor_body: "pelvis" + vr_3point_body: ["left_wrist_yaw_link", "right_wrist_yaw_link", "torso_link"] + vr_3point_body_offset: [[0.18, -0.025, 0.0],[0.18, +0.025, 0.0],[0.0, 0.0, 0.35]] + reward_point_body: ["pelvis", "left_wrist_yaw_link", "right_wrist_yaw_link", "left_ankle_roll_link", "right_ankle_roll_link"] + reward_point_body_offset: [[0.0, 0.0, 0.0], [0.18, -0.025, 0.0], [0.18, +0.025, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] + body_names: [ + "pelvis", + "left_hip_roll_link", + "left_knee_link", + "left_ankle_roll_link", + "right_hip_roll_link", + "right_knee_link", + "right_ankle_roll_link", + "torso_link", + "left_shoulder_roll_link", + "left_elbow_link", + "left_wrist_yaw_link", + "right_shoulder_roll_link", + "right_elbow_link", + "right_wrist_yaw_link", + ] diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/commands/tracking/base.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/commands/tracking/base.yaml new file mode 100644 index 0000000000000000000000000000000000000000..2636ee1cff4274ce1f319479095e63bb0de7cd6a --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/commands/tracking/base.yaml @@ -0,0 +1,6 @@ +# Standard command composition + +defaults: + - terms/motion@_here_ + +_target_: gear_sonic.envs.manager_env.mdp.commands.CommandsCfg diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/curriculum/empty.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/curriculum/empty.yaml new file mode 100644 index 0000000000000000000000000000000000000000..0732bf8a94f92e784e9a40c3eb95745859916c89 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/curriculum/empty.yaml @@ -0,0 +1 @@ +_target_: gear_sonic.envs.manager_env.mdp.curriculum.CurriculumCfg diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/events/terms/add_joint_default_pos.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/events/terms/add_joint_default_pos.yaml new file mode 100644 index 0000000000000000000000000000000000000000..d6d2f50c3a228a56fc4287c759fb01906fe6c3b2 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/events/terms/add_joint_default_pos.yaml @@ -0,0 +1,11 @@ +add_joint_default_pos: + _target_: isaaclab.managers.EventTermCfg + func: gear_sonic.envs.manager_env.mdp:randomize_joint_default_pos + mode: "startup" + params: + asset_cfg: + _target_: isaaclab.managers.SceneEntityCfg + name: "robot" + joint_names: [".*"] + pos_distribution_params: [-0.01, 0.01] + operation: "add" diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/events/terms/base_com.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/events/terms/base_com.yaml new file mode 100644 index 0000000000000000000000000000000000000000..afb1ac7844c6aa49ab299894367f416901a11b54 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/events/terms/base_com.yaml @@ -0,0 +1,13 @@ +base_com: + _target_: isaaclab.managers.EventTermCfg + func: gear_sonic.envs.manager_env.mdp:randomize_rigid_body_com + mode: "startup" + params: + asset_cfg: + _target_: isaaclab.managers.SceneEntityCfg + name: "robot" + body_names: "torso_link" + com_range: + x: [-0.025, 0.025] + y: [-0.05, 0.05] + z: [-0.05, 0.05] diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/events/terms/physics_material.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/events/terms/physics_material.yaml new file mode 100644 index 0000000000000000000000000000000000000000..adc6451e7f3437fb1734a6427dd0a8c866463bcb --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/events/terms/physics_material.yaml @@ -0,0 +1,13 @@ +physics_material: + _target_: isaaclab.managers.EventTermCfg + func: gear_sonic.envs.manager_env.mdp:randomize_rigid_body_material + mode: "startup" + params: + asset_cfg: + _target_: isaaclab.managers.SceneEntityCfg + name: "robot" + body_names: ".*" + static_friction_range: [0.3, 1.6] + dynamic_friction_range: [0.3, 1.2] + restitution_range: [0.0, 0.5] + num_buckets: 64 diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/events/terms/push_robot.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/events/terms/push_robot.yaml new file mode 100644 index 0000000000000000000000000000000000000000..d04ad5cf5d85d345339a50f77b928fde910ac53e --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/events/terms/push_robot.yaml @@ -0,0 +1,37 @@ +# ============================================================================= +# Velocity Push Event (for balance/robustness training) +# ============================================================================= +# +# This event applies VELOCITY IMPULSES to the robot's base, simulating external +# disturbances like being bumped. It's used for general robustness training. +# +# ┌────────────────────────────────────────────────────────────────────────────┐ +# │ NOTE: This event does NOT affect the compliance mechanism! │ +# │ │ +# │ For compliance training, you ALSO need: │ +# │ - terms/compliance_force_push@_here_ │ +# │ - terms/chip_change_compliance_discrete/unified@_here_ │ +# └────────────────────────────────────────────────────────────────────────────┘ +# +# ============================================================================= + +push_robot: + _target_: isaaclab.managers.EventTermCfg + func: gear_sonic.envs.manager_env.mdp:push_by_setting_velocity + mode: "interval" + # Random push every 1-3 seconds + interval_range_s: [1.0, 3.0] + params: + velocity_range: + # Forward/backward velocity (m/s) + x: [-0.5, 0.5] + # Left/right velocity (m/s) + y: [-0.5, 0.5] + # Up/down velocity (m/s) + z: [-0.2, 0.2] + # Roll angular velocity (rad/s) + roll: [-0.52, 0.52] + # Pitch angular velocity (rad/s) + pitch: [-0.52, 0.52] + # Yaw angular velocity (rad/s) + yaw: [-0.78, 0.78] diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/events/terms/randomize_rigid_body_mass.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/events/terms/randomize_rigid_body_mass.yaml new file mode 100644 index 0000000000000000000000000000000000000000..f29f5c6bb931ee67e5d9af2375cd235ed6959509 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/events/terms/randomize_rigid_body_mass.yaml @@ -0,0 +1,11 @@ +randomize_rigid_body_mass: + _target_: isaaclab.managers.EventTermCfg + func: gear_sonic.envs.manager_env.mdp:randomize_rigid_body_mass + mode: "reset" + params: + asset_cfg: + _target_: isaaclab.managers.SceneEntityCfg + name: "robot" + body_names: ".*" + mass_distribution_params: [0.8, 1.2] + operation: "scale" diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/events/tracking/base.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/events/tracking/base.yaml new file mode 100644 index 0000000000000000000000000000000000000000..7ccaa5c8ea7c544e148ad3aa9896dbe0a6e00f21 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/events/tracking/base.yaml @@ -0,0 +1,9 @@ +# Standard event composition + +defaults: + - terms/physics_material@_here_ + - terms/add_joint_default_pos@_here_ + - terms/base_com@_here_ + - terms/push_robot@_here_ + +_target_: gear_sonic.envs.manager_env.mdp.events.EventCfg diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/events/tracking/level0_4.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/events/tracking/level0_4.yaml new file mode 100644 index 0000000000000000000000000000000000000000..d143d7249ccd7ad25796fcad9a35a025d7e247f7 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/events/tracking/level0_4.yaml @@ -0,0 +1,33 @@ +# Standard event composition + +defaults: + - terms/physics_material@_here_ + - terms/add_joint_default_pos@_here_ + - terms/base_com@_here_ + - terms/push_robot@_here_ + - terms/randomize_rigid_body_mass@_here_ + +_target_: gear_sonic.envs.manager_env.mdp.events.EventCfg + +randomize_rigid_body_mass: + mode: "startup" + params: + asset_cfg: + _target_: isaaclab.managers.SceneEntityCfg + name: "robot" + body_names: ".*wrist_yaw.*|torso_link" + mass_distribution_params: [0.8, 2.5] + operation: "scale" + + +push_robot: + mode: "interval" + interval_range_s: [4.0, 6.0] + params: + velocity_range: + x: [-0.5, 0.5] + y: [-0.5, 0.5] + z: [-0.2, 0.2] + roll: [-0.52, 0.52] + pitch: [-0.52, 0.52] + yaw: [-0.78, 0.78] diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/critic/privileged.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/critic/privileged.yaml new file mode 100644 index 0000000000000000000000000000000000000000..db1260bf787f5bcc13d06d304e6606b8962e69cc --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/critic/privileged.yaml @@ -0,0 +1,15 @@ +# Standard critic observations composition + +defaults: + - ../terms/command@_here_ + - ../terms/motion_anchor_pos_b@_here_ + - ../terms/motion_anchor_ori_b@_here_ + - ../terms/body_pos@_here_ + - ../terms/body_ori@_here_ + - ../terms/base_lin_vel@_here_ + - ../terms/base_ang_vel@_here_ + - ../terms/joint_pos@_here_ + - ../terms/joint_vel@_here_ + - ../terms/actions@_here_ + +_target_: gear_sonic.envs.manager_env.mdp.observations.PrivilegedCfg diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/critic/privileged_mf_hist.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/critic/privileged_mf_hist.yaml new file mode 100644 index 0000000000000000000000000000000000000000..41a68687c0c7473f1580973bb1d2839a228bfa50 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/critic/privileged_mf_hist.yaml @@ -0,0 +1,30 @@ +# Standard critic observations composition + +defaults: + - ../terms/command_multi_future@_here_ + - ../terms/motion_anchor_pos_b@_here_ + - ../terms/motion_anchor_ori_b@_here_ + - ../terms/body_pos@_here_ + - ../terms/body_ori@_here_ + - ../terms/base_lin_vel@_here_ + - ../terms/base_ang_vel@_here_ + - ../terms/joint_pos@_here_ + - ../terms/joint_vel@_here_ + - ../terms/actions@_here_ + +_target_: gear_sonic.envs.manager_env.mdp.observations.PrivilegedCfg + +base_ang_vel: + history_length: ${critic_prop_history_length} + +base_lin_vel: + history_length: ${critic_prop_history_length} + +joint_pos: + history_length: ${critic_prop_history_length} + +joint_vel: + history_length: ${critic_prop_history_length} + +actions: + history_length: ${critic_actions_history_length} diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/policy/global.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/policy/global.yaml new file mode 100644 index 0000000000000000000000000000000000000000..448013e5a02cd81de4c581ee5831f6f946ffe667 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/policy/global.yaml @@ -0,0 +1,53 @@ +# Standard policy observations composition + +defaults: + - ../terms/command@_here_ + - ../terms/motion_anchor_pos_b@_here_ + - ../terms/motion_anchor_ori_b@_here_ + - ../terms/base_lin_vel@_here_ + - ../terms/base_ang_vel@_here_ + - ../terms/joint_pos@_here_ + - ../terms/joint_vel@_here_ + - ../terms/actions@_here_ + +_target_: gear_sonic.envs.manager_env.mdp.observations.PolicyCfg +enable_corruption: True +concatenate_terms: True + + +# define noise for certain terms +motion_anchor_pos_b: + noise: + _target_: isaaclab.utils.noise.AdditiveUniformNoiseCfg + n_min: -0.25 + n_max: 0.25 + +motion_anchor_ori_b: + noise: + _target_: isaaclab.utils.noise.AdditiveUniformNoiseCfg + n_min: -0.05 + n_max: 0.05 + +base_lin_vel: + noise: + _target_: isaaclab.utils.noise.AdditiveUniformNoiseCfg + n_min: -0.5 + n_max: 0.5 + +base_ang_vel: + noise: + _target_: isaaclab.utils.noise.AdditiveUniformNoiseCfg + n_min: -0.2 + n_max: 0.2 + +joint_pos: + noise: + _target_: isaaclab.utils.noise.AdditiveUniformNoiseCfg + n_min: -0.01 + n_max: 0.01 + +joint_vel: + noise: + _target_: isaaclab.utils.noise.AdditiveUniformNoiseCfg + n_min: -0.5 + n_max: 0.5 diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/policy/local_dir_hist.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/policy/local_dir_hist.yaml new file mode 100644 index 0000000000000000000000000000000000000000..41d36a7cf2a35b0be26e7505aa910dc5f064d8a0 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/policy/local_dir_hist.yaml @@ -0,0 +1,45 @@ +# Standard policy observations composition + +defaults: + - ../terms/gravity_dir@_here_ + - ../terms/base_ang_vel@_here_ + - ../terms/joint_pos@_here_ + - ../terms/joint_vel@_here_ + - ../terms/actions@_here_ + +_target_: gear_sonic.envs.manager_env.mdp.observations.PolicyCfg +enable_corruption: True +concatenate_terms: True + + +# define noise for certain terms +gravity_dir: + history_length: ${actor_prop_history_length} + noise: + _target_: isaaclab.utils.noise.AdditiveUniformNoiseCfg + n_min: -0.05 + n_max: 0.05 + +base_ang_vel: + history_length: ${actor_prop_history_length} + noise: + _target_: isaaclab.utils.noise.AdditiveUniformNoiseCfg + n_min: -0.2 + n_max: 0.2 + +joint_pos: + history_length: ${actor_prop_history_length} + noise: + _target_: isaaclab.utils.noise.AdditiveUniformNoiseCfg + n_min: -0.01 + n_max: 0.01 + +joint_vel: + history_length: ${actor_prop_history_length} + noise: + _target_: isaaclab.utils.noise.AdditiveUniformNoiseCfg + n_min: -0.5 + n_max: 0.5 + +actions: + history_length: ${actor_actions_history_length} diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/actions.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/actions.yaml new file mode 100644 index 0000000000000000000000000000000000000000..d9048febced5e6e042691adf752bd017fce4a17e --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/actions.yaml @@ -0,0 +1,3 @@ +actions: + _target_: isaaclab.managers.ObservationTermCfg + func: gear_sonic.envs.manager_env.mdp:last_action diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/base_ang_vel.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/base_ang_vel.yaml new file mode 100644 index 0000000000000000000000000000000000000000..888bdfe744710ae60a7ad7de7f1a9be9f4679a72 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/base_ang_vel.yaml @@ -0,0 +1,3 @@ +base_ang_vel: + _target_: isaaclab.managers.ObservationTermCfg + func: gear_sonic.envs.manager_env.mdp:base_ang_vel diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/base_lin_vel.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/base_lin_vel.yaml new file mode 100644 index 0000000000000000000000000000000000000000..773c9a91911b294cc1fd29e45594718c24be4975 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/base_lin_vel.yaml @@ -0,0 +1,3 @@ +base_lin_vel: + _target_: isaaclab.managers.ObservationTermCfg + func: gear_sonic.envs.manager_env.mdp:base_lin_vel diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/body_ori.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/body_ori.yaml new file mode 100644 index 0000000000000000000000000000000000000000..9e8b9676942c939ec09efc874b9713cc5bf34f5b --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/body_ori.yaml @@ -0,0 +1,5 @@ +body_ori: + _target_: isaaclab.managers.ObservationTermCfg + func: gear_sonic.envs.manager_env.mdp:robot_body_ori_b + params: + command_name: "motion" diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/body_pos.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/body_pos.yaml new file mode 100644 index 0000000000000000000000000000000000000000..d1fb0985704bd2005b618e90a61e87d0822988b9 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/body_pos.yaml @@ -0,0 +1,5 @@ +body_pos: + _target_: isaaclab.managers.ObservationTermCfg + func: gear_sonic.envs.manager_env.mdp:robot_body_pos_b + params: + command_name: "motion" diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/command.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/command.yaml new file mode 100644 index 0000000000000000000000000000000000000000..171538fe913be4fc1fe796fe3fa83e0d95a64b83 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/command.yaml @@ -0,0 +1,5 @@ +command: + _target_: isaaclab.managers.ObservationTermCfg + func: gear_sonic.envs.manager_env.mdp:generated_commands + params: + command_name: "motion" diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/command_multi_future.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/command_multi_future.yaml new file mode 100644 index 0000000000000000000000000000000000000000..acf26afadf8771594571a056035991154d6e0b1c --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/command_multi_future.yaml @@ -0,0 +1,5 @@ +command_multi_future: + _target_: isaaclab.managers.ObservationTermCfg + func: gear_sonic.envs.manager_env.mdp:command_multi_future + params: + command_name: "motion" diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/command_multi_future_lower_body.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/command_multi_future_lower_body.yaml new file mode 100644 index 0000000000000000000000000000000000000000..fcdbc459adec97eff008c4e0213685fc4c6f2b26 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/command_multi_future_lower_body.yaml @@ -0,0 +1,5 @@ +command_multi_future_lower_body: + _target_: isaaclab.managers.ObservationTermCfg + func: gear_sonic.envs.manager_env.mdp:command_multi_future_lower_body + params: + command_name: "motion" diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/command_multi_future_nonflat.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/command_multi_future_nonflat.yaml new file mode 100644 index 0000000000000000000000000000000000000000..53672cf1eac69f83a6494c3bbadca4d1c56c6383 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/command_multi_future_nonflat.yaml @@ -0,0 +1,6 @@ +command_multi_future_nonflat: + _target_: isaaclab.managers.ObservationTermCfg + func: gear_sonic.envs.manager_env.mdp:command_multi_future + params: + command_name: "motion" + non_flatten: True diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/command_z.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/command_z.yaml new file mode 100644 index 0000000000000000000000000000000000000000..5e22a7bb71af8a2415751222bd09dafe50abf848 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/command_z.yaml @@ -0,0 +1,5 @@ +command_z: + _target_: isaaclab.managers.ObservationTermCfg + func: gear_sonic.envs.manager_env.mdp:command_z + params: + command_name: "motion" diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/command_z_multi_future_nonflat.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/command_z_multi_future_nonflat.yaml new file mode 100644 index 0000000000000000000000000000000000000000..a30bc9c6feac9d41ca32e2444fcf091d9f892c8c --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/command_z_multi_future_nonflat.yaml @@ -0,0 +1,6 @@ +command_z_multi_future_nonflat: + _target_: isaaclab.managers.ObservationTermCfg + func: gear_sonic.envs.manager_env.mdp:command_z_multi_future + params: + command_name: "motion" + non_flatten: True diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/encoder_index.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/encoder_index.yaml new file mode 100644 index 0000000000000000000000000000000000000000..ee92583be53ad527a2ff05fb0c775c30ba4d6dae --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/encoder_index.yaml @@ -0,0 +1,6 @@ +encoder_index: + _target_: isaaclab.managers.ObservationTermCfg + func: gear_sonic.envs.manager_env.mdp:get_command_obs + params: + command_name: "motion" + obs_name: "episode_encoder_index" diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/gravity_dir.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/gravity_dir.yaml new file mode 100644 index 0000000000000000000000000000000000000000..fd04bd3f20bac3bf776dd726549bdceba3a0d1fd --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/gravity_dir.yaml @@ -0,0 +1,5 @@ +gravity_dir: + _target_: isaaclab.managers.ObservationTermCfg + func: gear_sonic.envs.manager_env.mdp:gravity_dir + params: + command_name: "motion" diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/joint_pos.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/joint_pos.yaml new file mode 100644 index 0000000000000000000000000000000000000000..a9d89c6eebc733a8890ba9d60eba3d27399c2388 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/joint_pos.yaml @@ -0,0 +1,3 @@ +joint_pos: + _target_: isaaclab.managers.ObservationTermCfg + func: gear_sonic.envs.manager_env.mdp:joint_pos_rel diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/joint_pos_multi_future_wrist_for_smpl.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/joint_pos_multi_future_wrist_for_smpl.yaml new file mode 100644 index 0000000000000000000000000000000000000000..685e1c2922bcbbb9e27c0b7b94bd79aef7cf70e1 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/joint_pos_multi_future_wrist_for_smpl.yaml @@ -0,0 +1,7 @@ +joint_pos_multi_future_wrist_for_smpl: + _target_: isaaclab.managers.ObservationTermCfg + func: gear_sonic.envs.manager_env.mdp:joint_pos_multi_future_select_joints_for_smpl + params: + command_name: "motion" + joints_idx: [23, 24, 25, 26, 27, 28] + # Wrist joints: left_wrist_roll_joint, right_wrist_roll_joint, left_wrist_pitch_joint, right_wrist_pitch_joint, left_wrist_yaw_joint, right_wrist_yaw_joint diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/joint_pos_multi_future_wrist_for_soma.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/joint_pos_multi_future_wrist_for_soma.yaml new file mode 100644 index 0000000000000000000000000000000000000000..b3e98804fc5ac80009e8985356568dad290700ad --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/joint_pos_multi_future_wrist_for_soma.yaml @@ -0,0 +1,7 @@ +joint_pos_multi_future_wrist_for_soma: + _target_: isaaclab.managers.ObservationTermCfg + func: gear_sonic.envs.manager_env.mdp:joint_pos_multi_future_select_joints_for_smpl + params: + command_name: "motion" + joints_idx: [23, 24, 25, 26, 27, 28] + # Wrist joints: left_wrist_roll_joint, right_wrist_roll_joint, left_wrist_pitch_joint, right_wrist_pitch_joint, left_wrist_yaw_joint, right_wrist_yaw_joint diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/joint_vel.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/joint_vel.yaml new file mode 100644 index 0000000000000000000000000000000000000000..24fd0c90d659a0e8fb4b2975ed42d972704a9e79 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/joint_vel.yaml @@ -0,0 +1,3 @@ +joint_vel: + _target_: isaaclab.managers.ObservationTermCfg + func: gear_sonic.envs.manager_env.mdp:joint_vel_rel diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/motion_anchor_ori_b.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/motion_anchor_ori_b.yaml new file mode 100644 index 0000000000000000000000000000000000000000..624de85844b6f2d3e8ff392d7891c05b39df3b1f --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/motion_anchor_ori_b.yaml @@ -0,0 +1,5 @@ +motion_anchor_ori_b: + _target_: isaaclab.managers.ObservationTermCfg + func: gear_sonic.envs.manager_env.mdp:motion_anchor_ori_b + params: + command_name: "motion" diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/motion_anchor_ori_b_mf_nonflat.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/motion_anchor_ori_b_mf_nonflat.yaml new file mode 100644 index 0000000000000000000000000000000000000000..15bc2e45b115585c4ab4c3c0b00af09ad8b0cff5 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/motion_anchor_ori_b_mf_nonflat.yaml @@ -0,0 +1,6 @@ +motion_anchor_ori_b_mf_nonflat: + _target_: isaaclab.managers.ObservationTermCfg + func: gear_sonic.envs.manager_env.mdp:motion_anchor_ori_b_mf + params: + command_name: "motion" + non_flatten: True diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/motion_anchor_ori_heading.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/motion_anchor_ori_heading.yaml new file mode 100644 index 0000000000000000000000000000000000000000..acf94feca9a3e826bdb136e616ace5bcfcee8840 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/motion_anchor_ori_heading.yaml @@ -0,0 +1,5 @@ +motion_anchor_ori_heading: + _target_: isaaclab.managers.ObservationTermCfg + func: gear_sonic.envs.manager_env.mdp:motion_anchor_ori_heading + params: + command_name: "motion" diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/motion_anchor_ori_heading_mf_nonflat.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/motion_anchor_ori_heading_mf_nonflat.yaml new file mode 100644 index 0000000000000000000000000000000000000000..5f53c5649813f75e0d25467292fb55ba5a93aaa3 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/motion_anchor_ori_heading_mf_nonflat.yaml @@ -0,0 +1,6 @@ +motion_anchor_ori_heading_mf_nonflat: + _target_: isaaclab.managers.ObservationTermCfg + func: gear_sonic.envs.manager_env.mdp:motion_anchor_ori_heading_mf + params: + command_name: "motion" + non_flatten: true diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/motion_anchor_pos_b.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/motion_anchor_pos_b.yaml new file mode 100644 index 0000000000000000000000000000000000000000..04941b5fefcab629a3d243591df800290f5b9817 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/motion_anchor_pos_b.yaml @@ -0,0 +1,5 @@ +motion_anchor_pos_b: + _target_: isaaclab.managers.ObservationTermCfg + func: gear_sonic.envs.manager_env.mdp:motion_anchor_pos_b + params: + command_name: "motion" diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/smpl_joints_multi_future_local_nonflat.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/smpl_joints_multi_future_local_nonflat.yaml new file mode 100644 index 0000000000000000000000000000000000000000..a972de31722fa408015b1eec3895f5d46ec4eaba --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/smpl_joints_multi_future_local_nonflat.yaml @@ -0,0 +1,6 @@ +smpl_joints_multi_future_local_nonflat: + _target_: isaaclab.managers.ObservationTermCfg + func: gear_sonic.envs.manager_env.mdp:smpl_joints_multi_future_local + params: + command_name: "motion" + non_flatten: True diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/smpl_root_ori_b_multi_future.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/smpl_root_ori_b_multi_future.yaml new file mode 100644 index 0000000000000000000000000000000000000000..1df27f990fedb135d26d5d5e0a278b47b29b48e0 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/smpl_root_ori_b_multi_future.yaml @@ -0,0 +1,6 @@ +smpl_root_ori_b_multi_future: + _target_: isaaclab.managers.ObservationTermCfg + func: gear_sonic.envs.manager_env.mdp:smpl_root_ori_b_mf + params: + command_name: "motion" + non_flatten: True diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/smpl_root_ori_heading_multi_future.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/smpl_root_ori_heading_multi_future.yaml new file mode 100644 index 0000000000000000000000000000000000000000..09b1b224d4ef073f6b250c4077bf7ed60949d652 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/smpl_root_ori_heading_multi_future.yaml @@ -0,0 +1,6 @@ +smpl_root_ori_heading_multi_future: + _target_: isaaclab.managers.ObservationTermCfg + func: gear_sonic.envs.manager_env.mdp:smpl_root_ori_heading_mf + params: + command_name: "motion" + non_flatten: true diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/soma_joints_multi_future_local_nonflat.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/soma_joints_multi_future_local_nonflat.yaml new file mode 100644 index 0000000000000000000000000000000000000000..1ebf769751accfeff4812bd2d5af7dabbe5c6109 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/soma_joints_multi_future_local_nonflat.yaml @@ -0,0 +1,6 @@ +soma_joints_multi_future_local_nonflat: + _target_: isaaclab.managers.ObservationTermCfg + func: gear_sonic.envs.manager_env.mdp:soma_joints_multi_future_local + params: + command_name: "motion" + non_flatten: true diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/soma_root_ori_b_multi_future.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/soma_root_ori_b_multi_future.yaml new file mode 100644 index 0000000000000000000000000000000000000000..26fff201b464e671ff2651203643355709a05b6a --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/soma_root_ori_b_multi_future.yaml @@ -0,0 +1,6 @@ +soma_root_ori_b_multi_future: + _target_: isaaclab.managers.ObservationTermCfg + func: gear_sonic.envs.manager_env.mdp:soma_root_ori_b_mf + params: + command_name: "motion" + non_flatten: true diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/vr_3point_local_orn_target.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/vr_3point_local_orn_target.yaml new file mode 100644 index 0000000000000000000000000000000000000000..eb6591ff760a94b3e7965bc612f8d2ca73888c7d --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/vr_3point_local_orn_target.yaml @@ -0,0 +1,5 @@ +vr_3point_local_orn_target: + _target_: isaaclab.managers.ObservationTermCfg + func: gear_sonic.envs.manager_env.mdp:vr_3point_local_orn_target + params: + command_name: "motion" diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/vr_3point_local_target.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/vr_3point_local_target.yaml new file mode 100644 index 0000000000000000000000000000000000000000..f4e325d4a6765ead44cbd10554953ca20e6b4029 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/terms/vr_3point_local_target.yaml @@ -0,0 +1,5 @@ +vr_3point_local_target: + _target_: isaaclab.managers.ObservationTermCfg + func: gear_sonic.envs.manager_env.mdp:vr_3point_local_target + params: + command_name: "motion" diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/tokenizer/unitoken_all_noz.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/tokenizer/unitoken_all_noz.yaml new file mode 100644 index 0000000000000000000000000000000000000000..b78c8db7b1cc66b0b974010439ad51901f61adb2 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/tokenizer/unitoken_all_noz.yaml @@ -0,0 +1,46 @@ +# Universal token observations (3 encoders: G1, teleop, SMPL — no SOMA) + +defaults: + - ../terms/encoder_index@_here_ + - ../terms/command_multi_future_nonflat@_here_ + - ../terms/command_z_multi_future_nonflat@_here_ + - ../terms/motion_anchor_ori_b_mf_nonflat@_here_ + # teleop + - ../terms/command_multi_future_lower_body@_here_ + - ../terms/vr_3point_local_target@_here_ + - ../terms/vr_3point_local_orn_target@_here_ + - ../terms/motion_anchor_ori_b@_here_ + - ../terms/command_z@_here_ + # smpl + - ../terms/smpl_joints_multi_future_local_nonflat@_here_ + - ../terms/smpl_root_ori_b_multi_future@_here_ + - ../terms/joint_pos_multi_future_wrist_for_smpl@_here_ + +_target_: gear_sonic.envs.manager_env.mdp.observations.TokenizerCfg +enable_corruption: true +concatenate_terms: false + +# define noise for certain terms +motion_anchor_ori_b_mf_nonflat: + noise: + _target_: isaaclab.utils.noise.AdditiveUniformNoiseCfg + n_min: -0.05 + n_max: 0.05 + +motion_anchor_ori_b: + noise: + _target_: isaaclab.utils.noise.AdditiveUniformNoiseCfg + n_min: -0.05 + n_max: 0.05 + +smpl_joints_multi_future_local_nonflat: + noise: + _target_: isaaclab.utils.noise.AdditiveUniformNoiseCfg + n_min: -0.05 + n_max: 0.05 + +smpl_root_ori_b_multi_future: + noise: + _target_: isaaclab.utils.noise.AdditiveUniformNoiseCfg + n_min: -0.05 + n_max: 0.05 diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/tokenizer/unitoken_all_noz_heading.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/tokenizer/unitoken_all_noz_heading.yaml new file mode 100644 index 0000000000000000000000000000000000000000..2f271e0003abbafc713c5085a1d8fa7c3194726a --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/tokenizer/unitoken_all_noz_heading.yaml @@ -0,0 +1,45 @@ +# Universal-token observations with robot-heading-normalized root orientation. + +defaults: + - ../terms/encoder_index@_here_ + - ../terms/command_multi_future_nonflat@_here_ + - ../terms/command_z_multi_future_nonflat@_here_ + - ../terms/motion_anchor_ori_heading_mf_nonflat@_here_ + # teleop + - ../terms/command_multi_future_lower_body@_here_ + - ../terms/vr_3point_local_target@_here_ + - ../terms/vr_3point_local_orn_target@_here_ + - ../terms/motion_anchor_ori_heading@_here_ + - ../terms/command_z@_here_ + # smpl + - ../terms/smpl_joints_multi_future_local_nonflat@_here_ + - ../terms/smpl_root_ori_heading_multi_future@_here_ + - ../terms/joint_pos_multi_future_wrist_for_smpl@_here_ + +_target_: gear_sonic.envs.manager_env.mdp.observations.TokenizerCfg +enable_corruption: true +concatenate_terms: false + +motion_anchor_ori_heading_mf_nonflat: + noise: + _target_: isaaclab.utils.noise.AdditiveUniformNoiseCfg + n_min: -0.05 + n_max: 0.05 + +motion_anchor_ori_heading: + noise: + _target_: isaaclab.utils.noise.AdditiveUniformNoiseCfg + n_min: -0.05 + n_max: 0.05 + +smpl_joints_multi_future_local_nonflat: + noise: + _target_: isaaclab.utils.noise.AdditiveUniformNoiseCfg + n_min: -0.05 + n_max: 0.05 + +smpl_root_ori_heading_multi_future: + noise: + _target_: isaaclab.utils.noise.AdditiveUniformNoiseCfg + n_min: -0.05 + n_max: 0.05 diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/tokenizer/unitoken_all_noz_soma.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/tokenizer/unitoken_all_noz_soma.yaml new file mode 100644 index 0000000000000000000000000000000000000000..2f5323be7bb9166e5ee2fd13d1f11bb646de32be --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/observations/tokenizer/unitoken_all_noz_soma.yaml @@ -0,0 +1,62 @@ +# Universal token observations with SOMA skeleton encoder + +defaults: + - ../terms/encoder_index@_here_ + - ../terms/command_multi_future_nonflat@_here_ + - ../terms/command_z_multi_future_nonflat@_here_ + - ../terms/motion_anchor_ori_b_mf_nonflat@_here_ + # teleop + - ../terms/command_multi_future_lower_body@_here_ + - ../terms/vr_3point_local_target@_here_ + - ../terms/vr_3point_local_orn_target@_here_ + - ../terms/motion_anchor_ori_b@_here_ + - ../terms/command_z@_here_ + # smpl + - ../terms/smpl_joints_multi_future_local_nonflat@_here_ + - ../terms/smpl_root_ori_b_multi_future@_here_ + - ../terms/joint_pos_multi_future_wrist_for_smpl@_here_ + # soma + - ../terms/soma_joints_multi_future_local_nonflat@_here_ + - ../terms/soma_root_ori_b_multi_future@_here_ + - ../terms/joint_pos_multi_future_wrist_for_soma@_here_ + +_target_: gear_sonic.envs.manager_env.mdp.observations.TokenizerCfg +enable_corruption: true +concatenate_terms: false + +# define noise for certain terms +motion_anchor_ori_b_mf_nonflat: + noise: + _target_: isaaclab.utils.noise.AdditiveUniformNoiseCfg + n_min: -0.05 + n_max: 0.05 + +motion_anchor_ori_b: + noise: + _target_: isaaclab.utils.noise.AdditiveUniformNoiseCfg + n_min: -0.05 + n_max: 0.05 + +smpl_joints_multi_future_local_nonflat: + noise: + _target_: isaaclab.utils.noise.AdditiveUniformNoiseCfg + n_min: -0.05 + n_max: 0.05 + +smpl_root_ori_b_multi_future: + noise: + _target_: isaaclab.utils.noise.AdditiveUniformNoiseCfg + n_min: -0.05 + n_max: 0.05 + +soma_joints_multi_future_local_nonflat: + noise: + _target_: isaaclab.utils.noise.AdditiveUniformNoiseCfg + n_min: -0.05 + n_max: 0.05 + +soma_root_ori_b_multi_future: + noise: + _target_: isaaclab.utils.noise.AdditiveUniformNoiseCfg + n_min: -0.05 + n_max: 0.05 diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/recorders/empty.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/recorders/empty.yaml new file mode 100644 index 0000000000000000000000000000000000000000..8cfe8cded1bd86fc1e28660341f16dd4df663537 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/recorders/empty.yaml @@ -0,0 +1 @@ +_target_: gear_sonic.envs.manager_env.mdp.recorders.RecordersCfg diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/recorders/render.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/recorders/render.yaml new file mode 100644 index 0000000000000000000000000000000000000000..b4b565af03a09eedf9841311167536ed3c83fe7c --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/recorders/render.yaml @@ -0,0 +1,6 @@ +_target_: gear_sonic.envs.manager_env.mdp.recorders.RecordersCfg + +render_envs: + _target_: gear_sonic.envs.manager_env.mdp.recorders.RenderEnvsRecorderCfg + video_save_path: ${manager_env.config.save_rendering_dir} + video_quality: 5 diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/action_rate_l2.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/action_rate_l2.yaml new file mode 100644 index 0000000000000000000000000000000000000000..abb6eb00fa99f4dabda103e437d2ed2bab2d88b2 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/action_rate_l2.yaml @@ -0,0 +1,4 @@ +action_rate_l2: + _target_: isaaclab.managers.RewardTermCfg + func: gear_sonic.envs.manager_env.mdp:action_rate_l2 + weight: -1e-1 diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/anti_shake_ang_vel.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/anti_shake_ang_vel.yaml new file mode 100644 index 0000000000000000000000000000000000000000..e05824e476d27b61e24dd98aa6191f95340a2446 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/anti_shake_ang_vel.yaml @@ -0,0 +1,8 @@ +anti_shake_ang_vel: + _target_: isaaclab.managers.RewardTermCfg + func: gear_sonic.envs.manager_env.mdp:anti_shake_ang_vel_l2 + weight: -5e-3 + params: + command_name: "motion" + threshold: 1.5 + body_names: ["left_wrist_yaw_link", "right_wrist_yaw_link", "head_link"] diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/energy_consumption.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/energy_consumption.yaml new file mode 100644 index 0000000000000000000000000000000000000000..5085e3be249fa1e5e445f2d95c147b46d9fbe439 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/energy_consumption.yaml @@ -0,0 +1,7 @@ +energy_consumption: + _target_: isaaclab.managers.RewardTermCfg + func: gear_sonic.envs.manager_env.mdp:energy_consumption + weight: -1e-4 + params: + asset_cfg: + name: "robot" diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/feet_acc.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/feet_acc.yaml new file mode 100644 index 0000000000000000000000000000000000000000..adf90fe27c9736b2fd7c6f437872dace55db25c1 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/feet_acc.yaml @@ -0,0 +1,9 @@ +feet_acc: + _target_: isaaclab.managers.RewardTermCfg + func: gear_sonic.envs.manager_env.mdp:joint_acc_l2 + weight: -2.5e-7 + params: + asset_cfg: + _target_: isaaclab.managers.SceneEntityCfg + name: "robot" + joint_names: [".*ankle.*"] diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/joint_limit.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/joint_limit.yaml new file mode 100644 index 0000000000000000000000000000000000000000..f0a96562117321e63455a309f33e9a3175b977d7 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/joint_limit.yaml @@ -0,0 +1,9 @@ +joint_limit: + _target_: isaaclab.managers.RewardTermCfg + func: gear_sonic.envs.manager_env.mdp:joint_pos_limits + weight: -10.0 + params: + asset_cfg: + _target_: isaaclab.managers.SceneEntityCfg + name: "robot" + joint_names: [".*"] diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/tracking_anchor_ori.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/tracking_anchor_ori.yaml new file mode 100644 index 0000000000000000000000000000000000000000..b6b68697b116d59c3bee0eeb66e82dcec5831f87 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/tracking_anchor_ori.yaml @@ -0,0 +1,7 @@ +tracking_anchor_ori: + _target_: isaaclab.managers.RewardTermCfg + func: gear_sonic.envs.manager_env.mdp:tracking_anchor_ori_error + weight: 0.5 + params: + command_name: "motion" + std: 0.4 diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/tracking_anchor_pos.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/tracking_anchor_pos.yaml new file mode 100644 index 0000000000000000000000000000000000000000..592c6611d5e31a1857cacb1f1e506955b4ae03d8 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/tracking_anchor_pos.yaml @@ -0,0 +1,7 @@ +tracking_anchor_pos: + _target_: isaaclab.managers.RewardTermCfg + func: gear_sonic.envs.manager_env.mdp:tracking_anchor_pos_error + weight: 0.5 + params: + command_name: "motion" + std: 0.3 diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/tracking_body_angvel.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/tracking_body_angvel.yaml new file mode 100644 index 0000000000000000000000000000000000000000..a6be64ec7759a3f0928ded92e8d0f210121a7214 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/tracking_body_angvel.yaml @@ -0,0 +1,7 @@ +tracking_body_angvel: + _target_: isaaclab.managers.RewardTermCfg + func: gear_sonic.envs.manager_env.mdp:tracking_body_angvel_error + weight: 1.0 + params: + command_name: "motion" + std: 3.14 diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/tracking_body_linvel.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/tracking_body_linvel.yaml new file mode 100644 index 0000000000000000000000000000000000000000..4b75907a7481c2557e55a3e605acc502c63cdc20 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/tracking_body_linvel.yaml @@ -0,0 +1,7 @@ +tracking_body_linvel: + _target_: isaaclab.managers.RewardTermCfg + func: gear_sonic.envs.manager_env.mdp:tracking_body_linvel_error + weight: 1.0 + params: + command_name: "motion" + std: 1.0 diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/tracking_relative_body_ori.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/tracking_relative_body_ori.yaml new file mode 100644 index 0000000000000000000000000000000000000000..1aadde1bd30d48865e3216dbf0b79f5c7db4088b --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/tracking_relative_body_ori.yaml @@ -0,0 +1,7 @@ +tracking_relative_body_ori: + _target_: isaaclab.managers.RewardTermCfg + func: gear_sonic.envs.manager_env.mdp:tracking_relative_body_ori_error + weight: 1.0 + params: + command_name: "motion" + std: 0.4 diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/tracking_relative_body_pos.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/tracking_relative_body_pos.yaml new file mode 100644 index 0000000000000000000000000000000000000000..d1e6b02528d031d8dd09e42d75d80a8bc277c21b --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/tracking_relative_body_pos.yaml @@ -0,0 +1,7 @@ +tracking_relative_body_pos: + _target_: isaaclab.managers.RewardTermCfg + func: gear_sonic.envs.manager_env.mdp:tracking_relative_body_pos_error + weight: 1.0 + params: + command_name: "motion" + std: 0.3 diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/tracking_vr_2wrists_local_ori.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/tracking_vr_2wrists_local_ori.yaml new file mode 100644 index 0000000000000000000000000000000000000000..9c6b9a548c62702c2e389763502cb791a057f950 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/tracking_vr_2wrists_local_ori.yaml @@ -0,0 +1,8 @@ +tracking_vr_2wrists_local_ori: + _target_: isaaclab.managers.RewardTermCfg + func: gear_sonic.envs.manager_env.mdp:tracking_local_vr_2wrists_ori_error + weight: 0.4 + params: + command_name: "motion" + std: 0.1 + body_names: ["left_wrist_yaw_link", "right_wrist_yaw_link"] diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/tracking_vr_5point_local.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/tracking_vr_5point_local.yaml new file mode 100644 index 0000000000000000000000000000000000000000..48921fa93f1c669552d0fad558ce9bbaf262eac2 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/tracking_vr_5point_local.yaml @@ -0,0 +1,7 @@ +tracking_vr_5point_local: + _target_: isaaclab.managers.RewardTermCfg + func: gear_sonic.envs.manager_env.mdp:tracking_local_vr_5point_error + weight: 2.0 + params: + command_name: "motion" + std: 0.1 diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/undesired_contacts.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/undesired_contacts.yaml new file mode 100644 index 0000000000000000000000000000000000000000..e3a6849a7406bf56e76f7ea984ec68a4b208bf8f --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/terms/undesired_contacts.yaml @@ -0,0 +1,11 @@ +undesired_contacts: + _target_: isaaclab.managers.RewardTermCfg + func: gear_sonic.envs.manager_env.mdp:undesired_contacts + weight: -0.1 + params: + sensor_cfg: + _target_: isaaclab.managers.SceneEntityCfg + name: "contact_forces" + body_names: + - "^(?!left_ankle_roll_link$)(?!right_ankle_roll_link$)(?!left_wrist_yaw_link$)(?!right_wrist_yaw_link$)(?!left_elbow_link$)(?!right_elbow_link$).+$" + threshold: 1.0 diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/tracking/base.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/tracking/base.yaml new file mode 100644 index 0000000000000000000000000000000000000000..916c05d64c5d40dbee1282cd4af7d059322b1261 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/tracking/base.yaml @@ -0,0 +1,14 @@ +# Standard reward composition - combines all reward terms + +defaults: + - terms/tracking_anchor_pos@_here_ + - terms/tracking_anchor_ori@_here_ + - terms/tracking_relative_body_pos@_here_ + - terms/tracking_relative_body_ori@_here_ + - terms/tracking_body_linvel@_here_ + - terms/tracking_body_angvel@_here_ + - terms/action_rate_l2@_here_ + - terms/joint_limit@_here_ + - terms/undesired_contacts@_here_ + +_target_: gear_sonic.envs.manager_env.mdp.rewards.RewardsCfg diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/tracking/base_5point_local_feet_acc.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/tracking/base_5point_local_feet_acc.yaml new file mode 100644 index 0000000000000000000000000000000000000000..8944f8d4596dc254cc30a3f92fd98a412238763a --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/tracking/base_5point_local_feet_acc.yaml @@ -0,0 +1,17 @@ +# Standard reward composition - combines all reward terms + +defaults: + - terms/tracking_anchor_pos@_here_ + - terms/tracking_anchor_ori@_here_ + - terms/tracking_relative_body_pos@_here_ + - terms/tracking_relative_body_ori@_here_ + - terms/tracking_body_linvel@_here_ + - terms/tracking_body_angvel@_here_ + - terms/action_rate_l2@_here_ + - terms/joint_limit@_here_ + - terms/undesired_contacts@_here_ + - terms/anti_shake_ang_vel@_here_ + - terms/tracking_vr_5point_local@_here_ + - terms/feet_acc@_here_ + +_target_: gear_sonic.envs.manager_env.mdp.rewards.RewardsCfg diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/tracking/local_feet_acc_energy_5pt.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/tracking/local_feet_acc_energy_5pt.yaml new file mode 100644 index 0000000000000000000000000000000000000000..4b6cc356e2589088f15a7716f8ab668d81f2ea67 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/rewards/tracking/local_feet_acc_energy_5pt.yaml @@ -0,0 +1,19 @@ +# SONIC v1.1 reward composition. + +defaults: + - terms/tracking_anchor_pos@_here_ + - terms/tracking_anchor_ori@_here_ + - terms/tracking_relative_body_pos@_here_ + - terms/tracking_relative_body_ori@_here_ + - terms/tracking_body_linvel@_here_ + - terms/tracking_body_angvel@_here_ + - terms/action_rate_l2@_here_ + - terms/joint_limit@_here_ + - terms/undesired_contacts@_here_ + - terms/anti_shake_ang_vel@_here_ + - terms/tracking_vr_5point_local@_here_ + - terms/tracking_vr_2wrists_local_ori@_here_ + - terms/feet_acc@_here_ + - terms/energy_consumption@_here_ + +_target_: gear_sonic.envs.manager_env.mdp.rewards.RewardsCfg diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/terminations/terms/anchor_ori_full.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/terminations/terms/anchor_ori_full.yaml new file mode 100644 index 0000000000000000000000000000000000000000..43ea62aaa006b186b05ec857bb93cca7715918e9 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/terminations/terms/anchor_ori_full.yaml @@ -0,0 +1,9 @@ +anchor_ori_full: + _target_: isaaclab.managers.TerminationTermCfg + func: gear_sonic.envs.manager_env.mdp:exceeded_anchor_ori + params: + asset_cfg: + _target_: isaaclab.managers.SceneEntityCfg + name: "robot" + command_name: "motion" + threshold: 1 diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/terminations/terms/anchor_pos.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/terminations/terms/anchor_pos.yaml new file mode 100644 index 0000000000000000000000000000000000000000..554ac3ad4d669d8568244e8f36ce1c16b554301e --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/terminations/terms/anchor_pos.yaml @@ -0,0 +1,6 @@ +anchor_pos: + _target_: isaaclab.managers.TerminationTermCfg + func: gear_sonic.envs.manager_env.mdp:exceeded_anchor_height + params: + command_name: "motion" + threshold: 0.25 diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/terminations/terms/anchor_pos_adaptive.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/terminations/terms/anchor_pos_adaptive.yaml new file mode 100644 index 0000000000000000000000000000000000000000..7f7a060d92ade85ec89e9e5dae02653dce656cb2 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/terminations/terms/anchor_pos_adaptive.yaml @@ -0,0 +1,9 @@ +anchor_pos: + _target_: isaaclab.managers.TerminationTermCfg + func: gear_sonic.envs.manager_env.mdp:exceeded_anchor_height + params: + command_name: "motion" + threshold: 0.5 + threshold_adaptive: True + down_threshold: 0.75 + root_height_threshold: 0.5 diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/terminations/terms/ee_body_pos.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/terminations/terms/ee_body_pos.yaml new file mode 100644 index 0000000000000000000000000000000000000000..8c94aebdfdfcba2993310c1fe523f73e1bda7d69 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/terminations/terms/ee_body_pos.yaml @@ -0,0 +1,11 @@ +ee_body_pos: + _target_: isaaclab.managers.TerminationTermCfg + func: gear_sonic.envs.manager_env.mdp:exceeded_body_height + params: + command_name: "motion" + threshold: 0.25 + body_names: + - "left_ankle_roll_link" + - "right_ankle_roll_link" + - "left_wrist_yaw_link" + - "right_wrist_yaw_link" diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/terminations/terms/ee_body_pos_adaptive.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/terminations/terms/ee_body_pos_adaptive.yaml new file mode 100644 index 0000000000000000000000000000000000000000..cf7ad73cc3380b73865ac77bfff35ec6e8567102 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/terminations/terms/ee_body_pos_adaptive.yaml @@ -0,0 +1,14 @@ +ee_body_pos: + _target_: isaaclab.managers.TerminationTermCfg + func: gear_sonic.envs.manager_env.mdp:exceeded_body_height + params: + command_name: "motion" + threshold: 0.5 + threshold_adaptive: True + down_threshold: 0.75 + root_height_threshold: 0.5 + body_names: + - "left_ankle_roll_link" + - "right_ankle_roll_link" + - "left_wrist_yaw_link" + - "right_wrist_yaw_link" diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/terminations/terms/foot_pos_xyz.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/terminations/terms/foot_pos_xyz.yaml new file mode 100644 index 0000000000000000000000000000000000000000..f1aa444f03d49628c33e8e03edc6b89ca674cfd9 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/terminations/terms/foot_pos_xyz.yaml @@ -0,0 +1,9 @@ +foot_pos_xyz: + _target_: isaaclab.managers.TerminationTermCfg + func: gear_sonic.envs.manager_env.mdp:exceeded_body_pos + params: + command_name: "motion" + threshold: 0.5 + body_names: + - "left_ankle_roll_link" + - "right_ankle_roll_link" diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/terminations/terms/motion_time_out.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/terminations/terms/motion_time_out.yaml new file mode 100644 index 0000000000000000000000000000000000000000..7a53caeae5b0d9c1d4e347c29191e3d3ac66192a --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/terminations/terms/motion_time_out.yaml @@ -0,0 +1,6 @@ +time_out: + _target_: isaaclab.managers.TerminationTermCfg + func: gear_sonic.envs.manager_env.mdp:tracking_time_out + time_out: true + params: + command_name: "motion" diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/terminations/tracking/base.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/terminations/tracking/base.yaml new file mode 100644 index 0000000000000000000000000000000000000000..60acfee1e9b50777a5291d91efd2bc529e0a2998 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/terminations/tracking/base.yaml @@ -0,0 +1,9 @@ +# Standard termination composition + +defaults: + - terms/anchor_pos@_here_ + - terms/anchor_ori_full@_here_ + - terms/ee_body_pos@_here_ + - terms/motion_time_out@_here_ + +_target_: gear_sonic.envs.manager_env.mdp.terminations.TerminationsCfg diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/terminations/tracking/base_adaptive_strict_ori_foot_xyz.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/terminations/tracking/base_adaptive_strict_ori_foot_xyz.yaml new file mode 100644 index 0000000000000000000000000000000000000000..e3a3c87e3d8b20e37b5f7cc0d8bcd2ed73fd46c6 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/terminations/tracking/base_adaptive_strict_ori_foot_xyz.yaml @@ -0,0 +1,26 @@ +# Standard termination composition + +defaults: + - terms/anchor_pos_adaptive@_here_ + - terms/anchor_ori_full@_here_ + - terms/ee_body_pos_adaptive@_here_ + - terms/motion_time_out@_here_ + - terms/foot_pos_xyz@_here_ + +_target_: gear_sonic.envs.manager_env.mdp.terminations.TerminationsCfg + +foot_pos_xyz: + params: + threshold: 0.2 + +anchor_pos: + params: + threshold: 0.15 + +ee_body_pos: + params: + threshold: 0.15 + +anchor_ori_full: + params: + threshold: 0.2 diff --git a/GR00T-WholeBodyControl/gear_sonic/config/manager_env/terminations/tracking/eval.yaml b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/terminations/tracking/eval.yaml new file mode 100644 index 0000000000000000000000000000000000000000..1ef79b567aab7b4773236c57720bfd66056c3d3b --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/manager_env/terminations/tracking/eval.yaml @@ -0,0 +1,25 @@ +# Evaluation terminations — relaxed thresholds vs training for fairer eval metrics + +defaults: + - terms/anchor_pos@_here_ + - terms/anchor_ori_full@_here_ + - terms/ee_body_pos@_here_ + - terms/motion_time_out@_here_ + +_target_: gear_sonic.envs.manager_env.mdp.terminations.TerminationsCfg + +anchor_pos: + params: + threshold: 0.25 + threshold_adaptive: false + down_threshold: 0.25 + +anchor_ori_full: + params: + threshold: 1.0 + +ee_body_pos: + params: + threshold: 0.25 + threshold_adaptive: false + down_threshold: 0.25 diff --git a/GR00T-WholeBodyControl/gear_sonic/config/opt/wandb.yaml b/GR00T-WholeBodyControl/gear_sonic/config/opt/wandb.yaml new file mode 100644 index 0000000000000000000000000000000000000000..f91c1666a7e558f9597a4056f701379c5f09da37 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/opt/wandb.yaml @@ -0,0 +1,12 @@ +# @package _global_ + +use_wandb: True + +wandb: + wandb_project: null + wandb_tags: 'online' # 'online' or 'offline' or 'disabled' + wandb_group: null + wandb_id: null + wandb_entity: null + wandb_dir: '/tmp' # use /tmp so that it will automatically be cleaned up + # wandb_dir: ${experiment_dir}/.wandb diff --git a/GR00T-WholeBodyControl/gear_sonic/config/trainer/trl.yaml b/GR00T-WholeBodyControl/gear_sonic/config/trainer/trl.yaml new file mode 100644 index 0000000000000000000000000000000000000000..11e5eb0a950e4921194d54a7fe5e21aaa191eb95 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/trainer/trl.yaml @@ -0,0 +1 @@ +_target_: gear_sonic.trl.trainer.ppo_trainer.TRLPPOTrainer diff --git a/GR00T-WholeBodyControl/gear_sonic/config/trainer/trl_ppo_aux.yaml b/GR00T-WholeBodyControl/gear_sonic/config/trainer/trl_ppo_aux.yaml new file mode 100644 index 0000000000000000000000000000000000000000..63aec020a622313b42d6fd722899c37c38f6d935 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/config/trainer/trl_ppo_aux.yaml @@ -0,0 +1 @@ +_target_: gear_sonic.trl.trainer.ppo_trainer_aux_loss.TRLAuxLossPPOTrainer diff --git a/GR00T-WholeBodyControl/gear_sonic/examples/live_camera_teleop/README.md b/GR00T-WholeBodyControl/gear_sonic/examples/live_camera_teleop/README.md new file mode 100644 index 0000000000000000000000000000000000000000..0787ab96d23d4990bda4b40ae004e2166f8322f7 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/examples/live_camera_teleop/README.md @@ -0,0 +1,152 @@ +# Live Camera Teleoperation with GEM-X + +Drive the Unitree G1 from a **live webcam** by feeding +[GEM-X](https://github.com/NVlabs/GEM-X) 3D human pose estimation into GEAR-SONIC. +No motion-capture suit or VR trackers; just a camera. + +GEM-X estimates full-body human motion (the SOMA body model); this example bridges +that motion into SONIC's existing ZMQ streaming interface, and SONIC's policy +tracks it while keeping balance. + +> GEM-X is an **optional external dependency** and is **not** bundled here. Install +> it separately and point these scripts at it with `--gemx-root` (or `$GEMX_ROOT`). + +## How it works + +| Script(s) | SONIC protocol | SONIC encoder | +|---|---|---| +| `webcam_stream.py` + `soma_to_smpl.py` | v3 (SMPL) | `smpl` (mode 2) | +| `soma_pt_to_sonic_v3.py` (offline verify) | v3 (SMPL) | `smpl` (mode 2) | + +GEM-X runs in a rolling-window loop producing SOMA per frame; `soma_to_smpl.py` +converts SOMA (77 joints) -> SMPL (24 joints, root-local, gravity-aligned) and +streams Protocol v3 so SONIC's learned `smpl` encoder does the human->robot +mapping (no offline retargeter in the loop). + +## Prerequisites + +1. **SONIC deployment** built and runnable (see the repo Quick Start), with the + released policy that has the `smpl` encoder (`policy/release/`). +2. **GEM-X** cloned and installed: https://github.com/NVlabs/GEM-X + (its checkpoints/assets download from HuggingFace on first run). +3. Python deps in the environment you run these scripts in: `pyzmq`, `scipy`, + `numpy`, `opencv-python`, and a CUDA-matched `onnxruntime-gpu`. + +## Camera setup + +Verify the camera before wiring up the robot. `webcam_stream.py` doubles as the +test tool — `--kp-only` skips the 3D denoiser, so it starts fast and needs no +checkpoint. + +### 1. Find the device + +```bash +ls /dev/video* +v4l2-ctl --list-devices # sudo apt install v4l-utils +v4l2-ctl -d /dev/video0 --list-formats-ext # modes the camera actually supports +``` + +The index in `/dev/videoN` is what you pass to `--source` (`/dev/video0` → +`--source 0`). Many USB cameras expose several nodes for one physical device; the +lowest-numbered one is usually the capture node. + +### 2. Preview the stream + +```bash +export GEMX_ROOT=/path/to/GEM-X + +# with a display: live overlay window, press q to quit +python gear_sonic/examples/live_camera_teleop/webcam_stream.py \ + --source 0 --kp-only --show + +# headless (SSH/VNC): write an overlay clip instead +python gear_sonic/examples/live_camera_teleop/webcam_stream.py \ + --source 0 --kp-only --save webcam_test.mp4 --max-frames 60 +``` + +You should see keypoints tracking your body and a steady frame rate in the status +line. If keypoints are missing or jumping, fix that before streaming to the robot +— the controller can only track what the estimator sees. + +### 3. Request a capture mode (optional) + +A camera's default mode is often low-resolution or low-fps, which caps the teleop +rate. Request one of the modes reported by `--list-formats-ext`: + +```bash +... --source 0 --resolution 1280x720 --cap-fps 30 +``` + +Cameras silently ignore unsupported settings, so the script logs the mode it +actually negotiated — check that line rather than assuming the request applied. + +### Framing + +Monocular estimation only knows what it can see: + +- keep the **full body in frame, including feet** — cropped legs make the lower + body unreliable, and that's what the controller balances on +- stand roughly 2–3 m back, camera near torso height, lens roughly level +- even front-facing light; avoid strong backlight +- one person in frame (the largest detection is the one tracked) + +### Troubleshooting + +| Symptom | Cause / fix | +|---|---| +| `Could not open video source: 0` | Wrong index (`ls /dev/video*`), or user not in the `video` group (`sudo usermod -aG video $USER`, then re-login) | +| Opens but frames fail or hang | Another process holds the device (browser tab, earlier run). Check with `sudo fuser /dev/video0` | +| Very low fps | Camera negotiated a slow mode — set `--resolution` / `--cap-fps`, and try a smaller `--window` | +| `--show` fails on a headless box | No display available; use `--save` instead | +| No keypoints detected | Body not fully in frame, too dark, or too far away | + +> No camera handy? Pass a video file to `--source` (e.g. `--source clip.mp4`) to +> exercise the full path, or use `soma_pt_to_sonic_v3.py` for a camera-free test +> against the SONIC sim. + +## Usage + +Run the SONIC deploy in ZMQ mode first (sim shown; use `real` for hardware): + +```bash +# terminal 1 (repo root): MuJoCo sim +python gear_sonic/scripts/run_sim_loop.py +# terminal 2 (gear_sonic_deploy/): deploy, ZMQ input +bash deploy.sh --input-type zmq --zmq-host localhost --zmq-port 5556 --zmq-topic pose --zmq-conflate sim +``` + +### Live webcam (Protocol v3 / smpl encoder) +```bash +export GEMX_ROOT=/path/to/GEM-X +python gear_sonic/examples/live_camera_teleop/webcam_stream.py --source 0 \ + --stream-sonic --window 30 --smooth 0.8 +``` + +### Offline SMPL verify (no camera) +```bash +python gear_sonic/examples/live_camera_teleop/soma_pt_to_sonic_v3.py \ + --gemx-root /path/to/GEM-X --pt /path/to/hpe_results.pt --fps 30 --loop +``` + +On the deploy side: press `]` to start, drop the robot (`9` in MuJoCo), then +`ENTER` to enable ZMQ streaming. Use `O` for emergency stop. On real hardware, +keep a safety operator on the E-stop. + +## Key options +- `--window`: rolling-window length; smaller = higher fps. +- `--no-imgfeat`: skip GEM-X's SAM-3D-Body image features for speed (lower + quality legs/depth). +- `--smooth`: temporal smoothing of the streamed reference (0 = off, + 0.6-0.85 steadier). +- `--source`: camera index (`ls /dev/video*`) or a video file (stand-in). +- `--resolution` / `--cap-fps`: request a camera capture mode (see Camera setup). +- `--kp-only` / `--show` / `--save`: 2D-only preview modes for camera checks. + +## Notes / limitations +- The live path streams root-local SMPL pose + heading; it does not command an + explicit root translation, so controlled forward navigation is limited (best + combined with the kinematic planner for the lower body). +- `smpl_pose` and wrist joints are streamed as zeros (the `smpl` encoder consumes + `smpl_joints` + anchor + wrists); wrist/finger fidelity is future work. +- Monocular lower-body/foot estimation is the weakest signal; image features + (`--no-imgfeat` off) improve grounding at the cost of frame rate. diff --git a/GR00T-WholeBodyControl/gear_sonic/examples/live_camera_teleop/soma_pt_to_sonic_v3.py b/GR00T-WholeBodyControl/gear_sonic/examples/live_camera_teleop/soma_pt_to_sonic_v3.py new file mode 100644 index 0000000000000000000000000000000000000000..cf632130925673393c4f19e79a3a851bb06015c6 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/examples/live_camera_teleop/soma_pt_to_sonic_v3.py @@ -0,0 +1,121 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +"""Offline driver: stream a saved GEM-X SOMA result to SONIC as SMPL (Protocol v3). + +Replays ``hpe_results.pt`` (the SOMA body params GEM-X already produced for a +video) through the SOMA->SMPL converter and publishes the v3 stream SONIC's +``smpl`` encoder expects. Useful to verify the SMPL path against the SONIC sim +WITHOUT a camera. + +Requires GEM-X (https://github.com/NVlabs/GEM-X); pass --gemx-root or set GEMX_ROOT. + +Example: + python soma_pt_to_sonic_v3.py --gemx-root /path/to/GEM-X \ + --pt /path/to/hpe_results.pt --fps 30 --loop +""" + +import argparse +import os +from pathlib import Path +import sys +import time + +import torch + + +def _add_gemx_to_path() -> str | None: + root = os.environ.get("GEMX_ROOT") + for i, a in enumerate(sys.argv): + if a == "--gemx-root" and i + 1 < len(sys.argv): + root = sys.argv[i + 1] + elif a.startswith("--gemx-root="): + root = a.split("=", 1)[1] + if root and root not in sys.path: + sys.path.insert(0, root) + return root + + +GEMX_ROOT = _add_gemx_to_path() +sys.path.insert(0, str(Path(__file__).resolve().parent)) # sibling modules + + +def main(): + ap = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) + ap.add_argument("--gemx-root", default=GEMX_ROOT, help="GEM-X repo root (or set $GEMX_ROOT)") + ap.add_argument("--pt", required=True, help="Path to hpe_results.pt") + ap.add_argument("--port", type=int, default=5556) + ap.add_argument("--fps", type=float, default=30.0) + ap.add_argument("--loop", action="store_true") + ap.add_argument("--max-frames", type=int, default=0) + ap.add_argument( + "--sonic-root", + default=os.environ.get("SONIC_ROOT"), + help="SONIC repo root, or set $SONIC_ROOT (only needed when run outside the repo)", + ) + ap.add_argument("--smooth", type=float, default=0.0, help="Temporal smoothing (0=off)") + ap.add_argument("--dry-run", action="store_true", help="Convert frame 0 and print, no ZMQ") + args = ap.parse_args() + + if not args.gemx_root: + raise SystemExit("Provide --gemx-root or set GEMX_ROOT.") + + try: + from gem.utils.soma_utils.soma_layer import SomaLayer + except ModuleNotFoundError as e: + raise SystemExit( + "GEM-X not found (%s). Install https://github.com/NVlabs/GEM-X and pass " + "--gemx-root or set GEMX_ROOT." % e + ) + from soma_to_smpl import SomaToSmpl, SonicV3Publisher + + # hpe_results.pt holds non-tensor objects, so weights_only=False is required. + pred = torch.load(args.pt, weights_only=False) + g = pred["body_params_global"] + T = g["body_pose"].shape[0] + print(f"[soma->v3] loaded {T} frames from {args.pt}") + + soma = SomaLayer( + data_root=str(Path(args.gemx_root) / "inputs" / "soma_assets"), + low_lod=True, + device="cuda", + identity_model_type="mhr", + mode="warp", + ) + conv = SomaToSmpl(soma, device="cuda", smooth=args.smooth, sonic_root=args.sonic_root) + + def frame_at(t): + return {k: g[k][t] for k in ("body_pose", "global_orient", "identity_coeffs", "scale_params")} + + if args.dry_run: + out = conv.convert(frame_at(0)) + for k, v in out.items(): + print(k, v.shape) + return + + pub = SonicV3Publisher(port=args.port, sonic_root=args.sonic_root) + print(f"[soma->v3] publishing 'pose' v3 on tcp://*:{args.port}; enable streaming (ENTER) on deploy") + time.sleep(1.0) + dt = 1.0 / args.fps + sent = 0 + try: + while True: + for t in range(T): + pub.publish(conv.convert(frame_at(t))) + sent += 1 + if t % int(max(1, args.fps)) == 0: + print(f"\r[soma->v3] frame {t + 1}/{T} (sent {sent})", end="") + time.sleep(dt) + if args.max_frames and sent >= args.max_frames: + return + if not args.loop: + break + print("\n[soma->v3] loop restart") + except KeyboardInterrupt: + print("\n[soma->v3] stopped") + finally: + pub.close() + print("\n[soma->v3] done") + + +if __name__ == "__main__": + main() diff --git a/GR00T-WholeBodyControl/gear_sonic/examples/live_camera_teleop/soma_to_smpl.py b/GR00T-WholeBodyControl/gear_sonic/examples/live_camera_teleop/soma_to_smpl.py new file mode 100644 index 0000000000000000000000000000000000000000..efa2bd6e5caf01dbfc73af360363202b69c561d6 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/examples/live_camera_teleop/soma_to_smpl.py @@ -0,0 +1,328 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +"""SOMA -> SMPL conversion + SONIC Protocol-v3 publishing (live-webcam path). + +GEM-X estimates SOMA (77-joint) body params. SONIC's deployed policy has an +``smpl`` encoder mode (Protocol v3) whose encoder observations are: + - smpl_joints (24x3, root-local) + - smpl_anchor_orientation (derived by the deploy from the streamed body_quat) + - motion_joint_positions_wrists (6 G1 wrist joints) + +This module converts a per-frame SOMA decode into the v3 stream fields, matching +SONIC's own convention in +``gear_sonic/scripts/pico_manager_thread_server.py:process_smpl_joints`` exactly: +the root quaternion goes aa -> quat -> smpl_root_ytoz_up (+90 deg about X) -> +remove_smpl_base_rot; smpl_joints are the SMPL joints with that root orientation +removed (root-local), in Z-up. Optional temporal smoothing steadies the live +signal. + +Note: this file imports ``gear_sonic`` (native when run from inside the SONIC +repo). The SOMA body-model layer is passed in by the caller, so GEM-X is not +imported here directly. +""" + +from __future__ import annotations + +import json +import sys + +import numpy as np +import torch + +# Canonical SMPL 24 joint names (SMPL body kinematic tree order). +SMPL_24_NAMES = [ + "pelvis", + "left_hip", + "right_hip", + "spine1", + "left_knee", + "right_knee", + "spine2", + "left_ankle", + "right_ankle", + "spine3", + "left_foot", + "right_foot", + "neck", + "left_collar", + "right_collar", + "head", + "left_shoulder", + "right_shoulder", + "left_elbow", + "right_elbow", + "left_wrist", + "right_wrist", + "left_hand", + "right_hand", +] + +# Aliases mapping canonical SMPL joints -> SOMA rig joint names (Maya-style). +# SOMA rig convention: "Foot" = ankle, "ToeBase" = foot/ball; "Arm" = shoulder, +# "ForeArm" = elbow, "Hand" = wrist, "Shoulder" = collar/clavicle. +_ALIASES = { + "pelvis": ["hips", "pelvis", "root"], + "left_hip": ["leftleg", "left_hip", "l_hip", "left_upleg"], + "right_hip": ["rightleg", "right_hip", "r_hip", "right_upleg"], + "spine1": ["spine1", "spine_1", "spine"], + "left_knee": ["leftshin", "left_knee", "l_knee"], + "right_knee": ["rightshin", "right_knee", "r_knee"], + "spine2": ["spine2", "spine_2"], + "left_ankle": ["leftfoot", "left_ankle", "l_ankle"], + "right_ankle": ["rightfoot", "right_ankle", "r_ankle"], + "spine3": ["chest", "spine3", "spine_3"], + "left_foot": ["lefttoebase", "left_foot", "l_foot", "left_toe"], + "right_foot": ["righttoebase", "right_foot", "r_foot", "right_toe"], + "neck": ["neck1", "neck"], + "left_collar": ["leftshoulder", "left_collar", "l_collar", "left_clavicle"], + "right_collar": ["rightshoulder", "right_collar", "r_collar", "right_clavicle"], + "head": ["head"], + "left_shoulder": ["leftarm", "left_shoulder", "l_shoulder", "left_upperarm"], + "right_shoulder": ["rightarm", "right_shoulder", "r_shoulder", "right_upperarm"], + "left_elbow": ["leftforearm", "left_elbow", "l_elbow", "left_lowerarm"], + "right_elbow": ["rightforearm", "right_elbow", "r_elbow"], + "left_wrist": ["lefthand", "left_wrist", "l_wrist"], + "right_wrist": ["righthand", "right_wrist", "r_wrist"], + "left_hand": ["lefthandmiddle1", "left_hand", "l_hand", "left_middle1"], + "right_hand": ["righthandmiddle1", "right_hand", "r_hand", "right_middle1"], +} + + +def _norm(name: str) -> str: + return name.lower().replace("-", "_").replace(" ", "_") + + +def build_soma_to_smpl_index(soma_joint_names: list[str]) -> list[int]: + """Map each of the 24 SMPL joints to a SOMA joint index, by name alias.""" + norm_names = [_norm(n) for n in soma_joint_names] + name_to_idx = {n: i for i, n in enumerate(norm_names)} + idx, unmatched = [], [] + for smpl_name in SMPL_24_NAMES: + found = None + for alias in _ALIASES[smpl_name]: + if _norm(alias) in name_to_idx: + found = name_to_idx[_norm(alias)] + break + if found is None: + unmatched.append(smpl_name) + found = 0 + idx.append(found) + if unmatched: + raise ValueError( + f"Could not map SMPL joints {unmatched} to SOMA joints. " + f"Available SOMA joint names: {soma_joint_names}" + ) + return idx + + +# Y-up -> Z-up rotation (+90 deg about X): same as smpl_root_ytoz_up on points. +_YUP_TO_ZUP = torch.tensor([[1.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]], dtype=torch.float32) + + +class SomaToSmpl: + """Convert per-frame SOMA decode -> SONIC v3 SMPL fields.""" + + def __init__(self, soma_layer, device="cuda", y_to_z_up=True, smooth=0.0, sonic_root=None): + self.soma = soma_layer + self.device = device + self.y_to_z_up = y_to_z_up + # Temporal smoothing weight on history (0 = off; 0.6-0.85 = progressively smoother). + self.smooth = float(smooth) + self._ema_joints = None + self._ema_quat = None + + names = self._resolve_joint_names(soma_layer) + if len(names) == 78 and _norm(names[0]) == "root": + names = names[1:] # align to the 77-joint forward() output + self.soma_joint_names = names + self.smpl_idx = build_soma_to_smpl_index(names) + self._R_up = _YUP_TO_ZUP.to(device) + + # Deferred: sonic_root must be on sys.path before gear_sonic resolves. + if sonic_root and sonic_root not in sys.path: + sys.path.insert(0, sonic_root) + from gear_sonic.isaac_utils.rotations import ( + remove_smpl_base_rot, + smpl_root_ytoz_up, + ) + from gear_sonic.trl.utils.torch_transform import ( + angle_axis_to_quaternion, + quat_apply, + quat_inv, + ) + + self._aa2quat = angle_axis_to_quaternion + self._quat_apply = quat_apply + self._quat_inv = quat_inv + self._remove_base_rot = remove_smpl_base_rot + self._ytoz = smpl_root_ytoz_up + + @staticmethod + def _resolve_joint_names(soma_layer): + inner = getattr(soma_layer, "soma", soma_layer) + rig = getattr(inner, "rig_data", None) + # rig_data is an NpzFile: .files lists key names, arrays come off the object. + if rig is not None and hasattr(rig, "files") and "joint_names" in rig.files: + return [str(x) for x in rig["joint_names"]] + if isinstance(rig, dict) and "joint_names" in rig: + return [str(x) for x in rig["joint_names"]] + names = getattr(inner, "joint_names", None) or getattr(soma_layer, "joint_names", None) + if names is not None: + return [str(x) for x in names] + raise AttributeError("Could not find SOMA joint_names (checked .soma.rig_data['joint_names']).") + + @torch.no_grad() + def convert(self, soma_params: dict) -> dict: + """soma_params: per-frame tensors (body_pose, global_orient, + identity_coeffs, scale_params[, transl]). Returns v3 stream fields.""" + missing = [ + k for k in ("body_pose", "global_orient", "identity_coeffs", "scale_params") if k not in soma_params + ] + if missing: + raise KeyError( + f"SomaToSmpl.convert() requires {missing}; these come from GEM-X's " + "EnDecoder.decode() (soma_v2). There is no safe zero default here: " + "scale_params[0] is a global scale factor, so zeros would collapse the " + "rest shape and stream an all-zeros skeleton to the controller." + ) + + def _t(x): + x = torch.as_tensor(x, dtype=torch.float32, device=self.device) + return x.unsqueeze(0) if x.dim() == 1 else x + + body_pose = _t(soma_params["body_pose"]) + global_orient = _t(soma_params["global_orient"]) + identity = _t(soma_params["identity_coeffs"]) + scale = _t(soma_params["scale_params"]) + transl = _t(soma_params.get("transl", torch.zeros(3, device=self.device))) + + out = self.soma( + body_pose=body_pose, + global_orient=global_orient, + transl=transl, + identity_coeffs=identity, + scale_params=scale, + ) + joints77 = out["joints"][0] # (77,3) GEM y-up, global applied + joints24 = joints77[self.smpl_idx] # (24,3) + + # Mirror SONIC's process_smpl_joints convention exactly. + g_quat = self._aa2quat(global_orient) # (1,4) wxyz, y-up + g_quat_z = self._ytoz(g_quat) # (1,4) z-up + g_quat_nobase = self._remove_base_rot(g_quat_z, w_last=False) # (1,4) + + joints0 = joints24 - joints24[0:1] # root at origin (y-up) + joints_z = joints0 @ self._R_up.T # (24,3) z-up + inv = self._quat_inv(g_quat_nobase).repeat(joints_z.shape[0], 1) # (24,4) + smpl_joints_local = self._quat_apply(inv, joints_z) # (24,3) + + smpl_joints = smpl_joints_local.unsqueeze(0).cpu().numpy().astype(np.float32) + body_quat = g_quat_nobase.reshape(1, 4).cpu().numpy().astype(np.float32) + + # Temporal smoothing (reduces live jitter -> stepping/wobble). + if self.smooth > 0.0: + w = self.smooth + if self._ema_joints is None: + self._ema_joints = smpl_joints.copy() + self._ema_quat = body_quat.copy() + else: + self._ema_joints = w * self._ema_joints + (1.0 - w) * smpl_joints + q_prev, q_new = self._ema_quat, body_quat.copy() + if float((q_prev * q_new).sum()) < 0.0: # sign-align before lerp + q_new = -q_new + q = w * q_prev + (1.0 - w) * q_new + self._ema_quat = q / (np.linalg.norm(q) + 1e-8) + smpl_joints = self._ema_joints.astype(np.float32) + body_quat = self._ema_quat.astype(np.float32) + + smpl_pose = np.zeros((1, 21, 3), dtype=np.float32) + wrists = np.zeros((1, 6), dtype=np.float32) + return { + "smpl_joints": smpl_joints, + "body_quat": body_quat, + "smpl_pose": smpl_pose, + "wrists": wrists, + } + + +HEADER_SIZE = 1280 # SONIC ZMQ header size (see gear_sonic zmq_planner_sender.py) + + +def _fallback_pack_pose_message(pose_data: dict, topic: str = "pose", version: int = 3) -> bytes: + """Byte-identical fallback for SONIC's pack_pose_message. + + Layout: [topic_bytes][1280-byte JSON header][concatenated little-endian binary fields]. + """ + dtype_map = { + np.dtype(np.float32): "f32", + np.dtype(np.float64): "f64", + np.dtype(np.int32): "i32", + np.dtype(np.int64): "i64", + np.dtype(bool): "bool", + } + fields, binary = [], [] + for key, value in pose_data.items(): + if not isinstance(value, np.ndarray): + continue + dtype_str = dtype_map.get(value.dtype, "f32") + if dtype_str == "f32" and value.dtype != np.float32: + value = value.astype(np.float32) + if not value.flags["C_CONTIGUOUS"]: + value = np.ascontiguousarray(value) + fields.append({"name": key, "dtype": dtype_str, "shape": list(value.shape)}) + binary.append(value.tobytes()) + header = {"v": version, "endian": "le", "count": 1, "fields": fields} + header_json = json.dumps(header, separators=(",", ":")).encode("utf-8") + if len(header_json) > HEADER_SIZE: + raise ValueError(f"Header too large: {len(header_json)} > {HEADER_SIZE}") + return topic.encode("utf-8") + header_json.ljust(HEADER_SIZE, b"\x00") + b"".join(binary) + + +def _get_packer(sonic_root: str | None = None): + """Return SONIC's pack_pose_message if importable, else the local fallback. + + Native import works when run from inside the SONIC repo; pass ``sonic_root`` + only if running from outside. + """ + if sonic_root and sonic_root not in sys.path: + sys.path.insert(0, sonic_root) + try: + from gear_sonic.utils.teleop.zmq.zmq_planner_sender import pack_pose_message + + print("[bridge] Using SONIC's pack_pose_message (exact wire format).") + return pack_pose_message + except Exception as exc: + print(f"[bridge] gear_sonic not importable ({exc}); using built-in fallback packer.") + return _fallback_pack_pose_message + + +class SonicV3Publisher: + """ZMQ PUB publisher for SONIC Protocol v3 (SMPL) using SONIC's exact packer.""" + + def __init__(self, port=5556, topic="pose", sonic_root=None): + import zmq + + self.pack = _get_packer(sonic_root) + self.topic = topic + self.frame_index = 0 + self.ctx = zmq.Context() + self.sock = self.ctx.socket(zmq.PUB) + self.sock.bind(f"tcp://*:{port}") + + def publish(self, fields: dict): + joint_pos = np.zeros((1, 29), dtype=np.float32) + joint_pos[:, 23:29] = fields["wrists"] # only wrists meaningful in v3 + pose_data = { + "smpl_joints": fields["smpl_joints"].reshape(1, 24, 3), + "smpl_pose": fields["smpl_pose"].reshape(1, 21, 3), + "joint_pos": joint_pos, + "joint_vel": np.zeros((1, 29), dtype=np.float32), + "body_quat": fields["body_quat"].reshape(1, 4), # wxyz, SONIC convention + "frame_index": np.array([self.frame_index], dtype=np.int64), + } + self.sock.send(self.pack(pose_data, topic=self.topic, version=3)) + self.frame_index += 1 + + def close(self): + self.sock.close(0) + self.ctx.term() diff --git a/GR00T-WholeBodyControl/gear_sonic/examples/live_camera_teleop/webcam_stream.py b/GR00T-WholeBodyControl/gear_sonic/examples/live_camera_teleop/webcam_stream.py new file mode 100644 index 0000000000000000000000000000000000000000..b1e0a7fd37740c5378b3a2808cccd3dc25b019ac --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/examples/live_camera_teleop/webcam_stream.py @@ -0,0 +1,337 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +"""Real-time GEM-X pose estimation from a live webcam -> SONIC (live-webcam path). + +Captures frames from a webcam (or a video file used as a stand-in), runs the +GEM-X ONNX pipeline (YOLOX detect -> VitPose 2D -> GEM denoiser) over a rolling +sliding window, decodes per-frame SOMA body params, and (with --stream-sonic) +converts each frame SOMA->SMPL and publishes Protocol v3 to SONIC's smpl encoder. + +Requires GEM-X (https://github.com/NVlabs/GEM-X) installed separately. Point to +its repo root with --gemx-root or the GEMX_ROOT environment variable. + +Examples: + # camera-only sanity check (no robot): live overlay window + python webcam_stream.py --gemx-root /path/to/GEM-X --source 0 --kp-only --show + + # same, headless: write an overlay clip instead + python webcam_stream.py --gemx-root /path/to/GEM-X --source 0 \ + --kp-only --save webcam_test.mp4 --max-frames 60 + + # live teleop -> SONIC + python webcam_stream.py --gemx-root /path/to/GEM-X --source 0 \ + --stream-sonic --window 30 --smooth 0.8 +""" + +# ruff: noqa: E402 +import argparse +from collections import deque +import os +from pathlib import Path +import sys +import time + +os.environ.setdefault("PYOPENGL_PLATFORM", "egl") +os.environ.setdefault("EGL_PLATFORM", "surfaceless") + + +def _add_gemx_to_path() -> str | None: + """Resolve the GEM-X repo root (from --gemx-root or $GEMX_ROOT) and add it to + sys.path so ``gem`` and GEM-X's ``scripts.demo`` modules import.""" + root = os.environ.get("GEMX_ROOT") + argv = sys.argv + for i, a in enumerate(argv): + if a == "--gemx-root" and i + 1 < len(argv): + root = argv[i + 1] + elif a.startswith("--gemx-root="): + root = a.split("=", 1)[1] + if root and root not in sys.path: + sys.path.insert(0, root) + return root + + +GEMX_ROOT = _add_gemx_to_path() + +import cv2 +import torch + +try: + from gem.utils.cam_utils import estimate_K + from gem.utils.geo_transform import compute_cam_angvel, get_bbx_xys_from_xyxy + from gem.utils.pylogger import Log + from scripts.demo.demo_soma_onnx import ( + load_denoiser, + load_vitpose, + run_denoiser_onnx, + run_vitpose_onnx, + ) +except ModuleNotFoundError as e: + raise SystemExit( + "GEM-X not found (%s). Install https://github.com/NVlabs/GEM-X and pass " + "--gemx-root or set GEMX_ROOT to its repo root." % e + ) + +# sibling module (same folder) +sys.path.insert(0, str(Path(__file__).resolve().parent)) + + +def _open_source(source: str, resolution=None, cap_fps=None, fourcc="MJPG"): + """Open a webcam index or video file, optionally requesting a capture mode. + + Cameras silently ignore unsupported settings, so the negotiated mode must be + read back rather than assumed. + """ + is_camera = source.isdigit() + cap = cv2.VideoCapture(int(source)) if is_camera else cv2.VideoCapture(source) + if not cap.isOpened(): + raise RuntimeError( + f"Could not open video source: {source}. For a camera, check the index " + "(ls /dev/video*), that no other process holds the device, and that you " + "are in the 'video' group." + ) + if is_camera: + if fourcc: + cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*fourcc)) + if resolution: + try: + w, h = (int(v) for v in resolution.lower().split("x")) + except ValueError: + raise SystemExit(f"--resolution must look like 1280x720, got {resolution!r}") + cap.set(cv2.CAP_PROP_FRAME_WIDTH, w) + cap.set(cv2.CAP_PROP_FRAME_HEIGHT, h) + if cap_fps: + cap.set(cv2.CAP_PROP_FPS, cap_fps) + return cap + + +class GemWebcamStreamer: + """Rolling-window GEM-X inference over a live frame source.""" + + def __init__(self, gemx_root, window=120, no_imgfeat=True, device="cuda"): + self.gemx_root = gemx_root + self.window = window + self.no_imgfeat = no_imgfeat + self.device = device + + from gem.utils.yolox_detector import YOLOXDetector + + self.yolox = YOLOXDetector(device=device) + + self.vitpose_runner, self.vitpose_backend = load_vitpose() + self.denoiser_runner, self.denoiser_backend = load_denoiser(no_imgfeat=no_imgfeat) + Log.info(f"[webcam] backends: vitpose={self.vitpose_backend}, denoiser={self.denoiser_backend}") + + self._endecoder = None + self._get_body_params_w_Rt_v2 = None + self.buf_kp2d = deque(maxlen=window) + self.buf_bbx = deque(maxlen=window) + self.K = None + + def _ensure_decoder(self, ckpt_path=None): + if self._endecoder is not None: + return + from gem.pipeline.gem_pipeline import get_body_params_w_Rt_v2 + import hydra + from hydra import compose, initialize_config_dir + + cfg_dir = str(Path(self.gemx_root) / "configs") + with initialize_config_dir(version_base="1.3", config_dir=cfg_dir): + cfg = compose( + config_name="demo_soma", + overrides=[ + "exp=gem_soma_regression", + "video_name=stream", + "video_path=stream", + "use_wandb=false", + "task=test", + ], + ) + model = hydra.utils.instantiate(cfg.model, _recursive_=False) + if ckpt_path is None: + from gem.utils.hf_utils import download_checkpoint + + ckpt_path = download_checkpoint() + model.load_pretrained_model(ckpt_path) + model = model.eval().to(self.device) + self._endecoder = model.endecoder + if self._endecoder.obs_indices_dict is None: + self._endecoder.build_obs_indices_dict() + self._get_body_params_w_Rt_v2 = get_body_params_w_Rt_v2 + + def detect_bbox(self, frame_bgr, W, H): + from gem.utils.yolox_detector import detect_and_track + + bbx_xyxy_np, _ = detect_and_track(frame_bgr[None], self.yolox) + bbx_xyxy = torch.from_numpy(bbx_xyxy_np).float() + bbx_xyxy[:, [0, 2]] = bbx_xyxy[:, [0, 2]].clamp(0, W - 1) + bbx_xyxy[:, [1, 3]] = bbx_xyxy[:, [1, 3]].clamp(0, H - 1) + return get_bbx_xys_from_xyxy(bbx_xyxy, base_enlarge=1.2).float()[0] + + @torch.no_grad() + def process_frame(self, frame_rgb, W, H, decode=True): + """Ingest one frame; return (kp2d [77,3], soma_params or None).""" + if self.K is None: + self.K = estimate_K(W, H) + + bbx_xys = self.detect_bbox(frame_rgb, W, H) + vitpose = run_vitpose_onnx(self.vitpose_runner, self.vitpose_backend, frame_rgb[None], bbx_xys[None]) + kp2d = vitpose[0] if isinstance(vitpose, tuple) else vitpose + kp2d = torch.as_tensor(kp2d)[0] # [77,3] + + self.buf_kp2d.append(kp2d) + self.buf_bbx.append(bbx_xys) + if not decode or len(self.buf_kp2d) < 2: + return kp2d, None + + L = len(self.buf_kp2d) + obs = torch.stack(list(self.buf_kp2d)).unsqueeze(0) + bbx = torch.stack(list(self.buf_bbx)).unsqueeze(0) + K = self.K.repeat(L, 1, 1).unsqueeze(0) + f_imgseq = torch.zeros(1, L, 1024) # no-imgfeat + cam_angvel = compute_cam_angvel(torch.eye(3).repeat(L, 1, 1)).unsqueeze(0) # static cam + + batch = {"obs": obs, "bbx_xys": bbx, "K_fullimg": K, "f_imgseq": f_imgseq, "f_cam_angvel": cam_angvel} + pred_x, _pred_cam = run_denoiser_onnx(self.denoiser_runner, self.denoiser_backend, batch) + + self._ensure_decoder() + decode_dict = self._endecoder.decode(pred_x) + + # decode_dict["global_orient"] is camera-frame; fuse to the gravity-aligned + # view so the robot doesn't pitch to match a tilted camera "up". + world_global_orient = decode_dict["global_orient"][0, -1].cpu() # fallback + if "global_orient_gv" in decode_dict and "local_transl_vel" in decode_dict: + gp = self._get_body_params_w_Rt_v2( + global_orient_gv=decode_dict["global_orient_gv"], + local_transl_vel=decode_dict["local_transl_vel"], + global_orient_c=decode_dict["global_orient"], + cam_angvel=cam_angvel.to(pred_x.device), + ) + world_global_orient = gp["global_orient"][0, -1].cpu() + + soma = { + "body_pose": decode_dict["body_pose"][0, -1].cpu(), + "global_orient": world_global_orient, + "identity_coeffs": decode_dict["identity_coeffs"][0, -1].cpu(), + "scale_params": decode_dict["scale_params"][0, -1].cpu(), + } + return kp2d, soma + + +def _draw_overlay(frame_bgr, kp2d, conf_thr=0.4): + for x, y, c in kp2d.numpy(): + if c > conf_thr: + cv2.circle(frame_bgr, (int(x), int(y)), 2, (0, 255, 0), -1) + return frame_bgr + + +def main(): + ap = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) + ap.add_argument("--gemx-root", default=GEMX_ROOT, help="GEM-X repo root (or set $GEMX_ROOT)") + ap.add_argument("--source", default="0", help="Webcam index (e.g. 0) or video file path") + ap.add_argument("--resolution", default=None, help="Request capture resolution for a camera, e.g. 1280x720") + ap.add_argument( + "--cap-fps", + type=float, + default=None, + help="Request capture fps for a camera (see v4l2-ctl --list-formats-ext)", + ) + ap.add_argument("--window", type=int, default=120, help="Rolling window length") + ap.add_argument("--no-imgfeat", action="store_true", help="Skip SAM3DB image features (faster)") + ap.add_argument("--kp-only", action="store_true", help="2D keypoints only (skip denoiser/decode)") + ap.add_argument("--max-frames", type=int, default=0, help="Stop after N frames (0 = run forever)") + ap.add_argument("--show", action="store_true", help="Show cv2 preview window (needs a display)") + ap.add_argument("--save", default=None, help="Optional path to save 2D-overlay preview mp4") + ap.add_argument("--stream-sonic", action="store_true", help="Publish SMPL v3 stream to SONIC") + ap.add_argument("--port", type=int, default=5556, help="ZMQ PUB port for SONIC stream") + ap.add_argument( + "--sonic-root", + default=os.environ.get("SONIC_ROOT"), + help="SONIC repo root, or set $SONIC_ROOT (only needed when run outside the repo)", + ) + ap.add_argument( + "--smooth", type=float, default=0.75, help="Temporal smoothing of streamed SMPL (0=off, 0.6-0.85 smoother)" + ) + args = ap.parse_args() + + if not args.gemx_root: + raise SystemExit("Provide --gemx-root or set GEMX_ROOT.") + + cap = _open_source(args.source, resolution=args.resolution, cap_fps=args.cap_fps) + W = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH)) or 640 + H = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)) or 480 + src_fps = cap.get(cv2.CAP_PROP_FPS) or 30.0 + Log.info(f"[webcam] source={args.source} negotiated {W}x{H} @ ~{src_fps:.1f}fps") + if args.resolution and f"{W}x{H}" != args.resolution.lower(): + Log.info(f"[webcam] note: requested {args.resolution}, camera gave {W}x{H}") + + streamer = GemWebcamStreamer(args.gemx_root, window=args.window, no_imgfeat=args.no_imgfeat or True) + + converter, publisher = None, None + if args.stream_sonic: + from gem.utils.soma_utils.soma_layer import SomaLayer + from soma_to_smpl import SomaToSmpl, SonicV3Publisher + + soma_layer = SomaLayer( + data_root=str(Path(args.gemx_root) / "inputs" / "soma_assets"), + low_lod=True, + device="cuda", + identity_model_type="mhr", + mode="warp", + ) + converter = SomaToSmpl(soma_layer, device="cuda", smooth=args.smooth, sonic_root=args.sonic_root) + publisher = SonicV3Publisher(port=args.port, sonic_root=args.sonic_root) + Log.info(f"[webcam] streaming SMPL v3 to SONIC on tcp://*:{args.port}") + + writer = None + if args.save: + Path(args.save).parent.mkdir(parents=True, exist_ok=True) + writer = cv2.VideoWriter(args.save, cv2.VideoWriter_fourcc(*"mp4v"), src_fps, (W, H)) + + n, t_start, t_last = 0, time.time(), time.time() + try: + while True: + ok, frame_bgr = cap.read() + if not ok: + break + frame_rgb = frame_bgr[..., ::-1].copy() + kp2d, soma = streamer.process_frame(frame_rgb, W, H, decode=not args.kp_only) + + if publisher is not None and soma is not None: + publisher.publish(converter.convert(soma)) + + n += 1 + now = time.time() + inst_fps = 1.0 / max(1e-6, now - t_last) + t_last = now + print( + f"\r[webcam] frame {n} | {inst_fps:5.1f} fps | " f"soma={'yes' if soma else 'warmup/kp-only'}", + end="", + flush=True, + ) + + if writer is not None or args.show: + vis = _draw_overlay(frame_bgr, kp2d) + if writer is not None: + writer.write(vis) + if args.show: + cv2.imshow("GEM webcam", vis) + if cv2.waitKey(1) & 0xFF == ord("q"): + break + if args.max_frames and n >= args.max_frames: + break + except KeyboardInterrupt: + pass + finally: + cap.release() + if publisher is not None: + publisher.close() + if writer is not None: + writer.release() + if args.show: + cv2.destroyAllWindows() + dur = time.time() - t_start + print(f"\n[webcam] processed {n} frames in {dur:.1f}s ({n / max(1e-6, dur):.1f} fps avg)") + + +if __name__ == "__main__": + main() diff --git a/GR00T-WholeBodyControl/gear_sonic/scripts/__pycache__/run_camera_viewer.cpython-310.pyc b/GR00T-WholeBodyControl/gear_sonic/scripts/__pycache__/run_camera_viewer.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..baecfaf61755863fad02617bd17beed965d5bca6 Binary files /dev/null and b/GR00T-WholeBodyControl/gear_sonic/scripts/__pycache__/run_camera_viewer.cpython-310.pyc differ diff --git a/GR00T-WholeBodyControl/gear_sonic/scripts/__pycache__/run_vla_inference.cpython-310.pyc b/GR00T-WholeBodyControl/gear_sonic/scripts/__pycache__/run_vla_inference.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..814c4a9f76b82d4e0918dfb40353f67558ecda9a Binary files /dev/null and b/GR00T-WholeBodyControl/gear_sonic/scripts/__pycache__/run_vla_inference.cpython-310.pyc differ diff --git a/GR00T-WholeBodyControl/gear_sonic/scripts/real_robot_deployment/01_start_sonic.sh b/GR00T-WholeBodyControl/gear_sonic/scripts/real_robot_deployment/01_start_sonic.sh new file mode 100644 index 0000000000000000000000000000000000000000..9fa29419b3ca94cf67cb66255b7dceb428f92646 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/scripts/real_robot_deployment/01_start_sonic.sh @@ -0,0 +1,54 @@ +#!/usr/bin/env bash + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +source "$SCRIPT_DIR/common.sh" + +DEPLOY_DIR="$WBC_REPO_ROOT/gear_sonic_deploy" +SONIC_MODEL_ROOT="${SONIC_MODEL_ROOT:-/mnt/data/model/nvidia/GEAR-SONIC}" +SONIC_VARIANT="${SONIC_VARIANT:-low_latency}" +SONIC_CHECKPOINT="${SONIC_CHECKPOINT:-$SONIC_MODEL_ROOT/$SONIC_VARIANT/model}" +SONIC_OBS_CONFIG="${SONIC_OBS_CONFIG:-$SONIC_MODEL_ROOT/$SONIC_VARIANT/observation_config.yaml}" +SONIC_PLANNER="${SONIC_PLANNER:-$SONIC_MODEL_ROOT/planner_sonic.onnx}" +SONIC_MOTION_DATA="${SONIC_MOTION_DATA:-reference/example/}" +ROBOT_INTERFACE="${ROBOT_INTERFACE:-real}" +SONIC_ZMQ_HOST="${SONIC_ZMQ_HOST:-localhost}" + +resolve_deploy_path() { + local path="$1" + if [[ "$path" = /* ]]; then + printf '%s\n' "$path" + else + printf '%s/%s\n' "$DEPLOY_DIR" "$path" + fi +} + +SONIC_CHECKPOINT_PATH="$(resolve_deploy_path "$SONIC_CHECKPOINT")" +SONIC_OBS_CONFIG_PATH="$(resolve_deploy_path "$SONIC_OBS_CONFIG")" +SONIC_PLANNER_PATH="$(resolve_deploy_path "$SONIC_PLANNER")" +SONIC_MOTION_DATA_PATH="$(resolve_deploy_path "$SONIC_MOTION_DATA")" + +require_file "$DEPLOY_DIR/deploy.sh" +require_file "${SONIC_CHECKPOINT_PATH}_encoder.onnx" +require_file "${SONIC_CHECKPOINT_PATH}_decoder.onnx" +require_file "$SONIC_OBS_CONFIG_PATH" +require_file "$SONIC_PLANNER_PATH" +require_dir "$SONIC_MOTION_DATA_PATH" + +echo "Starting SONIC C++ deploy" +echo " variant: $SONIC_VARIANT" +echo " checkpoint: $SONIC_CHECKPOINT_PATH" +echo " obs config: $SONIC_OBS_CONFIG_PATH" +echo " planner: $SONIC_PLANNER_PATH" +echo " interface: $ROBOT_INTERFACE" +echo " ZMQ host: $SONIC_ZMQ_HOST" + +cd "$DEPLOY_DIR" +exec ./deploy.sh \ + --cp "$SONIC_CHECKPOINT_PATH" \ + --obs-config "$SONIC_OBS_CONFIG_PATH" \ + --planner "$SONIC_PLANNER_PATH" \ + --motion-data "$SONIC_MOTION_DATA_PATH" \ + --input-type zmq_manager \ + --output-type all \ + --zmq-host "$SONIC_ZMQ_HOST" \ + "$ROBOT_INTERFACE" diff --git a/GR00T-WholeBodyControl/gear_sonic/scripts/real_robot_deployment/02_start_policy_server.sh b/GR00T-WholeBodyControl/gear_sonic/scripts/real_robot_deployment/02_start_policy_server.sh new file mode 100644 index 0000000000000000000000000000000000000000..1a2df6d588ac6c6f0255a6ff82912ae4fb5ccd35 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/scripts/real_robot_deployment/02_start_policy_server.sh @@ -0,0 +1,49 @@ +#!/usr/bin/env bash + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +source "$SCRIPT_DIR/common.sh" + +GR00T_ROOT="${GR00T_ROOT:-/mnt/data/code/Isaac-GR00T}" +MODEL_PATH="${MODEL_PATH:-/mnt/data/model/groot_ckpt_ns/checkpoint-20000}" +EMBODIMENT_TAG="${EMBODIMENT_TAG:-UNITREE_G1_SONIC}" +DEVICE="${DEVICE:-cuda:0}" +POLICY_BIND_HOST="${POLICY_BIND_HOST:-0.0.0.0}" +POLICY_PORT="${POLICY_PORT:-5550}" +GR00T_PYTHON="${GR00T_PYTHON:-}" + +require_dir "$GR00T_ROOT" +require_file "$GR00T_ROOT/gr00t/eval/run_gr00t_server.py" +require_dir "$MODEL_PATH" + +python3 "$SCRIPT_DIR/check_checkpoint.py" \ + --checkpoint "$MODEL_PATH" \ + --embodiment unitree_g1_sonic + +server_args=( + "$GR00T_ROOT/gr00t/eval/run_gr00t_server.py" + --model-path "$MODEL_PATH" + --embodiment-tag "$EMBODIMENT_TAG" + --device "$DEVICE" + --host "$POLICY_BIND_HOST" + --port "$POLICY_PORT" +) + +echo "Starting the official standard GR00T PolicyServer" +echo " GR00T root: $GR00T_ROOT" +echo " checkpoint: $MODEL_PATH" +echo " embodiment: $EMBODIMENT_TAG" +echo " endpoint: $POLICY_BIND_HOST:$POLICY_PORT" +echo " RTC: disabled (official Gr00tPolicy path)" + +if python_bin="$(resolve_python "$GR00T_PYTHON" "$GR00T_ROOT/.venv/bin/python")"; then + export PYTHONPATH="$GR00T_ROOT${PYTHONPATH:+:$PYTHONPATH}" + exec "$python_bin" "${server_args[@]}" +fi + +if command -v uv >/dev/null 2>&1; then + cd "$GR00T_ROOT" + exec uv run python "${server_args[@]}" +fi + +die "no GR00T runtime found. Set GR00T_PYTHON or run 'uv sync --all-extras' in $GR00T_ROOT" + diff --git a/GR00T-WholeBodyControl/gear_sonic/scripts/real_robot_deployment/03_start_camera_server.sh b/GR00T-WholeBodyControl/gear_sonic/scripts/real_robot_deployment/03_start_camera_server.sh new file mode 100644 index 0000000000000000000000000000000000000000..f9ee54759122a5b6ca96814d26e7afeccfc31aef --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/scripts/real_robot_deployment/03_start_camera_server.sh @@ -0,0 +1,51 @@ +#!/usr/bin/env bash + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +source "$SCRIPT_DIR/common.sh" + +CAMERA_PYTHON="${CAMERA_PYTHON:-}" +EGO_CAMERA="${EGO_CAMERA:-oak}" +LEFT_WRIST_CAMERA="${LEFT_WRIST_CAMERA:-oak}" +RIGHT_WRIST_CAMERA="${RIGHT_WRIST_CAMERA:-oak}" +EGO_CAMERA_ID="${EGO_CAMERA_ID:-}" +LEFT_WRIST_CAMERA_ID="${LEFT_WRIST_CAMERA_ID:-}" +RIGHT_WRIST_CAMERA_ID="${RIGHT_WRIST_CAMERA_ID:-}" +CAMERA_PORT="${CAMERA_PORT:-5555}" +CAMERA_FPS="${CAMERA_FPS:-30}" +CAMERA_QUEUE_SIZE="${CAMERA_QUEUE_SIZE:-1}" +CAMERA_USE_MJPEG="${CAMERA_USE_MJPEG:-0}" +CAMERA_MJPEG_QUALITY="${CAMERA_MJPEG_QUALITY:-80}" + +python_bin="$( + resolve_python \ + "$CAMERA_PYTHON" \ + "$WBC_REPO_ROOT/.venv_camera/bin/python" \ + "$WBC_REPO_ROOT/.venv_data_collection/bin/python" +)" || die "camera Python not found; set CAMERA_PYTHON or install .venv_camera" + +args=( + -m gear_sonic.camera.composed_camera + --ego-view-camera "$EGO_CAMERA" + --left-wrist-camera "$LEFT_WRIST_CAMERA" + --right-wrist-camera "$RIGHT_WRIST_CAMERA" + --fps "$CAMERA_FPS" + --port "$CAMERA_PORT" + --queue-size "$CAMERA_QUEUE_SIZE" +) + +[[ -z "$EGO_CAMERA_ID" ]] || args+=(--ego-view-device-id "$EGO_CAMERA_ID") +[[ -z "$LEFT_WRIST_CAMERA_ID" ]] || args+=(--left-wrist-device-id "$LEFT_WRIST_CAMERA_ID") +[[ -z "$RIGHT_WRIST_CAMERA_ID" ]] || args+=(--right-wrist-device-id "$RIGHT_WRIST_CAMERA_ID") +if [[ "$CAMERA_USE_MJPEG" == "1" ]]; then + args+=(--use-mjpeg --mjpeg-quality "$CAMERA_MJPEG_QUALITY") +fi + +echo "Starting three-view camera server on port $CAMERA_PORT" +echo " ego_view: $EGO_CAMERA ${EGO_CAMERA_ID:-}" +echo " left_wrist: $LEFT_WRIST_CAMERA ${LEFT_WRIST_CAMERA_ID:-}" +echo " right_wrist:$RIGHT_WRIST_CAMERA ${RIGHT_WRIST_CAMERA_ID:-}" +echo "The three views are published as independent keys; no image mosaic is created." + +cd "$WBC_REPO_ROOT" +exec "$python_bin" "${args[@]}" + diff --git a/GR00T-WholeBodyControl/gear_sonic/scripts/real_robot_deployment/04_run_inference.sh b/GR00T-WholeBodyControl/gear_sonic/scripts/real_robot_deployment/04_run_inference.sh new file mode 100644 index 0000000000000000000000000000000000000000..fbf754ddf5d3d20182121e63ed65dc62fb152dbd --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/scripts/real_robot_deployment/04_run_inference.sh @@ -0,0 +1,56 @@ +#!/usr/bin/env bash + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +source "$SCRIPT_DIR/common.sh" + +INFERENCE_PYTHON="${INFERENCE_PYTHON:-}" +ROBOT_HOST="${ROBOT_HOST:-192.168.123.164}" +POLICY_HOST="${POLICY_HOST:-localhost}" +POLICY_PORT="${POLICY_PORT:-5550}" +CAMERA_HOST="${CAMERA_HOST:-$ROBOT_HOST}" +CAMERA_PORT="${CAMERA_PORT:-5555}" +STATE_ZMQ_HOST="${STATE_ZMQ_HOST:-localhost}" +HAND_STATE_HOST="${HAND_STATE_HOST:-$ROBOT_HOST}" +HEAD_STATE_HOST="${HEAD_STATE_HOST:-$ROBOT_HOST}" +OUTPUT_BIND_HOST="${OUTPUT_BIND_HOST:-0.0.0.0}" +PROMPT="${PROMPT:-Pick up the bottle and put it in the box}" +DRY_RUN="${DRY_RUN:-0}" +DEBUG="${DEBUG:-0}" + +python_bin="$( + resolve_python \ + "$INFERENCE_PYTHON" \ + "$WBC_REPO_ROOT/.venv_inference/bin/python" +)" || die "inference Python not found; set INFERENCE_PYTHON or run install_scripts/install_inference.sh" + +args=( + "$WBC_REPO_ROOT/gear_sonic/scripts/run_vla_inference_dex1_head.py" + --host "$POLICY_HOST" + --port "$POLICY_PORT" + --embodiment-tag unitree_g1_sonic + --prompt "$PROMPT" + --camera-host "$CAMERA_HOST" + --camera-port "$CAMERA_PORT" + --state-zmq-host "$STATE_ZMQ_HOST" + --hand-state-host "$HAND_STATE_HOST" + --head-state-host "$HEAD_STATE_HOST" + --output-bind-host "$OUTPUT_BIND_HOST" + --action-horizon 40 + --action-publish-rate 50 + --rate 2.5 +) +[[ "$DRY_RUN" == "1" ]] && args+=(--dry-run) +[[ "$DEBUG" == "1" ]] && args+=(--debug) + +echo "Starting standard GR00T + SONIC real-robot inference" +echo " policy: $POLICY_HOST:$POLICY_PORT" +echo " camera: $CAMERA_HOST:$CAMERA_PORT" +echo " robot: $ROBOT_HOST" +echo " prompt: $PROMPT" +echo " RTC: disabled" +echo " input: ego_view, left_wrist, right_wrist (separate)" +echo "Enter commands in this terminal: k -> i -> p" + +cd "$WBC_REPO_ROOT" +exec "$python_bin" "${args[@]}" + diff --git a/GR00T-WholeBodyControl/gear_sonic/scripts/real_robot_deployment/ENVIRONMENT_SETUP.md b/GR00T-WholeBodyControl/gear_sonic/scripts/real_robot_deployment/ENVIRONMENT_SETUP.md new file mode 100644 index 0000000000000000000000000000000000000000..b8c9961a4bb017febdc7acc9d04c150d9b0e6411 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/scripts/real_robot_deployment/ENVIRONMENT_SETUP.md @@ -0,0 +1,398 @@ +# GR00T N1.7 + SONIC 真机部署环境配置 + +本文只负责环境安装与验证。四阶段启动命令、端口和真机操作顺序见同目录的 +[`README.md`](README.md)。 + +## 1. 环境拆分 + +四个阶段不是使用同一个 uv virtual environment： + +| 阶段 | 运行机器 | Python/系统环境 | +| --- | --- | --- | +| 1. SONIC C++ deploy | 与 G1 同局域网的控制机 | TensorRT、ROS/Unitree SDK、C++；不使用 uv Python | +| 2. GR00T PolicyServer | GPU server `Humanoid_pretrain` | `/mnt/data/code/Isaac-GR00T/.venv`，Python 3.12 | +| 3. 三相机 server | 机器人计算单元 | `GR00T-WholeBodyControl/.venv_camera`，Python 3.10 即可 | +| 4. VLA inference client | 与 G1 同局域网的控制机 | `GR00T-WholeBodyControl/.venv_inference`，Python 3.12 | + +截至 2026-08-15，`Humanoid_pretrain` 上只有 `.venv_sim`，没有上述 GR00T、 +inference 或 camera 环境；系统 `/usr/bin/python3` 也没有安装 Torch。 + +## 2. 重要的磁盘与版本约束 + +### 2.1 不要把 uv cache 放在服务器系统盘 + +当前 `Humanoid_pretrain` 的 `/` 只有 100 GB，检查时只剩约 16 GB，且 +`/root/.cache` 已占约 59 GB。Isaac-GR00T 的 CUDA Torch、TensorRT 和相关 wheel +较大，应把 uv cache 放到 `/mnt/data`： + +```bash +mkdir -p /mnt/data/cache/uv +export UV_CACHE_DIR=/mnt/data/cache/uv +``` + +建议把下面一行手工加入 GPU server 的 `~/.zshrc`： + +```bash +export UV_CACHE_DIR=/mnt/data/cache/uv +``` + +不要在确认内容和用途前删除 `/root/.cache`。 + +### 2.2 Python 版本不能混用 + +- 当前 `/mnt/data/code/Isaac-GR00T/pyproject.toml` 要求 Python `>=3.12,<3.13`。 +- `GR00T-WholeBodyControl/install_scripts/install_inference.sh` 当前固定创建 Python + 3.10 环境，并从 GitHub 安装最新 Isaac-GR00T；这两个约束不兼容。 +- 因此 inference client 按本文手工创建 Python 3.12 环境，不直接运行未修改的 + `install_inference.sh`。 +- camera server 不导入 Isaac-GR00T，可以继续使用官方安装脚本创建的 Python 3.10 + `.venv_camera`。 + +## 3. GPU server：配置 GR00T PolicyServer 环境 + +以下命令在 `Humanoid_pretrain` 上执行。 + +### 3.1 安装 uv + +```bash +ssh Humanoid_pretrain + +mkdir -p /mnt/data/cache/uv +export UV_CACHE_DIR=/mnt/data/cache/uv +export PATH="$HOME/.local/bin:$PATH" + +curl -LsSf https://astral.sh/uv/install.sh | sh +source "$HOME/.local/bin/env" +uv --version +``` + +重新登录后如果提示 `uv: command not found`，再次执行： + +```bash +source "$HOME/.local/bin/env" +``` + +### 3.2 根据 lockfile 创建 Isaac-GR00T 环境 + +```bash +cd /mnt/data/code/Isaac-GR00T +export UV_CACHE_DIR=/mnt/data/cache/uv + +uv sync --locked +``` + +环境应生成在： + +```text +/mnt/data/code/Isaac-GR00T/.venv +``` + +这里不需要 `--all-extras`；PolicyServer 使用项目基础依赖即可。`--locked` 可以避免 +安装时静默修改 `uv.lock` 或解析到另一套版本。 + +### 3.3 验证 Python、Torch、CUDA 和 GR00T + +```bash +/mnt/data/code/Isaac-GR00T/.venv/bin/python -c \ + 'import sys, torch, gr00t, zmq; print(sys.version); print(torch.__version__); print(torch.cuda.is_available()); print(gr00t.__file__)' + +nvidia-smi +``` + +期望： + +- Python 为 3.12； +- `torch.cuda.is_available()` 为 `True`； +- `gr00t.__file__` 指向 `/mnt/data/code/Isaac-GR00T/gr00t/`； +- `nvidia-smi` 能看到 H100。 + +### 3.4 检查 checkpoint 上传完整性 + +```bash +python3 \ + /mnt/data/code/GR00T-WholeBodyControl/gear_sonic/scripts/real_robot_deployment/check_checkpoint.py \ + --checkpoint /mnt/data/model/groot_ckpt_ns/checkpoint-20000 \ + --embodiment unitree_g1_sonic +``` + +只有出现 `Checkpoint preflight passed` 才能启动 PolicyServer。该检查会解析 +safetensors header 并核对每个 shard 的最终长度；仅看到文件名并不代表上传完成。 + +### 3.5 启动前的无模型导入检查 + +```bash +cd /mnt/data/code/Isaac-GR00T +.venv/bin/python gr00t/eval/run_gr00t_server.py --help +``` + +### 3.6 启动 PolicyServer + +```bash +cd /mnt/data/code/GR00T-WholeBodyControl + +GR00T_PYTHON=/mnt/data/code/Isaac-GR00T/.venv/bin/python \ +MODEL_PATH=/mnt/data/model/groot_ckpt_ns/checkpoint-20000 \ +DEVICE=cuda:0 \ +bash gear_sonic/scripts/real_robot_deployment/02_start_policy_server.sh +``` + +`GR00T_PYTHON` 可以省略；`02_start_policy_server.sh` 默认就会查找上述路径。显式写出 +有助于排查环境串用问题。 + +## 4. 控制机：配置 VLA inference client 环境 + +控制机需要同时能访问： + +- GR00T PolicyServer 的 5550； +- 机器人相机的 5555； +- 机器人 Dex1/head state 的 5559/5561； +- 本机 SONIC state 的 5557。 + +推荐让控制机拥有与 GPU server 同一 commit 的两个仓库： + +```text +/path/to/Isaac-GR00T +/path/to/GR00T-WholeBodyControl +``` + +先在 GPU server 和控制机分别运行以下命令，确认 Isaac-GR00T commit 一致： + +```bash +git -C /path/to/Isaac-GR00T rev-parse HEAD +``` + +### 4.1 安装 uv 并选择持久化 cache + +```bash +export PATH="$HOME/.local/bin:$PATH" +curl -LsSf https://astral.sh/uv/install.sh | sh +source "$HOME/.local/bin/env" + +mkdir -p /path/to/persistent-cache/uv +export UV_CACHE_DIR=/path/to/persistent-cache/uv +uv python install 3.12 +``` + +将 `/path/to/persistent-cache` 替换为控制机上空间充足的持久化磁盘。 + +### 4.2 用 Isaac-GR00T lockfile 创建 `.venv_inference` + +```bash +export WBC_ROOT=/path/to/GR00T-WholeBodyControl +export GR00T_ROOT=/path/to/Isaac-GR00T +export UV_PROJECT_ENVIRONMENT="$WBC_ROOT/.venv_inference" + +uv sync --project "$GR00T_ROOT" --locked +unset UV_PROJECT_ENVIRONMENT +``` + +然后把本地 `gear_sonic` 和 Pinocchio 安装到该环境。这里故意不使用 +`gear_sonic[inference]`，避免它再次从 GitHub 拉取另一份未固定 commit 的 +Isaac-GR00T： + +```bash +uv pip install \ + --python "$WBC_ROOT/.venv_inference/bin/python" \ + -e "$WBC_ROOT/gear_sonic" + +uv pip install \ + --python "$WBC_ROOT/.venv_inference/bin/python" \ + pin +``` + +### 4.3 验证 inference 环境 + +```bash +"$WBC_ROOT/.venv_inference/bin/python" -c \ + 'import sys, gr00t, gear_sonic, pinocchio, zmq, msgpack, cv2; print(sys.version); print(gr00t.__file__); print(gear_sonic.__file__)' + +cd "$WBC_ROOT" +.venv_inference/bin/python \ + gear_sonic/scripts/run_vla_inference_dex1_head.py --help +``` + +期望 Python 为 3.12，且 `gr00t` 与 `gear_sonic` 都指向控制机本地仓库。 + +### 4.4 如果控制机和 GPU server 是同一台机器 + +仅在该机器也能直连 G1 局域网时，可以复用 Isaac-GR00T `.venv`： + +```bash +uv pip install \ + --python /mnt/data/code/Isaac-GR00T/.venv/bin/python \ + -e /mnt/data/code/GR00T-WholeBodyControl/gear_sonic + +uv pip install \ + --python /mnt/data/code/Isaac-GR00T/.venv/bin/python \ + pin + +INFERENCE_PYTHON=/mnt/data/code/Isaac-GR00T/.venv/bin/python \ +bash /mnt/data/code/GR00T-WholeBodyControl/gear_sonic/scripts/real_robot_deployment/04_run_inference.sh +``` + +通常 `Humanoid_pretrain` 是云端 GPU server，不能直连 G1，所以实际部署仍应在机器人 +控制机创建独立 `.venv_inference`。 + +## 5. 机器人计算单元：配置三相机环境 + +以下命令在连接三台物理相机的机器人计算单元上执行： + +```bash +cd /path/to/GR00T-WholeBodyControl +bash install_scripts/install_camera_server.sh +``` + +该脚本会： + +1. 安装 uv； +2. 安装 uv-managed Python 3.10； +3. 创建 `.venv_camera`； +4. 安装 `gear_sonic[camera]` 和 OAK `depthai`； +5. 询问是否创建 systemd service。 + +如果机器人磁盘空间紧张，也应提前设置 cache： + +```bash +mkdir -p /path/to/persistent-cache/uv +export UV_CACHE_DIR=/path/to/persistent-cache/uv +``` + +### 5.1 验证相机 SDK 和设备 ID + +```bash +cd /path/to/GR00T-WholeBodyControl +.venv_camera/bin/python -c \ + 'import depthai as dai; print(dai.Device.getAllAvailableDevices())' +``` + +记录 ego、左腕、右腕三个 MxID，随后按部署 README 的阶段 3 命令启动。必须确保输出键 +分别为： + +```text +ego_view +left_wrist +right_wrist +``` + +### 5.2 systemd 与手动脚本二选一 + +- 长期部署：在安装脚本中选择安装 systemd，并填入三台相机的 type/MxID。 +- 临时调试：不安装 systemd，运行 `03_start_camera_server.sh`。 +- 不要同时运行二者，否则都会尝试绑定 5555。 + +systemd 验证： + +```bash +sudo systemctl status composed_camera_server.service +journalctl -u composed_camera_server.service -f +``` + +## 6. 控制机：SONIC C++ 环境 + +SONIC 不使用 uv。需要按仓库主 README 准备： + +- TensorRT； +- ROS 2 / Unitree SDK； +- `just`、CMake、Clang； +- SONIC Low Latency encoder/decoder 和与之配套的 observation config； +- 连接 G1 `192.168.123.x` 网段的网卡。 + +基本检查： + +```bash +cd /path/to/GR00T-WholeBodyControl/gear_sonic_deploy + +echo "$TensorRT_ROOT" +command -v just +command -v cmake +command -v clang +./deploy.sh --help +``` + +默认 Low Latency 模型检查： + +```bash +test -f /mnt/data/model/nvidia/GEAR-SONIC/low_latency/model_encoder.onnx +test -f /mnt/data/model/nvidia/GEAR-SONIC/low_latency/model_decoder.onnx +test -f /mnt/data/model/nvidia/GEAR-SONIC/low_latency/observation_config.yaml +test -f /mnt/data/model/nvidia/GEAR-SONIC/planner_sonic.onnx +``` + +`01_start_sonic.sh` 默认使用上述目录。若控制机上的模型位置不同，设置 +`SONIC_MODEL_ROOT=/path/to/GEAR-SONIC`；encoder、decoder 和 observation config +必须始终来自同一个 Low Latency checkpoint，不能只替换其中一个文件。 + +## 7. PolicyServer 网络连接 + +如果控制机可以直接访问 GPU server 的 5550，阶段 4 设置 GPU server 的可达 IP： + +```bash +POLICY_HOST=GPU_SERVER_IP \ +bash gear_sonic/scripts/real_robot_deployment/04_run_inference.sh +``` + +如果平台没有暴露 5550，在控制机建立 SSH tunnel： + +```bash +ssh -N -L 5550:127.0.0.1:5550 Humanoid_pretrain +``` + +保持该终端运行，在另一个终端使用： + +```bash +POLICY_HOST=127.0.0.1 \ +bash gear_sonic/scripts/real_robot_deployment/04_run_inference.sh +``` + +## 8. 完整环境验收清单 + +### GPU server + +```bash +test -x /mnt/data/code/Isaac-GR00T/.venv/bin/python +/mnt/data/code/Isaac-GR00T/.venv/bin/python -c \ + 'import torch, gr00t, zmq; assert torch.cuda.is_available(); print("GPU server env OK")' +``` + +### 控制机 inference + +```bash +test -x /path/to/GR00T-WholeBodyControl/.venv_inference/bin/python +/path/to/GR00T-WholeBodyControl/.venv_inference/bin/python -c \ + 'import gr00t, gear_sonic, pinocchio, zmq, msgpack, cv2; print("inference env OK")' +``` + +### 机器人相机 + +```bash +test -x /path/to/GR00T-WholeBodyControl/.venv_camera/bin/python +/path/to/GR00T-WholeBodyControl/.venv_camera/bin/python -c \ + 'import depthai, cv2, zmq, msgpack; print("camera env OK")' +``` + +### 部署顺序 + +环境全部通过后，再依次运行： + +```text +01_start_sonic.sh +02_start_policy_server.sh +03_start_camera_server.sh（或 systemd camera service） +04_run_inference.sh +``` + +真机第一次执行必须先使用 `DRY_RUN=1 DEBUG=1`，确认三路 video、36D state 和 +68D action schema 后，再进入真实 `k -> i -> p` 流程。 + +## 9. 常见环境错误 + +- `uv: command not found`：执行 `source "$HOME/.local/bin/env"`，并检查 + `$HOME/.local/bin` 是否在 `PATH`。 +- `No module named torch`：正在使用系统 Python，而不是 Isaac-GR00T `.venv`。 +- `Package requires Python >=3.12`：误用了旧 `install_inference.sh` 创建的 Python + 3.10 环境。 +- `No module named pinocchio`：在 inference Python 中安装 `pin`。 +- 根分区空间不足：确认 `UV_CACHE_DIR` 指向大容量持久化磁盘。 +- `Checkpoint preflight failed`：checkpoint 仍在上传或 schema 与本客户端不匹配。 +- `Address already in use`：camera systemd 与手动脚本同时运行，或旧的 ZMQ 进程未退出。 +- `State key ... missing`：使用了错误的 checkpoint/embodiment，或机器人状态服务未启动。 diff --git a/GR00T-WholeBodyControl/gear_sonic/scripts/real_robot_deployment/README.md b/GR00T-WholeBodyControl/gear_sonic/scripts/real_robot_deployment/README.md new file mode 100644 index 0000000000000000000000000000000000000000..743122f7a1d37c6373c7101f19b717143ae5d797 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/scripts/real_robot_deployment/README.md @@ -0,0 +1,215 @@ +# GR00T N1.7 + SONIC Low Latency + Dex1 + 舵机头真机部署 + +本目录对应四个部署阶段：启动 SONIC、启动标准 GR00T PolicyServer、启动三相机服务、运行真机测试客户端。实现以 NVIDIA 的 [VLA Inference 教程](https://nvlabs.github.io/GR00T-WholeBodyControl/tutorials/vla_inference.html)和已跑通的 Dex1/舵机头 ZMQ 协议为基础，但严格匹配本次 checkpoint，而不是复制 RTC/拼图版本。 + +首次配置机器时，请先按照 [`ENVIRONMENT_SETUP.md`](ENVIRONMENT_SETUP.md) 创建并验证 +GPU server、控制机和机器人相机端各自的运行环境，再执行本文的四阶段启动流程。 + +## 本次 checkpoint 的实际接口 + +脚本会直接检查 `/mnt/data/model/groot_ckpt_ns/checkpoint-20000` 内的 `processor_config.json`、`statistics.json` 和 safetensors shards。当前接口是： + +| 模态 | 键与维度 | +| --- | --- | +| Video | `ego_view`、`left_wrist`、`right_wrist`，三个独立的 `[1,1,H,W,3]` tensor | +| State | body 29D + 左右 Dex1 标量 2D + 头部 yaw/pitch 2D + projected gravity 3D = 36D | +| Action | motion token 64D + 左右 Dex1 标量 2D + 头部 yaw/pitch 2D = 68D | +| Horizon | 40 steps | +| Embodiment | `UNITREE_G1_SONIC` / `unitree_g1_sonic` | + +这不是 64+7+7+2 的 Head80 模型。参考目录中的 Head80 客户端与当前 checkpoint 不匹配，不能直接使用。 + +本实现没有 RTC：PolicyServer 使用 `/mnt/data/code/Isaac-GR00T/gr00t/eval/run_gr00t_server.py` 的标准 `Gr00tPolicy`，客户端只调用 `PolicyClient.get_action(observation)`，不会发送 previous chunk 或 RTC options。客户端保留官方的异步推理和 latency-compensated start index；延迟补偿只是跳过已经过时的 action index，不是 RTC sampling。 + +## 机器和端口 + +推荐拓扑： + +| 机器 | 组件 | +| --- | --- | +| GPU server（`Humanoid_pretrain`） | GR00T PolicyServer | +| 与 G1 同一局域网的控制机 | SONIC C++ deploy、VLA inference client | +| G1/机器人计算单元 | 三相机 server、现有 Dex1 服务、现有舵机头服务 | + +| 端口 | 方向 | 用途 | +| ---: | --- | --- | +| 5550 | client → GPU server | GR00T PolicyServer | +| 5555 | client ← robot | 三路相机 | +| 5556 | client → SONIC | motion token 与 control command | +| 5557 | client ← SONIC | `g1_debug` robot state | +| 5558 | client → robot | Dex1 command | +| 5559 | client ← robot | Dex1 feedback | +| 5560 | client → robot | 头部 command | +| 5561 | client ← robot | 头部 feedback | + +确保机器人侧已有同事代码对应的 Dex1/head 服务，并让它们连接控制机的 5558/5560，同时发布 5559/5561。相机、Dex1、head 的默认机器人 IP 是 `192.168.123.164`，可通过环境变量覆盖。 + +## 一次性环境准备 + +GPU server： + +```bash +ssh Humanoid_pretrain +export UV_CACHE_DIR=/mnt/data/cache/uv +cd /mnt/data/code/Isaac-GR00T +uv sync --locked +``` + +若不是用 `uv`，准备一个可以 `import torch, gr00t, tyro, zmq` 的 Python，并在阶段 2 设置 `GR00T_PYTHON=/path/to/python`。 + +控制机的 VLA client 必须使用 Python 3.12。当前 `install_inference.sh` 固定创建 +Python 3.10，与最新版 Isaac-GR00T 不兼容；请按 +[`ENVIRONMENT_SETUP.md`](ENVIRONMENT_SETUP.md) 的“控制机”章节手工创建 +`.venv_inference`。 + +机器人相机端： + +```bash +cd /path/to/GR00T-WholeBodyControl +bash install_scripts/install_camera_server.sh +``` + +该安装脚本会同时启用 systemd camera service。如果阶段 3 选择直接运行 +`03_start_camera_server.sh`，先停止占用 5555 的默认 service；长期部署则把阶段 3 +的三个 camera 参数写入 systemd service，二者只运行一个。 + +`gear_sonic_deploy` 仍需按主仓库 README 安装 TensorRT、ROS/Unitree 依赖。`01_start_sonic.sh` 会调用仓库现有的 `deploy.sh`，后者负责 build 和真机确认。 + +## 阶段 1：启动 SONIC（控制机） + +本项目使用 SONIC **Low Latency** 版本。脚本默认从 +`/mnt/data/model/nvidia/GEAR-SONIC/low_latency/` 读取成套的 encoder、decoder 和 +observation config，并从 `/mnt/data/model/nvidia/GEAR-SONIC/planner_sonic.onnx` +读取 planner。先检查文件： + +```bash +test -f /mnt/data/model/nvidia/GEAR-SONIC/low_latency/model_encoder.onnx +test -f /mnt/data/model/nvidia/GEAR-SONIC/low_latency/model_decoder.onnx +test -f /mnt/data/model/nvidia/GEAR-SONIC/low_latency/observation_config.yaml +test -f /mnt/data/model/nvidia/GEAR-SONIC/planner_sonic.onnx +``` + +然后启动： + +```bash +cd /path/to/GR00T-WholeBodyControl +bash gear_sonic/scripts/real_robot_deployment/01_start_sonic.sh +``` + +如果阶段 1 所在机器的模型根目录不同，只需覆盖根目录： + +```bash +SONIC_MODEL_ROOT=/path/to/GEAR-SONIC \ +bash gear_sonic/scripts/real_robot_deployment/01_start_sonic.sh +``` + +也可以用 `SONIC_CHECKPOINT`、`SONIC_OBS_CONFIG` 和 `SONIC_PLANNER` 分别覆盖， +但 encoder、decoder 与 observation config 必须来自同一个 Low Latency checkpoint， +不能与默认 release 或 SONIC v1.1 混用。若阶段 1 不在 `Humanoid_pretrain` 上运行， +先把 `GEAR-SONIC` 模型目录复制到控制机，再设置 `SONIC_MODEL_ROOT`。 + +## 阶段 2：启动模型服务（Humanoid_pretrain） + +```bash +ssh Humanoid_pretrain +cd /mnt/data/code/GR00T-WholeBodyControl +bash gear_sonic/scripts/real_robot_deployment/02_start_policy_server.sh +``` + +常用覆盖项： + +```bash +GR00T_PYTHON=/path/to/isaac-groot-python \ +MODEL_PATH=/mnt/data/model/groot_ckpt_ns/checkpoint-20000 \ +DEVICE=cuda:0 POLICY_PORT=5550 \ +bash gear_sonic/scripts/real_robot_deployment/02_start_policy_server.sh +``` + +启动前的 checkpoint preflight 会： + +- 检查 3 个独立视频键、36D state、68D action、40-step horizon； +- 根据 `model.safetensors.index.json` 找齐所有 shard； +- 解析每个 safetensors header，并核对文件长度，避免对半上传文件启动模型。 + +如果 GPU server 的 5550 不能被控制机直接访问，在控制机另开终端建立 SSH tunnel： + +```bash +ssh -N -L 5550:127.0.0.1:5550 Humanoid_pretrain +``` + +此时阶段 4 使用 `POLICY_HOST=127.0.0.1 POLICY_PORT=5550`。 + +## 阶段 3：启动三相机服务（机器人） + +先查出三台相机的设备 ID。OAK 可用： + +```bash +/path/to/.venv_camera/bin/python -c \ + "import depthai as dai; print(dai.Device.getAllAvailableDevices())" +``` + +然后显式指定 ego、左腕、右腕，避免多 OAK 设备枚举顺序变化： + +```bash +cd /path/to/GR00T-WholeBodyControl +EGO_CAMERA=oak EGO_CAMERA_ID=REPLACE_WITH_EGO_MXID \ +LEFT_WRIST_CAMERA=oak LEFT_WRIST_CAMERA_ID=REPLACE_WITH_LEFT_MXID \ +RIGHT_WRIST_CAMERA=oak RIGHT_WRIST_CAMERA_ID=REPLACE_WITH_RIGHT_MXID \ +bash gear_sonic/scripts/real_robot_deployment/03_start_camera_server.sh +``` + +相机可以分别覆盖成 `realsense`、`usb` 或视频文件。脚本发布的键固定为 `ego_view`、`left_wrist`、`right_wrist`，不会拼图。 + +在控制机验证三路输入： + +```bash +cd /path/to/GR00T-WholeBodyControl +source .venv_inference/bin/activate +python gear_sonic/scripts/run_camera_viewer.py \ + --camera-host 192.168.123.164 --camera-port 5555 +``` + +必须看到三个独立 stream 名称。少一路时客户端会拒绝推理，不会静默用拼图或复制视角。 + +## 阶段 4：执行真机测试（控制机） + +先做无输出检查；这仍需要阶段 1 提供 robot state、阶段 2 提供 policy、阶段 3 和 Dex1/head 服务提供传感器反馈： + +```bash +cd /path/to/GR00T-WholeBodyControl +ROBOT_HOST=192.168.123.164 \ +POLICY_HOST=127.0.0.1 \ +PROMPT="Pick up the bottle and put it in the box" \ +DRY_RUN=1 DEBUG=1 \ +bash gear_sonic/scripts/real_robot_deployment/04_run_inference.sh +``` + +确认日志中的 video/state/action shapes 后，退出并去掉 `DRY_RUN=1`： + +```bash +ROBOT_HOST=192.168.123.164 \ +POLICY_HOST=127.0.0.1 \ +PROMPT="Pick up the bottle and put it in the box" \ +bash gear_sonic/scripts/real_robot_deployment/04_run_inference.sh +``` + +客户端默认直接从当前终端读命令，安全顺序为： + +1. `k`：启动 SONIC control loop，进入 PLANNER mode。 +2. 等机器人站稳后输入 `i`：发送初始 motion token，保持当前 Dex1/head 实测位置，切到 POSE mode；policy 仍暂停。 +3. 检查环境和急停人员后输入 `p`：请求新 action chunk 并开始执行。 +4. 需要暂停时先输入 `p`，结束时再输入 `k` 停止 SONIC。 +5. `t new instruction`：更换 prompt，同时清掉旧 chunk 并暂停，确认后再输入 `p`。 + +Ctrl-C 会停止客户端，并在退出前尝试发送一次 SONIC stop command。物理急停仍应始终可用，不能只依赖软件键盘。 + +## 常见问题 + +- `checkpoint does not contain embodiment new_embodiment`：本 checkpoint 应使用 `UNITREE_G1_SONIC`，不要使用参考脚本的 `new_embodiment` 默认值。 +- `left_hand_joints must have shape [T, 1]`：当前模型输出是单标量 Dex1，不是参考 Head80 的 7D 手部输出。 +- `State key head_joints must be in observation`：必须运行头部 feedback 服务；头部 state 是训练输入，不能只做安全检查后丢弃。 +- 缺少 `left_wrist` 或 `right_wrist`：检查阶段 3 的设备 ID 和相机日志；不要改成 mosaic。 +- `Address already in use`：检查 5550/5555/5556/5558/5560 是否有旧进程占用。 +- policy timeout：优先确认 SSH tunnel/防火墙和 server `ping`，再检查 GPU 推理耗时。 +- `no GR00T runtime found`：服务器当前只有源码不等于依赖已安装；按环境文档完成 + `uv sync --locked`，或设置 `GR00T_PYTHON`。 diff --git a/GR00T-WholeBodyControl/gear_sonic/scripts/real_robot_deployment/check_checkpoint.py b/GR00T-WholeBodyControl/gear_sonic/scripts/real_robot_deployment/check_checkpoint.py new file mode 100644 index 0000000000000000000000000000000000000000..4bd1af8e094c970c0868a8473042d901defa3cf1 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/scripts/real_robot_deployment/check_checkpoint.py @@ -0,0 +1,197 @@ +#!/usr/bin/env python3 +"""Dependency-free structural preflight for a GR00T SONIC checkpoint.""" + +from __future__ import annotations + +import argparse +import json +from pathlib import Path +import struct +import sys + + +EXPECTED_MODALITIES = { + "video": ["ego_view", "left_wrist", "right_wrist"], + "state": [ + "left_leg", + "right_leg", + "waist", + "left_arm", + "right_arm", + "left_gripper", + "right_gripper", + "head_joints", + "projected_gravity", + ], + "action": [ + "motion_token", + "left_hand_joints", + "right_hand_joints", + "head_joints", + ], + "language": ["annotation.human.task_description"], +} +EXPECTED_DIMS = { + "state": { + "left_leg": 6, + "right_leg": 6, + "waist": 3, + "left_arm": 7, + "right_arm": 7, + "left_gripper": 1, + "right_gripper": 1, + "head_joints": 2, + "projected_gravity": 3, + }, + "action": { + "motion_token": 64, + "left_hand_joints": 1, + "right_hand_joints": 1, + "head_joints": 2, + }, +} +REQUIRED_FILES = ( + "config.json", + "embodiment_id.json", + "model.safetensors.index.json", + "processor_config.json", + "statistics.json", +) + + +def load_json(path: Path): + try: + with path.open("r", encoding="utf-8") as stream: + return json.load(stream) + except (OSError, json.JSONDecodeError) as error: + raise ValueError(f"cannot read JSON {path}: {error}") from error + + +def validate_safetensors(path: Path) -> int: + """Return tensor payload bytes after verifying the complete shard layout.""" + size = path.stat().st_size + if size < 10: + raise ValueError(f"safetensors shard is too small: {path} ({size} bytes)") + with path.open("rb") as stream: + header_size_raw = stream.read(8) + if len(header_size_raw) != 8: + raise ValueError(f"truncated safetensors header length: {path}") + header_size = struct.unpack(" size: + raise ValueError(f"invalid safetensors header size in {path}: {header_size}") + header_raw = stream.read(header_size) + try: + header = json.loads(header_raw.decode("utf-8")) + except (UnicodeDecodeError, json.JSONDecodeError) as error: + raise ValueError(f"invalid safetensors JSON header in {path}: {error}") from error + + max_end = 0 + tensor_count = 0 + for name, metadata in header.items(): + if name == "__metadata__": + continue + offsets = metadata.get("data_offsets") if isinstance(metadata, dict) else None + if not isinstance(offsets, list) or len(offsets) != 2: + raise ValueError(f"tensor {name!r} has invalid offsets in {path}") + start, end = offsets + if not isinstance(start, int) or not isinstance(end, int) or start < 0 or end < start: + raise ValueError(f"tensor {name!r} has invalid offsets {offsets} in {path}") + max_end = max(max_end, end) + tensor_count += 1 + + expected_size = 8 + header_size + max_end + if tensor_count == 0 or size != expected_size: + raise ValueError( + f"incomplete/invalid safetensors shard {path}: size={size}, expected={expected_size}" + ) + return max_end + + +def stat_width(statistics: dict, modality: str, key: str) -> int: + values = statistics[modality][key] + for field in ("q01", "mean", "min"): + value = values.get(field) + if isinstance(value, list): + return len(value) + raise ValueError(f"cannot determine {modality}.{key} width from statistics") + + +def check_checkpoint(checkpoint: Path, embodiment: str) -> None: + if not checkpoint.is_dir(): + raise ValueError(f"checkpoint directory does not exist: {checkpoint}") + for filename in REQUIRED_FILES: + path = checkpoint / filename + if not path.is_file() or path.stat().st_size == 0: + raise ValueError(f"missing or empty checkpoint file: {path}") + + config = load_json(checkpoint / "config.json") + if config.get("action_horizon") != 40: + raise ValueError(f"expected action_horizon=40, got {config.get('action_horizon')}") + + processor = load_json(checkpoint / "processor_config.json") + try: + modalities = processor["processor_kwargs"]["modality_configs"][embodiment] + except KeyError as error: + raise ValueError(f"checkpoint does not contain embodiment {embodiment!r}") from error + for modality, expected_keys in EXPECTED_MODALITIES.items(): + actual = modalities[modality]["modality_keys"] + if actual != expected_keys: + raise ValueError( + f"{embodiment}.{modality} keys differ: expected {expected_keys}, got {actual}" + ) + + all_statistics = load_json(checkpoint / "statistics.json") + if embodiment not in all_statistics: + raise ValueError(f"statistics do not contain embodiment {embodiment!r}") + statistics = all_statistics[embodiment] + for modality, expected in EXPECTED_DIMS.items(): + for key, width in expected.items(): + actual_width = stat_width(statistics, modality, key) + if actual_width != width: + raise ValueError( + f"{modality}.{key} width differs: expected {width}, got {actual_width}" + ) + + index = load_json(checkpoint / "model.safetensors.index.json") + shards = sorted(set(index.get("weight_map", {}).values())) + if not shards: + raise ValueError("model.safetensors.index.json contains no shards") + payload_total = 0 + for filename in shards: + path = checkpoint / filename + if not path.is_file(): + raise ValueError(f"checkpoint shard is still missing: {path}") + payload = validate_safetensors(path) + payload_total += payload + print(f" valid shard: {filename} ({path.stat().st_size:,} bytes)") + + indexed_total = index.get("metadata", {}).get("total_size") + if isinstance(indexed_total, int) and payload_total != indexed_total: + raise ValueError( + f"tensor payload total differs: index={indexed_total}, shards={payload_total}" + ) + + print("Checkpoint preflight passed") + print(f" checkpoint: {checkpoint}") + print(f" embodiment: {embodiment}") + print(" video: ego_view + left_wrist + right_wrist (separate)") + print(" state dim: 36") + print(" action dim: 68 = 64 + 1 + 1 + 2") + print(" horizon: 40") + + +def main() -> int: + parser = argparse.ArgumentParser() + parser.add_argument("--checkpoint", type=Path, required=True) + parser.add_argument("--embodiment", default="unitree_g1_sonic") + args = parser.parse_args() + try: + check_checkpoint(args.checkpoint.expanduser(), args.embodiment) + except (OSError, ValueError, KeyError, TypeError) as error: + print(f"Checkpoint preflight failed: {error}", file=sys.stderr) + return 1 + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/GR00T-WholeBodyControl/gear_sonic/scripts/real_robot_deployment/common.sh b/GR00T-WholeBodyControl/gear_sonic/scripts/real_robot_deployment/common.sh new file mode 100644 index 0000000000000000000000000000000000000000..1f35115248c70bbba87076c5b4cd39db61d40b45 --- /dev/null +++ b/GR00T-WholeBodyControl/gear_sonic/scripts/real_robot_deployment/common.sh @@ -0,0 +1,39 @@ +#!/usr/bin/env bash + +set -Eeuo pipefail + +DEPLOY_SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +WBC_REPO_ROOT="$(cd "$DEPLOY_SCRIPT_DIR/../../.." && pwd)" + +die() { + echo "ERROR: $*" >&2 + exit 1 +} + +require_file() { + [[ -f "$1" ]] || die "missing file: $1" +} + +require_dir() { + [[ -d "$1" ]] || die "missing directory: $1" +} + +resolve_python() { + local explicit_python="$1" + shift + if [[ -n "$explicit_python" ]]; then + [[ -x "$explicit_python" ]] || die "Python is not executable: $explicit_python" + printf '%s\n' "$explicit_python" + return + fi + + local candidate + for candidate in "$@"; do + if [[ -x "$candidate" ]]; then + printf '%s\n' "$candidate" + return + fi + done + return 1 +} + diff --git a/GR00T-WholeBodyControl/gear_sonic/trl/modules/__pycache__/__init__.cpython-311.pyc b/GR00T-WholeBodyControl/gear_sonic/trl/modules/__pycache__/__init__.cpython-311.pyc new file mode 100644 index 0000000000000000000000000000000000000000..ac6a9f1c0e30cae9e889f11cfe645af13ac53d64 Binary files /dev/null and b/GR00T-WholeBodyControl/gear_sonic/trl/modules/__pycache__/__init__.cpython-311.pyc differ diff --git a/GR00T-WholeBodyControl/gear_sonic/trl/modules/__pycache__/__init__.cpython-312.pyc b/GR00T-WholeBodyControl/gear_sonic/trl/modules/__pycache__/__init__.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..cf7d373e2b9c5d0409d76335941993241a5f5e23 Binary files /dev/null and b/GR00T-WholeBodyControl/gear_sonic/trl/modules/__pycache__/__init__.cpython-312.pyc differ diff --git a/GR00T-WholeBodyControl/gear_sonic/trl/modules/__pycache__/actor_critic_modules.cpython-311.pyc b/GR00T-WholeBodyControl/gear_sonic/trl/modules/__pycache__/actor_critic_modules.cpython-311.pyc new file mode 100644 index 0000000000000000000000000000000000000000..71efdf75b69d78b9854d6ceeb6aa9b64379ca6e0 Binary files /dev/null and b/GR00T-WholeBodyControl/gear_sonic/trl/modules/__pycache__/actor_critic_modules.cpython-311.pyc differ diff --git a/GR00T-WholeBodyControl/gear_sonic/trl/modules/__pycache__/actor_critic_modules.cpython-312.pyc b/GR00T-WholeBodyControl/gear_sonic/trl/modules/__pycache__/actor_critic_modules.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..1af85c4f77d83d74a1b834f7f9ce6559c10358db Binary files /dev/null and b/GR00T-WholeBodyControl/gear_sonic/trl/modules/__pycache__/actor_critic_modules.cpython-312.pyc differ diff --git a/GR00T-WholeBodyControl/gear_sonic/trl/modules/__pycache__/base_module.cpython-311.pyc b/GR00T-WholeBodyControl/gear_sonic/trl/modules/__pycache__/base_module.cpython-311.pyc new file mode 100644 index 0000000000000000000000000000000000000000..dbfa423725018989ca9c885f96e021d9e4895fd6 Binary files /dev/null and b/GR00T-WholeBodyControl/gear_sonic/trl/modules/__pycache__/base_module.cpython-311.pyc differ diff --git a/GR00T-WholeBodyControl/gear_sonic/trl/modules/__pycache__/base_module.cpython-312.pyc b/GR00T-WholeBodyControl/gear_sonic/trl/modules/__pycache__/base_module.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..2685ed8322287d84f0f6457fafc8c06583a9778d Binary files /dev/null and b/GR00T-WholeBodyControl/gear_sonic/trl/modules/__pycache__/base_module.cpython-312.pyc differ diff --git a/GR00T-WholeBodyControl/gear_sonic/trl/modules/__pycache__/data_utils.cpython-311.pyc b/GR00T-WholeBodyControl/gear_sonic/trl/modules/__pycache__/data_utils.cpython-311.pyc new file mode 100644 index 0000000000000000000000000000000000000000..05303c9121a25d130f6d86bcc3730782fb901ab1 Binary files /dev/null and b/GR00T-WholeBodyControl/gear_sonic/trl/modules/__pycache__/data_utils.cpython-311.pyc differ diff --git a/GR00T-WholeBodyControl/gear_sonic/trl/modules/__pycache__/data_utils.cpython-312.pyc b/GR00T-WholeBodyControl/gear_sonic/trl/modules/__pycache__/data_utils.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..0a79c36f97f76ca765169693321ac28c71b2cca1 Binary files /dev/null and b/GR00T-WholeBodyControl/gear_sonic/trl/modules/__pycache__/data_utils.cpython-312.pyc differ diff --git a/GR00T-WholeBodyControl/gear_sonic/trl/modules/__pycache__/universal_token_modules.cpython-311.pyc b/GR00T-WholeBodyControl/gear_sonic/trl/modules/__pycache__/universal_token_modules.cpython-311.pyc new file mode 100644 index 0000000000000000000000000000000000000000..d1c6f17833c207f236a75ac5b9aaa82defa7cabd Binary files /dev/null and b/GR00T-WholeBodyControl/gear_sonic/trl/modules/__pycache__/universal_token_modules.cpython-311.pyc differ diff --git a/GR00T-WholeBodyControl/gear_sonic/trl/modules/__pycache__/universal_token_modules.cpython-312.pyc b/GR00T-WholeBodyControl/gear_sonic/trl/modules/__pycache__/universal_token_modules.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..a779460595fe5039486fbab1bf9aa86ddba583d2 Binary files /dev/null and b/GR00T-WholeBodyControl/gear_sonic/trl/modules/__pycache__/universal_token_modules.cpython-312.pyc differ diff --git a/GR00T-WholeBodyControl/gear_sonic/trl/trainer/__pycache__/__init__.cpython-311.pyc b/GR00T-WholeBodyControl/gear_sonic/trl/trainer/__pycache__/__init__.cpython-311.pyc new file mode 100644 index 0000000000000000000000000000000000000000..3fadac87a7c4b3df659041664e98df06bd18ce0e Binary files /dev/null and b/GR00T-WholeBodyControl/gear_sonic/trl/trainer/__pycache__/__init__.cpython-311.pyc differ diff --git a/GR00T-WholeBodyControl/gear_sonic/trl/trainer/__pycache__/__init__.cpython-312.pyc b/GR00T-WholeBodyControl/gear_sonic/trl/trainer/__pycache__/__init__.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..4cb4b9efa1ba49bceb81226d3d4c1896643d00b7 Binary files /dev/null and b/GR00T-WholeBodyControl/gear_sonic/trl/trainer/__pycache__/__init__.cpython-312.pyc differ diff --git a/GR00T-WholeBodyControl/hardware/camera_mount/README.md b/GR00T-WholeBodyControl/hardware/camera_mount/README.md new file mode 100644 index 0000000000000000000000000000000000000000..ca5d68380741de75478eba972db5dc7c4978525f --- /dev/null +++ b/GR00T-WholeBodyControl/hardware/camera_mount/README.md @@ -0,0 +1,50 @@ +# OAK-D W Camera Mount + +3D-printed mount for the ego / head-view camera used in the GR00T-WholeBodyControl +data-collection and VLA pipeline. The mount positions the camera that produces the +`observation.images.ego_view` stream, so reproducing this mount reproduces the +camera viewpoint the policies were trained with. + +| | | +|---|---| +| **File** | [`oak_d_w_mount.step`](./oak_d_w_mount.step) (STEP AP242) | +| **Version** | v1 | +| **Camera** | [Luxonis OAK-D W](https://shop.luxonis.com/products/oak-d-w) (OV9782 global-shutter sensor) | + +> **Note:** The `.step` file is stored directly in Git (it is ~44 KB of plain ASCII +> text), so a normal `git clone` or the GitHub "Download" button gives you the real +> geometry — no Git LFS required. + +## Manufacturing + +| Parameter | Value | +|---|---| +| Process | FDM 3D printing | +| Material | PLA | +| Layer height | 0.2 mm | +| Infill / supports | Slicer defaults — no special settings required | + +Open the STEP file in any CAD tool (FreeCAD, Fusion 360, SolidWorks, or any +online STEP viewer). + +## Bill of materials + +| Fastener | Spec | Notes | +|---|---|---| +| Camera → mount | 2 × M4 × 8 mm | Threads directly into the printed PLA — no heat-set inserts. The OAK-D W sits flush against the mount face. | +| Mount → G1 | Stock G1 screws | Reuses the default screws at the G1 mounting location. | + +## Mounting on the G1 + +The mount attaches at the **same location as the G1's stock Intel RealSense head +camera** — it screws into the existing RealSense mounting point using the stock +G1 screws, so no new holes or modifications are needed. + +The OAK-D W sits flush against the printed face and keeps the **same orientation +plane as the stock RealSense**, angled approximately **40° relative to the head**. +This reproduces the ego-view camera pose the policies were trained with. + +## Related documentation + +- [Data Collection for VLA](../../docs/source/tutorials/data_collection.md) — camera + server setup and the `ego_view` image stream this mount provides. diff --git a/GR00T-WholeBodyControl/hardware/camera_mount/oak_d_w_mount.step b/GR00T-WholeBodyControl/hardware/camera_mount/oak_d_w_mount.step new file mode 100644 index 0000000000000000000000000000000000000000..8c93f52bf764528ea6a1d101feb3c84b9d9328c2 --- /dev/null +++ b/GR00T-WholeBodyControl/hardware/camera_mount/oak_d_w_mount.step @@ -0,0 +1,1054 @@ +ISO-10303-21; +HEADER; +/* Generated by software containing ST-Developer + * from STEP Tools, Inc. (www.steptools.com) + */ + +FILE_DESCRIPTION( +/* description */ ('STEP AP242'), +/* implementation_level */ '2;1'); + +FILE_NAME( +/* name */ 'oak - v1.step', +/* time_stamp */ '2025-12-23T12:09:19-08:00', +/* author */ (''), +/* organization */ (''), +/* preprocessor_version */ 'ST-DEVELOPER v20.1', +/* originating_system */ 'Autodesk Translation Framework v14.21.0.0', +/* authorisation */ ''); + +FILE_SCHEMA (('AUTOMOTIVE_DESIGN { 1 0 10303 214 3 1 1 }')); +ENDSEC; + +DATA; +#10=MECHANICAL_DESIGN_GEOMETRIC_PRESENTATION_REPRESENTATION('',(#13),#971); +#11=SHAPE_REPRESENTATION_RELATIONSHIP('SRR','None',#977,#12); +#12=ADVANCED_BREP_SHAPE_REPRESENTATION('',(#14),#970); +#13=STYLED_ITEM('',(#987),#14); +#14=MANIFOLD_SOLID_BREP('Body1',#587); +#15=CYLINDRICAL_SURFACE('',#620,0.0016); +#16=CYLINDRICAL_SURFACE('',#630,0.005); +#17=CYLINDRICAL_SURFACE('',#631,0.005); +#18=CYLINDRICAL_SURFACE('',#632,0.002); +#19=CYLINDRICAL_SURFACE('',#635,0.0016); +#20=CYLINDRICAL_SURFACE('',#644,0.005); +#21=CYLINDRICAL_SURFACE('',#645,0.005); +#22=CYLINDRICAL_SURFACE('',#646,0.002); +#23=ELLIPSE('',#610,0.00503989870734971,0.005); +#24=ELLIPSE('',#611,0.00503989870734971,0.005); +#25=ELLIPSE('',#612,0.00503989870734971,0.005); +#26=ELLIPSE('',#613,0.00503989870734971,0.005); +#27=ELLIPSE('',#624,0.00503989870734971,0.005); +#28=ELLIPSE('',#625,0.00503989870734971,0.005); +#29=ELLIPSE('',#638,0.00503989870734971,0.005); +#30=ELLIPSE('',#639,0.00503989870734971,0.005); +#31=FACE_BOUND('',#79,.T.); +#32=FACE_BOUND('',#80,.T.); +#33=FACE_BOUND('',#82,.T.); +#34=FACE_BOUND('',#83,.T.); +#35=FACE_BOUND('',#86,.T.); +#36=FACE_BOUND('',#87,.T.); +#37=FACE_BOUND('',#96,.T.); +#38=FACE_BOUND('',#108,.T.); +#39=CIRCLE('',#603,0.0016); +#40=CIRCLE('',#604,0.0016); +#41=CIRCLE('',#606,0.0016); +#42=CIRCLE('',#607,0.0016); +#43=CIRCLE('',#614,0.002); +#44=CIRCLE('',#615,0.002); +#45=CIRCLE('',#626,0.002); +#46=CIRCLE('',#640,0.002); +#47=FACE_OUTER_BOUND('',#77,.T.); +#48=FACE_OUTER_BOUND('',#78,.T.); +#49=FACE_OUTER_BOUND('',#81,.T.); +#50=FACE_OUTER_BOUND('',#84,.T.); +#51=FACE_OUTER_BOUND('',#85,.T.); +#52=FACE_OUTER_BOUND('',#88,.T.); +#53=FACE_OUTER_BOUND('',#89,.T.); +#54=FACE_OUTER_BOUND('',#90,.T.); +#55=FACE_OUTER_BOUND('',#91,.T.); +#56=FACE_OUTER_BOUND('',#92,.T.); +#57=FACE_OUTER_BOUND('',#93,.T.); +#58=FACE_OUTER_BOUND('',#94,.T.); +#59=FACE_OUTER_BOUND('',#95,.T.); +#60=FACE_OUTER_BOUND('',#97,.T.); +#61=FACE_OUTER_BOUND('',#98,.T.); +#62=FACE_OUTER_BOUND('',#99,.T.); +#63=FACE_OUTER_BOUND('',#100,.T.); +#64=FACE_OUTER_BOUND('',#101,.T.); +#65=FACE_OUTER_BOUND('',#102,.T.); +#66=FACE_OUTER_BOUND('',#103,.T.); +#67=FACE_OUTER_BOUND('',#104,.T.); +#68=FACE_OUTER_BOUND('',#105,.T.); +#69=FACE_OUTER_BOUND('',#106,.T.); +#70=FACE_OUTER_BOUND('',#107,.T.); +#71=FACE_OUTER_BOUND('',#109,.T.); +#72=FACE_OUTER_BOUND('',#110,.T.); +#73=FACE_OUTER_BOUND('',#111,.T.); +#74=FACE_OUTER_BOUND('',#112,.T.); +#75=FACE_OUTER_BOUND('',#113,.T.); +#76=FACE_OUTER_BOUND('',#114,.T.); +#77=EDGE_LOOP('',(#375,#376,#377,#378,#379,#380,#381,#382,#383,#384)); +#78=EDGE_LOOP('',(#385,#386,#387,#388,#389,#390,#391,#392)); +#79=EDGE_LOOP('',(#393)); +#80=EDGE_LOOP('',(#394)); +#81=EDGE_LOOP('',(#395,#396,#397,#398,#399,#400,#401,#402)); +#82=EDGE_LOOP('',(#403)); +#83=EDGE_LOOP('',(#404)); +#84=EDGE_LOOP('',(#405,#406,#407,#408)); +#85=EDGE_LOOP('',(#409,#410,#411,#412,#413,#414,#415,#416,#417,#418,#419, +#420)); +#86=EDGE_LOOP('',(#421)); +#87=EDGE_LOOP('',(#422)); +#88=EDGE_LOOP('',(#423,#424,#425,#426)); +#89=EDGE_LOOP('',(#427,#428,#429,#430)); +#90=EDGE_LOOP('',(#431,#432,#433,#434)); +#91=EDGE_LOOP('',(#435,#436,#437,#438)); +#92=EDGE_LOOP('',(#439,#440,#441,#442)); +#93=EDGE_LOOP('',(#443,#444,#445,#446)); +#94=EDGE_LOOP('',(#447,#448,#449,#450)); +#95=EDGE_LOOP('',(#451,#452,#453,#454,#455,#456)); +#96=EDGE_LOOP('',(#457)); +#97=EDGE_LOOP('',(#458,#459,#460,#461)); +#98=EDGE_LOOP('',(#462,#463,#464,#465)); +#99=EDGE_LOOP('',(#466,#467,#468,#469)); +#100=EDGE_LOOP('',(#470,#471,#472,#473)); +#101=EDGE_LOOP('',(#474,#475,#476,#477)); +#102=EDGE_LOOP('',(#478,#479,#480,#481)); +#103=EDGE_LOOP('',(#482,#483,#484,#485,#486,#487,#488,#489,#490,#491)); +#104=EDGE_LOOP('',(#492,#493,#494,#495)); +#105=EDGE_LOOP('',(#496,#497,#498,#499)); +#106=EDGE_LOOP('',(#500,#501,#502,#503)); +#107=EDGE_LOOP('',(#504,#505,#506,#507,#508,#509)); +#108=EDGE_LOOP('',(#510)); +#109=EDGE_LOOP('',(#511,#512,#513,#514)); +#110=EDGE_LOOP('',(#515,#516,#517,#518)); +#111=EDGE_LOOP('',(#519,#520,#521,#522)); +#112=EDGE_LOOP('',(#523,#524,#525,#526)); +#113=EDGE_LOOP('',(#527,#528,#529,#530)); +#114=EDGE_LOOP('',(#531,#532,#533,#534)); +#115=LINE('',#809,#179); +#116=LINE('',#811,#180); +#117=LINE('',#813,#181); +#118=LINE('',#815,#182); +#119=LINE('',#817,#183); +#120=LINE('',#819,#184); +#121=LINE('',#821,#185); +#122=LINE('',#823,#186); +#123=LINE('',#825,#187); +#124=LINE('',#826,#188); +#125=LINE('',#829,#189); +#126=LINE('',#831,#190); +#127=LINE('',#833,#191); +#128=LINE('',#835,#192); +#129=LINE('',#837,#193); +#130=LINE('',#839,#194); +#131=LINE('',#840,#195); +#132=LINE('',#848,#196); +#133=LINE('',#850,#197); +#134=LINE('',#852,#198); +#135=LINE('',#854,#199); +#136=LINE('',#855,#200); +#137=LINE('',#857,#201); +#138=LINE('',#858,#202); +#139=LINE('',#865,#203); +#140=LINE('',#866,#204); +#141=LINE('',#869,#205); +#142=LINE('',#873,#206); +#143=LINE('',#876,#207); +#144=LINE('',#878,#208); +#145=LINE('',#882,#209); +#146=LINE('',#886,#210); +#147=LINE('',#887,#211); +#148=LINE('',#894,#212); +#149=LINE('',#895,#213); +#150=LINE('',#898,#214); +#151=LINE('',#899,#215); +#152=LINE('',#901,#216); +#153=LINE('',#903,#217); +#154=LINE('',#905,#218); +#155=LINE('',#907,#219); +#156=LINE('',#908,#220); +#157=LINE('',#913,#221); +#158=LINE('',#916,#222); +#159=LINE('',#918,#223); +#160=LINE('',#923,#224); +#161=LINE('',#925,#225); +#162=LINE('',#927,#226); +#163=LINE('',#928,#227); +#164=LINE('',#932,#228); +#165=LINE('',#936,#229); +#166=LINE('',#937,#230); +#167=LINE('',#938,#231); +#168=LINE('',#939,#232); +#169=LINE('',#941,#233); +#170=LINE('',#943,#234); +#171=LINE('',#948,#235); +#172=LINE('',#951,#236); +#173=LINE('',#953,#237); +#174=LINE('',#958,#238); +#175=LINE('',#960,#239); +#176=LINE('',#962,#240); +#177=LINE('',#963,#241); +#178=LINE('',#967,#242); +#179=VECTOR('',#651,1.); +#180=VECTOR('',#652,1.); +#181=VECTOR('',#653,1.); +#182=VECTOR('',#654,1.); +#183=VECTOR('',#655,1.); +#184=VECTOR('',#656,1.); +#185=VECTOR('',#657,1.); +#186=VECTOR('',#658,1.); +#187=VECTOR('',#659,1.); +#188=VECTOR('',#660,1.); +#189=VECTOR('',#663,1.); +#190=VECTOR('',#664,1.); +#191=VECTOR('',#665,1.); +#192=VECTOR('',#666,1.); +#193=VECTOR('',#667,1.); +#194=VECTOR('',#668,1.); +#195=VECTOR('',#669,1.); +#196=VECTOR('',#676,1.); +#197=VECTOR('',#677,1.); +#198=VECTOR('',#678,1.); +#199=VECTOR('',#679,1.); +#200=VECTOR('',#680,1.); +#201=VECTOR('',#681,1.); +#202=VECTOR('',#682,1.); +#203=VECTOR('',#689,1.); +#204=VECTOR('',#690,1.); +#205=VECTOR('',#693,1.); +#206=VECTOR('',#696,1.); +#207=VECTOR('',#699,1.); +#208=VECTOR('',#700,1.); +#209=VECTOR('',#703,1.); +#210=VECTOR('',#706,1.); +#211=VECTOR('',#707,1.); +#212=VECTOR('',#714,1.); +#213=VECTOR('',#715,1.); +#214=VECTOR('',#718,1.); +#215=VECTOR('',#719,1.); +#216=VECTOR('',#722,1.); +#217=VECTOR('',#725,1.); +#218=VECTOR('',#728,0.0016); +#219=VECTOR('',#731,1.); +#220=VECTOR('',#732,1.); +#221=VECTOR('',#737,1.); +#222=VECTOR('',#740,1.); +#223=VECTOR('',#741,1.); +#224=VECTOR('',#748,1.); +#225=VECTOR('',#751,1.); +#226=VECTOR('',#754,1.); +#227=VECTOR('',#755,1.); +#228=VECTOR('',#762,0.002); +#229=VECTOR('',#765,1.); +#230=VECTOR('',#766,1.); +#231=VECTOR('',#767,1.); +#232=VECTOR('',#768,1.); +#233=VECTOR('',#771,1.); +#234=VECTOR('',#774,0.0016); +#235=VECTOR('',#779,1.); +#236=VECTOR('',#782,1.); +#237=VECTOR('',#783,1.); +#238=VECTOR('',#790,1.); +#239=VECTOR('',#793,1.); +#240=VECTOR('',#796,1.); +#241=VECTOR('',#797,1.); +#242=VECTOR('',#804,0.002); +#243=VERTEX_POINT('',#807); +#244=VERTEX_POINT('',#808); +#245=VERTEX_POINT('',#810); +#246=VERTEX_POINT('',#812); +#247=VERTEX_POINT('',#814); +#248=VERTEX_POINT('',#816); +#249=VERTEX_POINT('',#818); +#250=VERTEX_POINT('',#820); +#251=VERTEX_POINT('',#822); +#252=VERTEX_POINT('',#824); +#253=VERTEX_POINT('',#828); +#254=VERTEX_POINT('',#830); +#255=VERTEX_POINT('',#832); +#256=VERTEX_POINT('',#834); +#257=VERTEX_POINT('',#836); +#258=VERTEX_POINT('',#838); +#259=VERTEX_POINT('',#841); +#260=VERTEX_POINT('',#843); +#261=VERTEX_POINT('',#846); +#262=VERTEX_POINT('',#847); +#263=VERTEX_POINT('',#849); +#264=VERTEX_POINT('',#851); +#265=VERTEX_POINT('',#853); +#266=VERTEX_POINT('',#856); +#267=VERTEX_POINT('',#859); +#268=VERTEX_POINT('',#861); +#269=VERTEX_POINT('',#864); +#270=VERTEX_POINT('',#868); +#271=VERTEX_POINT('',#870); +#272=VERTEX_POINT('',#872); +#273=VERTEX_POINT('',#874); +#274=VERTEX_POINT('',#877); +#275=VERTEX_POINT('',#879); +#276=VERTEX_POINT('',#881); +#277=VERTEX_POINT('',#883); +#278=VERTEX_POINT('',#885); +#279=VERTEX_POINT('',#888); +#280=VERTEX_POINT('',#890); +#281=VERTEX_POINT('',#893); +#282=VERTEX_POINT('',#897); +#283=VERTEX_POINT('',#911); +#284=VERTEX_POINT('',#912); +#285=VERTEX_POINT('',#914); +#286=VERTEX_POINT('',#917); +#287=VERTEX_POINT('',#920); +#288=VERTEX_POINT('',#934); +#289=VERTEX_POINT('',#935); +#290=VERTEX_POINT('',#946); +#291=VERTEX_POINT('',#947); +#292=VERTEX_POINT('',#949); +#293=VERTEX_POINT('',#952); +#294=VERTEX_POINT('',#955); +#295=EDGE_CURVE('',#243,#244,#115,.T.); +#296=EDGE_CURVE('',#244,#245,#116,.T.); +#297=EDGE_CURVE('',#246,#245,#117,.T.); +#298=EDGE_CURVE('',#247,#246,#118,.T.); +#299=EDGE_CURVE('',#248,#247,#119,.T.); +#300=EDGE_CURVE('',#249,#248,#120,.T.); +#301=EDGE_CURVE('',#250,#249,#121,.T.); +#302=EDGE_CURVE('',#251,#250,#122,.T.); +#303=EDGE_CURVE('',#252,#251,#123,.T.); +#304=EDGE_CURVE('',#243,#252,#124,.T.); +#305=EDGE_CURVE('',#248,#253,#125,.T.); +#306=EDGE_CURVE('',#254,#253,#126,.T.); +#307=EDGE_CURVE('',#254,#255,#127,.T.); +#308=EDGE_CURVE('',#256,#255,#128,.T.); +#309=EDGE_CURVE('',#256,#257,#129,.T.); +#310=EDGE_CURVE('',#257,#258,#130,.T.); +#311=EDGE_CURVE('',#249,#258,#131,.T.); +#312=EDGE_CURVE('',#259,#259,#39,.T.); +#313=EDGE_CURVE('',#260,#260,#40,.T.); +#314=EDGE_CURVE('',#261,#262,#132,.T.); +#315=EDGE_CURVE('',#261,#263,#133,.T.); +#316=EDGE_CURVE('',#264,#263,#134,.T.); +#317=EDGE_CURVE('',#265,#264,#135,.T.); +#318=EDGE_CURVE('',#251,#265,#136,.T.); +#319=EDGE_CURVE('',#250,#266,#137,.T.); +#320=EDGE_CURVE('',#262,#266,#138,.T.); +#321=EDGE_CURVE('',#267,#267,#41,.F.); +#322=EDGE_CURVE('',#268,#268,#42,.F.); +#323=EDGE_CURVE('',#269,#265,#139,.T.); +#324=EDGE_CURVE('',#252,#269,#140,.T.); +#325=EDGE_CURVE('',#270,#245,#141,.T.); +#326=EDGE_CURVE('',#270,#271,#23,.T.); +#327=EDGE_CURVE('',#271,#272,#142,.T.); +#328=EDGE_CURVE('',#272,#273,#24,.T.); +#329=EDGE_CURVE('',#273,#252,#143,.T.); +#330=EDGE_CURVE('',#274,#269,#144,.T.); +#331=EDGE_CURVE('',#275,#274,#25,.T.); +#332=EDGE_CURVE('',#276,#275,#145,.T.); +#333=EDGE_CURVE('',#277,#276,#26,.T.); +#334=EDGE_CURVE('',#277,#278,#146,.T.); +#335=EDGE_CURVE('',#245,#278,#147,.T.); +#336=EDGE_CURVE('',#279,#279,#43,.T.); +#337=EDGE_CURVE('',#280,#280,#44,.T.); +#338=EDGE_CURVE('',#281,#278,#148,.T.); +#339=EDGE_CURVE('',#246,#281,#149,.T.); +#340=EDGE_CURVE('',#282,#281,#150,.T.); +#341=EDGE_CURVE('',#247,#282,#151,.T.); +#342=EDGE_CURVE('',#253,#282,#152,.T.); +#343=EDGE_CURVE('',#266,#258,#153,.T.); +#344=EDGE_CURVE('',#260,#267,#154,.T.); +#345=EDGE_CURVE('',#262,#257,#155,.T.); +#346=EDGE_CURVE('',#261,#256,#156,.T.); +#347=EDGE_CURVE('',#283,#284,#157,.T.); +#348=EDGE_CURVE('',#284,#285,#27,.T.); +#349=EDGE_CURVE('',#285,#244,#158,.T.); +#350=EDGE_CURVE('',#286,#243,#159,.T.); +#351=EDGE_CURVE('',#286,#283,#28,.T.); +#352=EDGE_CURVE('',#287,#287,#45,.T.); +#353=EDGE_CURVE('',#273,#286,#160,.F.); +#354=EDGE_CURVE('',#285,#270,#161,.T.); +#355=EDGE_CURVE('',#271,#284,#162,.F.); +#356=EDGE_CURVE('',#283,#272,#163,.T.); +#357=EDGE_CURVE('',#280,#287,#164,.T.); +#358=EDGE_CURVE('',#288,#289,#165,.T.); +#359=EDGE_CURVE('',#288,#269,#166,.T.); +#360=EDGE_CURVE('',#264,#254,#167,.T.); +#361=EDGE_CURVE('',#289,#278,#168,.T.); +#362=EDGE_CURVE('',#263,#255,#169,.T.); +#363=EDGE_CURVE('',#259,#268,#170,.T.); +#364=EDGE_CURVE('',#290,#291,#171,.T.); +#365=EDGE_CURVE('',#292,#290,#29,.T.); +#366=EDGE_CURVE('',#292,#288,#172,.T.); +#367=EDGE_CURVE('',#293,#289,#173,.T.); +#368=EDGE_CURVE('',#291,#293,#30,.T.); +#369=EDGE_CURVE('',#294,#294,#46,.T.); +#370=EDGE_CURVE('',#274,#292,#174,.F.); +#371=EDGE_CURVE('',#293,#277,#175,.T.); +#372=EDGE_CURVE('',#290,#275,#176,.T.); +#373=EDGE_CURVE('',#276,#291,#177,.F.); +#374=EDGE_CURVE('',#279,#294,#178,.T.); +#375=ORIENTED_EDGE('',*,*,#295,.T.); +#376=ORIENTED_EDGE('',*,*,#296,.T.); +#377=ORIENTED_EDGE('',*,*,#297,.F.); +#378=ORIENTED_EDGE('',*,*,#298,.F.); +#379=ORIENTED_EDGE('',*,*,#299,.F.); +#380=ORIENTED_EDGE('',*,*,#300,.F.); +#381=ORIENTED_EDGE('',*,*,#301,.F.); +#382=ORIENTED_EDGE('',*,*,#302,.F.); +#383=ORIENTED_EDGE('',*,*,#303,.F.); +#384=ORIENTED_EDGE('',*,*,#304,.F.); +#385=ORIENTED_EDGE('',*,*,#305,.T.); +#386=ORIENTED_EDGE('',*,*,#306,.F.); +#387=ORIENTED_EDGE('',*,*,#307,.T.); +#388=ORIENTED_EDGE('',*,*,#308,.F.); +#389=ORIENTED_EDGE('',*,*,#309,.T.); +#390=ORIENTED_EDGE('',*,*,#310,.T.); +#391=ORIENTED_EDGE('',*,*,#311,.F.); +#392=ORIENTED_EDGE('',*,*,#300,.T.); +#393=ORIENTED_EDGE('',*,*,#312,.F.); +#394=ORIENTED_EDGE('',*,*,#313,.T.); +#395=ORIENTED_EDGE('',*,*,#314,.F.); +#396=ORIENTED_EDGE('',*,*,#315,.T.); +#397=ORIENTED_EDGE('',*,*,#316,.F.); +#398=ORIENTED_EDGE('',*,*,#317,.F.); +#399=ORIENTED_EDGE('',*,*,#318,.F.); +#400=ORIENTED_EDGE('',*,*,#302,.T.); +#401=ORIENTED_EDGE('',*,*,#319,.T.); +#402=ORIENTED_EDGE('',*,*,#320,.F.); +#403=ORIENTED_EDGE('',*,*,#321,.T.); +#404=ORIENTED_EDGE('',*,*,#322,.F.); +#405=ORIENTED_EDGE('',*,*,#318,.T.); +#406=ORIENTED_EDGE('',*,*,#323,.F.); +#407=ORIENTED_EDGE('',*,*,#324,.F.); +#408=ORIENTED_EDGE('',*,*,#303,.T.); +#409=ORIENTED_EDGE('',*,*,#325,.F.); +#410=ORIENTED_EDGE('',*,*,#326,.T.); +#411=ORIENTED_EDGE('',*,*,#327,.T.); +#412=ORIENTED_EDGE('',*,*,#328,.T.); +#413=ORIENTED_EDGE('',*,*,#329,.T.); +#414=ORIENTED_EDGE('',*,*,#324,.T.); +#415=ORIENTED_EDGE('',*,*,#330,.F.); +#416=ORIENTED_EDGE('',*,*,#331,.F.); +#417=ORIENTED_EDGE('',*,*,#332,.F.); +#418=ORIENTED_EDGE('',*,*,#333,.F.); +#419=ORIENTED_EDGE('',*,*,#334,.T.); +#420=ORIENTED_EDGE('',*,*,#335,.F.); +#421=ORIENTED_EDGE('',*,*,#336,.F.); +#422=ORIENTED_EDGE('',*,*,#337,.T.); +#423=ORIENTED_EDGE('',*,*,#335,.T.); +#424=ORIENTED_EDGE('',*,*,#338,.F.); +#425=ORIENTED_EDGE('',*,*,#339,.F.); +#426=ORIENTED_EDGE('',*,*,#297,.T.); +#427=ORIENTED_EDGE('',*,*,#339,.T.); +#428=ORIENTED_EDGE('',*,*,#340,.F.); +#429=ORIENTED_EDGE('',*,*,#341,.F.); +#430=ORIENTED_EDGE('',*,*,#298,.T.); +#431=ORIENTED_EDGE('',*,*,#341,.T.); +#432=ORIENTED_EDGE('',*,*,#342,.F.); +#433=ORIENTED_EDGE('',*,*,#305,.F.); +#434=ORIENTED_EDGE('',*,*,#299,.T.); +#435=ORIENTED_EDGE('',*,*,#343,.F.); +#436=ORIENTED_EDGE('',*,*,#319,.F.); +#437=ORIENTED_EDGE('',*,*,#301,.T.); +#438=ORIENTED_EDGE('',*,*,#311,.T.); +#439=ORIENTED_EDGE('',*,*,#313,.F.); +#440=ORIENTED_EDGE('',*,*,#344,.T.); +#441=ORIENTED_EDGE('',*,*,#321,.F.); +#442=ORIENTED_EDGE('',*,*,#344,.F.); +#443=ORIENTED_EDGE('',*,*,#314,.T.); +#444=ORIENTED_EDGE('',*,*,#345,.T.); +#445=ORIENTED_EDGE('',*,*,#309,.F.); +#446=ORIENTED_EDGE('',*,*,#346,.F.); +#447=ORIENTED_EDGE('',*,*,#320,.T.); +#448=ORIENTED_EDGE('',*,*,#343,.T.); +#449=ORIENTED_EDGE('',*,*,#310,.F.); +#450=ORIENTED_EDGE('',*,*,#345,.F.); +#451=ORIENTED_EDGE('',*,*,#347,.T.); +#452=ORIENTED_EDGE('',*,*,#348,.T.); +#453=ORIENTED_EDGE('',*,*,#349,.T.); +#454=ORIENTED_EDGE('',*,*,#295,.F.); +#455=ORIENTED_EDGE('',*,*,#350,.F.); +#456=ORIENTED_EDGE('',*,*,#351,.T.); +#457=ORIENTED_EDGE('',*,*,#352,.F.); +#458=ORIENTED_EDGE('',*,*,#329,.F.); +#459=ORIENTED_EDGE('',*,*,#353,.T.); +#460=ORIENTED_EDGE('',*,*,#350,.T.); +#461=ORIENTED_EDGE('',*,*,#304,.T.); +#462=ORIENTED_EDGE('',*,*,#349,.F.); +#463=ORIENTED_EDGE('',*,*,#354,.T.); +#464=ORIENTED_EDGE('',*,*,#325,.T.); +#465=ORIENTED_EDGE('',*,*,#296,.F.); +#466=ORIENTED_EDGE('',*,*,#327,.F.); +#467=ORIENTED_EDGE('',*,*,#355,.T.); +#468=ORIENTED_EDGE('',*,*,#347,.F.); +#469=ORIENTED_EDGE('',*,*,#356,.T.); +#470=ORIENTED_EDGE('',*,*,#348,.F.); +#471=ORIENTED_EDGE('',*,*,#355,.F.); +#472=ORIENTED_EDGE('',*,*,#326,.F.); +#473=ORIENTED_EDGE('',*,*,#354,.F.); +#474=ORIENTED_EDGE('',*,*,#351,.F.); +#475=ORIENTED_EDGE('',*,*,#353,.F.); +#476=ORIENTED_EDGE('',*,*,#328,.F.); +#477=ORIENTED_EDGE('',*,*,#356,.F.); +#478=ORIENTED_EDGE('',*,*,#337,.F.); +#479=ORIENTED_EDGE('',*,*,#357,.T.); +#480=ORIENTED_EDGE('',*,*,#352,.T.); +#481=ORIENTED_EDGE('',*,*,#357,.F.); +#482=ORIENTED_EDGE('',*,*,#358,.F.); +#483=ORIENTED_EDGE('',*,*,#359,.T.); +#484=ORIENTED_EDGE('',*,*,#323,.T.); +#485=ORIENTED_EDGE('',*,*,#317,.T.); +#486=ORIENTED_EDGE('',*,*,#360,.T.); +#487=ORIENTED_EDGE('',*,*,#306,.T.); +#488=ORIENTED_EDGE('',*,*,#342,.T.); +#489=ORIENTED_EDGE('',*,*,#340,.T.); +#490=ORIENTED_EDGE('',*,*,#338,.T.); +#491=ORIENTED_EDGE('',*,*,#361,.F.); +#492=ORIENTED_EDGE('',*,*,#362,.T.); +#493=ORIENTED_EDGE('',*,*,#307,.F.); +#494=ORIENTED_EDGE('',*,*,#360,.F.); +#495=ORIENTED_EDGE('',*,*,#316,.T.); +#496=ORIENTED_EDGE('',*,*,#312,.T.); +#497=ORIENTED_EDGE('',*,*,#363,.T.); +#498=ORIENTED_EDGE('',*,*,#322,.T.); +#499=ORIENTED_EDGE('',*,*,#363,.F.); +#500=ORIENTED_EDGE('',*,*,#315,.F.); +#501=ORIENTED_EDGE('',*,*,#346,.T.); +#502=ORIENTED_EDGE('',*,*,#308,.T.); +#503=ORIENTED_EDGE('',*,*,#362,.F.); +#504=ORIENTED_EDGE('',*,*,#364,.F.); +#505=ORIENTED_EDGE('',*,*,#365,.F.); +#506=ORIENTED_EDGE('',*,*,#366,.T.); +#507=ORIENTED_EDGE('',*,*,#358,.T.); +#508=ORIENTED_EDGE('',*,*,#367,.F.); +#509=ORIENTED_EDGE('',*,*,#368,.F.); +#510=ORIENTED_EDGE('',*,*,#369,.T.); +#511=ORIENTED_EDGE('',*,*,#330,.T.); +#512=ORIENTED_EDGE('',*,*,#359,.F.); +#513=ORIENTED_EDGE('',*,*,#366,.F.); +#514=ORIENTED_EDGE('',*,*,#370,.F.); +#515=ORIENTED_EDGE('',*,*,#367,.T.); +#516=ORIENTED_EDGE('',*,*,#361,.T.); +#517=ORIENTED_EDGE('',*,*,#334,.F.); +#518=ORIENTED_EDGE('',*,*,#371,.F.); +#519=ORIENTED_EDGE('',*,*,#332,.T.); +#520=ORIENTED_EDGE('',*,*,#372,.F.); +#521=ORIENTED_EDGE('',*,*,#364,.T.); +#522=ORIENTED_EDGE('',*,*,#373,.F.); +#523=ORIENTED_EDGE('',*,*,#368,.T.); +#524=ORIENTED_EDGE('',*,*,#371,.T.); +#525=ORIENTED_EDGE('',*,*,#333,.T.); +#526=ORIENTED_EDGE('',*,*,#373,.T.); +#527=ORIENTED_EDGE('',*,*,#365,.T.); +#528=ORIENTED_EDGE('',*,*,#372,.T.); +#529=ORIENTED_EDGE('',*,*,#331,.T.); +#530=ORIENTED_EDGE('',*,*,#370,.T.); +#531=ORIENTED_EDGE('',*,*,#336,.T.); +#532=ORIENTED_EDGE('',*,*,#374,.T.); +#533=ORIENTED_EDGE('',*,*,#369,.F.); +#534=ORIENTED_EDGE('',*,*,#374,.F.); +#535=PLANE('',#601); +#536=PLANE('',#602); +#537=PLANE('',#605); +#538=PLANE('',#608); +#539=PLANE('',#609); +#540=PLANE('',#616); +#541=PLANE('',#617); +#542=PLANE('',#618); +#543=PLANE('',#619); +#544=PLANE('',#621); +#545=PLANE('',#622); +#546=PLANE('',#623); +#547=PLANE('',#627); +#548=PLANE('',#628); +#549=PLANE('',#629); +#550=PLANE('',#633); +#551=PLANE('',#634); +#552=PLANE('',#636); +#553=PLANE('',#637); +#554=PLANE('',#641); +#555=PLANE('',#642); +#556=PLANE('',#643); +#557=ADVANCED_FACE('',(#47),#535,.T.); +#558=ADVANCED_FACE('',(#48,#31,#32),#536,.T.); +#559=ADVANCED_FACE('',(#49,#33,#34),#537,.T.); +#560=ADVANCED_FACE('',(#50),#538,.T.); +#561=ADVANCED_FACE('',(#51,#35,#36),#539,.T.); +#562=ADVANCED_FACE('',(#52),#540,.T.); +#563=ADVANCED_FACE('',(#53),#541,.T.); +#564=ADVANCED_FACE('',(#54),#542,.T.); +#565=ADVANCED_FACE('',(#55),#543,.T.); +#566=ADVANCED_FACE('',(#56),#15,.F.); +#567=ADVANCED_FACE('',(#57),#544,.T.); +#568=ADVANCED_FACE('',(#58),#545,.T.); +#569=ADVANCED_FACE('',(#59,#37),#546,.T.); +#570=ADVANCED_FACE('',(#60),#547,.F.); +#571=ADVANCED_FACE('',(#61),#548,.T.); +#572=ADVANCED_FACE('',(#62),#549,.T.); +#573=ADVANCED_FACE('',(#63),#16,.T.); +#574=ADVANCED_FACE('',(#64),#17,.T.); +#575=ADVANCED_FACE('',(#65),#18,.F.); +#576=ADVANCED_FACE('',(#66),#550,.T.); +#577=ADVANCED_FACE('',(#67),#551,.T.); +#578=ADVANCED_FACE('',(#68),#19,.F.); +#579=ADVANCED_FACE('',(#69),#552,.T.); +#580=ADVANCED_FACE('',(#70,#38),#553,.T.); +#581=ADVANCED_FACE('',(#71),#554,.F.); +#582=ADVANCED_FACE('',(#72),#555,.T.); +#583=ADVANCED_FACE('',(#73),#556,.T.); +#584=ADVANCED_FACE('',(#74),#20,.T.); +#585=ADVANCED_FACE('',(#75),#21,.T.); +#586=ADVANCED_FACE('',(#76),#22,.F.); +#587=CLOSED_SHELL('',(#557,#558,#559,#560,#561,#562,#563,#564,#565,#566, +#567,#568,#569,#570,#571,#572,#573,#574,#575,#576,#577,#578,#579,#580,#581, +#582,#583,#584,#585,#586)); +#588=DERIVED_UNIT_ELEMENT(#590,1.); +#589=DERIVED_UNIT_ELEMENT(#972,-3.); +#590=( +MASS_UNIT() +NAMED_UNIT(*) +SI_UNIT(.KILO.,.GRAM.) +); +#591=DERIVED_UNIT((#588,#589)); +#592=MEASURE_REPRESENTATION_ITEM('density measure', +POSITIVE_RATIO_MEASURE(7850.),#591); +#593=PROPERTY_DEFINITION_REPRESENTATION(#598,#595); +#594=PROPERTY_DEFINITION_REPRESENTATION(#599,#596); +#595=REPRESENTATION('material name',(#597),#970); +#596=REPRESENTATION('density',(#592),#970); +#597=DESCRIPTIVE_REPRESENTATION_ITEM('Steel','Steel'); +#598=PROPERTY_DEFINITION('material property','material name',#979); +#599=PROPERTY_DEFINITION('material property','density of part',#979); +#600=AXIS2_PLACEMENT_3D('',#805,#647,#648); +#601=AXIS2_PLACEMENT_3D('',#806,#649,#650); +#602=AXIS2_PLACEMENT_3D('',#827,#661,#662); +#603=AXIS2_PLACEMENT_3D('',#842,#670,#671); +#604=AXIS2_PLACEMENT_3D('',#844,#672,#673); +#605=AXIS2_PLACEMENT_3D('',#845,#674,#675); +#606=AXIS2_PLACEMENT_3D('',#860,#683,#684); +#607=AXIS2_PLACEMENT_3D('',#862,#685,#686); +#608=AXIS2_PLACEMENT_3D('',#863,#687,#688); +#609=AXIS2_PLACEMENT_3D('',#867,#691,#692); +#610=AXIS2_PLACEMENT_3D('',#871,#694,#695); +#611=AXIS2_PLACEMENT_3D('',#875,#697,#698); +#612=AXIS2_PLACEMENT_3D('',#880,#701,#702); +#613=AXIS2_PLACEMENT_3D('',#884,#704,#705); +#614=AXIS2_PLACEMENT_3D('',#889,#708,#709); +#615=AXIS2_PLACEMENT_3D('',#891,#710,#711); +#616=AXIS2_PLACEMENT_3D('',#892,#712,#713); +#617=AXIS2_PLACEMENT_3D('',#896,#716,#717); +#618=AXIS2_PLACEMENT_3D('',#900,#720,#721); +#619=AXIS2_PLACEMENT_3D('',#902,#723,#724); +#620=AXIS2_PLACEMENT_3D('',#904,#726,#727); +#621=AXIS2_PLACEMENT_3D('',#906,#729,#730); +#622=AXIS2_PLACEMENT_3D('',#909,#733,#734); +#623=AXIS2_PLACEMENT_3D('',#910,#735,#736); +#624=AXIS2_PLACEMENT_3D('',#915,#738,#739); +#625=AXIS2_PLACEMENT_3D('',#919,#742,#743); +#626=AXIS2_PLACEMENT_3D('',#921,#744,#745); +#627=AXIS2_PLACEMENT_3D('',#922,#746,#747); +#628=AXIS2_PLACEMENT_3D('',#924,#749,#750); +#629=AXIS2_PLACEMENT_3D('',#926,#752,#753); +#630=AXIS2_PLACEMENT_3D('',#929,#756,#757); +#631=AXIS2_PLACEMENT_3D('',#930,#758,#759); +#632=AXIS2_PLACEMENT_3D('',#931,#760,#761); +#633=AXIS2_PLACEMENT_3D('',#933,#763,#764); +#634=AXIS2_PLACEMENT_3D('',#940,#769,#770); +#635=AXIS2_PLACEMENT_3D('',#942,#772,#773); +#636=AXIS2_PLACEMENT_3D('',#944,#775,#776); +#637=AXIS2_PLACEMENT_3D('',#945,#777,#778); +#638=AXIS2_PLACEMENT_3D('',#950,#780,#781); +#639=AXIS2_PLACEMENT_3D('',#954,#784,#785); +#640=AXIS2_PLACEMENT_3D('',#956,#786,#787); +#641=AXIS2_PLACEMENT_3D('',#957,#788,#789); +#642=AXIS2_PLACEMENT_3D('',#959,#791,#792); +#643=AXIS2_PLACEMENT_3D('',#961,#794,#795); +#644=AXIS2_PLACEMENT_3D('',#964,#798,#799); +#645=AXIS2_PLACEMENT_3D('',#965,#800,#801); +#646=AXIS2_PLACEMENT_3D('',#966,#802,#803); +#647=DIRECTION('axis',(0.,0.,1.)); +#648=DIRECTION('refdir',(1.,0.,0.)); +#649=DIRECTION('center_axis',(0.,-1.,0.)); +#650=DIRECTION('ref_axis',(1.,0.,0.)); +#651=DIRECTION('',(0.719339800338651,0.,-0.694658370458997)); +#652=DIRECTION('',(-0.779494124126418,0.,-0.626409538921933)); +#653=DIRECTION('',(-1.,0.,0.)); +#654=DIRECTION('',(0.719339800338651,0.,-0.694658370458997)); +#655=DIRECTION('',(9.09177391738478E-17,0.,-1.)); +#656=DIRECTION('',(1.,0.,0.)); +#657=DIRECTION('',(0.,0.,1.)); +#658=DIRECTION('',(-1.,0.,0.)); +#659=DIRECTION('',(7.60207536292217E-17,0.,1.)); +#660=DIRECTION('',(-0.779494124126418,0.,-0.626409538921933)); +#661=DIRECTION('center_axis',(0.,0.,1.)); +#662=DIRECTION('ref_axis',(1.,0.,0.)); +#663=DIRECTION('',(0.,1.,0.)); +#664=DIRECTION('',(1.,0.,0.)); +#665=DIRECTION('',(0.,-1.,0.)); +#666=DIRECTION('',(-1.,0.,0.)); +#667=DIRECTION('',(1.3010426069826E-16,-1.,0.)); +#668=DIRECTION('',(-1.,0.,0.)); +#669=DIRECTION('',(0.,1.,0.)); +#670=DIRECTION('center_axis',(0.,0.,1.)); +#671=DIRECTION('ref_axis',(-1.,0.,0.)); +#672=DIRECTION('center_axis',(0.,0.,-1.)); +#673=DIRECTION('ref_axis',(-1.,0.,0.)); +#674=DIRECTION('center_axis',(0.,0.,-1.)); +#675=DIRECTION('ref_axis',(-1.,0.,0.)); +#676=DIRECTION('',(1.3010426069826E-16,-1.,0.)); +#677=DIRECTION('',(-1.,0.,0.)); +#678=DIRECTION('',(0.,-1.,0.)); +#679=DIRECTION('',(-1.,0.,0.)); +#680=DIRECTION('',(0.,1.,0.)); +#681=DIRECTION('',(0.,1.,0.)); +#682=DIRECTION('',(-1.,0.,0.)); +#683=DIRECTION('center_axis',(0.,0.,-1.)); +#684=DIRECTION('ref_axis',(-1.,0.,0.)); +#685=DIRECTION('center_axis',(0.,0.,1.)); +#686=DIRECTION('ref_axis',(-1.,0.,0.)); +#687=DIRECTION('center_axis',(-1.,0.,7.60207536292217E-17)); +#688=DIRECTION('ref_axis',(7.60207536292217E-17,0.,1.)); +#689=DIRECTION('',(7.60207536292217E-17,0.,1.)); +#690=DIRECTION('',(0.,1.,0.)); +#691=DIRECTION('center_axis',(-0.694658370458997,0.,-0.719339800338651)); +#692=DIRECTION('ref_axis',(-0.719339800338651,0.,0.694658370458997)); +#693=DIRECTION('',(0.,1.,0.)); +#694=DIRECTION('center_axis',(-0.694658370458997,0.,-0.719339800338651)); +#695=DIRECTION('ref_axis',(-0.719339800338651,0.,0.694658370458997)); +#696=DIRECTION('',(-0.719339800338651,0.,0.694658370458997)); +#697=DIRECTION('center_axis',(-0.694658370458997,0.,-0.719339800338651)); +#698=DIRECTION('ref_axis',(0.719339800338651,0.,-0.694658370458997)); +#699=DIRECTION('',(0.,1.,0.)); +#700=DIRECTION('',(0.,-1.,0.)); +#701=DIRECTION('center_axis',(0.694658370458997,0.,0.719339800338651)); +#702=DIRECTION('ref_axis',(0.719339800338651,0.,-0.694658370458997)); +#703=DIRECTION('',(-0.719339800338651,0.,0.694658370458997)); +#704=DIRECTION('center_axis',(0.694658370458997,0.,0.719339800338651)); +#705=DIRECTION('ref_axis',(-0.719339800338651,0.,0.694658370458997)); +#706=DIRECTION('',(0.,-1.,0.)); +#707=DIRECTION('',(0.,1.,0.)); +#708=DIRECTION('center_axis',(-0.694658370458997,0.,-0.719339800338651)); +#709=DIRECTION('ref_axis',(0.719339800338651,0.,-0.694658370458997)); +#710=DIRECTION('center_axis',(0.694658370458997,0.,0.719339800338651)); +#711=DIRECTION('ref_axis',(0.719339800338651,0.,-0.694658370458997)); +#712=DIRECTION('center_axis',(0.,0.,-1.)); +#713=DIRECTION('ref_axis',(-1.,0.,0.)); +#714=DIRECTION('',(-1.,0.,0.)); +#715=DIRECTION('',(0.,1.,0.)); +#716=DIRECTION('center_axis',(0.694658370458997,0.,0.719339800338651)); +#717=DIRECTION('ref_axis',(0.719339800338651,0.,-0.694658370458997)); +#718=DIRECTION('',(0.719339800338651,0.,-0.694658370458997)); +#719=DIRECTION('',(0.,1.,0.)); +#720=DIRECTION('center_axis',(1.,0.,9.09177391738478E-17)); +#721=DIRECTION('ref_axis',(9.09177391738478E-17,0.,-1.)); +#722=DIRECTION('',(9.09177391738478E-17,0.,-1.)); +#723=DIRECTION('center_axis',(-1.,0.,0.)); +#724=DIRECTION('ref_axis',(0.,0.,1.)); +#725=DIRECTION('',(0.,0.,1.)); +#726=DIRECTION('center_axis',(0.,0.,-1.)); +#727=DIRECTION('ref_axis',(-1.,0.,0.)); +#728=DIRECTION('',(0.,0.,-1.)); +#729=DIRECTION('center_axis',(-1.,-1.3010426069826E-16,0.)); +#730=DIRECTION('ref_axis',(1.3010426069826E-16,-1.,0.)); +#731=DIRECTION('',(0.,0.,1.)); +#732=DIRECTION('',(0.,0.,1.)); +#733=DIRECTION('center_axis',(0.,1.,0.)); +#734=DIRECTION('ref_axis',(0.,0.,1.)); +#735=DIRECTION('center_axis',(0.694658370458997,0.,0.719339800338652)); +#736=DIRECTION('ref_axis',(0.719339800338652,0.,-0.694658370458997)); +#737=DIRECTION('',(0.719339800338651,0.,-0.694658370458997)); +#738=DIRECTION('center_axis',(0.694658370458997,0.,0.719339800338652)); +#739=DIRECTION('ref_axis',(-0.719339800338652,0.,0.694658370458997)); +#740=DIRECTION('',(0.,1.,0.)); +#741=DIRECTION('',(0.,1.,0.)); +#742=DIRECTION('center_axis',(0.694658370458997,0.,0.719339800338652)); +#743=DIRECTION('ref_axis',(0.719339800338652,0.,-0.694658370458997)); +#744=DIRECTION('center_axis',(0.694658370458997,0.,0.719339800338651)); +#745=DIRECTION('ref_axis',(0.719339800338651,0.,-0.694658370458997)); +#746=DIRECTION('center_axis',(0.626409538921933,0.,-0.779494124126418)); +#747=DIRECTION('ref_axis',(-0.779494124126418,0.,-0.626409538921933)); +#748=DIRECTION('',(-0.779494124126418,0.,-0.626409538921933)); +#749=DIRECTION('center_axis',(0.626409538921933,0.,-0.779494124126418)); +#750=DIRECTION('ref_axis',(-0.779494124126418,0.,-0.626409538921933)); +#751=DIRECTION('',(-0.779494124126418,0.,-0.626409538921933)); +#752=DIRECTION('center_axis',(0.,-1.,0.)); +#753=DIRECTION('ref_axis',(1.,0.,0.)); +#754=DIRECTION('',(-0.779494124126418,0.,-0.626409538921933)); +#755=DIRECTION('',(-0.779494124126418,0.,-0.626409538921933)); +#756=DIRECTION('center_axis',(-0.779494124126418,0.,-0.626409538921933)); +#757=DIRECTION('ref_axis',(-0.626409538921933,0.,0.779494124126418)); +#758=DIRECTION('center_axis',(0.779494124126418,0.,0.626409538921933)); +#759=DIRECTION('ref_axis',(0.626409538921933,0.,-0.779494124126418)); +#760=DIRECTION('center_axis',(0.694658370458997,0.,0.719339800338651)); +#761=DIRECTION('ref_axis',(0.719339800338651,0.,-0.694658370458997)); +#762=DIRECTION('',(0.694658370458997,0.,0.719339800338651)); +#763=DIRECTION('center_axis',(0.,1.,0.)); +#764=DIRECTION('ref_axis',(1.,0.,0.)); +#765=DIRECTION('',(0.719339800338651,0.,-0.694658370458997)); +#766=DIRECTION('',(-0.779494124126418,0.,-0.626409538921933)); +#767=DIRECTION('',(0.,0.,1.)); +#768=DIRECTION('',(-0.779494124126418,0.,-0.626409538921933)); +#769=DIRECTION('center_axis',(-1.,0.,0.)); +#770=DIRECTION('ref_axis',(0.,0.,1.)); +#771=DIRECTION('',(0.,0.,1.)); +#772=DIRECTION('center_axis',(0.,0.,-1.)); +#773=DIRECTION('ref_axis',(-1.,0.,0.)); +#774=DIRECTION('',(0.,0.,-1.)); +#775=DIRECTION('center_axis',(0.,-1.,0.)); +#776=DIRECTION('ref_axis',(0.,0.,1.)); +#777=DIRECTION('center_axis',(0.694658370458997,0.,0.719339800338652)); +#778=DIRECTION('ref_axis',(0.719339800338652,0.,-0.694658370458997)); +#779=DIRECTION('',(0.719339800338651,0.,-0.694658370458997)); +#780=DIRECTION('center_axis',(-0.694658370458997,0.,-0.719339800338652)); +#781=DIRECTION('ref_axis',(0.719339800338652,0.,-0.694658370458997)); +#782=DIRECTION('',(0.,-1.,0.)); +#783=DIRECTION('',(0.,-1.,0.)); +#784=DIRECTION('center_axis',(-0.694658370458997,0.,-0.719339800338652)); +#785=DIRECTION('ref_axis',(-0.719339800338652,0.,0.694658370458997)); +#786=DIRECTION('center_axis',(-0.694658370458997,0.,-0.719339800338651)); +#787=DIRECTION('ref_axis',(0.719339800338651,0.,-0.694658370458997)); +#788=DIRECTION('center_axis',(0.626409538921933,0.,-0.779494124126418)); +#789=DIRECTION('ref_axis',(-0.779494124126418,0.,-0.626409538921933)); +#790=DIRECTION('',(-0.779494124126418,0.,-0.626409538921933)); +#791=DIRECTION('center_axis',(0.626409538921933,0.,-0.779494124126418)); +#792=DIRECTION('ref_axis',(-0.779494124126418,0.,-0.626409538921933)); +#793=DIRECTION('',(-0.779494124126418,0.,-0.626409538921933)); +#794=DIRECTION('center_axis',(0.,1.,0.)); +#795=DIRECTION('ref_axis',(1.,0.,0.)); +#796=DIRECTION('',(-0.779494124126418,0.,-0.626409538921933)); +#797=DIRECTION('',(-0.779494124126418,0.,-0.626409538921933)); +#798=DIRECTION('center_axis',(-0.779494124126418,0.,-0.626409538921933)); +#799=DIRECTION('ref_axis',(-0.626409538921933,0.,0.779494124126418)); +#800=DIRECTION('center_axis',(0.779494124126418,0.,0.626409538921933)); +#801=DIRECTION('ref_axis',(0.626409538921933,0.,-0.779494124126418)); +#802=DIRECTION('center_axis',(0.694658370458997,0.,0.719339800338651)); +#803=DIRECTION('ref_axis',(0.719339800338651,0.,-0.694658370458997)); +#804=DIRECTION('',(0.694658370458997,0.,0.719339800338651)); +#805=CARTESIAN_POINT('',(0.,0.,0.)); +#806=CARTESIAN_POINT('Origin',(0.00754316629139926,-0.02,-0.013307266963672)); +#807=CARTESIAN_POINT('',(0.00275,-0.02,-0.0172900715621611)); +#808=CARTESIAN_POINT('',(0.0142594368054184,-0.02,-0.0284046054895051)); +#809=CARTESIAN_POINT('',(0.00850471840270921,-0.02,-0.0228473385258331)); +#810=CARTESIAN_POINT('',(0.0115094368054184,-0.02,-0.030614533927344)); +#811=CARTESIAN_POINT('',(0.0128844368054184,-0.02,-0.0295095697084245)); +#812=CARTESIAN_POINT('',(0.0215863325827985,-0.02,-0.030614533927344)); +#813=CARTESIAN_POINT('',(0.0165478846941085,-0.02,-0.030614533927344)); +#814=CARTESIAN_POINT('',(0.0055,-0.02,-0.0150801431243222)); +#815=CARTESIAN_POINT('',(0.0135431662913993,-0.02,-0.0228473385258331)); +#816=CARTESIAN_POINT('',(0.0055,-0.02,0.004)); +#817=CARTESIAN_POINT('',(0.0055,-0.02,-0.00554007156216108)); +#818=CARTESIAN_POINT('',(-0.0065,-0.02,0.004)); +#819=CARTESIAN_POINT('',(-0.00325,-0.02,0.004)); +#820=CARTESIAN_POINT('',(-0.0065,-0.02,0.)); +#821=CARTESIAN_POINT('',(-0.0065,-0.02,0.002)); +#822=CARTESIAN_POINT('',(0.,-0.02,0.)); +#823=CARTESIAN_POINT('',(-0.00325,-0.02,0.)); +#824=CARTESIAN_POINT('',(-1.48240469576982E-18,-0.02,-0.0195)); +#825=CARTESIAN_POINT('',(-7.41202347884911E-19,-0.02,-0.00975)); +#826=CARTESIAN_POINT('',(0.001375,-0.02,-0.0183950357810805)); +#827=CARTESIAN_POINT('Origin',(-0.00325,-0.02,0.004)); +#828=CARTESIAN_POINT('',(0.0055,0.02,0.004)); +#829=CARTESIAN_POINT('',(0.0055,-0.02,0.004)); +#830=CARTESIAN_POINT('',(-0.0065,0.02,0.004)); +#831=CARTESIAN_POINT('',(-0.00325,0.02,0.004)); +#832=CARTESIAN_POINT('',(-0.0065,0.005,0.004)); +#833=CARTESIAN_POINT('',(-0.0065,0.02,0.004)); +#834=CARTESIAN_POINT('',(-0.0015,0.005,0.004)); +#835=CARTESIAN_POINT('',(-0.004,0.005,0.004)); +#836=CARTESIAN_POINT('',(-0.0015,-0.005,0.004)); +#837=CARTESIAN_POINT('',(-0.0015,-0.0025,0.004)); +#838=CARTESIAN_POINT('',(-0.0065,-0.005,0.004)); +#839=CARTESIAN_POINT('',(-0.004,-0.005,0.004)); +#840=CARTESIAN_POINT('',(-0.0065,-0.02,0.004)); +#841=CARTESIAN_POINT('',(-0.0019,0.01,0.004)); +#842=CARTESIAN_POINT('Origin',(-0.0035,0.01,0.004)); +#843=CARTESIAN_POINT('',(-0.0019,-0.01,0.004)); +#844=CARTESIAN_POINT('Origin',(-0.0035,-0.01,0.004)); +#845=CARTESIAN_POINT('Origin',(-0.00325,-0.02,0.)); +#846=CARTESIAN_POINT('',(-0.0015,0.005,0.)); +#847=CARTESIAN_POINT('',(-0.0015,-0.005,0.)); +#848=CARTESIAN_POINT('',(-0.0015,-0.0025,0.)); +#849=CARTESIAN_POINT('',(-0.0065,0.005,0.)); +#850=CARTESIAN_POINT('',(-0.004,0.005,0.)); +#851=CARTESIAN_POINT('',(-0.0065,0.02,0.)); +#852=CARTESIAN_POINT('',(-0.0065,0.02,0.)); +#853=CARTESIAN_POINT('',(0.,0.02,0.)); +#854=CARTESIAN_POINT('',(-0.00325,0.02,0.)); +#855=CARTESIAN_POINT('',(0.,-0.02,0.)); +#856=CARTESIAN_POINT('',(-0.0065,-0.005,0.)); +#857=CARTESIAN_POINT('',(-0.0065,-0.02,0.)); +#858=CARTESIAN_POINT('',(-0.004,-0.005,0.)); +#859=CARTESIAN_POINT('',(-0.0019,-0.01,0.)); +#860=CARTESIAN_POINT('Origin',(-0.0035,-0.01,0.)); +#861=CARTESIAN_POINT('',(-0.0019,0.01,0.)); +#862=CARTESIAN_POINT('Origin',(-0.0035,0.01,0.)); +#863=CARTESIAN_POINT('Origin',(-7.41202347884911E-19,-0.02,-0.00975)); +#864=CARTESIAN_POINT('',(-1.48240469576982E-18,0.02,-0.0195)); +#865=CARTESIAN_POINT('',(-7.41202347884911E-19,0.02,-0.00975)); +#866=CARTESIAN_POINT('',(-1.48240469576982E-18,-0.02,-0.0195)); +#867=CARTESIAN_POINT('Origin',(0.00575471840270921,-0.02,-0.025057266963672)); +#868=CARTESIAN_POINT('',(0.0115094368054184,-0.037,-0.030614533927344)); +#869=CARTESIAN_POINT('',(0.0115094368054184,-0.042,-0.030614533927344)); +#870=CARTESIAN_POINT('',(0.00788403707554645,-0.042,-0.027113526104018)); +#871=CARTESIAN_POINT('Origin',(0.00788403707554645,-0.037,-0.027113526104018)); +#872=CARTESIAN_POINT('',(0.00362539972987196,-0.042,-0.0230010078233259)); +#873=CARTESIAN_POINT('',(0.00575471840270921,-0.042,-0.025057266963672)); +#874=CARTESIAN_POINT('',(-1.48240469576982E-18,-0.037,-0.0195)); +#875=CARTESIAN_POINT('Origin',(0.00362539972987196,-0.037,-0.0230010078233259)); +#876=CARTESIAN_POINT('',(-1.48240469576982E-18,-0.042,-0.0195)); +#877=CARTESIAN_POINT('',(-1.48240469576982E-18,0.037,-0.0195)); +#878=CARTESIAN_POINT('',(-1.48240469576982E-18,0.042,-0.0195)); +#879=CARTESIAN_POINT('',(0.00362539972987196,0.042,-0.0230010078233259)); +#880=CARTESIAN_POINT('Origin',(0.00362539972987196,0.037,-0.0230010078233259)); +#881=CARTESIAN_POINT('',(0.00788403707554645,0.042,-0.027113526104018)); +#882=CARTESIAN_POINT('',(0.00575471840270921,0.042,-0.025057266963672)); +#883=CARTESIAN_POINT('',(0.0115094368054184,0.037,-0.030614533927344)); +#884=CARTESIAN_POINT('Origin',(0.00788403707554645,0.037,-0.027113526104018)); +#885=CARTESIAN_POINT('',(0.0115094368054184,0.02,-0.030614533927344)); +#886=CARTESIAN_POINT('',(0.0115094368054184,0.042,-0.030614533927344)); +#887=CARTESIAN_POINT('',(0.0115094368054184,-0.02,-0.030614533927344)); +#888=CARTESIAN_POINT('',(0.00431603880203191,0.0375,-0.023667950222754)); +#889=CARTESIAN_POINT('Origin',(0.00575471840270921,0.0375,-0.025057266963672)); +#890=CARTESIAN_POINT('',(0.00431603880203191,-0.0375,-0.023667950222754)); +#891=CARTESIAN_POINT('Origin',(0.00575471840270921,-0.0375,-0.025057266963672)); +#892=CARTESIAN_POINT('Origin',(0.0165478846941085,-0.02,-0.030614533927344)); +#893=CARTESIAN_POINT('',(0.0215863325827985,0.02,-0.030614533927344)); +#894=CARTESIAN_POINT('',(0.0165478846941085,0.02,-0.030614533927344)); +#895=CARTESIAN_POINT('',(0.0215863325827985,-0.02,-0.030614533927344)); +#896=CARTESIAN_POINT('Origin',(0.0135431662913993,-0.02,-0.0228473385258331)); +#897=CARTESIAN_POINT('',(0.0055,0.02,-0.0150801431243222)); +#898=CARTESIAN_POINT('',(0.0135431662913993,0.02,-0.0228473385258331)); +#899=CARTESIAN_POINT('',(0.0055,-0.02,-0.0150801431243222)); +#900=CARTESIAN_POINT('Origin',(0.0055,-0.02,-0.00554007156216108)); +#901=CARTESIAN_POINT('',(0.0055,0.02,-0.00554007156216108)); +#902=CARTESIAN_POINT('Origin',(-0.0065,-0.02,0.002)); +#903=CARTESIAN_POINT('',(-0.0065,-0.005,-0.0306165339273439)); +#904=CARTESIAN_POINT('Origin',(-0.0035,-0.01,0.004)); +#905=CARTESIAN_POINT('',(-0.0019,-0.01,0.004)); +#906=CARTESIAN_POINT('Origin',(-0.0015,-0.0025,-0.0306165339273439)); +#907=CARTESIAN_POINT('',(-0.0015,-0.005,-0.0306165339273439)); +#908=CARTESIAN_POINT('',(-0.0015,0.005,-0.0306165339273439)); +#909=CARTESIAN_POINT('Origin',(-0.004,-0.005,-0.0306165339273439)); +#910=CARTESIAN_POINT('Origin',(0.00850471840270921,-0.042,-0.0228473385258331)); +#911=CARTESIAN_POINT('',(0.00637539972987196,-0.042,-0.020791079385487)); +#912=CARTESIAN_POINT('',(0.0106340370755465,-0.042,-0.0249035976661791)); +#913=CARTESIAN_POINT('',(0.00850471840270921,-0.042,-0.0228473385258331)); +#914=CARTESIAN_POINT('',(0.0142594368054184,-0.037,-0.0284046054895051)); +#915=CARTESIAN_POINT('Origin',(0.0106340370755465,-0.037,-0.0249035976661791)); +#916=CARTESIAN_POINT('',(0.0142594368054184,-0.042,-0.0284046054895051)); +#917=CARTESIAN_POINT('',(0.00275,-0.037,-0.0172900715621611)); +#918=CARTESIAN_POINT('',(0.00275,-0.042,-0.0172900715621611)); +#919=CARTESIAN_POINT('Origin',(0.00637539972987196,-0.037,-0.020791079385487)); +#920=CARTESIAN_POINT('',(0.00674734309863841,-0.0375,-0.0211502609215687)); +#921=CARTESIAN_POINT('Origin',(0.0081860226993157,-0.0375,-0.0225395776624867)); +#922=CARTESIAN_POINT('Origin',(0.001375,-0.042,-0.0183950357810805)); +#923=CARTESIAN_POINT('',(0.00275,-0.037,-0.0172900715621611)); +#924=CARTESIAN_POINT('Origin',(0.0128844368054184,-0.042,-0.0295095697084245)); +#925=CARTESIAN_POINT('',(0.0128844368054184,-0.037,-0.0295095697084245)); +#926=CARTESIAN_POINT('Origin',(0.00754316629139926,-0.042,-0.013307266963672)); +#927=CARTESIAN_POINT('',(0.0144182756934675,-0.042,-0.0218625443798838)); +#928=CARTESIAN_POINT('',(0.00313204769460966,-0.042,-0.0233974706206321)); +#929=CARTESIAN_POINT('Origin',(0.00975238911080876,-0.037,-0.0256120990877924)); +#930=CARTESIAN_POINT('Origin',(0.0107391626922731,-0.037,-0.0172843143430875)); +#931=CARTESIAN_POINT('Origin',(0.00575471840270921,-0.0375,-0.025057266963672)); +#932=CARTESIAN_POINT('',(0.00431603880203191,-0.0375,-0.023667950222754)); +#933=CARTESIAN_POINT('Origin',(0.00754316629139926,0.02,-0.013307266963672)); +#934=CARTESIAN_POINT('',(0.00275,0.02,-0.0172900715621611)); +#935=CARTESIAN_POINT('',(0.0142594368054184,0.02,-0.0284046054895051)); +#936=CARTESIAN_POINT('',(0.00850471840270921,0.02,-0.0228473385258331)); +#937=CARTESIAN_POINT('',(0.001375,0.02,-0.0183950357810805)); +#938=CARTESIAN_POINT('',(-0.0065,0.02,0.002)); +#939=CARTESIAN_POINT('',(0.0128844368054184,0.02,-0.0295095697084245)); +#940=CARTESIAN_POINT('Origin',(-0.0065,0.02,0.002)); +#941=CARTESIAN_POINT('',(-0.0065,0.005,-0.0306165339273439)); +#942=CARTESIAN_POINT('Origin',(-0.0035,0.01,0.004)); +#943=CARTESIAN_POINT('',(-0.0019,0.01,0.004)); +#944=CARTESIAN_POINT('Origin',(-0.004,0.005,-0.0306165339273439)); +#945=CARTESIAN_POINT('Origin',(0.00850471840270921,0.042,-0.0228473385258331)); +#946=CARTESIAN_POINT('',(0.00637539972987196,0.042,-0.020791079385487)); +#947=CARTESIAN_POINT('',(0.0106340370755465,0.042,-0.0249035976661791)); +#948=CARTESIAN_POINT('',(0.00850471840270921,0.042,-0.0228473385258331)); +#949=CARTESIAN_POINT('',(0.00275,0.037,-0.0172900715621611)); +#950=CARTESIAN_POINT('Origin',(0.00637539972987196,0.037,-0.020791079385487)); +#951=CARTESIAN_POINT('',(0.00275,0.042,-0.0172900715621611)); +#952=CARTESIAN_POINT('',(0.0142594368054184,0.037,-0.0284046054895051)); +#953=CARTESIAN_POINT('',(0.0142594368054184,0.042,-0.0284046054895051)); +#954=CARTESIAN_POINT('Origin',(0.0106340370755465,0.037,-0.0249035976661791)); +#955=CARTESIAN_POINT('',(0.00674734309863841,0.0375,-0.0211502609215687)); +#956=CARTESIAN_POINT('Origin',(0.0081860226993157,0.0375,-0.0225395776624867)); +#957=CARTESIAN_POINT('Origin',(0.001375,0.042,-0.0183950357810805)); +#958=CARTESIAN_POINT('',(0.00275,0.037,-0.0172900715621611)); +#959=CARTESIAN_POINT('Origin',(0.0128844368054184,0.042,-0.0295095697084245)); +#960=CARTESIAN_POINT('',(0.0128844368054184,0.037,-0.0295095697084245)); +#961=CARTESIAN_POINT('Origin',(0.00754316629139926,0.042,-0.013307266963672)); +#962=CARTESIAN_POINT('',(0.00313204769460966,0.042,-0.0233974706206321)); +#963=CARTESIAN_POINT('',(0.0144182756934675,0.042,-0.0218625443798838)); +#964=CARTESIAN_POINT('Origin',(0.00975238911080876,0.037,-0.0256120990877924)); +#965=CARTESIAN_POINT('Origin',(0.0107391626922731,0.037,-0.0172843143430875)); +#966=CARTESIAN_POINT('Origin',(0.00575471840270921,0.0375,-0.025057266963672)); +#967=CARTESIAN_POINT('',(0.00431603880203191,0.0375,-0.023667950222754)); +#968=UNCERTAINTY_MEASURE_WITH_UNIT(LENGTH_MEASURE(1.E-5),#972, +'DISTANCE_ACCURACY_VALUE', +'Maximum model space distance between geometric entities at asserted c +onnectivities'); +#969=UNCERTAINTY_MEASURE_WITH_UNIT(LENGTH_MEASURE(1.E-5),#972, +'DISTANCE_ACCURACY_VALUE', +'Maximum model space distance between geometric entities at asserted c +onnectivities'); +#970=( +GEOMETRIC_REPRESENTATION_CONTEXT(3) +GLOBAL_UNCERTAINTY_ASSIGNED_CONTEXT((#968)) +GLOBAL_UNIT_ASSIGNED_CONTEXT((#972,#973,#974)) +REPRESENTATION_CONTEXT('','3D') +); +#971=( +GEOMETRIC_REPRESENTATION_CONTEXT(3) +GLOBAL_UNCERTAINTY_ASSIGNED_CONTEXT((#969)) +GLOBAL_UNIT_ASSIGNED_CONTEXT((#972,#973,#974)) +REPRESENTATION_CONTEXT('','3D') +); +#972=( +LENGTH_UNIT() +NAMED_UNIT(*) +SI_UNIT($,.METRE.) +); +#973=( +NAMED_UNIT(*) +PLANE_ANGLE_UNIT() +SI_UNIT($,.RADIAN.) +); +#974=( +NAMED_UNIT(*) +SI_UNIT($,.STERADIAN.) +SOLID_ANGLE_UNIT() +); +#975=SHAPE_DEFINITION_REPRESENTATION(#976,#977); +#976=PRODUCT_DEFINITION_SHAPE('',$,#979); +#977=SHAPE_REPRESENTATION('',(#600),#970); +#978=PRODUCT_DEFINITION_CONTEXT('part definition',#983,'design'); +#979=PRODUCT_DEFINITION('2025-07-25-23-29-53-268','oak - v1',#980,#978); +#980=PRODUCT_DEFINITION_FORMATION('',$,#985); +#981=PRODUCT_RELATED_PRODUCT_CATEGORY('oak - v1','oak - v1',(#985)); +#982=APPLICATION_PROTOCOL_DEFINITION('international standard', +'automotive_design',2009,#983); +#983=APPLICATION_CONTEXT( +'Core Data for Automotive Mechanical Design Process'); +#984=PRODUCT_CONTEXT('part definition',#983,'mechanical'); +#985=PRODUCT('2025-07-25-23-29-53-268','oak - v1','STEP AP242',(#984)); +#986=PRESENTATION_STYLE_ASSIGNMENT((#988)); +#987=PRESENTATION_STYLE_ASSIGNMENT((#989)); +#988=SURFACE_STYLE_USAGE(.BOTH.,#990); +#989=SURFACE_STYLE_USAGE(.BOTH.,#991); +#990=SURFACE_SIDE_STYLE('',(#992)); +#991=SURFACE_SIDE_STYLE('',(#993)); +#992=SURFACE_STYLE_FILL_AREA(#994); +#993=SURFACE_STYLE_FILL_AREA(#995); +#994=FILL_AREA_STYLE('Steel - Satin',(#996)); +#995=FILL_AREA_STYLE('Opaque(157,207,237)',(#997)); +#996=FILL_AREA_STYLE_COLOUR('Steel - Satin',#998); +#997=FILL_AREA_STYLE_COLOUR('Opaque(157,207,237)',#999); +#998=COLOUR_RGB('Steel - Satin',0.627450980392157,0.627450980392157,0.627450980392157); +#999=COLOUR_RGB('Opaque(157,207,237)',0.615686274509804,0.811764705882353, +0.929411764705882); +ENDSEC; +END-ISO-10303-21;