diff --git a/docs/index.rst b/docs/index.rst index 6d7c06c607..bdb6b26d0e 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -27,6 +27,7 @@ Welcome to robosuite's documentation! modules/devices modules/renderers modules/controllers + modules/sonic_g1 .. toctree:: :maxdepth: 1 diff --git a/docs/modules/sonic_g1.rst b/docs/modules/sonic_g1.rst new file mode 100644 index 0000000000..dac45441e0 --- /dev/null +++ b/docs/modules/sonic_g1.rst @@ -0,0 +1,194 @@ +SONIC G1 Setup +=============== + +The **SONIC G1** is the Unitree G1 humanoid registered as a first-class **robosuite** robot +(``SonicG1`` / ``SonicG1Fixed``): a 29-DOF body (legs, 3-DOF waist, 7-DOF arms) plus two Dex3 +three-finger hands (7 DOF each), for 43 actuated DOF. It can be driven either by standard +**robosuite** part controllers (e.g. ``OSC_POSE`` via a ``BASIC`` config) or by NVIDIA's external +**SONIC** GR00T whole-body controller -- an unchanged C++ policy stack (``g1_deploy_onnx_ref``) +that drives the robot over `Unitree DDS `_. The +SONIC path is wired in through the ``SONIC_WBC`` `composite controller `_, which +routes the per-motor PD command streamed by the C++ stack through robosuite's per-part +``JointPositionController`` s. **robosuite** owns the physics clock; on the SONIC path the +``env.step`` action is ignored and control comes from DDS. + +This page covers installing and setting up the integration. For the full design, internals, and +the open `known limitations`_, see the handoff doc referenced at the bottom of this page. + +Prerequisites +------------- + +The two ways of driving the robot have different requirements: + +* **Standard controllers (OSC, etc.)** -- need only a working **robosuite** + MuJoCo install (see + `Installation <../installation.html>`_). **None** of the SONIC stack is required. The + ``SonicG1`` / ``SonicG1Fixed`` robot, the Dex3 grippers, and the assets are all part of + **robosuite**, so a plain ``robosuite.make(...)`` with a ``BASIC`` controller works out of the box. + +* **The live SONIC whole-body controller (**\ ``SONIC_WBC``\ **)** -- additionally needs the external + `GR00T-WholeBodyControl `_ / ``gear_sonic`` + stack, locally at ``/home/ajay/code/GR00T-WholeBodyControl/``: + + - the compiled C++ deploy binary ``gear_sonic_deploy/target/release/g1_deploy_onnx_ref`` + (built per that repo's instructions; ``deploy.sh`` will build it on first run); + - the Unitree SDK2 DDS layer (the C++ and the sim exchange over DDS on domain 0, interface + ``lo``); + - the SONIC model config (PD gains / effort limits) + ``gear_sonic/utils/mujoco_sim/wbc_configs/g1_29dof_sonic_model12.yaml`` and the source model + ``gear_sonic/data/robot_model/model_data/g1/g1_29dof_with_hand.xml``. + +.. admonition:: Interpreter / virtual environment + :class: note + + Run the robosuite side from the SONIC stack's virtual environment so the DDS / Unitree SDK + bindings resolve: ``/home/ajay/code/GR00T-WholeBodyControl/.venv_sim/bin/python`` (managed by + ``uv``; this is the interpreter referenced as ``$VENV`` throughout this page). + +Assets +------ + +The G1 body and Dex3 gripper MJCF assets are generated from the SONIC ``model_data`` and are +**already committed** to the package under ``robosuite/models/assets/robots/sonic_g1/`` and +``robosuite/models/assets/grippers/sonic_dex3_{left,right}.xml`` -- so a normal install needs no +asset build step. + +You only need to regenerate them if the upstream SONIC model changes (or you want to rebuild from +a different ``model_data``). The build script splits SONIC's validated 29-DOF +``g1_29dof_with_hand.xml`` into a robosuite-conformant body plus two Dex3 grippers (copying meshes, +relabeling joints/actuators to robosuite's part-classification convention) so the reassembled robot +is physically identical to the source model (43 DOF, mass preserved): + +.. code-block:: sh + + # needs the GR00T model_data + meshes available locally + python -m robosuite.scripts.build_sonic_g1_assets + +Registered components +--------------------- + +Importing **robosuite** registers all of the following (no extra setup): + +.. list-table:: + :widths: 30 70 + :header-rows: 1 + + * - Component + - Notes + * - ``SonicG1`` + - 29-DOF G1 with a true free-floating 6-DOF base (default base ``NullBase``, which preserves + the top-level pelvis ````); bimanual Dex3 grippers. Registered in + ``robosuite/robots/__init__.py`` ``ROBOT_CLASS_MAPPING``. + * - ``SonicG1Fixed`` + - Same robot with the floating base removed (pelvis welded, default base ``NoActuationBase``) + -- the variant to use for fixed-stance manipulation / OSC tasks. + * - ``SonicDex3LeftGripper`` / ``SonicDex3RightGripper`` + - Dex3 three-finger hands, 7 DOF each (``robosuite/models/grippers/sonic_dex3_gripper.py``). + * - ``SONIC_WBC`` + - Composite controller (``name = "SONIC_WBC"``) that drives ``SonicG1`` from the external SONIC + C++ stack over DDS (``robosuite/controllers/composite/sonic_whole_body_controller.py``). + * - ``default_sonic_g1.json`` + - Default ``SONIC_WBC`` controller config (all parts ``JOINT_POSITION``), at + ``robosuite/controllers/config/robots/default_sonic_g1.json``. + +Quick start +----------- + +Standard controllers (no SONIC stack) +************************************* + +Drop ``SonicG1Fixed`` into any task env and control the arms with the ``BASIC`` (OSC) controller -- +no DDS, no C++, no SONIC config: + +.. code-block:: python + + import numpy as np + import robosuite + from robosuite.controllers import load_composite_controller_config + + cfg = load_composite_controller_config(controller="BASIC") # OSC_POSE arms + env = robosuite.make( + "TwoArmLift", + robots=["SonicG1Fixed"], + controller_configs=cfg, + has_renderer=False, + has_offscreen_renderer=False, + use_camera_obs=False, + control_freq=20, + ) + env.reset() + low, _ = env.action_spec + action = np.zeros_like(low) + action[0] = 0.6 # +x pose delta on the right arm + for _ in range(50): + env.step(action) + env.close() + +SONIC whole-body controller over live DDS +****************************************** + +The C++ SONIC controller and the **robosuite** sim are **separate processes** that exchange over +Unitree DDS, so this is a two-terminal recipe. Start the C++ controller first, then the sim. + +.. code-block:: sh + + VENV=/home/ajay/code/GR00T-WholeBodyControl/.venv_sim/bin/python + + # Terminal 1 -- the C++ SONIC controller (from gear_sonic_deploy): + cd /home/ajay/code/GR00T-WholeBodyControl/gear_sonic_deploy + ./target/release/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 \ + --input-type keyboard --output-type zmq --disable-crc-check + # (or `bash deploy.sh sim`, which builds if needed and defaults to the planner.) + + # Terminal 2 -- the robosuite backend (needs a display; do NOT set MUJOCO_GL=egl): + cd /home/ajay/code/robosuite + $VENV -m robosuite.scripts.collect_sonic_g1_demos --mode dds + +Once both are up, drive the robot from Terminal 1 (e.g. press ``]`` to engage the policy, then +``T`` / ``N`` / ``P`` for recorded motions, or enter planner mode for walking). + +.. admonition:: DDS hygiene and rendering + :class: warning + + * Run exactly **one** C++ controller and **one** backend at a time. Kill a stale C++ process + with ``kill -9 $(pgrep -x g1_deploy_onnx_ref)`` -- **never** ``pkill -f`` (it matches your own + shell command line). + * The on-screen viewer needs a display and must **not** set ``MUJOCO_GL=egl`` (that selects the + headless EGL backend). Offscreen rendering does use ``MUJOCO_GL=egl``. + +Deterministic self-test (no C++) +******************************** + +To exercise the full robot + ``SONIC_WBC`` data-collection pipeline without the SONIC stack, replay +a recorded golden command stream headlessly. This records a robosuite-format ``demo.hdf5``: + +.. code-block:: sh + + python -m robosuite.scripts.collect_sonic_g1_demos \ + --mode replay --motion squat_001__A359 --no-render + +Verifying the install +--------------------- + +Run the SonicG1 test suite. It covers registration, model assembly (43 DOF, mass preserved), OSC +via ``robosuite.make``, the ``SONIC_WBC`` PD dispatch, the floating base, and replay data +collection. With the external SONIC config + golden streams present, all **7** tests pass: + +.. code-block:: sh + + VENV=/home/ajay/code/GR00T-WholeBodyControl/.venv_sim/bin/python + cd /home/ajay/code/robosuite + MUJOCO_GL=egl $VENV -m pytest tests/test_robots/test_sonic_g1.py -q # 7/7 + +Tests that depend on the external SONIC config or golden command streams skip automatically when +those are unavailable, so the registration / assembly / OSC tests still pass on a SONIC-free +install. + +Known limitations +----------------- + +Motion-file playback and offline tracking on the native path reproduce the reference (base_sim) +behavior; interactive **planner** walking via the native path is a known open issue. The +install/setup of the integration is unaffected. For the full design, internals, file index, and the +status of that open issue, see ``/home/ajay/code/sonic_robosuite_design.md``. diff --git a/robosuite/controllers/composite/__init__.py b/robosuite/controllers/composite/__init__.py index 84173bb274..44aa69129c 100644 --- a/robosuite/controllers/composite/__init__.py +++ b/robosuite/controllers/composite/__init__.py @@ -1,5 +1,6 @@ from .composite_controller import CompositeController, HybridMobileBase, WholeBodyIK from .composite_controller import REGISTERED_COMPOSITE_CONTROLLERS_DICT +from .sonic_whole_body_controller import SonicWholeBodyController # registers "SONIC_WBC" ALL_COMPOSITE_CONTROLLERS = REGISTERED_COMPOSITE_CONTROLLERS_DICT.keys() diff --git a/robosuite/controllers/composite/sonic_whole_body_controller.py b/robosuite/controllers/composite/sonic_whole_body_controller.py new file mode 100644 index 0000000000..98f5008fd3 --- /dev/null +++ b/robosuite/controllers/composite/sonic_whole_body_controller.py @@ -0,0 +1,237 @@ +"""SonicWholeBodyController: composite controller that applies the SONIC PD law from the **action**. + +The env action is the per-motor joint-position target q* (MOTOR order: [body q*(29), left-hand q*(7), +right-hand q*(7)] = 43 for SonicG1). The controller routes it to robosuite's per-part +JointPosition(Velocity)Controllers, which evaluate the gravity-comp-free PD law against the live state + + tau_i = kp_i*(q*_i - q_i) + kd_i*(0 - dq_i) (dq*≡0, tau_ff≡0; clipped to per-motor effort) + +The action is produced by a pluggable **source** (live DDS / replay / policy) -- see +``robosuite.utils.sonic.action_sources`` -- NOT pulled from DDS inside the controller. The per-motor +kp/kd are CONSTANT over an episode (the SONIC policy modulates only q*; dq*/tau_ff are zero), so they +are not carried in the action: the live source captures them once from the first command and calls +``set_command_gains``. Until both an action and gains are available the controller holds (no torque), +while a startup elastic band on the pelvis keeps the floating base up during the C++ handoff +(force only, never a qpos write); release it via ``release_band()`` / the viewer '9' key. + +``set_goal`` stores the action; ``run_controller`` routes it. Select via a composite config with +"type": "SONIC_WBC"; the same SonicG1 can instead be driven by standard controllers (OSC etc.). +""" +import os + +import mujoco +import numpy as np +import yaml +from robosuite.controllers.composite.composite_controller import ( + CompositeController, register_composite_controller) +from robosuite.utils.sonic.controller import G1SonicController +from scipy.spatial.transform import Rotation + + +def _wbc_config_path(): + """Path to the SONIC WBC config (effort + joint limits) inside the installed gear_sonic.""" + import gear_sonic + return os.path.join(os.path.dirname(gear_sonic.__file__), + "utils", "mujoco_sim", "wbc_configs", "g1_29dof_sonic_model12.yaml") + + +@register_composite_controller +class SonicWholeBodyController(CompositeController): + name = "SONIC_WBC" + + def __init__(self, sim, robot_model, grippers): + super().__init__(sim, robot_model, grippers) + self.config_path = _wbc_config_path() + with open(self.config_path) as f: + self._cfg = yaml.load(f, Loader=yaml.FullLoader) + self._maps = None # G1SonicController used ONLY for motor maps (no DDS, no exchange) + self._part_plan = None # part -> [(source_key, index-within-source) per joint] + self._action_split = None # (n_body, n_hand) to slice the flat q* action into sources + self._action_q = None # latest q* action (set by set_goal) + self._cmd_gains = {} # {"body": (kp, kd), "lhand": (kp, kd), "rhand": (kp, kd)} (constant) + # Startup elastic band: spring-damper force on the PELVIS body only (never a qpos write), + # holding the floating base up during the C++ handoff. Released via release_band()/'9'. + self._band = None + self._band_ref_rot = None + self._pelvis_bid = None + self.band_enabled = True + + # --- action space: per-motor q* targets in MOTOR order [body(29), L-hand(7), R-hand(7)]. + # --- engine-free (valid before the lazy maps-engine is built): body bounds from the config + # --- joint limits (29, body motor order); hands have no config limits, so use generous bounds + # --- (their values are not used for faithfulness -- only the action dim is load-bearing -- and + # --- the commanded q* is within range). --- + def _num_hand_motors(self): + """Per-hand actuated Dex3 motor count from the model (0 if the robot has no hands).""" + m = self.sim.model._model if hasattr(self.sim.model, "_model") else self.sim.model + acted = {int(m.actuator_trnid[a, 0]) for a in range(m.nu)} + return sum(1 for j in range(m.njnt) if j in acted + and "left_hand" in (mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_JOINT, j) or "")) + + @property + def action_limits(self): + c = self._cfg + body_lo = np.array(c["motor_pos_lower_limit_list"], dtype=float) # 29, body motor order + body_hi = np.array(c["motor_pos_upper_limit_list"], dtype=float) + n_hand = self._num_hand_motors() + hand_lo = np.full(2 * n_hand, -np.pi) + hand_hi = np.full(2 * n_hand, np.pi) + return np.concatenate([body_lo, hand_lo]), np.concatenate([body_hi, hand_hi]) + + def set_goal(self, all_action): + self._action_q = np.asarray(all_action, dtype=float) + + def set_command_gains(self, gains): + """Set the constant per-source PD gains, e.g. from the live source the first time it has a + command. ``gains``: dict {"body": (kp, kd), "lhand": (kp, kd), "rhand": (kp, kd)} (any subset; + entries accumulate). Until a part's source gains are present, that part holds.""" + if gains: + self._cmd_gains.update(gains) + + def update_state(self): + pass + + def reset(self): + self._maps = None + self._part_plan = None + self._action_split = None + self._action_q = None + self._cmd_gains = {} + self._band = None + self._band_ref_rot = None + self._pelvis_bid = None + self.band_enabled = True + + def release_band(self): + """Drop the startup elastic band (manual handoff release). Idempotent.""" + self.band_enabled = False + + def toggle_band(self): + """Flip the startup band on/off (for a viewer key, mirroring base_sim's '9').""" + self.band_enabled = not self.band_enabled + + def _prepare(self): + """Build, once, the per-part action-routing plan + the flat-action split, and configure each + part controller for SONIC (clip output torque to per-motor effort; no gravity comp).""" + engine = self._maps + model = engine._mj_model + # joint name -> (source_key, index-within-that-source, per-motor effort) + cmd_index = {name: ("body", i, float(engine.effort_limit[i])) + for i, name in enumerate(engine.motor_joint_names)} + if engine.has_hands: + for i, name in enumerate(engine._lh_names): + cmd_index[name] = ("lhand", i, float(engine._lh_eff[i])) + for i, name in enumerate(engine._rh_names): + cmd_index[name] = ("rhand", i, float(engine._rh_eff[i])) + + self._part_plan = {} + for part, part_ctrl in self.part_controllers.items(): + joint_names = [mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, int(j)) + for j in part_ctrl.joint_index] + plan, efforts = [], [] + for name in joint_names: + source_key, cmd_idx, effort = cmd_index[name] + plan.append((source_key, cmd_idx)); efforts.append(effort) + self._part_plan[part] = plan + efforts = np.array(efforts) + part_ctrl.torque_limits = np.array([-efforts, efforts]) # == SONIC's effort clip + if hasattr(part_ctrl, "use_torque_compensation"): + part_ctrl.use_torque_compensation = False # SONIC adds no gravity comp + part_ctrl.interpolator = None # apply the command as-is + + # how the flat action splits into per-source blocks (matches the source's concat order) + self._action_split = (engine.num_motors, 7 if engine.has_hands else 0) + + # PELVIS body carrying the floating base (the engine's chosen freejoint) -- where the + # startup band applies its force. None on a fixed base (no freejoint). + self._pelvis_bid = None + if engine.free_qadr is not None: + self._pelvis_bid = next( + (int(model.jnt_bodyid[j]) for j in range(model.njnt) + if model.jnt_type[j] == mujoco.mjtJoint.mjJNT_FREE + and int(model.jnt_qposadr[j]) == engine.free_qadr), None) + + def _apply_band(self, mj_data): + """Hold the floating base up during the C++ startup/handoff with an elastic band. + + The band applies a spring-damper force plus an orientation-hold torque on + the PELVIS body, written to xfrc_applied[pelvis] ONLY. The orientation + reference is the spawn/reset pelvis pose, so kitchens that spawn Sonic + facing a counter are not rotated back to world-zero yaw. + """ + bid = self._pelvis_bid + if not self.band_enabled: + mj_data.xfrc_applied[bid] = 0.0 + return + if self._band is None: + from gear_sonic.utils.mujoco_sim.unitree_sdk2py_bridge import ElasticBand + self._band = ElasticBand() + self._band.point = np.array(mj_data.xpos[bid]) # anchor at the spawn stand pose + self._band.length = 0.0 + quat = mj_data.xquat[bid] + self._band_ref_rot = Rotation.from_quat([quat[1], quat[2], quat[3], quat[0]]) + model = self._maps._mj_model + vel = np.zeros(6) # mj_objectVelocity -> [angular(3), linear(3)] in world frame + mujoco.mj_objectVelocity(model, mj_data, mujoco.mjtObj.mjOBJ_BODY, bid, vel, 0) + lin_vel, ang_vel = vel[3:6], vel[0:3] + quat = mj_data.xquat[bid] + rot = Rotation.from_quat([quat[1], quat[2], quat[3], quat[0]]) + rotvec = (rot * self._band_ref_rot.inv()).as_rotvec() + force = ( + self._band.kp_pos + * (self._band.point - mj_data.xpos[bid] + np.array([0.0, 0.0, self._band.length])) + - self._band.kd_pos * lin_vel + ) + torque = -self._band.kp_ang * rotvec - self._band.kd_ang * ang_vel + mj_data.xfrc_applied[bid] = np.concatenate([force, torque]) + + def _action_by_source(self): + """Slice the flat q* action into {body, lhand, rhand} blocks (matching the source's concat).""" + n_body, n_hand = self._action_split + a = self._action_q + out = {"body": a[0:n_body]} + if n_hand: + out["lhand"] = a[n_body:n_body + n_hand] + out["rhand"] = a[n_body + n_hand:n_body + 2 * n_hand] + return out + + def run_controller(self, enabled_parts): + if self._maps is None: + self._maps = G1SonicController(self.sim, None, self._cfg) # maps only; no DDS / no exchange + if self._part_plan is None: + self._prepare() + + # Startup hold: elastic band on the pelvis until released. Object-safe (force only). + mj_data = self.sim.data._data if hasattr(self.sim.data, "_data") else self.sim.data + if self._pelvis_bid is not None: + self._apply_band(mj_data) + + outputs = {} + if self._action_q is None or not self._cmd_gains: + return outputs # no command/gains yet -> hold current ctrl (band keeps the pelvis up) + + abys = self._action_by_source() + for part, part_ctrl in self.part_controllers.items(): + if not enabled_parts.get(part, False): + continue + plan = self._part_plan[part] + srcs = {sk for sk, _ in plan} + if any(sk not in self._cmd_gains or sk not in abys for sk in srcs): + continue # this part's source gains/action not available yet (e.g. hands at startup) + q_des = np.array([abys[source_key][i] for source_key, i in plan]) + kp = np.array([self._cmd_gains[source_key][0][i] for source_key, i in plan]) + kd = np.array([self._cmd_gains[source_key][1][i] for source_key, i in plan]) + dq_des = np.zeros_like(q_des) # SONIC commands dq*≡0 (pure position PD + kd damping) + tau = np.zeros_like(q_des) # tau_ff≡0 + part_ctrl.set_pd_command(q_des, dq_des, kp, kd, tau) # JointPositionController PD law + outputs[part] = part_ctrl.run_controller() + + return outputs + + +# Convenience export for tests/scripts; "" if gear_sonic isn't installed (so `import robosuite` +# stays safe and the SONIC_WBC tests' skipif resolves correctly). +try: + WBC_CONFIG = _wbc_config_path() +except Exception: + WBC_CONFIG = "" diff --git a/robosuite/controllers/config/robots/default_sonic_g1.json b/robosuite/controllers/config/robots/default_sonic_g1.json new file mode 100644 index 0000000000..3fee8af3d2 --- /dev/null +++ b/robosuite/controllers/config/robots/default_sonic_g1.json @@ -0,0 +1,37 @@ +{ + "type": "SONIC_WBC", + "body_parts": { + "arms": { + "right": { + "type": "JOINT_POSITION_VELOCITY", + "use_torque_compensation": false, + "interpolation": null, + "gripper": { + "type": "JOINT_POSITION_VELOCITY", + "use_torque_compensation": false, + "interpolation": null + } + }, + "left": { + "type": "JOINT_POSITION_VELOCITY", + "use_torque_compensation": false, + "interpolation": null, + "gripper": { + "type": "JOINT_POSITION_VELOCITY", + "use_torque_compensation": false, + "interpolation": null + } + } + }, + "torso": { + "type": "JOINT_POSITION_VELOCITY", + "use_torque_compensation": false, + "interpolation": null + }, + "legs": { + "type": "JOINT_POSITION_VELOCITY", + "use_torque_compensation": false, + "interpolation": null + } + } +} diff --git a/robosuite/controllers/parts/controller_factory.py b/robosuite/controllers/parts/controller_factory.py index 947ef2826c..cd0ed5d9a5 100644 --- a/robosuite/controllers/parts/controller_factory.py +++ b/robosuite/controllers/parts/controller_factory.py @@ -136,6 +136,9 @@ def arm_controller_factory(name, params): if name == "JOINT_POSITION": return generic.JointPositionController(interpolator=interpolator, **params) + if name == "JOINT_POSITION_VELOCITY": + return generic.JointPositionVelocityController(interpolator=interpolator, **params) + if name == "JOINT_TORQUE": return generic.JointTorqueController(interpolator=interpolator, **params) @@ -165,6 +168,8 @@ def gripper_controller_factory(name, params): return gripper_controllers.SimpleGripController(interpolator=interpolator, **params) elif name == "JOINT_POSITION": return generic.JointPositionController(interpolator=interpolator, **params) + elif name == "JOINT_POSITION_VELOCITY": + return generic.JointPositionVelocityController(interpolator=interpolator, **params) raise ValueError("Unknown controller name: {}".format(name)) @@ -193,6 +198,8 @@ def torso_controller_factory(name, params): return generic.JointVelocityController(interpolator=interpolator, **params) elif name == "JOINT_POSITION": return generic.JointPositionController(interpolator=interpolator, **params) + elif name == "JOINT_POSITION_VELOCITY": + return generic.JointPositionVelocityController(interpolator=interpolator, **params) raise ValueError("Unknown controller name: {}".format(name)) @@ -226,6 +233,9 @@ def legs_controller_factory(name, params): if name == "JOINT_POSITION": return generic.JointPositionController(interpolator=interpolator, **params) + if name == "JOINT_POSITION_VELOCITY": + return generic.JointPositionVelocityController(interpolator=interpolator, **params) + if name == "JOINT_TORQUE": return generic.JointTorqueController(interpolator=interpolator, **params) diff --git a/robosuite/controllers/parts/generic/__init__.py b/robosuite/controllers/parts/generic/__init__.py index c334db11f3..ded39cf8db 100644 --- a/robosuite/controllers/parts/generic/__init__.py +++ b/robosuite/controllers/parts/generic/__init__.py @@ -1,3 +1,3 @@ -from .joint_pos import JointPositionController +from .joint_pos import JointPositionController, JointPositionVelocityController from .joint_vel import JointVelocityController from .joint_tor import JointTorqueController diff --git a/robosuite/controllers/parts/generic/joint_pos.py b/robosuite/controllers/parts/generic/joint_pos.py index 7665679157..e4fec5d5fb 100644 --- a/robosuite/controllers/parts/generic/joint_pos.py +++ b/robosuite/controllers/parts/generic/joint_pos.py @@ -310,3 +310,70 @@ def control_limits(self): @property def name(self): return "JOINT_POSITION" + + +class JointPositionVelocityController(JointPositionController): + """JointPositionController extended to track a streamed external joint PD+feedforward + command, as SONIC does over DDS. Standard JointPositionController is position-only + (drives dq -> 0, no feedforward); SONIC needs a velocity setpoint dq* and a + feedforward torque tau_ff, with an output clip to the per-motor effort limit: + + tau = clip(tau_ff + kp*(q* - q) + kd*(dq* - dq), -effort, +effort) + + set_pd_command(q*, dq*, kp, kd, tau_ff) sets the full command directly each step + (bypassing set_goal's delta/scaling); torque_limits is the per-joint effort clip. + reset_goal / control_limits are inherited from JointPositionController.""" + + def __init__(self, *args, **kwargs): + torque_limits = kwargs.pop("torque_limits", None) + super().__init__(*args, **kwargs) + njnt = len(self.qpos_index) + self.desired_vel = np.zeros(njnt) # dq* (default 0 -> kd damps to rest) + self.torque_feedforward = np.zeros(njnt) # tau_ff (default 0) + self.torque_limits = np.array(torque_limits) if torque_limits is not None else None + + def set_pd_command(self, qpos, qvel=None, kp=None, kd=None, tau_feedforward=None): + """Set an absolute joint-space PD command directly (q*, dq*, kp, kd, tau_ff), + bypassing set_goal's delta/scaling. Used when an external policy streams a full + per-joint command each step (SONIC over DDS) rather than a position action.""" + self.goal_qpos = np.asarray(qpos, dtype=float) + if qvel is not None: + self.desired_vel = np.asarray(qvel, dtype=float) + if kp is not None: + self.kp = np.asarray(kp, dtype=float) + if kd is not None: + self.kd = np.asarray(kd, dtype=float) + if tau_feedforward is not None: + self.torque_feedforward = np.asarray(tau_feedforward, dtype=float) + if self.interpolator is not None: + self.interpolator.set_goal(self.goal_qpos) + + def run_controller(self): + """PD law with velocity setpoint + feedforward + effort clip (see class doc).""" + if self.goal_qpos is None: + self.set_goal(np.zeros(self.control_dim)) + self.update() + + if self.interpolator is not None and self.interpolator.order == 1: + desired_qpos = self.interpolator.get_interpolated_goal() + else: + desired_qpos = np.array(self.goal_qpos) + + position_error = desired_qpos - self.joint_pos + vel_pos_error = self.desired_vel - self.joint_vel # dq* - dq + desired_torque = np.multiply(position_error, self.kp) + np.multiply(vel_pos_error, self.kd) + + if self.use_torque_compensation: + self.torques = np.dot(self.mass_matrix, desired_torque) + self.torque_compensation + self.torque_feedforward + else: + self.torques = desired_torque + self.torque_feedforward + + if self.torque_limits is not None: + self.torques = np.clip(self.torques, self.torque_limits[0], self.torque_limits[1]) + + Controller.run_controller(self) # base cleanup (skip JointPositionController.run_controller) + return self.torques + + @property + def name(self): + return "JOINT_POSITION_VELOCITY" diff --git a/robosuite/environments/base.py b/robosuite/environments/base.py index 09d6c299cd..bf89e84b98 100644 --- a/robosuite/environments/base.py +++ b/robosuite/environments/base.py @@ -1,4 +1,5 @@ import os +import time import xml.etree.ElementTree as ET from collections import OrderedDict from copy import deepcopy @@ -136,6 +137,16 @@ def __init__( self.cur_time = None self.model_timestep = None self.control_timestep = None + # How often (every Nth step()) to run the expensive once-per-step bookkeeping in step() + # -- reward()/_check_success() and any subclass _post_action work (e.g. robocasa's + # per-fixture update_state()). Default 1 == every step (unchanged behavior). Raise it for + # high-rate control where reward is unused and success only needs an occasional check + # (e.g. SONIC data collection); skipped steps reuse the previous (reward, done, info). + self.post_action_freq = 1 + # How often (every Nth step()) to refresh the built-in viewer (self.viewer.update()). + # Default 1 == every step (unchanged). Raise it to render at a lower rate than the control + # loop -- e.g. 20 Hz render under a 200 Hz control loop (render_freq = control_freq/20). + self.render_freq = 1 self.deterministic_reset = False # Whether to add randomized resetting of objects / robot joints self.renderer = renderer @@ -215,6 +226,13 @@ def initialize_time(self, control_freq): """ self.cur_time = 0 self.model_timestep = macros.SIMULATION_TIMESTEP + # RTF (real-time-factor) monitor: when console logging is verbose + # (macros.CONSOLE_LOGGING_LEVEL == "DEBUG"), step() prints a [real-time] line ~1x/sec with + # sim-time advanced vs wall-time elapsed. Read-only accounting -- the env does NOT throttle + # itself; callers that need real time pace their own step loop (e.g. the SONIC collectors). + self._rt_verbose = macros.CONSOLE_LOGGING_LEVEL == "DEBUG" + self._rt_n = 0 + self._rt_t0 = None if self.model_timestep <= 0: raise ValueError("Invalid simulation timestep defined!") self.control_freq = control_freq @@ -504,13 +522,48 @@ def step(self, action): self._update_observables() policy_step = False + # --- RTF accounting (verbose only; read-only -- this env does NOT throttle; the + # caller paces its own step loop if it needs real time) --- + if self._rt_verbose: + self._rt_n += 1 + if self._rt_t0 is None: + self._rt_t0 = time.perf_counter() + # Note: this is done all at once to avoid floating point inaccuracies self.cur_time += self.control_timestep - reward, done, info = self._post_action(action) + # --- RTF report (~1x/sec), printed only when console logging is verbose. Reports the + # achieved sim-vs-wall ratio (wall includes any pacing the caller's loop did between + # steps); flags sub-real-time so a too-heavy scene / loaded box is visible. --- + if self._rt_verbose and self._rt_t0 is not None: + _T = time.perf_counter() - self._rt_t0 + if _T >= 1.0: + _sim = self._rt_n * self.model_timestep + _rtf = _sim / _T + print( + f"[real-time] {self._rt_n} substeps | wall {_T * 1e3:.0f}ms | sim {_sim * 1e3:.0f}ms " + f"| RTF~{_rtf:.2f}x" + (" <-- BEHIND real-time" if _rtf < 0.97 else ""), + flush=True, + ) + self._rt_t0 = time.perf_counter() + self._rt_n = 0 + + # Expensive once-per-step bookkeeping -- reward()/_check_success() and (robocasa) the + # per-fixture update_state() loop -- dominates the step on heavy task scenes (~2.5 ms of a + # ~4.5 ms DivideBuffetTrays step at 200 Hz). self.post_action_freq (default 1 == every step) + # runs it only every Nth step for high-rate control where reward is unused and success only + # needs an occasional check; skipped steps reuse the last (reward, done, info) -- done stays + # put (high-rate collection runs with ignore_done). + if self.post_action_freq <= 1 or self.timestep % self.post_action_freq == 0 \ + or not hasattr(self, "_last_post_action"): + reward, done, info = self._post_action(action) + self._last_post_action = (reward, done, info) + else: + reward, done, info = self._last_post_action if self.viewer is not None and self.renderer != "mujoco": - self.viewer.update() + if self.render_freq <= 1 or self.timestep % self.render_freq == 0: + self.viewer.update() # throttle the built-in viewer render off the control loop elif self.has_renderer and self.renderer == "mjviewer" and self.viewer is None: # need to launch again after it was destroyed self.initialize_renderer() diff --git a/robosuite/models/assets/grippers/meshes/sonic_dex3/left_hand_index_0_link.STL b/robosuite/models/assets/grippers/meshes/sonic_dex3/left_hand_index_0_link.STL new file mode 100644 index 0000000000..8069369afd Binary files /dev/null and b/robosuite/models/assets/grippers/meshes/sonic_dex3/left_hand_index_0_link.STL differ diff --git a/robosuite/models/assets/grippers/meshes/sonic_dex3/left_hand_index_1_link.STL b/robosuite/models/assets/grippers/meshes/sonic_dex3/left_hand_index_1_link.STL new file mode 100644 index 0000000000..89d231d7e8 Binary files /dev/null and b/robosuite/models/assets/grippers/meshes/sonic_dex3/left_hand_index_1_link.STL differ diff --git a/robosuite/models/assets/grippers/meshes/sonic_dex3/left_hand_middle_0_link.STL b/robosuite/models/assets/grippers/meshes/sonic_dex3/left_hand_middle_0_link.STL new file mode 100644 index 0000000000..8069369afd Binary files /dev/null and b/robosuite/models/assets/grippers/meshes/sonic_dex3/left_hand_middle_0_link.STL differ diff --git a/robosuite/models/assets/grippers/meshes/sonic_dex3/left_hand_middle_1_link.STL b/robosuite/models/assets/grippers/meshes/sonic_dex3/left_hand_middle_1_link.STL new file mode 100644 index 0000000000..89d231d7e8 Binary files /dev/null and b/robosuite/models/assets/grippers/meshes/sonic_dex3/left_hand_middle_1_link.STL differ diff --git a/robosuite/models/assets/grippers/meshes/sonic_dex3/left_hand_thumb_0_link.STL b/robosuite/models/assets/grippers/meshes/sonic_dex3/left_hand_thumb_0_link.STL new file mode 100644 index 0000000000..3028bb4d6e Binary files /dev/null and b/robosuite/models/assets/grippers/meshes/sonic_dex3/left_hand_thumb_0_link.STL differ diff --git a/robosuite/models/assets/grippers/meshes/sonic_dex3/left_hand_thumb_1_link.STL b/robosuite/models/assets/grippers/meshes/sonic_dex3/left_hand_thumb_1_link.STL new file mode 100644 index 0000000000..d1c080c86e Binary files /dev/null and b/robosuite/models/assets/grippers/meshes/sonic_dex3/left_hand_thumb_1_link.STL differ diff --git a/robosuite/models/assets/grippers/meshes/sonic_dex3/left_hand_thumb_2_link.STL b/robosuite/models/assets/grippers/meshes/sonic_dex3/left_hand_thumb_2_link.STL new file mode 100644 index 0000000000..8b32e96634 Binary files /dev/null and b/robosuite/models/assets/grippers/meshes/sonic_dex3/left_hand_thumb_2_link.STL differ diff --git a/robosuite/models/assets/grippers/meshes/sonic_dex3/right_hand_index_0_link.STL b/robosuite/models/assets/grippers/meshes/sonic_dex3/right_hand_index_0_link.STL new file mode 100644 index 0000000000..f87ad3212b Binary files /dev/null and b/robosuite/models/assets/grippers/meshes/sonic_dex3/right_hand_index_0_link.STL differ diff --git a/robosuite/models/assets/grippers/meshes/sonic_dex3/right_hand_index_1_link.STL b/robosuite/models/assets/grippers/meshes/sonic_dex3/right_hand_index_1_link.STL new file mode 100644 index 0000000000..6dea51ad93 Binary files /dev/null and b/robosuite/models/assets/grippers/meshes/sonic_dex3/right_hand_index_1_link.STL differ diff --git a/robosuite/models/assets/grippers/meshes/sonic_dex3/right_hand_middle_0_link.STL b/robosuite/models/assets/grippers/meshes/sonic_dex3/right_hand_middle_0_link.STL new file mode 100644 index 0000000000..f87ad3212b Binary files /dev/null and b/robosuite/models/assets/grippers/meshes/sonic_dex3/right_hand_middle_0_link.STL differ diff --git a/robosuite/models/assets/grippers/meshes/sonic_dex3/right_hand_middle_1_link.STL b/robosuite/models/assets/grippers/meshes/sonic_dex3/right_hand_middle_1_link.STL new file mode 100644 index 0000000000..6dea51ad93 Binary files /dev/null and b/robosuite/models/assets/grippers/meshes/sonic_dex3/right_hand_middle_1_link.STL differ diff --git a/robosuite/models/assets/grippers/meshes/sonic_dex3/right_hand_thumb_0_link.STL b/robosuite/models/assets/grippers/meshes/sonic_dex3/right_hand_thumb_0_link.STL new file mode 100644 index 0000000000..1cae7f18e1 Binary files /dev/null and b/robosuite/models/assets/grippers/meshes/sonic_dex3/right_hand_thumb_0_link.STL differ diff --git a/robosuite/models/assets/grippers/meshes/sonic_dex3/right_hand_thumb_1_link.STL b/robosuite/models/assets/grippers/meshes/sonic_dex3/right_hand_thumb_1_link.STL new file mode 100644 index 0000000000..c141fbf5a6 Binary files /dev/null and b/robosuite/models/assets/grippers/meshes/sonic_dex3/right_hand_thumb_1_link.STL differ diff --git a/robosuite/models/assets/grippers/meshes/sonic_dex3/right_hand_thumb_2_link.STL b/robosuite/models/assets/grippers/meshes/sonic_dex3/right_hand_thumb_2_link.STL new file mode 100644 index 0000000000..e942923c50 Binary files /dev/null and b/robosuite/models/assets/grippers/meshes/sonic_dex3/right_hand_thumb_2_link.STL differ diff --git a/robosuite/models/assets/grippers/sonic_dex3_left.xml b/robosuite/models/assets/grippers/sonic_dex3_left.xml new file mode 100644 index 0000000000..1cf9646c70 --- /dev/null +++ b/robosuite/models/assets/grippers/sonic_dex3_left.xml @@ -0,0 +1,78 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/robosuite/models/assets/grippers/sonic_dex3_right.xml b/robosuite/models/assets/grippers/sonic_dex3_right.xml new file mode 100644 index 0000000000..83ced16709 --- /dev/null +++ b/robosuite/models/assets/grippers/sonic_dex3_right.xml @@ -0,0 +1,77 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/head_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/head_link.STL new file mode 100644 index 0000000000..2ee5fba15d Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/head_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/left_ankle_pitch_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/left_ankle_pitch_link.STL new file mode 100644 index 0000000000..69de849018 Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/left_ankle_pitch_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/left_ankle_roll_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/left_ankle_roll_link.STL new file mode 100644 index 0000000000..8864e9f981 Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/left_ankle_roll_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/left_elbow_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/left_elbow_link.STL new file mode 100644 index 0000000000..1a96d99ba4 Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/left_elbow_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/left_hand_palm_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/left_hand_palm_link.STL new file mode 100644 index 0000000000..7d595ed8fb Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/left_hand_palm_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/left_hip_pitch_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/left_hip_pitch_link.STL new file mode 100644 index 0000000000..5b751c767e Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/left_hip_pitch_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/left_hip_roll_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/left_hip_roll_link.STL new file mode 100644 index 0000000000..778437ffe6 Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/left_hip_roll_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/left_hip_yaw_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/left_hip_yaw_link.STL new file mode 100644 index 0000000000..383093ab96 Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/left_hip_yaw_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/left_knee_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/left_knee_link.STL new file mode 100644 index 0000000000..f2e98e54e8 Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/left_knee_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/left_shoulder_pitch_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/left_shoulder_pitch_link.STL new file mode 100644 index 0000000000..e698311fb1 Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/left_shoulder_pitch_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/left_shoulder_roll_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/left_shoulder_roll_link.STL new file mode 100644 index 0000000000..80bca84aca Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/left_shoulder_roll_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/left_shoulder_yaw_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/left_shoulder_yaw_link.STL new file mode 100644 index 0000000000..281e699055 Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/left_shoulder_yaw_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/left_wrist_pitch_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/left_wrist_pitch_link.STL new file mode 100644 index 0000000000..82cc224a8e Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/left_wrist_pitch_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/left_wrist_roll_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/left_wrist_roll_link.STL new file mode 100644 index 0000000000..f3c263a7ab Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/left_wrist_roll_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/left_wrist_yaw_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/left_wrist_yaw_link.STL new file mode 100644 index 0000000000..31be4fd45f Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/left_wrist_yaw_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/logo_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/logo_link.STL new file mode 100644 index 0000000000..e979209850 Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/logo_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/pelvis.STL b/robosuite/models/assets/robots/sonic_g1/meshes/pelvis.STL new file mode 100644 index 0000000000..691a779b9c Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/pelvis.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/pelvis_contour_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/pelvis_contour_link.STL new file mode 100644 index 0000000000..42434339ab Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/pelvis_contour_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/right_ankle_pitch_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/right_ankle_pitch_link.STL new file mode 100644 index 0000000000..e77d8a2fe1 Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/right_ankle_pitch_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/right_ankle_roll_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/right_ankle_roll_link.STL new file mode 100644 index 0000000000..d4261dd7cc Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/right_ankle_roll_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/right_elbow_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/right_elbow_link.STL new file mode 100644 index 0000000000..f259e3812e Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/right_elbow_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/right_hand_palm_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/right_hand_palm_link.STL new file mode 100644 index 0000000000..5ae00a783d Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/right_hand_palm_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/right_hip_pitch_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/right_hip_pitch_link.STL new file mode 100644 index 0000000000..998a0a0f5c Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/right_hip_pitch_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/right_hip_roll_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/right_hip_roll_link.STL new file mode 100644 index 0000000000..47b2eebdd3 Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/right_hip_roll_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/right_hip_yaw_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/right_hip_yaw_link.STL new file mode 100644 index 0000000000..371856427c Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/right_hip_yaw_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/right_knee_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/right_knee_link.STL new file mode 100644 index 0000000000..76d21a3d81 Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/right_knee_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/right_shoulder_pitch_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/right_shoulder_pitch_link.STL new file mode 100644 index 0000000000..3f5b4ed47a Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/right_shoulder_pitch_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/right_shoulder_roll_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/right_shoulder_roll_link.STL new file mode 100644 index 0000000000..179d61753d Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/right_shoulder_roll_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/right_shoulder_yaw_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/right_shoulder_yaw_link.STL new file mode 100644 index 0000000000..2ba6076a88 Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/right_shoulder_yaw_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/right_wrist_pitch_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/right_wrist_pitch_link.STL new file mode 100644 index 0000000000..da194543c4 Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/right_wrist_pitch_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/right_wrist_roll_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/right_wrist_roll_link.STL new file mode 100644 index 0000000000..26868d2282 Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/right_wrist_roll_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/right_wrist_yaw_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/right_wrist_yaw_link.STL new file mode 100644 index 0000000000..d788902867 Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/right_wrist_yaw_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/torso_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/torso_link.STL new file mode 100644 index 0000000000..17745af9ce Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/torso_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/waist_roll_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/waist_roll_link.STL new file mode 100644 index 0000000000..65831abd2a Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/waist_roll_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/waist_support_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/waist_support_link.STL new file mode 100644 index 0000000000..63660fb6ee Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/waist_support_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/meshes/waist_yaw_link.STL b/robosuite/models/assets/robots/sonic_g1/meshes/waist_yaw_link.STL new file mode 100644 index 0000000000..7d36b02cce Binary files /dev/null and b/robosuite/models/assets/robots/sonic_g1/meshes/waist_yaw_link.STL differ diff --git a/robosuite/models/assets/robots/sonic_g1/robot.xml b/robosuite/models/assets/robots/sonic_g1/robot.xml new file mode 100644 index 0000000000..2ca872668b --- /dev/null +++ b/robosuite/models/assets/robots/sonic_g1/robot.xml @@ -0,0 +1,393 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/robosuite/models/grippers/__init__.py b/robosuite/models/grippers/__init__.py index 57977d1774..03aa588e6a 100644 --- a/robosuite/models/grippers/__init__.py +++ b/robosuite/models/grippers/__init__.py @@ -43,3 +43,8 @@ def register_gripper(target_class): GRIPPER_MAPPING[target_class.__name__] = target_class return target_class + + +# Imported after register_gripper is defined so the @register_gripper decorator +# resolves (these grippers self-register into GRIPPER_MAPPING). +from .sonic_dex3_gripper import SonicDex3LeftGripper, SonicDex3RightGripper # noqa: E402,F401 diff --git a/robosuite/models/grippers/sonic_dex3_gripper.py b/robosuite/models/grippers/sonic_dex3_gripper.py new file mode 100644 index 0000000000..13d04a27c4 --- /dev/null +++ b/robosuite/models/grippers/sonic_dex3_gripper.py @@ -0,0 +1,53 @@ +"""Dex3 three-finger grippers for the SonicG1 (7 DOF/side). Split from SONIC's +validated model so joints/meshes/ranges match the integrated model the C++ dex3 +command stream maps to.""" +import numpy as np +from robosuite.models.grippers.gripper_model import GripperModel +from robosuite.models.grippers import register_gripper +from robosuite.utils.mjcf_utils import xml_path_completion + +_FINGER_SIGN = np.array([1.0, 1.0, 1.0, -1.0, -1.0, -1.0, -1.0]) + + +@register_gripper +class SonicDex3LeftGripper(GripperModel): + def __init__(self, idn=0): + super().__init__(xml_path_completion("grippers/sonic_dex3_left.xml"), idn=idn) + + def format_action(self, action): + # 1-D open/close convenience for robosuite's gripper controller; the SONIC + # composite controller drives all 7 finger torques directly. + return np.sign(action) * _FINGER_SIGN + + @property + def init_qpos(self): + return np.zeros(7) + + @property + def speed(self): + return 0.15 + + @property + def dof(self): + return 7 + + +@register_gripper +class SonicDex3RightGripper(GripperModel): + def __init__(self, idn=0): + super().__init__(xml_path_completion("grippers/sonic_dex3_right.xml"), idn=idn) + + def format_action(self, action): + return np.sign(action) * -_FINGER_SIGN + + @property + def init_qpos(self): + return np.zeros(7) + + @property + def speed(self): + return 0.15 + + @property + def dof(self): + return 7 diff --git a/robosuite/models/robots/manipulators/__init__.py b/robosuite/models/robots/manipulators/__init__.py index 9b56b54d9a..49b87f4409 100644 --- a/robosuite/models/robots/manipulators/__init__.py +++ b/robosuite/models/robots/manipulators/__init__.py @@ -10,3 +10,4 @@ from .tiago_robot import Tiago from .gr1_robot import GR1, GR1FixedLowerBody, GR1ArmsOnly, GR1FloatingBody from .xarm7_robot import XArm7 +from .sonic_g1_robot import SonicG1, SonicG1Fixed diff --git a/robosuite/models/robots/manipulators/sonic_g1_robot.py b/robosuite/models/robots/manipulators/sonic_g1_robot.py new file mode 100644 index 0000000000..a73ab8d575 --- /dev/null +++ b/robosuite/models/robots/manipulators/sonic_g1_robot.py @@ -0,0 +1,130 @@ +"""SonicG1: 29-DOF Unitree G1 (legs, 3-DOF waist, 7-DOF arms) + Dex3 grippers, +driven by the external SONIC C++ whole-body controller (over DDS) via the +SonicWholeBodyController composite controller, or by standard controllers. + +Assets generated by robosuite/scripts/build_sonic_g1_assets.py (split from SONIC's +validated 29-DOF model; physics identical to the integrated model).""" +import numpy as np +from robosuite.models.robots.manipulators.legged_manipulator_model import LeggedManipulatorModel +from robosuite.utils.mjcf_utils import xml_path_completion + +# NOTE: the SonicG1 -> LeggedRobot control-class mapping is registered in +# robosuite/robots/__init__.py ROBOT_CLASS_MAPPING (the hardcoded path used by the +# built-in robots), not via @register_robot_class -- that decorator imports +# robosuite.robots, which would create a circular import here (this module is +# imported from within robosuite.models.robots). The RobotModel itself auto-registers +# in REGISTERED_ROBOTS via its metaclass on class definition. + + +class SonicG1(LeggedManipulatorModel): + """29-DOF G1 with a free (un-actuated) floating base; bimanual Dex3 grippers.""" + + arms = ["right", "left"] + + def __init__(self, idn=0): + super().__init__(xml_path_completion("robots/sonic_g1/robot.xml"), idn=idn) + # robosuite derives _base_offset (-> bottom_offset) from the root body's authored + # pos; our pelvis is authored at its standing height (z=0.793), so set_base_xpos + # would otherwise place the pelvis that far underground. Zero it so base_xpos_offset + # z IS the pelvis standing height the env places it at (the GR1 convention -- GR1's + # root is authored at ~0, so it doesn't need this). + self._base_offset = np.zeros(3) + + @property + def default_base(self): + # NullBase (add_null_base) leaves the robot's worldbody untouched, so the + # pelvis's top-level survives -> a true free-floating 6-DOF base + # (needed: SONIC's policy controls a floating humanoid; squat/jump/walk move + # the base in z/pitch/roll, which robosuite's planar FloatingLeggedBase and the + # freejoint-dropping NoActuationBase/add_mobile_base cannot represent). base_pos + # reads root_body (the pelvis) directly (robot.py), so no base body is needed. + # The FIXED variant overrides this (it has no freejoint to preserve). + return "NullBase" + + @property + def default_gripper(self): + return {"right": "SonicDex3RightGripper", "left": "SonicDex3LeftGripper"} + + @property + def default_controller_config(self): + return {"right": "default_sonic_g1", "left": "default_sonic_g1"} + + @property + def init_qpos(self): + # SONIC neutral stand (gold squat frame-0 cmd_q), in THIS robot's joint order + # (robosuite applies init_qpos[i] to robot_joints[i]). The base height comes from + # base_xpos_offset; the Dex3 hands spawn open (gripper default) and are posed by the + # streamed command. So the robot spawns standing -- no external set-pose step needed. + return np.array([ + -0.1638, -0.0929, 0.0101, 0.2799, 0.0437, -0.0186, -0.1329, -0.0178, + -0.2169, 0.3958, 0.0487, 0.0261, -0.0033, -0.0519, -0.3514, -0.9683, + 0.7521, -0.9398, 0.9698, 0.0127, 0.1253, 0.0883, -1.0862, -0.66, + 1.0143, 1.1552, 0.1886, 0.0735, -0.0864]) + + @property + def base_xpos_offset(self): + # z = the pelvis standing height: the env places the free-floating base directly + # here (bottom_offset is zeroed in __init__). x,y stand the robot in front of the + # env's table / clear of its bins. + STAND_Z = 0.793 + return { + "bins": (-0.5, -0.1, STAND_Z), + "empty": (-0.29, 0, STAND_Z), + "table": lambda table_length: (-0.26 - table_length / 2, 0, STAND_Z), + } + + @property + def top_offset(self): + return np.array((0, 0, 1.0)) + + @property + def _horizontal_radius(self): + return 0.5 + + @property + def arm_type(self): + return "bimanual" + + @property + def _eef_name(self): + return {"right": "right_eef", "left": "left_eef"} + + @staticmethod + def _right_first(names): + # robosuite splits the arm joint/actuator lists evenly per side with the FIRST + # half -> "right" (robot.py _load_arm_controllers / fixed_base_robot.py:96). Our + # MOTOR_ORDER lists the LEFT arm first, so reorder both arm_joints AND + # arm_actuators right-first here. Both MUST use the same order: the "right" + # part controller takes joint_indexes from arm_joints[:split] and writes its + # torque to arm_actuators[:split] -- if only one is reordered, left/right + # swap (right-arm torque lands on left-arm actuators). This is a pure relabel + # for robosuite's part split; it does NOT touch the model's actuator order, so + # SONIC's own ctrl_idx mapping (built from joint names) is unaffected. + right = [n for n in names if any(k in n for k in ("r_shoulder", "r_elbow", "r_wrist"))] + left = [n for n in names if any(k in n for k in ("l_shoulder", "l_elbow", "l_wrist"))] + return right + left + + @property + def arm_joints(self): + return self._right_first(super().arm_joints) + + @property + def arm_actuators(self): + return self._right_first(super().arm_actuators) + + +class SonicG1Fixed(SonicG1): + """SonicG1 with the floating base removed (pelvis welded). Non-free-floating + version for manipulation envs -- avoids the FloatingLeggedBase machinery and the + freejoint-as-arm misclassification. Legs/waist/arms remain actuated (SONIC or + OSC can drive them).""" + + def __init__(self, idn=0): + super().__init__(idn=idn) + self._remove_free_joint() + + @property + def default_base(self): + # Fixed variant has no freejoint to preserve; NoActuationBase provides a base + # body + welds the pelvis -> fixed stance (used by the manipulation/OSC tests). + return "NoActuationBase" diff --git a/robosuite/robots/__init__.py b/robosuite/robots/__init__.py index 86714f7dbc..e806474fc0 100644 --- a/robosuite/robots/__init__.py +++ b/robosuite/robots/__init__.py @@ -31,6 +31,8 @@ "PandaDexRH": FixedBaseRobot, "PandaDexLH": FixedBaseRobot, "XArm7": FixedBaseRobot, + "SonicG1": LeggedRobot, + "SonicG1Fixed": LeggedRobot, } target_type_mapping = { diff --git a/robosuite/scripts/build_sonic_g1_assets.py b/robosuite/scripts/build_sonic_g1_assets.py new file mode 100644 index 0000000000..ae1a55d08e --- /dev/null +++ b/robosuite/scripts/build_sonic_g1_assets.py @@ -0,0 +1,201 @@ +"""Generate the SonicG1 robot + Dex3 gripper MJCF assets (robosuite-native). + +Splits SONIC's validated 29-DOF model (GR00T-WholeBodyControl model_data +g1_29dof_with_hand.xml) into a robosuite-conformant robot body + two Dex3 +three-finger grippers, copying meshes into the robosuite asset tree with relative +paths. Bodies/geoms/joints/meshes move verbatim -> the reassembled robot is +physically identical to the integrated model (mass + DOF + finger poses preserved). + +Run once (needs the GR00T model_data + meshes available): + python -m robosuite.scripts.build_sonic_g1_assets +Outputs: + models/assets/robots/sonic_g1/robot.xml (+ meshes/) + models/assets/grippers/sonic_dex3_{left,right}.xml (+ meshes/sonic_dex3/) +""" + +# Kept in-repo as the provenance/reproducer for the committed SonicG1 + Dex3 assets (run +# once, locally, when the GR00T model_data changes); it is not imported at runtime. + +import copy +import os +import shutil +import xml.etree.ElementTree as ET + +import robosuite + +GEAR = "/home/ajay/code/GR00T-WholeBodyControl/gear_sonic" +SRC = os.path.join(GEAR, "data/robot_model/model_data/g1/g1_29dof_with_hand.xml") +MESHDIR = os.path.join(GEAR, "data/robot_model/model_data/g1/meshes") + +ASSETS = os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(robosuite.__file__))), + "robosuite", "models", "assets") +ROBOT_DIR = os.path.join(ASSETS, "robots", "sonic_g1") +GRIP_DIR = os.path.join(ASSETS, "grippers") +GRIP_MESH_REL = os.path.join("meshes", "sonic_dex3") +SIDES = ["left", "right"] + + +def _find_body(root, name): + return next((b for b in root.iter("body") if b.get("name") == name), None) + + +def _copy_mesh(name, dst_dir, mesh_elems): + src = os.path.join(MESHDIR, os.path.basename(mesh_elems[name].get("file"))) + os.makedirs(dst_dir, exist_ok=True) + shutil.copy(src, os.path.join(dst_dir, os.path.basename(src))) + return os.path.basename(src) + + +def main(): + os.makedirs(os.path.join(ROBOT_DIR, "meshes"), exist_ok=True) + os.makedirs(os.path.join(GRIP_DIR, GRIP_MESH_REL), exist_ok=True) + tree = ET.parse(SRC) + root = tree.getroot() + asset = root.find("asset") + mesh_elems = {m.get("name"): m for m in asset.findall("mesh")} + orig_default = copy.deepcopy(root.find("default")) + + # --- detach finger chains; add eef body + center site at each wrist --- + grip_fingers, finger_meshes, hand_joints = {}, set(), set() + for side in SIDES: + wrist = _find_body(root, f"{side}_wrist_yaw_link") + fingers = [b for b in list(wrist) if b.tag == "body" and "hand" in (b.get("name") or "")] + assert len(fingers) == 3, side + grip_fingers[side] = [copy.deepcopy(b) for b in fingers] + for b in fingers: + for g in b.iter("geom"): + if g.get("mesh"): + finger_meshes.add(g.get("mesh")) + for j in b.iter("joint"): + hand_joints.add(j.get("name")) + wrist.remove(b) + eef = ET.SubElement(wrist, "body", {"name": f"{side}_eef", "pos": "0 0 0"}) + ET.SubElement(eef, "inertial", {"pos": "0 0 0", "mass": "0", "diaginertia": "0 0 0"}) + ET.SubElement(wrist, "site", {"name": f"{side}_center", "pos": "0 0 0", + "size": "0.01", "group": "2", "rgba": "1 0.3 0.3 1"}) + + # --- BODY: drop hand motors + hand-joint sensors + finger meshes; relative meshes --- + act = root.find("actuator") + body_hand_motors = {s: [] for s in SIDES} + for m in list(act): + if m.get("joint") in hand_joints: + side = "left" if m.get("joint").startswith("left") else "right" + body_hand_motors[side].append(copy.deepcopy(m)) + act.remove(m) + bsensor = root.find("sensor") + if bsensor is not None: + for s in list(bsensor): + if s.get("joint") in hand_joints: + bsensor.remove(s) + for mn in list(finger_meshes): + if mn in mesh_elems: + asset.remove(mesh_elems[mn]) + for m in asset.findall("mesh"): # copy body meshes, set relative path + fn = _copy_mesh(m.get("name"), os.path.join(ROBOT_DIR, "meshes"), + {m.get("name"): m}) + m.set("file", os.path.join("meshes", fn)) + comp = root.find("compiler") + if comp is not None: + comp.attrib.pop("meshdir", None) + wb = root.find("worldbody") + for geom in wb.findall("geom"): + if geom.get("type") == "plane" or geom.get("name") == "floor": + wb.remove(geom) + for light in wb.findall("light"): + wb.remove(light) + + # Rename body JOINTS/ACTUATORS/SENSORS to robosuite's part-classification + # convention (GR1-style: legs -> l_/r_leg_*, waist -> torso_waist_*, arms -> + # l_/r_*) so robosuite classifies legs/torso/arms correctly (manipulator_model + # keys on "leg"/"torso" substrings; arms split evenly per side). SONIC's keyword + # detection (hip/knee/ankle/waist/shoulder/elbow/wrist are still substrings) and + # the actuator MOTOR_ORDER are preserved. Body/link/mesh names are left alone + # (classification uses joint/actuator names, not bodies). + _RN = [("left_hip", "l_leg_hip"), ("right_hip", "r_leg_hip"), + ("left_knee", "l_leg_knee"), ("right_knee", "r_leg_knee"), + ("left_ankle", "l_leg_ankle"), ("right_ankle", "r_leg_ankle"), + ("waist_", "torso_waist_"), + ("left_shoulder", "l_shoulder"), ("right_shoulder", "r_shoulder"), + ("left_elbow", "l_elbow"), ("right_elbow", "r_elbow"), + ("left_wrist", "l_wrist"), ("right_wrist", "r_wrist")] + + def _rn(s): + for a, b in _RN: + if a in s: + return s.replace(a, b) + return s + + # Convert the named free joint -> (robosuite convention): the + # TAG (not ) keeps robosuite's joint classification from + # treating it as an arm joint, and lets LeggedManipulatorModel._remove_free_joint() drop + # it for the fixed variant (both match by tag). We KEEP a name on it ("root") so the joint + # is named: robocasa fixtures (e.g. oven/toaster_oven update_state) iterate + # env.sim.model.joint_names doing `"rack" in joint_name`, which crashes on an unnamed + # (None) joint -- so an unnamed freejoint broke every kitchen layout that has an oven. + for b in wb.iter("body"): + for j in list(b): + if j.tag == "joint" and j.get("type") == "free": + i = list(b).index(j) + b.remove(j) + b.insert(i, ET.Element("freejoint", {"name": "root"})) + for j in wb.iter("joint"): + if j.get("name"): + j.set("name", _rn(j.get("name"))) + for a in act.findall("motor"): + if a.get("name"): + a.set("name", _rn(a.get("name"))) + if a.get("joint"): + a.set("joint", _rn(a.get("joint"))) + if bsensor is not None: + for s in list(bsensor): + if s.get("name"): + s.set("name", _rn(s.get("name"))) + if s.get("joint"): + s.set("joint", _rn(s.get("joint"))) + tree.write(os.path.join(ROBOT_DIR, "robot.xml")) + + # --- GRIPPERS --- + for side in SIDES: + s0 = side[0] + g = ET.Element("mujoco", {"model": f"sonic_dex3_{side}"}) + ET.SubElement(g, "compiler", {"angle": "radian", "autolimits": "true"}) + g.append(copy.deepcopy(orig_default)) + gasset = ET.SubElement(g, "asset") + for mn in sorted(n for n in finger_meshes if n.startswith(f"{side}_hand")): + fn = _copy_mesh(mn, os.path.join(GRIP_DIR, GRIP_MESH_REL), mesh_elems) + me = copy.deepcopy(mesh_elems[mn]) + me.set("file", os.path.join(GRIP_MESH_REL, fn)) + gasset.append(me) + wbody = ET.SubElement(g, "worldbody") + rootb = ET.SubElement(wbody, "body", {"name": f"{s0}_gripper_base", "pos": "0 0 0", "quat": "1 0 0 0"}) + ET.SubElement(rootb, "inertial", {"pos": "0 0 0", "mass": "0", "diaginertia": "0 0 0"}) + ET.SubElement(rootb, "site", {"name": "ft_frame", "pos": "0 0 0", "size": "0.01", + "rgba": "1 0 0 0", "type": "sphere", "group": "1"}) + eefb = ET.SubElement(rootb, "body", {"name": "eef", "pos": "0.05 0 0"}) + ET.SubElement(eefb, "inertial", {"pos": "0 0 0", "mass": "0", "diaginertia": "0 0 0"}) + ET.SubElement(eefb, "site", {"name": "grip_site", "pos": "0 0 0", "size": "0.01", + "rgba": "1 1 0 1", "type": "sphere", "group": "2"}) + for ax, q, color, pos in (("x", "0.707105 0 0.707108 0", "1 0 0 0", "0.1 0 0"), + ("y", "0.707105 0.707108 0 0", "0 1 0 0", "0 0.1 0"), + ("z", "1 0 0 0", "0 0 1 0", "0 0 0.1")): + ET.SubElement(eefb, "site", {"name": f"ee_{ax}", "pos": pos, "size": "0.005 .1", + "quat": q, "rgba": color, "type": "cylinder", "group": "1"}) + ET.SubElement(eefb, "site", {"name": "grip_site_cylinder", "pos": "0 0 0", "quat": "1 0 0 0", + "size": "0.005 0.5", "rgba": "0 1 0 0.3", "type": "cylinder", "group": "1"}) + for fb in grip_fingers[side]: + rootb.append(fb) + gact = ET.SubElement(g, "actuator") + for m in body_hand_motors[side]: + gact.append(m) + gsensor = ET.SubElement(g, "sensor") # robosuite expects raw names force_ee/torque_ee + ET.SubElement(gsensor, "force", {"name": "force_ee", "site": "ft_frame"}) + ET.SubElement(gsensor, "torque", {"name": "torque_ee", "site": "ft_frame"}) + ET.ElementTree(g).write(os.path.join(GRIP_DIR, f"sonic_dex3_{side}.xml")) + + print(f"robot -> {ROBOT_DIR}/robot.xml ({len(asset.findall('mesh'))} meshes)") + print(f"grippers-> {GRIP_DIR}/sonic_dex3_{{left,right}}.xml") + print(f"body actuators: {len(act.findall('motor'))}; hand joints/side: {len(hand_joints)//2}") + + +if __name__ == "__main__": + main() diff --git a/robosuite/scripts/collect_sonic_g1_demos.py b/robosuite/scripts/collect_sonic_g1_demos.py new file mode 100644 index 0000000000..6fa770c4fa --- /dev/null +++ b/robosuite/scripts/collect_sonic_g1_demos.py @@ -0,0 +1,357 @@ +"""Drive the native robosuite SonicG1 live from the C++ SONIC controller over DDS. + +Uses the registered ``SonicG1`` robot (true free-floating base) + the ``SONIC_WBC`` +composite controller inside a real robosuite ``Environment``. The C++ SONIC stack runs +in ANOTHER terminal and drives the robot over Unitree DDS: this process publishes +``rt/lowstate`` and applies the received ``rt/lowcmd``, holding the floating base up with +an elastic band on the pelvis during the C++ handoff. Press '9' in the viewer to release +the band once the policy is balancing (and before commanding locomotion -- it anchors the +pelvis horizontally). There is NO fall recovery -- a collapse stays down (by design; this +env is mainly for interactive debugging). + +By default the robot spawns in a table-free ``EmptyArena`` (``SonicArenaEnv`` below). +``--environment`` instead loads ANY registered robosuite env (Lift, Stack, TwoArmLift, +...) -- safe for object/table envs because the band never pins qpos. The env places the +robot's x,y + facing (in front of the table); only its standing joint pose is set here. + +(Recording to a robosuite demo.hdf5 from the live loop is still a TODO; the recording +helpers gather_to_hdf5 / sonic_action below + DataCollectionWrapper are exercised by the +test suite, tests/test_robots/test_sonic_g1.py.) + +Example -- start the C++ SONIC controller in terminal 1 (gear_sonic_deploy; keyboard, OR a +planner via --planner-file + --input-type zmq/gamepad/ros2), then in terminal 2 (needs a +display; do NOT set MUJOCO_GL=egl): + python -m robosuite.scripts.collect_sonic_g1_demos --environment Lift +""" +import argparse +import datetime +import json +import os + +import mujoco +import numpy as np + +import robosuite # noqa: F401 (registers SonicG1 / Dex3 / SONIC_WBC) +from robosuite.controllers import load_composite_controller_config +from robosuite.environments.manipulation.manipulation_env import ManipulationEnv +from robosuite.models.arenas import EmptyArena +from robosuite.models.tasks import ManipulationTask +from robosuite.utils.sonic.action_sources import DDSActionSource + +SONIC_CFG = os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(robosuite.__file__))), + "robosuite", "controllers", "config", "robots", "default_sonic_g1.json") + + +class SonicArenaEnv(ManipulationEnv): + """Minimal table-free stage: an EmptyArena holding one legged robot, no task + objects. A clean place to drive the SonicG1 with SONIC (no table to obstruct + whole-body / locomotion motion). reward/success are trivial (driving, not a task).""" + + def reward(self, action=None): + return 0.0 + + def _check_success(self): + return False + + def _check_robot_configuration(self, robots): + pass + + def _load_model(self): + super()._load_model() # instantiates + loads the robot model(s) + arena = EmptyArena() + arena.set_origin([0, 0, 0]) + self.model = ManipulationTask( + mujoco_arena=arena, + mujoco_robots=[robot.robot_model for robot in self.robots], + mujoco_objects=[], + ) + + +def match_base_sim_physics(model, floor_friction=1.0, floor_torsion=0.005, timestep=0.002): + """Set the global MuJoCo options + floor contact to base_sim's (gold was recorded + there): Euler, pyramidal cone, impratio 1, no fluid, base_sim floor friction. + (Per-joint armature/damping/frictionloss already come from model_data via the asset + build, so they need no override.) + + timestep is the physics dt (== robosuite ``model_timestep``); keep it equal to + ``macros.SIMULATION_TIMESTEP`` so robosuite's substep count (control_timestep/ + model_timestep) matches the true MuJoCo advance. base_sim recorded gold at 500 Hz + (0.002); 200 Hz (0.005) is the lighter rate used for downstream policy collection + (1 substep == 1 PD command when control_freq == 1/timestep). PD is semi-implicit in + MuJoCo's Euler integrator, so it stays numerically stable at 0.005. + + floor_friction / floor_torsion default to base_sim's exact values (tangential 1.0, + torsional 0.005). MuJoCo combines contact friction as max(foot, floor), so RAISING + these grips the feet harder than base_sim -- useful if SONIC's slow walk slips / + pivots in robosuite (low torsional friction lets the planted foot spin during turns). + Keep the defaults to stay bit-faithful to base_sim.""" + model.opt.timestep = timestep + model.opt.integrator = int(mujoco.mjtIntegrator.mjINT_EULER) + model.opt.cone = int(mujoco.mjtCone.mjCONE_PYRAMIDAL) + model.opt.impratio = 1.0 + model.opt.density = 0.0 + model.opt.viscosity = 0.0 + for g in range(model.ngeom): + if model.geom_type[g] == mujoco.mjtGeom.mjGEOM_PLANE: + model.geom_friction[g] = [floor_friction, floor_torsion, 0.0001] + model.geom_solref[g] = [0.02, 1.0] + +def make_env(env_name, robot, cfg, has_renderer, control_freq=None): + """Build the env that SONIC drives: the table-free ``SonicArenaEnv`` (default) or ANY + registered robosuite env via ``robosuite.make`` (e.g. Lift / Stack / TwoArmLift). Real + object/table envs are now safe -- the startup elastic band no longer pins the whole + qpos, so task objects evolve under normal physics. The env places the robot's x,y + + facing (in front of the table); the robot spawns standing via SonicG1.init_qpos + + base_xpos_offset, so no separate set-pose step is needed. + + control_freq (None == env default): the step() rate. Set it to 1/``macros.SIMULATION_TIMESTEP`` + so each step() is exactly one physics substep == one SONIC PD command (e.g. 200 with dt + 0.005); leaving it at the default makes step() span many PD commands (the rate mismatch + that downstream per-step policy eval trips over).""" + kw = dict(robots=[robot], controller_configs=cfg, has_renderer=has_renderer, + has_offscreen_renderer=False, use_camera_obs=False, + hard_reset=False, ignore_done=True, horizon=10_000_000) + if control_freq is not None: + kw["control_freq"] = control_freq + if env_name in ("SonicArena", "Empty", "empty"): + return SonicArenaEnv(**kw) + return robosuite.make(env_name, **kw) + + +def sonic_action(env): + """The per-motor SONIC target actually commanded this step (body q* then hand q*), + recorded as the demo 'action' (meaningful trajectory; the robosuite action input + itself is ignored by SONIC_WBC).""" + last = env.robots[0].composite_controller.last_command + if last is None or last[0] is None: + return np.zeros(env.action_dim) + cmd, hands = last + parts = [cmd.q] + if hands is not None: + parts += [hands[0].q, hands[1].q] + a = np.concatenate(parts) + # pad/trim to action_dim so DataCollectionWrapper is happy + out = np.zeros(env.action_dim) + out[:min(len(a), env.action_dim)] = a[:env.action_dim] + return out + + +def gather_to_hdf5(directory, out_dir, env_name, env_info): + """Pack per-episode state_*.npz (written by DataCollectionWrapper) into a + robosuite-format demo.hdf5 (states + actions + per-episode model_file).""" + import h5py + os.makedirs(out_dir, exist_ok=True) + hdf5_path = os.path.join(out_dir, "demo.hdf5") + f = h5py.File(hdf5_path, "w") + grp = f.create_group("data") + n = 0 + for ep in sorted(os.listdir(directory)): + ep_dir = os.path.join(directory, ep) + if not os.path.isdir(ep_dir): + continue + states, actions = [], [] + for sf in sorted(s for s in os.listdir(ep_dir) if s.startswith("state_")): + dic = np.load(os.path.join(ep_dir, sf), allow_pickle=True) + states.extend(dic["states"]) + actions.extend(ai["actions"] for ai in dic["action_infos"]) + if len(states) == 0: + continue + del states[-1] # last state has no following action + assert len(states) == len(actions), (len(states), len(actions)) + n += 1 + g = grp.create_group(f"demo_{n}") + with open(os.path.join(ep_dir, "model.xml")) as mf: + g.attrs["model_file"] = mf.read() + g.create_dataset("states", data=np.array(states)) + g.create_dataset("actions", data=np.array(actions)) + now = datetime.datetime.now() + grp.attrs["date"] = f"{now.month}-{now.day}-{now.year}" + grp.attrs["repository_version"] = robosuite.__version__ + grp.attrs["env"] = env_name + grp.attrs["env_info"] = json.dumps(env_info) + f.close() + print(f"[demos] wrote {n} episode(s) -> {hdf5_path}", flush=True) + return hdf5_path + + +def run_dds_interactive(env, render=True, pace=True, render_hz=20.0): + """Live interactive driver: the robot is driven by the C++ SONIC controller running in + ANOTHER terminal (this process publishes rt/lowstate + applies the received rt/lowcmd). + SonicWholeBodyController owns the startup hold (an elastic band on the pelvis holds the + floating base up during the C++ handoff -- object-safe, no qpos pin) and builds its own + engine; the robot spawns standing via SonicG1.init_qpos. So this loop just steps in real + time and renders. Drive from the C++ terminal (']' start; '[ENTER]'->planner, 'W'/'T'/'N'/'P'). + + Press '9' in the viewer to drop the band once the policy is balancing (and before + commanding locomotion -- the band anchors the pelvis horizontally); that also marks the + start of the active phase. The loop then runs until the task succeeds (env._check_success + held briefly, as in collect_human_demonstrations) or the viewer is closed / Ctrl-C. + SonicArenaEnv has no success condition, so it runs until you stop it. NO fall recovery. + + Rendering uses a passive mujoco.viewer (cheap viewer.sync), NOT robosuite's synchronous + per-step OpenCVViewer (env.render) -- the C++ planner integrates its velocity command in + wall-clock and assumes the sim runs real time, so the heavy synchronous render must stay + off the loop.""" + import time + controller = env.robots[0].composite_controller + mj_data = env.sim.data._data if hasattr(env.sim.data, "_data") else env.sim.data + model = env.sim.model._model + sim_dt = float(model.opt.timestep) + # sync the passive viewer at ~render_hz (default 20), decoupled from the 200 Hz control loop -- + # syncing every step would starve the wall-clock-paced loop. + render_every = max(1, int(round(env.control_freq / render_hz))) + # The action IS the SONIC q* command, produced by the DDS source (the controller consumes it). + # The source runs OUTSIDE env.step (it publishes lowstate + reads lowcmd before stepping), so + # the loop -- not env.real_time -- paces the whole iteration to real time. + source = DDSActionSource(controller._cfg) + source.reset(env) + hold = np.zeros(env.action_dim) + pelvis = next((b for b in range(model.nbody) if "pelvis" in + (mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, b) or "")), -1) + + viewer = None + if render: + # NB: `from mujoco import viewer` (not `import mujoco.viewer`) so we don't rebind the + # module-level `mujoco` to a function-local -- the pelvis lookup above uses it. + from mujoco import viewer as mj_viewer + # '9' toggles the startup elastic band (mirrors base_sim): drop it once the policy is + # balancing and before commanding locomotion (it anchors the pelvis horizontally). + def _key_callback(key): + if key == ord("9"): + controller.toggle_band() + print(f"[collect] elastic band -> {'ON' if controller.band_enabled else 'OFF'}", flush=True) + try: + viewer = mj_viewer.launch_passive(model, mj_data, show_left_ui=False, + show_right_ui=False, key_callback=_key_callback) + cam = viewer.cam + cam.type = mujoco.mjtCamera.mjCAMERA_TRACKING + cam.trackbodyid = pelvis + cam.distance, cam.azimuth, cam.elevation = 3.0, 120, -20 + viewer.sync() + print("[collect] viewer open (close window or Ctrl-C to stop).", flush=True) + except Exception as exc: + print(f"[collect] viewer unavailable ({exc}); running headless. (For a window, " + f"ensure a display and do NOT set MUJOCO_GL=egl.)", flush=True) + viewer = None + + print("[collect] waiting for C++ -- start the SONIC controller, press ']' in its terminal, " + "then '9' in the viewer to release the band and begin.", flush=True) + started = False + success_hold = 0 + step_count = 0 + next_step_time = time.perf_counter() + try: + while True: + a = source.act(env) + if a is None: + env.step(hold) # C++ not engaged yet: controller holds, band keeps pelvis up + else: + if source.gains: + controller.set_command_gains(source.gains) # constant gains captured from the stream + env.step(a) + step_count += 1 + if not started and not controller.band_enabled: # band released via '9' + started = True + print("[collect] band released -- active (drive from the C++ terminal).", flush=True) + if started: + # run until the task succeeds (held briefly), like collect_human_demonstrations; + # SonicArenaEnv._check_success is always False -> runs until viewer close / Ctrl-C. + success_hold = success_hold + 1 if env._check_success() else 0 + if success_hold >= 10: + print("[collect] task success -- stopping.", flush=True) + break + if step_count % 1000 == 0: + print(f"[collect] live: pelvis_z={float(mj_data.xpos[pelvis][2]):.3f}", flush=True) + if viewer is not None: + if not viewer.is_running(): + print("[collect] viewer closed; stopping...", flush=True) + break + if step_count % render_every == 0: + viewer.sync() + if pace: # pace the WHOLE loop (source.act + env.step) to one control step of wall-clock + next_step_time += env.control_timestep + sleep_time = next_step_time - time.perf_counter() + if sleep_time > 0: + time.sleep(sleep_time) + finally: + if viewer is not None: + try: + viewer.close() + except Exception: + pass + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--environment", default="SonicArena", + help="env to drive in: 'SonicArena' (default, table-free EmptyArena) " + "or any registered robosuite env (e.g. Lift, Stack, TwoArmLift). " + "Object/table envs are safe (the band no longer pins qpos); the " + "env places the robot in front of its table automatically.") + ap.add_argument("--robot", default="SonicG1", help="SonicG1 (floating) or SonicG1Fixed") + ap.add_argument("--floor-friction", type=float, default=1.0, + help="floor tangential friction (base_sim=1.0; raise to grip feet harder)") + ap.add_argument("--floor-torsion", type=float, default=0.005, + help="floor torsional friction (base_sim=0.005; raise to reduce foot " + "pivot/spin on turns -- a common cause of visible 'slipping')") + ap.add_argument("--no-render", action="store_true") + ap.add_argument("--no-real-time", dest="real_time", action="store_false", default=True, + help="run the source+step loop flat-out instead of pacing it to the control rate. " + "The live C++ is wall-clock paced, so keep real-time on for live runs; use " + "this only for headless replay/throughput.") + ap.add_argument("--rtf-log", action="store_true", + help="flip console logging to DEBUG so base.py prints the [real-time] RTF line " + "~1x/sec (a real-time-factor sanity check).") + ap.add_argument("--sim-dt", type=float, default=0.005, + help="physics timestep (s). Default 0.005 (200 Hz): with --control-freq 200, " + "step()==1 substep==1 SONIC PD command, so the true control rate == the " + "recorded/PD-command rate. Use 0.002 for base_sim's 500 Hz (gold-faithful).") + ap.add_argument("--control-freq", type=int, default=200, + help="step() rate (Hz); set == 1/--sim-dt for one physics substep (one PD " + "command) per step().") + ap.add_argument("--post-action-hz", type=float, default=20.0, + help="rate (Hz) to run the expensive once-per-step bookkeeping (reward/" + "_check_success + any subclass update_state). Default 20 Hz (sets " + "base.post_action_freq = control_freq/20); reward is unused for SONIC demos.") + ap.add_argument("--render-hz", type=float, default=20.0, + help="passive-viewer sync rate (Hz). Default 20; decoupled from the 200 Hz " + "control loop so the render stays off it. Ignored when headless (--no-render).") + args = ap.parse_args() + + import robosuite.macros as macros + if args.rtf_log: + macros.CONSOLE_LOGGING_LEVEL = "DEBUG" # before make_env so initialize_time reads it + # physics rate: robosuite reads model_timestep from this macro in initialize_time (every + # reset), so set it BEFORE make_env; match_base_sim_physics stamps model.opt.timestep to match. + macros.SIMULATION_TIMESTEP = args.sim_dt + + cfg = load_composite_controller_config(controller=SONIC_CFG) + + # No robosuite per-step renderer: run_dds_interactive drives a passive viewer so the + # sim+DDS exchange stays real-time at the wall-clock-paced C++ planner's rate. + base = make_env(args.environment, args.robot, cfg, has_renderer=False, + control_freq=args.control_freq) + base.reset() + match_base_sim_physics(base.sim.model._model, args.floor_friction, args.floor_torsion, + timestep=args.sim_dt) + # throttle the expensive once-per-step bookkeeping (reward/_check_success/update_state) to + # ~--post-action-hz; base.post_action_freq is "every Nth step()" so derive it from control_freq. + base.post_action_freq = max(1, int(round(base.control_freq / args.post_action_hz))) + if base.post_action_freq > 1: + print(f"[collect] per-step bookkeeping (reward/_check_success) every {base.post_action_freq} " + f"steps (~{base.control_freq / base.post_action_freq:.0f} Hz).", flush=True) + if args.real_time: + print("[collect] real-time pacing ON (the driver paces the whole source+step loop to the " + "control rate, matching the wall-clock-paced C++); pass --no-real-time to run flat-out.", + flush=True) + try: + run_dds_interactive(base, render=not args.no_render, pace=args.real_time, + render_hz=args.render_hz) + except KeyboardInterrupt: + print("\n[collect] stopped.", flush=True) + finally: + base.close() + + +if __name__ == "__main__": + main() diff --git a/robosuite/utils/sonic/__init__.py b/robosuite/utils/sonic/__init__.py new file mode 100644 index 0000000000..cb3e521550 --- /dev/null +++ b/robosuite/utils/sonic/__init__.py @@ -0,0 +1,8 @@ +"""SONIC whole-body control engine for the robosuite G1 integration. + +- controller.G1SonicController : obs build + per-motor PD + effort clip (ported from + gear_sonic base_sim; the engine the SonicWholeBodyController composite controller wraps) +- sources.DDSCommandSource : live C++/DDS command source (+ MotorCommand) +""" +from .controller import G1SonicController, MotorCommand +from .sources import DDSCommandSource, init_dds_once diff --git a/robosuite/utils/sonic/action_sources.py b/robosuite/utils/sonic/action_sources.py new file mode 100644 index 0000000000..0beea66211 --- /dev/null +++ b/robosuite/utils/sonic/action_sources.py @@ -0,0 +1,105 @@ +"""Action sources for the SONIC whole-body controller. + +The SONIC refactor makes the per-motor joint-position target **q\*** the env action vector: +``SonicWholeBodyController`` consumes it (``set_goal``) and applies the PD law with the (constant, +per-motor) gains the deployment uses; the **source** of that action is pluggable: + + - ``DDSActionSource`` -- live: publishes lowstate to / reads lowcmd from the C++ SONIC controller + over Unitree DDS, and returns its commanded q\* as the action. + - ``ReplayActionSource`` -- headless: yields recorded q\* actions from a demo (Phase B). + - a learned policy is itself a source (it outputs q\*); no class needed. + +Why q\* alone is enough: the SONIC command's kp/kd are **constant** over an episode (verified: zero +variance in the gold streams), dq\*≡0 and tau_ff≡0. So gains are not in the action; the live source +**captures them once** from the first real command and hands them to the controller +(``set_command_gains``). Action layout (MOTOR order): ``[body q* (29), left-hand q* (7), +right-hand q* (7)]`` = 43 for SonicG1. +""" +import numpy as np + +from robosuite.utils.sonic.controller import G1SonicController +from robosuite.utils.sonic.sources import DDSCommandSource + + +class SonicActionSource: + """Interface: ``act(env) -> np.ndarray | None`` returns the per-motor q\* action (None until the + source has a command, e.g. before the live C++ engages). ``reset(env)`` clears per-episode state. + Live sources also expose ``gains`` -- the captured constant (kp, kd) per source -- once known.""" + + gains = None # dict: {"body": (kp, kd), "lhand": (kp, kd), "rhand": (kp, kd)}; entries appear as captured + + def reset(self, env): + pass + + def act(self, env): + raise NotImplementedError + + +class DDSActionSource(SonicActionSource): + """Live action source: drives the SONIC C++ controller over DDS and returns its commanded q\*. + + Owns its own ``G1SonicController`` engine (for ``build_obs`` + the motor-index maps) and a + ``DDSCommandSource`` (the Unitree SDK2 bridge). Each ``act`` does one exchange (publish lowstate, + read lowcmd) and assembles the 43-dim q\* action. Returns None before the C++ sends commands, so + the driver holds (the controller's startup elastic band keeps the pelvis up meanwhile).""" + + def __init__(self, config): + self._cfg = config + self._src = DDSCommandSource(config) # opens the DDS bridge (no hold_q: None until engaged) + self._engine = None # built lazily on first act (sim fully compiled by then) + self.gains = None + + def reset(self, env): + self._engine = None + self.gains = None + bridge = getattr(self._src, "bridge", None) + if bridge is not None and hasattr(bridge, "reset"): + bridge.reset() + + def _capture_gains(self, cmd, lh, rh): + """Record the (constant) per-source gains the first time each is available.""" + if self.gains is None: + self.gains = {} + if "body" not in self.gains and cmd is not None: + self.gains["body"] = (np.array(cmd.kp), np.array(cmd.kd)) + if "lhand" not in self.gains and lh is not None: + self.gains["lhand"] = (np.array(lh.kp), np.array(lh.kd)) + if "rhand" not in self.gains and rh is not None: + self.gains["rhand"] = (np.array(rh.kp), np.array(rh.kd)) + + def act(self, env): + if self._engine is None: + self._engine = G1SonicController(env.sim, self._src, self._cfg) + obs, cmd, hands = self._engine.exchange() + if cmd is None: + return None # C++ not engaged yet + lh, rh = hands if hands is not None else (None, None) + self._capture_gains(cmd, lh, rh) + # hands lag the body command on startup -> hold the current hand pose until commanded + lh_q = lh.q if lh is not None else obs.get("left_hand_q") + rh_q = rh.q if rh is not None else obs.get("right_hand_q") + parts = [np.asarray(cmd.q, dtype=float)] + if self._engine.has_hands: + parts += [np.asarray(lh_q, dtype=float), np.asarray(rh_q, dtype=float)] + return np.concatenate(parts) + + +class ReplayActionSource(SonicActionSource): + """Headless action source: replays a recorded q\* action sequence (one per control step), with + the constant gains captured at collection. Drives action-replay / dataset playback through the + same controller path as live (Phase B).""" + + def __init__(self, actions, gains=None): + self._actions = np.asarray(actions, dtype=float) + self.gains = gains # {"body": (kp,kd), ...} saved at collection; None -> controller must have them + self._t = 0 + + def reset(self, env): + self._t = 0 + + def act(self, env): + if self._t >= len(self._actions): + return None # sequence exhausted + a = self._actions[self._t] + self._t += 1 + return a diff --git a/robosuite/utils/sonic/controller.py b/robosuite/utils/sonic/controller.py new file mode 100644 index 0000000000..f0ec4347ec --- /dev/null +++ b/robosuite/utils/sonic/controller.py @@ -0,0 +1,212 @@ +"""G1SonicController: whole-body torque bridge for robosuite. + +Mirrors what ``gear_sonic``'s ``base_sim`` does on the MuJoCo side, but inside a +robosuite env: every control substep it + 1. builds the proprioceptive obs dict from ``sim.data`` (== base_sim.prepare_obs), + 2. hands it to a *command source* (publishes lowstate over DDS to the C++ + SONIC controller, or a local mock), + 3. reads back the per-motor command (q*, dq*, kp, kd, tau_ff), + 4. applies SONIC's per-motor PD law and writes torques to ``sim.data.ctrl``. + +The PD law is ported verbatim from ``base_sim.compute_body_torques``: + tau_i = tau_ff_i + kp_i*(q*_i - q_i) + kd_i*(dq*_i - dq_i) +with gains coming from the command stream (no mass-matrix decoupling, no gravity +compensation) — i.e. the real Unitree motor behavior, NOT robosuite's +JointPositionController. +""" + +import mujoco +import numpy as np + +# Body-part keywords used by base_sim to identify the 29 actuated body joints. +_BODY_JOINT_KEYWORDS = ["hip", "knee", "ankle", "waist", "shoulder", "elbow", "wrist"] + +# Dex3 hand joints, 7 per side, in the order the C++ sends rt/dex3/*/cmd +# (== gear_sonic joint_utils.G1_HAND_JOINTS). +_LEFT_HAND = ["left_hand_index_0_joint", "left_hand_index_1_joint", + "left_hand_middle_0_joint", "left_hand_middle_1_joint", + "left_hand_thumb_0_joint", "left_hand_thumb_1_joint", "left_hand_thumb_2_joint"] +_RIGHT_HAND = ["right_hand_index_0_joint", "right_hand_index_1_joint", + "right_hand_middle_0_joint", "right_hand_middle_1_joint", + "right_hand_thumb_0_joint", "right_hand_thumb_1_joint", "right_hand_thumb_2_joint"] + +# Canonical joint order of config["motor_effort_limit_list"] (model_data 43-DOF +# ACTUATOR order: legs, waist, L-arm, L-hand, R-arm, R-hand). We map effort limits +# by NAME (not actuator index) so it's correct regardless of how the model is +# assembled -- the integrated model (legs,waist,L-arm,L-hand,R-arm,R-hand) AND the +# native robosuite robot (body 29 then split-out grippers) both resolve right. +_EFFORT_NAMES = [ + "left_hip_pitch", "left_hip_roll", "left_hip_yaw", "left_knee", "left_ankle_pitch", "left_ankle_roll", + "right_hip_pitch", "right_hip_roll", "right_hip_yaw", "right_knee", "right_ankle_pitch", "right_ankle_roll", + "waist_yaw", "waist_roll", "waist_pitch", + "left_shoulder_pitch", "left_shoulder_roll", "left_shoulder_yaw", "left_elbow", + "left_wrist_roll", "left_wrist_pitch", "left_wrist_yaw", + "left_hand_thumb_0", "left_hand_thumb_1", "left_hand_thumb_2", + "left_hand_middle_0", "left_hand_middle_1", "left_hand_index_0", "left_hand_index_1", + "right_shoulder_pitch", "right_shoulder_roll", "right_shoulder_yaw", "right_elbow", + "right_wrist_roll", "right_wrist_pitch", "right_wrist_yaw", + "right_hand_thumb_0", "right_hand_thumb_1", "right_hand_thumb_2", + "right_hand_middle_0", "right_hand_middle_1", "right_hand_index_0", "right_hand_index_1", +] + + +def _effort_by_name(eff_list): + """name->effort from the canonical 43-DOF list, longest-key-first for matching.""" + d = dict(zip(_EFFORT_NAMES, eff_list)) + keys = sorted(d, key=len, reverse=True) + return d, keys + + +def _lookup_effort(joint_name, eff_d, eff_keys, default=5.0): + for k in eff_keys: # longest first so e.g. left_hip_pitch beats no shorter key + if k in joint_name: + return eff_d[k] + return default + + +class MotorCommand: + """Per-motor command (length = num_motors), all in Unitree motor order.""" + + __slots__ = ("q", "dq", "kp", "kd", "tau") + + def __init__(self, q, dq, kp, kd, tau): + self.q, self.dq, self.kp, self.kd, self.tau = q, dq, kp, kd, tau + + +class G1SonicController: + """Reads sim state, exchanges it with a command source, and writes torques. + + A command source needs ``update(obs)`` (consume latest obs, e.g. publish lowstate) + and ``read()`` (return a MotorCommand or None); optionally ``read_hands()``. The only + production source is DDSCommandSource; tests inject their own.""" + + def __init__(self, sim, command_source, config: dict): + self.sim = sim + self.src = command_source + self.last_obs = None + m = sim.model._model if hasattr(sim.model, "_model") else sim.model + + # --- motor index maps (actuator order == Unitree motor order) --- + self.num_motors = int(config["NUM_MOTORS"]) + self.qpos_adr, self.qvel_adr, self.ctrl_idx, self.motor_joint_names = [], [], [], [] + for a in range(m.nu): + jid = int(m.actuator_trnid[a, 0]) + name = mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_JOINT, jid) + if any(k in name for k in _BODY_JOINT_KEYWORDS): + self.qpos_adr.append(int(m.jnt_qposadr[jid])) + self.qvel_adr.append(int(m.jnt_dofadr[jid])) + self.ctrl_idx.append(a) + self.motor_joint_names.append(name) + assert len(self.ctrl_idx) == self.num_motors, ( + f"expected {self.num_motors} body actuators, found {len(self.ctrl_idx)}" + ) + self.qpos_adr = np.array(self.qpos_adr) + self.qvel_adr = np.array(self.qvel_adr) + self.ctrl_idx = np.array(self.ctrl_idx) + + # --- free joint + torso bookkeeping (for obs); prefix-tolerant so it works + # on bare model_data names AND robosuite's "robot0_"-prefixed names --- + fj = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_JOINT, "floating_base_joint") + if fj < 0: # native robosuite robot: locate the free joint by type + frees = [j for j in range(m.njnt) if m.jnt_type[j] == mujoco.mjtJoint.mjJNT_FREE] + # prefer the ROBOT's own base (pelvis) over any task-object freejoints + # (e.g. a TwoArmLift pot) so build_obs publishes the robot base, not clutter + fj = next((j for j in frees if "pelvis" in + (mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_BODY, m.jnt_bodyid[j]) or "")), + frees[0] if frees else -1) + self.free_qadr = int(m.jnt_qposadr[fj]) if fj >= 0 else None + self.free_vadr = int(m.jnt_dofadr[fj]) if fj >= 0 else None + self.torso_id = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_BODY, "torso_link") + if self.torso_id < 0: + self.torso_id = next((b for b in range(m.nbody) if "torso_link" in + (mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_BODY, b) or "")), -1) + + # Effort limits by ACTUATOR ORDER (rename-robust -- robosuite-conformant joint + # names like l_leg_hip_pitch break name matching). config list is 43-DOF + # actuator order: legs+waist [0:15], L-arm [15:22], L-hand [22:29], R-arm + # [29:36], R-hand [36:43]. Body actuators (ctrl_idx) are in MOTOR_ORDER (legs, + # waist, L-arm, R-arm) -> body slice = [0:22] + [29:36] (skip L-hand). + eff_list = config["motor_effort_limit_list"] + self.effort_limit = np.array(list(eff_list[0:22]) + list(eff_list[29:36])) + self._mj_model = m + + # --- Dex3 hand maps (7/side) in the MODEL's njnt order, exactly like + # base_sim's bridge (left_hand_index/right_hand_index). The rt/dex3 state + # we publish and the cmd we apply must be in this same order, NOT a + # hardcoded finger order (the model lists hands thumb-first). --- + jid_to_act = {int(m.actuator_trnid[a, 0]): a for a in range(m.nu)} + lqa, lva, lci, lnm, rqa, rva, rci, rnm = [], [], [], [], [], [], [], [] + for j in range(m.njnt): + name = mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_JOINT, j) or "" + if j not in jid_to_act: + continue + if "left_hand" in name: + lqa.append(int(m.jnt_qposadr[j])); lva.append(int(m.jnt_dofadr[j])); lci.append(jid_to_act[j]); lnm.append(name) + elif "right_hand" in name: + rqa.append(int(m.jnt_qposadr[j])); rva.append(int(m.jnt_dofadr[j])); rci.append(jid_to_act[j]); rnm.append(name) + self._lh = (np.array(lqa), np.array(lva), np.array(lci)) if len(lci) == 7 else None + self._rh = (np.array(rqa), np.array(rva), np.array(rci)) if len(rci) == 7 else None + self._lh_names = lnm if len(lci) == 7 else [] # hand joint names, aligned with + self._rh_names = rnm if len(rci) == 7 else [] # the hand cmd / _lh,_rh maps + self.has_hands = self._lh is not None and self._rh is not None + # hand effort limits by actuator order (L-hand [22:29], R-hand [36:43]); the + # hand maps are built in njnt order == config hand order (thumb-first). + self._lh_eff = (np.array(eff_list[22:29]) if self._lh is not None else np.full(7, 5.0)) + self._rh_eff = (np.array(eff_list[36:43]) if self._rh is not None else np.full(7, 5.0)) + + # ------------------------------------------------------------------ + def build_obs(self) -> dict: + """Replicates base_sim.prepare_obs for the 29-DOF body (no hands).""" + d = self.sim.data + m = self._mj_model + md = d._data if hasattr(d, "_data") else d + obs = {} + if self.free_qadr is not None: + obs["floating_base_pose"] = np.array(md.qpos[self.free_qadr:self.free_qadr + 7]) + obs["floating_base_vel"] = np.array(md.qvel[self.free_vadr:self.free_vadr + 6]) + obs["floating_base_acc"] = np.array(md.qacc[self.free_vadr:self.free_vadr + 6]) + else: + obs["floating_base_pose"] = np.zeros(7) + obs["floating_base_vel"] = np.zeros(6) + obs["floating_base_acc"] = np.zeros(6) + + obs["secondary_imu_quat"] = np.array(md.xquat[self.torso_id]) + vel6 = np.zeros(6) + mujoco.mj_objectVelocity(m, md, mujoco.mjtObj.mjOBJ_BODY, self.torso_id, vel6, 1) + # mj_objectVelocity returns [ang, lin]; swap to [lin, ang] (== base_sim) + vel6[0:3], vel6[3:6] = vel6[3:6].copy(), vel6[0:3].copy() + obs["secondary_imu_vel"] = vel6 + + obs["body_q"] = np.array(md.qpos[self.qpos_adr]) + obs["body_dq"] = np.array(md.qvel[self.qvel_adr]) + obs["body_ddq"] = np.array(md.qacc[self.qvel_adr]) + obs["body_tau_est"] = np.array(md.actuator_force[self.ctrl_idx]) + if self.has_hands: + lq, lv, _ = self._lh + rq, rv, _ = self._rh + obs["left_hand_q"] = np.array(md.qpos[lq]) + obs["left_hand_dq"] = np.array(md.qvel[lv]) + obs["right_hand_q"] = np.array(md.qpos[rq]) + obs["right_hand_dq"] = np.array(md.qvel[rv]) + obs["time"] = float(md.time) + return obs + + # ------------------------------------------------------------------ + def exchange(self): + """Build obs, publish state to the command source, and read back the per-motor + command WITHOUT computing or writing any torque. Returns + ``(obs, body_cmd, hand_cmds)`` where body_cmd is a MotorCommand (or None if the + source has nothing yet) and hand_cmds is ``(lcmd, rcmd)`` or None. + + The PD law is evaluated downstream: SonicWholeBodyController routes + (q*, dq*, kp, kd, tau_ff) to robosuite's per-part JointPosition(Velocity) + controllers, which apply it against the live joint state.""" + obs = self.build_obs() + self.last_obs = obs + self.src.update(obs) + cmd = self.src.read() + hands = None + if self.has_hands and hasattr(self.src, "read_hands"): + hands = self.src.read_hands() + return obs, cmd, hands + diff --git a/robosuite/utils/sonic/sources.py b/robosuite/utils/sonic/sources.py new file mode 100644 index 0000000000..f674402134 --- /dev/null +++ b/robosuite/utils/sonic/sources.py @@ -0,0 +1,88 @@ +"""DDSCommandSource: the live command source for G1SonicController -- publishes lowstate +to / reads lowcmd from the real C++ SONIC controller over Unitree SDK2 DDS (reuses +gear_sonic's UnitreeSdk2Bridge). The non-DDS mock/replay sources used by tests live in +the test tree (tests/test_robots/test_sonic_g1.py).""" + +import threading + +import numpy as np + +from .controller import MotorCommand + +_dds_initialized = False +_dds_lock = threading.Lock() + + +def init_dds_once(config): + """Call ChannelFactoryInitialize exactly once per process.""" + global _dds_initialized + with _dds_lock: + if _dds_initialized: + return + from unitree_sdk2py.core.channel import ChannelFactoryInitialize + + if config.get("INTERFACE"): + ChannelFactoryInitialize(config["DOMAIN_ID"], config["INTERFACE"]) + else: + ChannelFactoryInitialize(config["DOMAIN_ID"]) + _dds_initialized = True + + +class DDSCommandSource: + """Bridges to the C++ SONIC controller over DDS. + + Until the C++ starts sending commands, returns a local PD "hold" command + (q=hold_q, config kp/kd) so the robot actively stands on its own legs during + the backend's startup latency -- giving the policy a warm, realistically + moving robot to take over (no freeze, no band).""" + + def __init__(self, config, hold_q=None, hold_kp=None, hold_kd=None): + from gear_sonic.utils.mujoco_sim.unitree_sdk2py_bridge import UnitreeSdk2Bridge + + init_dds_once(config) + # The SonicG1 always has Dex3 hands -> always publish hand state + subscribe + # rt/dex3/{left,right}/cmd (7 motors/side). + self.n_hand = 7 + cfg = dict(config) + cfg["NUM_HAND_MOTORS"] = self.n_hand + self.bridge = UnitreeSdk2Bridge(cfg) + self.n = int(config["NUM_MOTORS"]) + self.hold_q = None if hold_q is None else np.asarray(hold_q, float) + self.hold_kp = None if hold_kp is None else np.asarray(hold_kp, float) + self.hold_kd = None if hold_kd is None else np.asarray(hold_kd, float) + + def update(self, obs: dict): + self.bridge.PublishLowState(obs) + + def read(self): + b = self.bridge + with b.low_cmd_lock: + if not b.low_cmd_received: + if self.hold_q is not None: + return MotorCommand(self.hold_q, np.zeros(self.n), + self.hold_kp, self.hold_kd, np.zeros(self.n)) + return None + mc = b.low_cmd.motor_cmd + q = np.array([mc[i].q for i in range(self.n)]) + dq = np.array([mc[i].dq for i in range(self.n)]) + kp = np.array([mc[i].kp for i in range(self.n)]) + kd = np.array([mc[i].kd for i in range(self.n)]) + tau = np.array([mc[i].tau for i in range(self.n)]) + return MotorCommand(q, dq, kp, kd, tau) + + def read_hands(self): + """Latest Dex3 hand commands (7/side) from rt/dex3/*/cmd, or None.""" + if self.n_hand != 7: + return None + b = self.bridge + with b.left_hand_cmd_lock: + lmc = b.left_hand_cmd.motor_cmd + lc = MotorCommand(*[np.array([getattr(lmc[i], f) for i in range(7)]) + for f in ("q", "dq", "kp", "kd", "tau")]) + with b.right_hand_cmd_lock: + rmc = b.right_hand_cmd.motor_cmd + rc = MotorCommand(*[np.array([getattr(rmc[i], f) for i in range(7)]) + for f in ("q", "dq", "kp", "kd", "tau")]) + return lc, rc + + diff --git a/tests/test_robots/test_sonic_g1.py b/tests/test_robots/test_sonic_g1.py new file mode 100644 index 0000000000..e313a16257 --- /dev/null +++ b/tests/test_robots/test_sonic_g1.py @@ -0,0 +1,519 @@ +"""Tests for the native SonicG1 robot + Dex3 grippers + SonicWholeBodyController. + +Covers: + - registration (SonicG1 / SonicG1Fixed / Dex3 grippers / SONIC_WBC) + - assembly: robosuite RobotModel + Dex3 grippers reassemble to the SAME physics as + the source model (43 DOF, mass preserved) + - OSC via robosuite.make: SonicG1Fixed in a task env, arms controlled by OSC_POSE + (the right EEF moves under an OSC pose command) [self-contained] + - SONIC_WBC: the SONIC composite controller drives the assembled robot (right arm + gets its real 25/5 Nm effort, ctrl finite) [needs external gear_sonic config] + - floating base: the free-floating SonicG1 keeps a top-level pelvis freejoint in a + real robosuite env (NullBase) and SONIC reads the robot's base, not task clutter + - data collection: the native SonicG1 + SONIC_WBC records a robosuite demo.hdf5 + (states + per-step SONIC targets) [needs gold command stream] +""" +import os + +import mujoco +import numpy as np +import pytest + +import robosuite # noqa: F401 registers SonicG1[/Fixed] + Dex3 grippers + SONIC_WBC +from robosuite.controllers import load_composite_controller_config +from robosuite.models import MujocoWorldBase +from robosuite.models.arenas import EmptyArena +from robosuite.models.grippers.sonic_dex3_gripper import SonicDex3LeftGripper, SonicDex3RightGripper +from robosuite.models.robots.manipulators.sonic_g1_robot import SonicG1, SonicG1Fixed +from robosuite.controllers.composite.sonic_whole_body_controller import ( + SonicWholeBodyController, WBC_CONFIG) +from robosuite.utils.sonic.controller import MotorCommand + + +def _first_existing(*paths): + for path in paths: + if os.path.exists(path): + return path + return paths[0] + + +GEAR_MODEL = _first_existing( + "/home/amaddukuri/Projects/GR00T-WholeBodyControl/gear_sonic/data/robot_model/model_data/g1/g1_29dof_with_hand.xml", + "/home/ajay/code/GR00T-WholeBodyControl/gear_sonic/data/robot_model/model_data/g1/g1_29dof_with_hand.xml", +) + + +def _assemble(): + """SonicG1Fixed + Dex3 grippers merged into an EmptyArena (robosuite machinery).""" + m = SonicG1Fixed(idn=0) + m.add_gripper(SonicDex3RightGripper(idn="0_right"), m.eef_name["right"]) + m.add_gripper(SonicDex3LeftGripper(idn="0_left"), m.eef_name["left"]) + world = MujocoWorldBase() + world.merge(EmptyArena()) + world.merge(m) + return world.get_model(mode="mujoco") + + +class _Sim: + """Minimal robosuite-sim duck type for G1SonicController.""" + def __init__(self, m, d): + self.model = type("M", (), {"_model": m})() + self.data = type("D", (), {"_data": d})() + + +def engine_pd_torques(engine, cmd, hands=None): + """The SONIC per-motor PD law evaluated against the engine's CURRENT sim state, + returned as ``{actuator_index: torque}`` clipped to per-motor effort: + ``tau_i = clip(tau_ff_i + kp_i*(q*_i - q_i) + kd_i*(dq*_i - dq_i))``. + This is the reference the SONIC_WBC part-controller dispatch must reproduce; it lives + in test code because production drives the same law through JointPosition(Velocity) + controllers, not this dict (read-only -- writes nothing).""" + md = engine.sim.data._data if hasattr(engine.sim.data, "_data") else engine.sim.data + out = {} + q = md.qpos[engine.qpos_adr] + dq = md.qvel[engine.qvel_adr] + body = np.clip(cmd.tau + cmd.kp * (cmd.q - q) + cmd.kd * (cmd.dq - dq), + -engine.effort_limit, engine.effort_limit) + for k, ci in enumerate(engine.ctrl_idx): + out[int(ci)] = float(body[k]) + if hands is not None and engine.has_hands: + lcmd, rcmd = hands + for hc, (qa, va, ci), eff in ((lcmd, engine._lh, engine._lh_eff), + (rcmd, engine._rh, engine._rh_eff)): + ht = np.clip(hc.tau + hc.kp * (hc.q - md.qpos[qa]) + hc.kd * (hc.dq - md.qvel[va]), + -eff, eff) + for k, c in enumerate(ci): + out[int(c)] = float(ht[k]) + return out + + +def drive_engine_direct(engine, data): + """Test-only direct PD substep (replaces the removed G1SonicController.apply): build + obs + read the command, then write engine_pd_torques straight to ctrl. Used to drive + the engine on a bare MjModel without robosuite's controller stack.""" + _obs, cmd, hands = engine.exchange() + if cmd is None: + data.ctrl[engine.ctrl_idx] = 0.0 + return + for actidx, tau in engine_pd_torques(engine, cmd, hands).items(): + data.ctrl[actidx] = tau + + +def _action_from_gold(gold, idx=0): + return np.concatenate([gold["cmd_q"][idx], gold["lh_cmd_q"][idx], gold["rh_cmd_q"][idx]]) + + +def _gains_from_gold(gold, idx=0): + return { + "body": (np.asarray(gold["cmd_kp"][idx], dtype=float), np.asarray(gold["cmd_kd"][idx], dtype=float)), + "lhand": (np.asarray(gold["lh_cmd_kp"][idx], dtype=float), np.asarray(gold["lh_cmd_kd"][idx], dtype=float)), + "rhand": (np.asarray(gold["rh_cmd_kp"][idx], dtype=float), np.asarray(gold["rh_cmd_kd"][idx], dtype=float)), + } + + +def _commands_from_action(action, gains): + action = np.asarray(action, dtype=float) + z_body = np.zeros(29) + z_hand = np.zeros(7) + cmd = MotorCommand( + q=action[:29], + dq=z_body.copy(), + kp=gains["body"][0], + kd=gains["body"][1], + tau=z_body.copy(), + ) + lh = MotorCommand( + q=action[29:36], + dq=z_hand.copy(), + kp=gains["lhand"][0], + kd=gains["lhand"][1], + tau=z_hand.copy(), + ) + rh = MotorCommand( + q=action[36:43], + dq=z_hand.copy(), + kp=gains["rhand"][0], + kd=gains["rhand"][1], + tau=z_hand.copy(), + ) + return cmd, (lh, rh) + + +# --- Test-only command sources (moved out of robosuite.utils.sonic.sources; production +# only ships DDSCommandSource). Both duck-type the source interface the engine needs: +# update(obs) + read() [+ read_hands()]. --- +class ReferenceMockSource: + """Replays per-motor joint targets (motor order) as PD targets; advances one frame + per read(), holds the last once exhausted.""" + def __init__(self, motor_targets, kp, kd): + self.targets = np.asarray(motor_targets, dtype=np.float64) # (T, n) + self.kp = np.asarray(kp, dtype=np.float64) + self.kd = np.asarray(kd, dtype=np.float64) + self.n = self.targets.shape[1] + self.t = 0 + + def update(self, obs): + pass + + def read(self): + idx = min(self.t, self.targets.shape[0] - 1) + self.t += 1 + return MotorCommand(q=self.targets[idx], dq=np.zeros(self.n), + kp=self.kp, kd=self.kd, tau=np.zeros(self.n)) + + +class ReplayCommandSource: + """Deterministically replays a recorded per-step command stream (body + Dex3 hands) + from a golden npz -- one frame per read(), holds the last once exhausted.""" + def __init__(self, gold): + self.q, self.dq = gold["cmd_q"], gold["cmd_dq"] + self.kp, self.kd, self.tau = gold["cmd_kp"], gold["cmd_kd"], gold["cmd_tau"] + self.T = self.q.shape[0] + self.has_hands = "lh_cmd_q" in gold + if self.has_hands: + self._lh = {f: gold[f"lh_cmd_{f}"] for f in ("q", "dq", "kp", "kd", "tau")} + self._rh = {f: gold[f"rh_cmd_{f}"] for f in ("q", "dq", "kp", "kd", "tau")} + self.t = self._cur = 0 + + def update(self, obs): + pass + + def read(self): + i = self._cur = min(self.t, self.T - 1) + self.t += 1 + return MotorCommand(self.q[i], self.dq[i], self.kp[i], self.kd[i], self.tau[i]) + + def read_hands(self): + if not self.has_hands: + return None + i = self._cur + lc = MotorCommand(*[self._lh[f][i] for f in ("q", "dq", "kp", "kd", "tau")]) + rc = MotorCommand(*[self._rh[f][i] for f in ("q", "dq", "kp", "kd", "tau")]) + return lc, rc + + +def test_registration(): + from robosuite.robots import ROBOT_CLASS_MAPPING + from robosuite.models.grippers import GRIPPER_MAPPING + from robosuite.controllers.composite.composite_controller import ( + REGISTERED_COMPOSITE_CONTROLLERS_DICT) + assert "SonicG1" in ROBOT_CLASS_MAPPING and "SonicG1Fixed" in ROBOT_CLASS_MAPPING + assert "SonicDex3LeftGripper" in GRIPPER_MAPPING + assert "SonicDex3RightGripper" in GRIPPER_MAPPING + assert "SONIC_WBC" in REGISTERED_COMPOSITE_CONTROLLERS_DICT + + +def test_assembles(): + model = _assemble() + assert model.nu == 43 # 29 body + 7 + 7 Dex3 + if os.path.exists(GEAR_MODEL): + orig = mujoco.MjModel.from_xml_path(GEAR_MODEL) + assert model.nu == orig.nu + rmass = sum(model.body_mass[b] for b in range(model.nbody) + if any(k in (mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, b) or "") + for k in ["hip", "knee", "ankle", "waist", "torso", "shoulder", "elbow", + "wrist", "hand", "pelvis", "link", "gripper", "eef", "base"])) + assert abs(rmass - sum(orig.body_mass)) < 1e-3 # physics preserved + + +def test_osc_robosuite_make(): + """SonicG1Fixed loads into a task env and OSC moves the right arm's EEF.""" + cfg = load_composite_controller_config(controller="BASIC") # OSC_POSE arms + env = robosuite.make("TwoArmLift", robots=["SonicG1Fixed"], controller_configs=cfg, + has_renderer=False, has_offscreen_renderer=False, + use_camera_obs=False, control_freq=20) + try: + env.reset() + low, _ = env.action_spec + grip = env.robots[0].gripper["right"].important_sites["grip_site"] + eef0 = env.sim.data.get_site_xpos(grip).copy() + action = np.zeros_like(low) + action[0] = 0.6 # +x pose delta on the right arm + for _ in range(50): + env.step(action) + eef1 = env.sim.data.get_site_xpos(grip).copy() + assert np.all(np.isfinite(env.sim.data.ctrl)) + assert np.linalg.norm(eef1 - eef0) > 0.02, "OSC did not move the right EEF" + finally: + env.close() + + +@pytest.mark.skipif(not os.path.exists(WBC_CONFIG), reason="gear_sonic config unavailable") +def test_engine_pd_and_effort_on_assembled_model(): + """The SONIC engine (G1SonicController) drives the assembled robot directly via its + PD law (bypass path): the right arm gets its real 25/5 Nm effort and the write + stays finite/bounded.""" + import yaml + from robosuite.utils.sonic.controller import G1SonicController + with open(WBC_CONFIG) as f: + cfg = yaml.load(f, Loader=yaml.FullLoader) + model = _assemble() + model.opt.timestep = 0.002 + model.opt.integrator = int(mujoco.mjtIntegrator.mjINT_EULER) + model.opt.cone = int(mujoco.mjtCone.mjCONE_PYRAMIDAL) + model.opt.impratio = 1.0 + data = mujoco.MjData(model) + mujoco.mj_forward(model, data) + + src = ReferenceMockSource(np.zeros((1, 29)), + np.array(cfg["MOTOR_KP"][:29], float), + np.array(cfg["MOTOR_KD"][:29], float)) + sonic = G1SonicController(_Sim(model, data), src, cfg) + assert sonic.num_motors == 29 and sonic.has_hands + # right arm must get its real effort (25/5 Nm), not the left-hand 0.7 Nm + ra = [i for i, n in enumerate(sonic.motor_joint_names) if "r_shoulder" in n or "r_elbow" in n] + assert ra and all(sonic.effort_limit[i] >= 25.0 for i in ra) + + for _ in range(200): + drive_engine_direct(sonic, data) + mujoco.mj_step(model, data) + assert np.all(np.isfinite(data.ctrl)) and np.all(np.isfinite(data.qpos)) + assert np.abs(data.ctrl).max() < 200.0 + + +@pytest.mark.skipif(not os.path.exists(WBC_CONFIG), reason="gear_sonic config unavailable") +def test_sonic_wbc_robosuite_make_matches_engine(): + """SONIC_WBC drives SonicG1Fixed inside a robosuite.make env by routing the DDS + command to per-part JointPositionControllers (PD law from live state). The applied + ctrl matches the SONIC engine's own PD torque (compute_torques) on every actuator + -- legs/torso/arms AND both grippers -- confirming the controller computes the law + (no left/right swap, no gravcomp/scaling drift, hands included).""" + + cfg_path = os.path.join(os.path.dirname(os.path.abspath(robosuite.__file__)), + "controllers", "config", "robots", "default_sonic_g1.json") + cfg = load_composite_controller_config(controller=cfg_path) + assert cfg["type"] == "SONIC_WBC" + # control_freq=500 -> one sim substep (hence one PD dispatch) per env.step, so the + # applied ctrl is computed at the same state we evaluate the engine reference at. + env = robosuite.make("TwoArmLift", robots=["SonicG1Fixed"], controller_configs=cfg, + has_renderer=False, has_offscreen_renderer=False, + use_camera_obs=False, control_freq=500) + try: + env.reset() + cc = env.robots[0].composite_controller + gains = { + "body": (np.array(cc._cfg["MOTOR_KP"][:29], float), np.array(cc._cfg["MOTOR_KD"][:29], float)), + "lhand": (np.full(7, 2.0), np.full(7, 0.1)), + "rhand": (np.full(7, 2.0), np.full(7, 0.1)), + } + action = np.zeros(env.action_dim) + cc.set_command_gains(gains) + env.step(action) # build the maps + part plan + ref = None + for _ in range(10): # stop before the zero-target command collapses the fixed-base legs + # The action command is constant, so the command we evaluate here is exactly the + # command env.step dispatches. Compute the reference at the CURRENT state, before + # env.step advances it. + cmd, hands = _commands_from_action(action, gains) + ref = engine_pd_torques(cc._maps, cmd, hands) # {actuator_index: torque} + env.step(action) # routed per-part PD dispatch + idx = np.array(sorted(ref)) # all 43 SONIC actuators + applied = env.sim.data.ctrl[idx] + engine = np.array([ref[i] for i in idx]) + assert np.allclose(applied, engine, atol=1e-9), \ + f"part-controller PD != engine PD (max {np.abs(applied - engine).max():.2e})" + s = cc._maps # both grippers actually exercised (14 hand actuators routed) + assert s.has_hands and len(s._lh[2]) == 7 and len(s._rh[2]) == 7 + assert {int(c) for c in list(s._lh[2]) + list(s._rh[2])} <= set(ref) + assert np.all(np.isfinite(env.sim.data.ctrl)) + finally: + env.close() + + +# gold command streams live in the sonic_robosuite harness repo; override with $SONIC_GOLD_DIR. +GOLD_DIR = os.environ.get("SONIC_GOLD_DIR") or _first_existing( + "/home/amaddukuri/Projects/sonic_robosuite/tests/gold", + "/home/ajay/code/sonic_robosuite/tests/gold", +) + + +@pytest.mark.skipif(not os.path.exists(WBC_CONFIG), reason="gear_sonic config unavailable") +def test_sonic_g1_floating_base_in_env(): + """The free-floating SonicG1 keeps a TRUE top-level pelvis freejoint inside a real + robosuite env (NullBase, not welded by the base machinery), and the SONIC obs reads + the robot's own freejoint.""" + from robosuite.scripts.collect_sonic_g1_demos import SonicArenaEnv, SONIC_CFG, match_base_sim_physics + env = SonicArenaEnv(robots=["SonicG1"], controller_configs=load_composite_controller_config(controller=SONIC_CFG), + has_renderer=False, has_offscreen_renderer=False, use_camera_obs=False, + control_freq=500, hard_reset=False, ignore_done=True, horizon=10_000) + try: + env.reset() + match_base_sim_physics(env.sim.model._model) + m = env.sim.model._model + pid = next(b for b in range(m.nbody) if "pelvis" in (mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_BODY, b) or "")) + # pelvis is top-level (child of world) and carries a free joint -> truly floating + assert mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_BODY, m.body_parentid[pid]) == "world" + assert m.body_jntnum[pid] == 1 + assert m.jnt_type[m.body_jntadr[pid]] == int(mujoco.mjtJoint.mjJNT_FREE) + # SONIC controller resolves the ROBOT's freejoint for its base obs + cc = env.robots[0].composite_controller + env.step(np.zeros(env.action_dim)) # build maps + assert cc._maps.free_qadr == int(m.jnt_qposadr[m.body_jntadr[pid]]) + for _ in range(20): + env.step(np.zeros(env.action_dim)) + assert np.all(np.isfinite(env.sim.data.ctrl)) + finally: + env.close() + + +@pytest.mark.skipif(not (os.path.exists(WBC_CONFIG) and os.path.isdir(GOLD_DIR)), + reason="gear_sonic config / gold command stream unavailable") +def test_sonic_data_collection_replay(tmp_path): + """End-to-end: drive the native floating SonicG1 with the golden command stream and + record a robosuite demo.hdf5 (states + per-step SONIC targets).""" + import h5py + from robosuite.scripts import collect_sonic_g1_demos as C + from robosuite.wrappers import DataCollectionWrapper + + gold = dict(np.load(os.path.join(GOLD_DIR, "squat_001__A359.npz"), allow_pickle=True)) + env = None + try: + base = C.SonicArenaEnv(robots=["SonicG1"], controller_configs=load_composite_controller_config(controller=C.SONIC_CFG), + has_renderer=False, has_offscreen_renderer=False, use_camera_obs=False, + control_freq=500, hard_reset=False, ignore_done=True, horizon=10_000) + tmp = str(tmp_path / "tmp") + env = DataCollectionWrapper(base, tmp) + env.reset() # robot spawns standing via SonicG1.init_qpos + C.match_base_sim_physics(base.sim.model._model) + base.robots[0].composite_controller.set_command_gains(_gains_from_gold(gold)) + for t in range(60): + env.step(_action_from_gold(gold, t)) + env.close() + out = str(tmp_path / "out") + C.gather_to_hdf5(tmp, out, "SonicArenaEnv", {"type": "SONIC_WBC", "robot": "SonicG1"}) + with h5py.File(os.path.join(out, "demo.hdf5"), "r") as f: + demos = list(f["data"].keys()) + assert len(demos) >= 1 + s = f["data"][demos[0]]["states"][:] + a = f["data"][demos[0]]["actions"][:] + assert s.shape[0] == a.shape[0] and s.shape[0] > 0 + assert a.shape[1] == base.action_dim + assert not np.all(a == 0) # SONIC targets recorded + assert f["data"][demos[0]].attrs["model_file"] # replayable model + finally: + if env is not None: + try: + env.close() + except Exception: + pass + + +@pytest.mark.skipif(not os.path.exists(WBC_CONFIG), reason="gear_sonic config unavailable") +def test_sonic_startup_band_holds_reference_yaw(): + """A startup band referenced to the spawn pose should not yaw Sonic back to + world identity when the kitchen spawns the robot facing a fixture.""" + from scipy.spatial.transform import Rotation + + cfg_path = os.path.join(os.path.dirname(os.path.abspath(robosuite.__file__)), + "controllers", "config", "robots", "default_sonic_g1.json") + env = robosuite.make("TwoArmLift", robots=["SonicG1"], + controller_configs=load_composite_controller_config(controller=cfg_path), + has_renderer=False, has_offscreen_renderer=False, + use_camera_obs=False, control_freq=20) + try: + env.reset() + cc = env.robots[0].composite_controller + m = env.sim.model._model + md = env.sim.data._data + pelvis_jid = next( + j for j in range(m.njnt) + if m.jnt_type[j] == mujoco.mjtJoint.mjJNT_FREE + and "pelvis" in (mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_BODY, m.jnt_bodyid[j]) or "") + ) + qadr = int(m.jnt_qposadr[pelvis_jid]) + qvadr = int(m.jnt_dofadr[pelvis_jid]) + + yaw = np.pi / 2.0 + ref_rot = Rotation.from_euler("z", yaw) + ref_quat = ref_rot.as_quat() + md.qpos[qadr + 3:qadr + 7] = [ref_quat[3], ref_quat[0], ref_quat[1], ref_quat[2]] + md.qvel[qvadr:qvadr + 6] = 0.0 + env.sim.forward() + env.step(np.zeros(env.action_dim)) # builds the engine, pelvis id, and reference-yaw band + pelvis_bid = cc._pelvis_bid + md.qpos[qadr + 3:qadr + 7] = [ref_quat[3], ref_quat[0], ref_quat[1], ref_quat[2]] + md.qvel[qvadr:qvadr + 6] = 0.0 + env.sim.forward() + cc._apply_band(md) + assert abs(md.xfrc_applied[pelvis_bid, 5]) < 1e-6 + + rolled = (ref_rot * Rotation.from_euler("x", 0.1)).as_quat() + md.qpos[qadr + 3:qadr + 7] = [rolled[3], rolled[0], rolled[1], rolled[2]] + md.qvel[qvadr:qvadr + 6] = 0.0 + env.sim.forward() + cc._apply_band(md) + assert np.linalg.norm(md.xfrc_applied[pelvis_bid, 3:5]) > 10.0 + assert abs(md.xfrc_applied[pelvis_bid, 5]) < 1e-6 + finally: + env.close() + + +@pytest.mark.skipif(not os.path.exists(WBC_CONFIG), reason="gear_sonic config unavailable") +def test_legacy_elastic_band_torques_yaw_to_identity(): + """The upstream gear_sonic band still yaws nonzero headings toward identity. + SONIC_WBC handles the RoboCasa spawn-yaw reference locally.""" + from gear_sonic.utils.mujoco_sim.unitree_sdk2py_bridge import ElasticBand + + yaw = np.pi / 2.0 + quat_wxyz = np.array([np.cos(yaw / 2.0), 0.0, 0.0, np.sin(yaw / 2.0)]) + pose = np.zeros(13) + pose[0:3] = np.array([0.0, 0.0, 1.0]) + pose[3:7] = quat_wxyz + + legacy = ElasticBand() + legacy.point = pose[0:3].copy() + legacy_wrench = legacy.Advance(pose) + + assert abs(legacy_wrench[5]) > 100.0 + + +@pytest.mark.skipif(not os.path.exists(WBC_CONFIG), reason="gear_sonic config unavailable") +def test_sonic_startup_band_is_object_safe(): + """The live-DDS startup hold is an elastic band -- a force on the PELVIS body only, + NEVER a qpos write. Regression: the old freeze/fall-recovery pinned the WHOLE qpos + vector, which would teleport any free task object (e.g. TwoArmLift's pot) back to a + snapshot each step. Here we perturb the pot, step, and assert (a) the pot evolves + freely (not snapped back), (b) the band applies a force to the pelvis, and (c) + releasing the band zeros that force.""" + + cfg_path = os.path.join(os.path.dirname(os.path.abspath(robosuite.__file__)), + "controllers", "config", "robots", "default_sonic_g1.json") + # floating SonicG1 in an env WITH a free task object (the pot) + env = robosuite.make("TwoArmLift", robots=["SonicG1"], + controller_configs=load_composite_controller_config(controller=cfg_path), + has_renderer=False, has_offscreen_renderer=False, + use_camera_obs=False, control_freq=20) + try: + env.reset() + cc = env.robots[0].composite_controller + m = env.sim.model._model + md = env.sim.data._data + env.step(np.zeros(env.action_dim)) # builds the engine + band; sets _pelvis_bid + assert cc._pelvis_bid is not None, "floating base not detected" + + # locate a NON-robot freejoint (the pot) by its body name + pelvis_bid = cc._pelvis_bid + pot_qadr = next( + int(m.jnt_qposadr[j]) for j in range(m.njnt) + if m.jnt_type[j] == mujoco.mjtJoint.mjJNT_FREE + and "pelvis" not in (mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_BODY, m.jnt_bodyid[j]) or "")) + + # (a) perturb the pot, step, and confirm it was NOT snapped back to a snapshot + before = np.array(md.qpos[pot_qadr:pot_qadr + 3]) + md.qpos[pot_qadr] += 0.15 + env.sim.forward() + moved_to = float(md.qpos[pot_qadr]) + env.step(np.zeros(env.action_dim)) + after = float(md.qpos[pot_qadr]) + assert abs(after - before[0]) > 0.1, "pot was teleported back -- qpos clobbered!" + assert abs(after - moved_to) < 0.05, "pot did not evolve from its perturbed pose" + + # (b) the band applies a force to the pelvis while enabled + assert np.any(md.xfrc_applied[pelvis_bid] != 0.0), "band applied no force" + + # (c) releasing the band zeros that force + cc.release_band() + env.step(np.zeros(env.action_dim)) + assert np.all(md.xfrc_applied[pelvis_bid] == 0.0), "band force not cleared on release" + finally: + env.close() diff --git a/tests/test_robots/test_sonic_g1_live.py b/tests/test_robots/test_sonic_g1_live.py new file mode 100644 index 0000000000..fdb7355229 --- /dev/null +++ b/tests/test_robots/test_sonic_g1_live.py @@ -0,0 +1,176 @@ +"""Opt-in LIVE-DDS faithfulness test for the native SonicG1 path. + +Launches the real C++ SONIC controller (gear_sonic_deploy), drives the registered +robosuite SonicG1 + SONIC_WBC over Unitree DDS, plays each motion, and asserts the native +backend's joint tracking is as good as base_sim's: the nearest-reference-frame joint error +must be within TOL of the recorded gold mean for that motion. (Closeness to gold, not just +"it ran" -- so a motion the policy itself tracks poorly still passes iff native is no worse +than base_sim, and a real integration regression fails.) + +Slow (~50 s/motion), launches a subprocess + opens DDS, and reuses the gold metric from the +sonic_robosuite harness, so it is gated behind an opt-in env var on top of the C++-binary / +config / gold checks. Set SONIC_LIVE_VIDEO_DIR to also render each run to an mp4 (needs EGL). + + RUN_SONIC_LIVE=1 MUJOCO_GL=egl SONIC_LIVE_VIDEO_DIR=/tmp/sonic_vids \ + pytest tests/test_robots/test_sonic_g1_live.py -q + +Overrides: SONIC_DEPLOY, SONIC_ROBOSUITE_DIR, SONIC_LIVE_VIDEO_DIR. +""" +import os +import subprocess +import sys +import tempfile +import threading +import time + +import numpy as np +import mujoco +import pytest + +import robosuite # noqa: F401 registers SonicG1 / Dex3 / SONIC_WBC +from robosuite.controllers import load_composite_controller_config +from robosuite.controllers.composite.sonic_whole_body_controller import WBC_CONFIG +from robosuite.scripts.collect_sonic_g1_demos import SonicArenaEnv, SONIC_CFG, match_base_sim_physics +from robosuite.utils.sonic.action_sources import DDSActionSource + +DEPLOY = os.environ.get("SONIC_DEPLOY", "/home/ajay/code/GR00T-WholeBodyControl/gear_sonic_deploy") +CPP_BIN = os.path.join(DEPLOY, "target", "release", "g1_deploy_onnx_ref") +REF_ROOT = os.path.join(DEPLOY, "reference", "example") +VIDEO_DIR = os.environ.get("SONIC_LIVE_VIDEO_DIR") +TOL = 0.04 # rad: native nearest-ref joint error may exceed gold by at most this (run-to-run noise) + +# The nearest-ref-frame metric + recorded gold streams live in the sonic_robosuite harness. +sys.path.insert(0, os.path.join(os.environ.get("SONIC_ROBOSUITE_DIR", "/home/ajay/code/sonic_robosuite"), "tests")) +try: + from golden_common import tracking_metrics, GOLD_DIR # noqa: E402 + _HAVE_GOLD = True +except Exception: + _HAVE_GOLD = False + GOLD_DIR = "" + +_LIVE_OK = (os.environ.get("RUN_SONIC_LIVE") == "1" + and os.path.exists(CPP_BIN) and os.path.exists(WBC_CONFIG) and _HAVE_GOLD) + +MOTIONS = ["squat_001__A359", "walking_quip_360_R_002__A428", + "neutral_kick_R_001__A543", "macarena_001__A545"] + + +def _launch_cpp(motion): + """Start the C++ SONIC controller for one motion (keyboard input over stdin).""" + os.system("kill -9 $(pgrep -x g1_deploy_onnx_ref) 2>/dev/null") # DDS hygiene: one at a time + time.sleep(1) + motion_dir = tempfile.mkdtemp(prefix="sonic_live_") + os.symlink(os.path.join(REF_ROOT, motion), os.path.join(motion_dir, motion)) + cmd = ["./target/release/g1_deploy_onnx_ref", "lo", "policy/release/model_decoder.onnx", + motion_dir, "--obs-config", "policy/release/observation_config.yaml", + "--encoder-file", "policy/release/model_encoder.onnx", + "--input-type", "keyboard", "--output-type", "zmq", "--disable-crc-check"] + return subprocess.Popen(cmd, cwd=DEPLOY, stdin=subprocess.PIPE, + stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL) + + +def _render(model, mj_data, qpos_seq, out_mp4, sim_dt, fps=30): + """Offscreen-render a recorded qpos sequence to mp4 (pelvis-tracking cam; needs EGL).""" + import imageio + os.environ.setdefault("MUJOCO_GL", "egl") + pelvis = next(b for b in range(model.nbody) + if "pelvis" in (mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, b) or "")) + renderer = mujoco.Renderer(model, 480, 640) + cam = mujoco.MjvCamera() + cam.type = mujoco.mjtCamera.mjCAMERA_TRACKING + cam.trackbodyid = pelvis + cam.distance, cam.azimuth, cam.elevation = 3.5, 120, -15 + stride = max(1, int(round((1.0 / fps) / sim_dt))) + writer = imageio.get_writer(out_mp4, fps=fps) + try: + for t in range(0, len(qpos_seq), stride): + mj_data.qpos[:] = qpos_seq[t] + mujoco.mj_forward(model, mj_data) + renderer.update_scene(mj_data, camera=cam) + writer.append_data(renderer.render()) + finally: + writer.close() + renderer.close() + + +@pytest.mark.skipif(not _LIVE_OK, + reason="opt-in: RUN_SONIC_LIVE=1 + C++ binary + gear_sonic config + gold") +@pytest.mark.parametrize("motion", MOTIONS) +def test_sonic_live_dds_tracking(motion): + if not os.path.exists(os.path.join(REF_ROOT, motion)): + pytest.skip(f"reference motion {motion} not under {REF_ROOT}") + + cfg = load_composite_controller_config(controller=SONIC_CFG) + env = SonicArenaEnv(robots=["SonicG1"], controller_configs=cfg, has_renderer=False, + has_offscreen_renderer=False, use_camera_obs=False, control_freq=500, + hard_reset=False, ignore_done=True, horizon=10_000_000) + env.reset() + match_base_sim_physics(env.sim.model._model) + controller = env.robots[0].composite_controller + mj_data, model = env.sim.data._data, env.sim.model._model + source = DDSActionSource(controller._cfg) + source.reset(env) + hold_action = np.zeros(env.action_dim) + sim_dt = float(model.opt.timestep) + + cpp = _launch_cpp(motion) + state = {"phase": "load", "rec": False} + + def drive(): + time.sleep(20); cpp.stdin.write(b"]"); cpp.stdin.flush() # control active + time.sleep(2); controller.release_band() # drop the startup band + time.sleep(8); cpp.stdin.write(b"T"); cpp.stdin.flush() # play the motion + state["rec"] = True + time.sleep(16); state["phase"] = "done" + threading.Thread(target=drive, daemon=True).start() + + body_q, fb_pose, qpos_seq = [], [], [] + nxt = time.perf_counter() + try: + while state["phase"] != "done": + action = source.act(env) + if action is None: + env.step(hold_action) + else: + if source.gains: + controller.set_command_gains(source.gains) + env.step(action) + if state["rec"]: + engine = source._engine + assert engine is not None, "DDS source did not initialize its SONIC map engine" + body_q.append(mj_data.qpos[engine.qpos_adr].copy()) # 29, MOTOR order + fb_pose.append(mj_data.qpos[engine.free_qadr:engine.free_qadr + 7].copy()) + if VIDEO_DIR: + qpos_seq.append(np.array(mj_data.qpos)) + nxt += sim_dt + slp = nxt - time.perf_counter() + if slp > 0: + time.sleep(slp) + finally: + try: + cpp.terminate(); cpp.wait(timeout=5) + except Exception: + os.system("kill -9 $(pgrep -x g1_deploy_onnx_ref) 2>/dev/null") + + body_q, fb_pose = np.asarray(body_q), np.asarray(fb_pose) + assert len(body_q) > 100 and np.all(np.isfinite(body_q)), "no / non-finite live data" + achieved_hz = len(body_q) / 16.0 # the record window is 16 s wall-clock + + if VIDEO_DIR: + os.makedirs(VIDEO_DIR, exist_ok=True) + _render(model, mj_data, np.asarray(qpos_seq), os.path.join(VIDEO_DIR, f"{motion}.mp4"), sim_dt) + + native = tracking_metrics(body_q, fb_pose, motion) + gold_npz = dict(np.load(os.path.join(GOLD_DIR, motion + ".npz"), allow_pickle=True)) + gold = tracking_metrics(gold_npz["body_q"], gold_npz["floating_base_pose"], motion) + print(f"[live] {motion}: native mean={native['mean']:.4f} drift={native['drift']:.2f}m vs " + f"gold mean={gold['mean']:.4f} drift={gold['drift']:.2f}m | {achieved_hz:.0f} Hz (tol={TOL})", flush=True) + env.close() + # The C++ is wall-clock paced: a sub-real-time sim over-advances its reference and tracking + # degrades, so don't false-fail -- skip. The native live loop's healthy steady state is + # ~350-415 Hz (high run-to-run variance) and stays gold-faithful there; below ~350 Hz the box + # is loaded / the motion has fallen, so faithfulness is undefined. (See design doc 13.5.) + if achieved_hz < 350: + pytest.skip(f"{motion}: sim ran sub-real-time ({achieved_hz:.0f} Hz); faithfulness undefined") + assert native["mean"] <= gold["mean"] + TOL, ( + f"{motion}: native joint error {native['mean']:.4f} exceeds gold {gold['mean']:.4f} + {TOL}")