diff --git a/CLAUDE.md b/CLAUDE.md index fd928cb7..9280fb2e 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -253,19 +253,19 @@ including `h12_control.launch.py`'s `sim`, which has no default. | Top-level launch file | Scenario | Includes | |---|---|---| -| `h12_sim.launch.py` | **sim** (`use_sim_time=true`), plus the camera optical-frame static TF and rviz (`use_rviz`) | state, control (`sim:=true`, `sim_safety_split` for both `safety_node` and `frame_task_server`, no stagger), lowerbody (run config `sim`), models (`use_skills`), navigation (`use_nav`) | -| `h12_real_robot.launch.py` | **real robot, onboard PC** (native, `ROS_DOMAIN_ID=0`) | real_drivers, navigation, state, control (`sim:=false`, `use_estop:=true`, the defaults `relax_safety_split` and `safety_split`), lowerbody (run config `real`). Leg control is interlocked behind `start_position_verified:=true` (default **false**) | -| `h12_real_desktop.launch.py` | **real: companion x86 desktop** — rviz, plus the hand cameras with `use_hand_cameras:=true` (default **false**) | models (`use_yolo:=true`) | +| `h12_sim.launch.py` | **sim** (`use_sim_time=true`), plus the camera optical-frame static TF and rviz (`use_rviz`). Runs the real robot's operator gates: the estop (the sim serves the box) and the legs' `/lowerbody/confirm_engage` | state, control (`sim:=true`, `estop_node` on the sim's estop box, the real defaults `relax_safety_split` and `safety_split` and the real stagger), lowerbody (run config `sim`: the real start-up and faults, plus the band release), vision models (`use_skills`), navigation (always) | +| `h12_real_robot.launch.py` | **real robot, onboard PC** (native, `ROS_DOMAIN_ID=0`) | real_drivers, navigation, state, control (`sim:=false`, the USB estop box, the defaults `relax_safety_split` and `safety_split`), lowerbody (run config `real`). Leg control is interlocked behind `start_position_verified:=true` (default **false**) | +| `h12_real_desktop.launch.py` | **real: companion x86 desktop** — rviz, plus the hand cameras with `use_hand_cameras:=true` (default **false**) | vision models (`use_yolo:=true`) | | `h12_safety_grippers.launch.py` | real: actuation-only debug bringup | control (`sim:=false`, estop + safety, no IK), grippers | -| `lmg_sim.launch.py` | **sim**, loco-manipulation game: the `lmg_deploy` MARL policy (its one H1-2 policy, `policies/h12/`) owns all 27 joints, no operator confirm. Refuses an unset or 0 `ROS_DOMAIN_ID` | state, control (`sim:=true`, no IK, no stagger), locomanipulationgame, navigation (`use_slam`), `golem_skills` (`use_skills`, default false: `/skill/*` without the service waits, on the H1-2's Nav2 footprint) | -| `lmg_real.launch.py` | **real, onboard PC**, loco-manipulation game, run *instead of* `h12_real_robot`. **Refuses to start, before any node**, until `lmg_deploy`'s handling of the MID-360's raw (upside-down) `/livox/imu`, checked in the sim, is checked on the robot. Once lifted, the policy is behind `start_position_verified:=true` + `/marl_deploy_node/confirm_engage` | real_drivers, state, control (`sim:=false`, `use_estop:=true`, no IK), locomanipulationgame, navigation (`use_slam`) | +| `lmg_sim.launch.py` | **sim**, loco-manipulation game: the `lmg_deploy` MARL policy (its one H1-2 policy, `policies/h12/`) owns all 27 joints, no operator confirm. Refuses an unset or 0 `ROS_DOMAIN_ID` | state, control (`sim:=true`, no IK, the sim's estop box, the real `safety_delay`), locomanipulationgame, navigation (`use_slam`), `golem_skills` (`use_skills`, default false: `/skill/*` without the service waits, on the H1-2's Nav2 footprint) | +| `lmg_real.launch.py` | **real, onboard PC**, loco-manipulation game, run *instead of* `h12_real_robot`. **Refuses to start, before any node**, until `lmg_deploy`'s handling of the MID-360's raw (upside-down) `/livox/imu`, checked in the sim, is checked on the robot. Once lifted, the policy is behind `start_position_verified:=true` + `/marl_deploy_node/confirm_engage` | real_drivers, state, control (`sim:=false`, no IK), locomanipulationgame, navigation (`use_slam`) | | Building block | Starts | |---|---| | `h12_state.launch.py` | `golem_upper_ik`'s `robot_state.launch.py` (`robot: h12`, `urdf_file` passed on): `joint_state_publisher` (the 27 body joints from `/lowstate`) + `robot_state_publisher` | -| `h12_control.launch.py` | [`estop_node`] → `golem_safety`'s `safety_node` (its `safety.launch.py`: `robot: h12`, `safety_config`, `sim`, `use_estop`) → `golem_upper_ik`'s `frame_task_server` (its `upper_ik.launch.py`: `robot: h12`, `frame_task_config`; `use_frame_task`), staggered by `safety_delay` (3 s) / `frame_task_delay` (5 s), passed on as each file's `start_delay`. `sim` has no default: a launch without it fails before any node starts. With `use_frame_task` it includes `upper_ik.launch.py` first, which refuses a `frame_task_config` that does not run with `safety_config` (`golem_upper_ik.pairing`) before anything starts. Only the pairs in `golem_upper_ik/config/h12/pairing.yaml` start, by name (a path only if its file name is a paired name), and sim and real configs do not mix | +| `h12_control.launch.py` | `estop_node` (on `estop_serial_port`: empty finds the real USB box, the sims pass `socket://127.0.0.1:<15700 + ROS_DOMAIN_ID>`) → `golem_safety`'s `safety_node` (its `safety.launch.py`: `robot: h12`, `safety_config`, `sim`, always `use_estop: true`) → `golem_upper_ik`'s `frame_task_server` (its `upper_ik.launch.py`: `robot: h12`, `frame_task_config`; `use_frame_task`), staggered by `safety_delay` (3 s) / `frame_task_delay` (5 s), passed on as each file's `start_delay`. `sim` has no default: a launch without it fails before any node starts. With `use_frame_task` it includes `upper_ik.launch.py` first, which refuses a `frame_task_config` that does not run with `safety_config` (`golem_upper_ik.pairing`) before anything starts. Only the pairs in `golem_upper_ik/config/h12/pairing.yaml` start, by name (a path only if its file name is a paired name); the H1-2 pairs no sim config, so its sims run the real pairs | | `h12_lowerbody.launch.py` | `golem_lowerbody_rl`'s `lowerbody.launch.py` (`robot: h12`, `policy:=`, run config `lowerbody_config`, `sim`, every argument passed), behind `start_position_verified`; a policy the H1-2 does not list is refused. `h12_sim` and `h12_real_robot` include it last and put its check (`golem_bringup.h12_legs.leg_check`, with the arguments they pass it) first, so a refused policy or run config starts no node | -| `h12_models.launch.py` | gemini + sam servers, [yolo], graspgen + `golem_skills` (`use_skills`) | +| `golem_vision_models.launch.py` | gemini + sam servers, [yolo], graspgen + `golem_skills` (`use_skills`) | | `h12_real_drivers.launch.py` | Livox MID360 (no extrinsic rotation: the points and `/livox/imu` are both in the sensor's raw, upside-down frame, the URDF's `livox_link` and `livox_imu_link`), RealSense head camera, `h12_grippers.launch.py` (left+right `gripper_node`) | | `h12_locomanipulationgame.launch.py` | `marl_deploy_node` (behind `start_position_verified`; holds arm goals in the world by the SLAM `/Odometry`) | | `golem_slam/launch/navigation.launch.py` | the shared nav stack, included with `robot: h12` (FAST-LIO → pointcloud_to_laserscan → slam_toolbox → nav2; `golem_slam`'s `config/h12/`) | @@ -285,7 +285,7 @@ for the includes after it, so pass consistent values (all four blocks share - `safety_node` streams zero-gain `/lowcmd` from boot (in the sim, from the first `/clock`). It engages each input on that input's first valid command once `/lowstate` reports `mode_machine` 6, - and, with `use_estop:=true`, once a first hardware estop sample has arrived; + and once a first estop sample has arrived; until then it logs `engage_refused` with the reason, at most once a second. - **`/safety/heartbeat` means relaying can begin.** `safety_node` publishes no beat until those two conditions hold; then it beats true at 10 Hz, and false @@ -301,21 +301,25 @@ for the includes after it, so pass consistent values (all four blocks share up to `controller.startup_timeout` (30 s). If `golem_safety` has not relayed them by then, it sends one zero-gain command and exits 1 with an error that lists the causes: no `/lowstate` reporting `mode_machine` 6 (an H1-2 sim - started before the sims reported 6), no first estop sample with - `use_estop:=true`, a safety config that does not pair with its controller + started before the sims reported 6), no first estop sample, a safety config that does not pair with its controller config, or an estop latched at this start-up (the limit checks run from boot, in the sim from the first `/clock`, so a joint already past `limits.estop` latches it) or in an earlier run. The other nodes keep running, without `/frame_task` and `/named_config`. -- **`use_estop`.** `use_estop:=true` (the real bringups and - `h12_safety_grippers`) starts `estop_node` and has `safety_node` read - `/estop/triggered` and `/estop/plugged_in`: no input engages before the first - sample, a sample that says triggered or unplugged latches the estop until - restart, and so does no new sample for `estop.timeout_seconds` (0.5 s) once - the first has arrived. With `use_estop:=false` (the sim bringups) those topics - are ignored. -- **The real robot's estop start-up.** `estop_node` publishes nothing until its +- **The estop, in the sim as on the real robot.** Every H1-2 bringup starts + `estop_node` and has `safety_node` read `/estop/triggered` and + `/estop/plugged_in` (`h12_control.launch.py` has no `use_estop` argument): no + input engages before the first sample, a sample that says triggered or + unplugged latches the estop until restart, and so does no new sample for + `estop.timeout_seconds` (0.5 s) once the first has arrived. The real box is an + Arduino on USB serial; the H1-2's RoboCasa and Isaac sims serve the same lines + (`ESTOP,BOOT`, then `ESTOP,0|1` at 50 Hz) on `127.0.0.1:<15700 + + ROS_DOMAIN_ID>` (`golem_robocasa/estop_server.py`, `CL_isaaclab_sim`'s copy), + which `estop_node` opens as a pyserial `socket://` URL. ENTER on the sim's + terminal (or the RoboCasa viewer) toggles the button; a stopped sim reads as + unplugged. The G1 has no estop node (`use_estop: false` in `g1_core`). +- **The estop start-up.** `estop_node` publishes nothing until its first attempt to connect ends; that attempt probes every serial port it finds (up to about 3.25 s for each port that opens, so another serial device such as a gripper board lengthens it). When an attempt fails, it publishes unplugged @@ -329,7 +333,8 @@ for the includes after it, so pass consistent values (all four blocks share cover the time `estop_node` takes to connect (the comment in `h12_control.launch.py`). - **The leg node** (`golem_lowerbody_rl`'s `lowerbody_controller_node`, its - README; the values are the run config's, `config/h12/{sim,real}.yaml`) is + README; the values are the run config's, `config/h12/{sim,real}.yaml`, which + differ only in the sim's band release and IMU trim) is silent until the first true heartbeat and an upright, finite `/lowstate`. It then ramps the legs over 2 s to the policy's crouch and holds there. The policy may start once `/lowstate` is steady over the last 1 s window (1 s @@ -338,12 +343,12 @@ for the includes after it, so pass consistent values (all four blocks share which counts publishes, never qualifies; the windows follow one another, so a recent stall or a slowed tick makes it not ready again within 1 s), the arm controller's commands are fresh within 0.5 s and, in the sim, `frame_task_server`'s first `/left_ee_pose` has come (or - `band.max_wait`, 34 s); the node logs why it waits. On the real robot it - then holds until `ros2 service call /lowerbody/confirm_engage + `band.max_wait`, 34 s); the node logs why it waits. It then holds, in the + sim as on the real robot, until `ros2 service call /lowerbody/confirm_engage std_srvs/srv/Trigger` (refused at any other time), and runs the policy. A `/cmd_vel` older than 0.5 s is zero velocity; `/lowerbody/height` (`std_msgs/Float32`) is the policy's height command. Once the legs are - commanded, a `/lowstate` gap over `max_state_gap` (0.25 s real, 0.5 s sim; + commanded, a `/lowstate` gap over `max_state_gap` (0.25 s, in the sim too; `/lowstate` has its own executor thread), a tick that goes back, a non-finite value or non-unit IMU quaternion, a fall, a policy step that fails or gives non-finite targets, and in every phase a false heartbeat or @@ -860,7 +865,10 @@ docker/scripts/docker_run.sh robocasa # or: isaac; add --headless if # Terminal 2 — ROS workspace (auto colcon-builds core_ws, drops to a shell) docker/scripts/docker_run.sh ros ros2 launch golem_bringup h12_sim.launch.py # manual step — the container does NOT launch it -# variants: use_skills:=false use_rviz:=false use_nav:=false +# variants: use_skills:=false use_rviz:=false +# As on the real robot, the legs hold their pre-pose until the operator's confirm (from +# terminal 3), and ENTER in terminal 1 presses the sim's estop (restart the bringup after): +# ros2 service call /lowerbody/confirm_engage std_srvs/srv/Trigger # Terminal 3 — drive skills: attach to the running ROS container docker exec -it golem_ros /bin/bash diff --git a/CL_isaaclab_sim b/CL_isaaclab_sim index 73959984..a4cc11bd 160000 --- a/CL_isaaclab_sim +++ b/CL_isaaclab_sim @@ -1 +1 @@ -Subproject commit 739599845d16af6cbf2be3744c4dfb3a33bb1e65 +Subproject commit a4cc11bd99203b3ba38335ccfc19ab3f8ca8d627 diff --git a/core_ws/src/estop b/core_ws/src/estop index c30fb50a..9021f650 160000 --- a/core_ws/src/estop +++ b/core_ws/src/estop @@ -1 +1 @@ -Subproject commit c30fb50afdd81cb974b80aaa78dbf9fc1c352898 +Subproject commit 9021f650408dcdaff60d1c8df0e8d22ab690b84a diff --git a/core_ws/src/golem_bringup/golem_bringup/sim_estop.py b/core_ws/src/golem_bringup/golem_bringup/sim_estop.py new file mode 100644 index 00000000..b19df255 --- /dev/null +++ b/core_ws/src/golem_bringup/golem_bringup/sim_estop.py @@ -0,0 +1,13 @@ +"""Name the estop box the H1-2 sims serve, for estop_node's serial_port.""" +import os + +# The sims' estop servers listen on BASE_PORT + ROS_DOMAIN_ID; keep in step with +# golem_robocasa/estop_server.py and CL_isaaclab_sim's estop_server.py. +BASE_PORT = 15700 + + +def sim_estop_url(environ=None) -> str: + """Return socket://127.0.0.1: of the sim on this shell's ROS_DOMAIN_ID (1 when unset).""" + env = os.environ if environ is None else environ + domain = env.get('ROS_DOMAIN_ID', '').strip() or '1' + return f'socket://127.0.0.1:{BASE_PORT + int(domain)}' diff --git a/core_ws/src/golem_bringup/launch/h12/h12_models.launch.py b/core_ws/src/golem_bringup/launch/h12/golem_vision_models.launch.py similarity index 100% rename from core_ws/src/golem_bringup/launch/h12/h12_models.launch.py rename to core_ws/src/golem_bringup/launch/h12/golem_vision_models.launch.py diff --git a/core_ws/src/golem_bringup/launch/h12/h12_control.launch.py b/core_ws/src/golem_bringup/launch/h12/h12_control.launch.py index 4104888f..b6b94a08 100644 --- a/core_ws/src/golem_bringup/launch/h12/h12_control.launch.py +++ b/core_ws/src/golem_bringup/launch/h12/h12_control.launch.py @@ -1,11 +1,11 @@ """ -Start the H1-2's command path: [estop_node], safety_node, frame_task_server. +Start the H1-2's command path: estop_node, safety_node, frame_task_server. golem_safety's safety_node is the only writer of /lowcmd; golem_upper_ik's frame_task_server (use_frame_task) commands the arms and torso through it. Each comes from its own package's launch file (safety.launch.py, upper_ik.launch.py). The order, -on the wall clock: estop_node (use_estop) at once, safety_node after safety_delay, -frame_task_server after frame_task_delay. +on the wall clock: estop_node at once, safety_node after safety_delay, +frame_task_server after frame_task_delay, in the sim as on the real robot. upper_ik.launch.py is included first: it refuses a frame_task_config that does not run with safety_config (golem_upper_ik.pairing) before this file starts any node. Without @@ -16,17 +16,21 @@ Domain 0 is the real robot's command bus, and safety_node refuses sim:=true on it, so a sim bringup needs a sim domain exported. -SAFETY: with use_estop, safety_node engages no input before estop_node's first sample, -and latches until restart on a sample that says triggered or unplugged. estop_node -publishes nothing until its first attempt to open and probe the serial port ends; when -that attempt fails it publishes unplugged and triggered +estop_node reads the hardware estop box on estop_serial_port: the real box on USB +serial, or the box the H1-2 sims serve on socket://127.0.0.1: (the estop package's +README), so the sim runs the real robot's estop path. + +SAFETY: safety_node always reads the estop (use_estop true) and engages no input before +estop_node's first sample, and latches until restart on a sample that says triggered or +unplugged. estop_node publishes nothing until its first attempt to open and probe the +serial port ends; when that attempt fails it publishes unplugged and triggered (fail_safe_triggered_on_disconnect) until a retry connects. A safety_node that starts -while estop_node is retrying latches, so on the real robot safety_delay must cover the -time estop_node takes to connect. A first attempt that outlasts safety_delay and then -connects only delays the start: safety_node engages nothing and publishes no -/safety/heartbeat, which the leg controller waits for before it pre-poses the legs, -until that first sample. frame_task_server waits up to its controller.startup_timeout -for safety_node to relay its commands. +while estop_node is retrying latches, so safety_delay must cover the time estop_node takes +to connect. A first attempt that outlasts safety_delay and then connects only delays the +start: safety_node engages nothing and publishes no /safety/heartbeat, which the leg +controller waits for before it pre-poses the legs, until that first sample. +frame_task_server waits up to its controller.startup_timeout for safety_node to relay its +commands. """ from golem_bringup.include import include @@ -51,9 +55,9 @@ def generate_launch_description(): description='Required. true for a simulator (safety_node refuses it on ROS ' 'domain 0), false for the real robot'), DeclareLaunchArgument( - 'use_estop', default_value='false', - description='true: start the serial e-stop bridge and have safety_node ' - 'require and obey it (real robot only)'), + 'estop_serial_port', default_value='', + description="estop_node's serial_port: empty to find the real box on USB " + 'serial, or the sim box\'s socket://127.0.0.1:'), DeclareLaunchArgument( 'use_frame_task', default_value='true', description='true: start frame_task_server, the arm IK action server ' @@ -70,7 +74,7 @@ def generate_launch_description(): description='golem_upper_ik config for frame_task_server. With use_frame_task ' 'it must be the one pairing.yaml pairs with safety_config, by name ' '(a path only if its file name is that one)'), - # SAFETY: on the real robot safety_delay must cover the time estop_node + # SAFETY: safety_delay must cover the time estop_node # takes to connect (the module docstring), or safety_node latches its estop. DeclareLaunchArgument( 'safety_delay', default_value='3.0', @@ -94,22 +98,26 @@ def generate_launch_description(): 'use_sim_time': use_sim_time, 'start_delay': LaunchConfiguration('frame_task_delay'), }, condition=IfCondition(LaunchConfiguration('use_frame_task'))), - # The hardware e-stop box on serial, published for safety_node. + # The hardware e-stop box, published for safety_node. Node( package='estop', executable='estop_node', name='estop_node', - parameters=[{'use_sim_time': ParameterValue(use_sim_time, value_type=bool)}], + parameters=[{ + 'use_sim_time': ParameterValue(use_sim_time, value_type=bool), + 'serial_port': ParameterValue(LaunchConfiguration('estop_serial_port'), + value_type=str), + }], output='screen', emulate_tty=True, - condition=IfCondition(LaunchConfiguration('use_estop')), ), # The only writer of /lowcmd. include('golem_safety', 'safety.launch.py', { 'robot': ROBOT, 'config': safety_config, 'sim': sim, - 'use_estop': LaunchConfiguration('use_estop'), + # SAFETY: the H1-2 always runs with its estop. + 'use_estop': 'true', 'use_sim_time': use_sim_time, 'start_delay': LaunchConfiguration('safety_delay'), }), diff --git a/core_ws/src/golem_bringup/launch/h12/h12_real_desktop.launch.py b/core_ws/src/golem_bringup/launch/h12/h12_real_desktop.launch.py index e4c2f380..c3ba77d8 100644 --- a/core_ws/src/golem_bringup/launch/h12/h12_real_desktop.launch.py +++ b/core_ws/src/golem_bringup/launch/h12/h12_real_desktop.launch.py @@ -54,7 +54,7 @@ def generate_launch_description(): condition=IfCondition(LaunchConfiguration('use_hand_cameras'))), # Perception servers, YOLO and the LLM-callable skills. - include('golem_bringup', 'h12/h12_models.launch.py', { + include('golem_bringup', 'h12/golem_vision_models.launch.py', { **wall_time, 'use_yolo': 'true', 'use_skills': LaunchConfiguration('use_skills'), diff --git a/core_ws/src/golem_bringup/launch/h12/h12_real_robot.launch.py b/core_ws/src/golem_bringup/launch/h12/h12_real_robot.launch.py index 0e83544e..820fbd49 100644 --- a/core_ws/src/golem_bringup/launch/h12/h12_real_robot.launch.py +++ b/core_ws/src/golem_bringup/launch/h12/h12_real_robot.launch.py @@ -69,7 +69,6 @@ def generate_launch_description(): include('golem_bringup', 'h12/h12_control.launch.py', { **wall_time, 'sim': 'false', - 'use_estop': 'true', }), # The legs. include('golem_bringup', 'h12/h12_lowerbody.launch.py', legs), diff --git a/core_ws/src/golem_bringup/launch/h12/h12_safety_grippers.launch.py b/core_ws/src/golem_bringup/launch/h12/h12_safety_grippers.launch.py index 52b0c414..253a4c96 100644 --- a/core_ws/src/golem_bringup/launch/h12/h12_safety_grippers.launch.py +++ b/core_ws/src/golem_bringup/launch/h12/h12_safety_grippers.launch.py @@ -34,7 +34,6 @@ def generate_launch_description(): { 'use_sim_time': 'false', 'sim': 'false', - 'use_estop': 'true', 'use_frame_task': 'false', 'safety_config': LaunchConfiguration('safety_config'), }, diff --git a/core_ws/src/golem_bringup/launch/h12/h12_sim.launch.py b/core_ws/src/golem_bringup/launch/h12/h12_sim.launch.py index ec1dabee..a09e7175 100644 --- a/core_ws/src/golem_bringup/launch/h12/h12_sim.launch.py +++ b/core_ws/src/golem_bringup/launch/h12/h12_sim.launch.py @@ -6,11 +6,16 @@ Start the simulator first: every node runs on its /clock. The leg node's check (golem_bringup.h12_legs.leg_check) runs first, so a refused policy starts no node; then everything starts at once: the state publishers (h12_state), the command path -(h12_control: the safety/controller pairing check before any node, then safety_node and -frame_task_server, with no estop and no stagger), the leg controller (h12_lowerbody, with -the start pose taken as verified), the model servers and golem_skills (h12_models), -navigation (use_nav), the static camera_link -> camera_color_optical_frame TF and RViz -(use_rviz). +(h12_control as on the real robot: the safety/controller pairing check before any node, +then estop_node on the sim's estop box, safety_node and frame_task_server with the real +robot's configs and delays), the leg controller (h12_lowerbody, with the start pose taken +as verified), the model servers and golem_skills (golem_vision_models), navigation, the +static camera_link -> camera_color_optical_frame TF and RViz (use_rviz). + +SAFETY: the sim runs the real robot's operator gates: the estop box the sim serves (ENTER +on the sim's terminal or viewer presses it; golem_safety then latches until restart), and +the leg policy holds its pre-pose until + ros2 service call /lowerbody/confirm_engage std_srvs/srv/Trigger Every node joins the launching shell's ROS_DOMAIN_ID; safety_node refuses sim:=true on domain 0, the real robot's command bus, so export a sim domain first. @@ -20,6 +25,7 @@ from ament_index_python.packages import get_package_share_directory from golem_bringup.h12_legs import leg_check from golem_bringup.include import include +from golem_bringup.sim_estop import sim_estop_url from golem_ros2_models.paths import urdf_path from launch import LaunchDescription from launch.actions import DeclareLaunchArgument @@ -36,7 +42,8 @@ def generate_launch_description(): # The simulator publishes /clock; every node uses it so TF lookups and sensor # stamps agree with the simulation. sim_time = {'use_sim_time': 'true'} - # The legs; the sim needs no operator check of the start pose. + # The legs; the sim needs no operator check of the start pose, but its run config + # waits for /lowerbody/confirm_engage as the real one does. legs = { **sim_time, 'start_position_verified': 'true', @@ -48,9 +55,6 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument('use_rviz', default_value='true', description='true: start RViz on rviz_config'), - DeclareLaunchArgument('use_nav', default_value='true', - description="true: start golem_slam's navigation (FAST-LIO, " - 'OctoMap, slam_toolbox, Nav2) for the H1-2'), DeclareLaunchArgument('use_skills', default_value='true', description='true: start graspgen_server and golem_skills ' '(/skill/*)'), @@ -84,16 +88,14 @@ def generate_launch_description(): include('golem_bringup', 'h12/h12_control.launch.py', { **sim_time, 'sim': 'true', - 'safety_config': 'sim_safety_split.yaml', - 'frame_task_config': 'sim_safety_split.yaml', - # No estop in the sim, so nothing to wait for. - 'safety_delay': '0.0', - 'frame_task_delay': '0.0', + # The real robot's configs and delays (h12_control's defaults); the + # sim's estop box in place of the USB one. + 'estop_serial_port': sim_estop_url(), }), # The legs. include('golem_bringup', 'h12/h12_lowerbody.launch.py', legs), # Perception servers and the LLM-callable skills. - include('golem_bringup', 'h12/h12_models.launch.py', { + include('golem_bringup', 'h12/golem_vision_models.launch.py', { **sim_time, 'use_skills': LaunchConfiguration('use_skills'), 'model_logging': LaunchConfiguration('model_logging'), @@ -102,8 +104,7 @@ def generate_launch_description(): 'record_runs': LaunchConfiguration('record_runs'), }), # SLAM and Nav2, driving the legs on /cmd_vel. - include('golem_slam', 'navigation.launch.py', {**sim_time, 'robot': 'h12'}, - condition=IfCondition(LaunchConfiguration('use_nav'))), + include('golem_slam', 'navigation.launch.py', {**sim_time, 'robot': 'h12'}), # For Isaac: its head camera's images are stamped # camera_color_optical_frame (CL_isaaclab_sim's ros_sensor_bridge.py, diff --git a/core_ws/src/golem_bringup/launch/h12/lmg_real.launch.py b/core_ws/src/golem_bringup/launch/h12/lmg_real.launch.py index 163c0f20..9ef7a68d 100644 --- a/core_ws/src/golem_bringup/launch/h12/lmg_real.launch.py +++ b/core_ws/src/golem_bringup/launch/h12/lmg_real.launch.py @@ -98,7 +98,6 @@ def generate_launch_description(): include('golem_bringup', 'h12/h12_control.launch.py', { **wall_time, 'sim': 'false', - 'use_estop': 'true', 'use_frame_task': 'false', 'safety_config': LaunchConfiguration('safety_config'), }), diff --git a/core_ws/src/golem_bringup/launch/h12/lmg_sim.launch.py b/core_ws/src/golem_bringup/launch/h12/lmg_sim.launch.py index d20aa919..2e82018b 100644 --- a/core_ws/src/golem_bringup/launch/h12/lmg_sim.launch.py +++ b/core_ws/src/golem_bringup/launch/h12/lmg_sim.launch.py @@ -2,14 +2,14 @@ Start the loco-manipulation game against the sim: lmg_deploy's MARL policy on all 27 joints. ros2 launch golem_bringup lmg_sim.launch.py [use_skills:=true] - -Start the simulator first. Everything starts at once: the state publishers -(h12_state), the command path (h12_control without frame_task_server, no estop, no -delay), the policy (h12_locomanipulationgame: lmg_deploy's policies/h12/, with the start -pose taken as verified and no operator confirm), navigation (use_slam), golem_skills -(use_skills: /skill/*, among them /skill/frontier_explore, which walks the policy through -Nav2) and RViz for Nav2 goals (use_rviz). golem_skills does not wait for the perception -and gripper services or frame_task_server, none of which run here, and plans its frontier + Start the simulator first. Everything starts at once: the state publishers (h12_state), +the command path (h12_control without frame_task_server, with the sim's estop box and the +real robot's delay), the policy (h12_locomanipulationgame: lmg_deploy's policies/h12/, +with the start pose taken as verified and no operator confirm), navigation (use_slam), +golem_skills (use_skills: /skill/*, among them /skill/frontier_explore, which walks the +policy through Nav2) and RViz for Nav2 goals (use_rviz). golem_skills does not wait for +the perception and gripper services or frame_task_server, none of which run here, and +plans its frontier goals with the H1-2's Nav2 footprint (golem_slam's config/h12/nav2.yaml). safety_config defaults to the real robot's preset, relax_safety_split: the policy was trained against its estops, and the sim is where to see that it does not trip them. @@ -23,6 +23,7 @@ from ament_index_python.packages import get_package_share_directory from golem_bringup.include import include +from golem_bringup.sim_estop import sim_estop_url from golem_ros2_models.paths import urdf_path from launch import LaunchDescription from launch.actions import DeclareLaunchArgument @@ -90,8 +91,8 @@ def generate_launch_description(): 'sim': 'true', 'use_frame_task': 'false', 'safety_config': LaunchConfiguration('safety_config'), - # No hardware estop in the sim, so nothing to wait for. - 'safety_delay': '0.0', + # The sim's estop box, as estop_node reads the real one. + 'estop_serial_port': sim_estop_url(), }), # The policy, on all 27 joints. include('golem_bringup', 'h12/h12_locomanipulationgame.launch.py', { diff --git a/core_ws/src/golem_bringup/test/test_bringup_packages.py b/core_ws/src/golem_bringup/test/test_bringup_packages.py index 77025346..7846ef7a 100644 --- a/core_ws/src/golem_bringup/test/test_bringup_packages.py +++ b/core_ws/src/golem_bringup/test/test_bringup_packages.py @@ -3,14 +3,14 @@ golem_bringup depends on golem_safety, golem_upper_ik and golem_lowerbody_rl. For the H1-2, h12_state.launch.py includes golem_upper_ik's robot_state.launch.py, and -h12_control.launch.py, which needs sim from -every top-level file (the real-robot ones pass use_estop), includes golem_upper_ik's -upper_ik.launch.py first, whose pairing check refuses a frame_task_config that does not -run with its safety_config, then golem_safety's safety.launch.py. For the G1, -g1_core.launch.py includes the same three files: safety_node with robot g1, the safety -config, sim as the top-level file says and no hardware estop, and frame_task_server and -joint_state_publisher with robot g1. The H1-2's h12_lowerbody.launch.py includes -golem_lowerbody_rl's lowerbody.launch.py with robot h12 and every argument it takes, behind +h12_control.launch.py, which needs sim from every top-level file and always runs +estop_node (the sims on their own box), includes golem_upper_ik's upper_ik.launch.py +first, whose pairing check refuses a frame_task_config that does not run with its +safety_config, then golem_safety's safety.launch.py. For the G1, g1_core.launch.py +includes the same three files: safety_node with robot g1, the safety config, sim as the +top-level file says and no hardware estop, and frame_task_server and joint_state_publisher +with robot g1. The H1-2's h12_lowerbody.launch.py includes golem_lowerbody_rl's +lowerbody.launch.py with robot h12 and every argument it takes, behind start_position_verified (g1_core's include, robot g1, is in test_g1_launch.py). The nodes are read by walking those includes, as ros2 launch runs them. The launch tests need the workspace built and sourced. @@ -96,25 +96,30 @@ def _walk(launch_file, **args): return _started(description.entities, context)[0] -@pytest.mark.parametrize('launch_file, sim, use_estop', [ - ('h12_sim.launch.py', 'true', 'false'), - ('lmg_sim.launch.py', 'true', 'false'), - ('h12_real_robot.launch.py', 'false', 'true'), - ('lmg_real.launch.py', 'false', 'true'), - ('h12_safety_grippers.launch.py', 'false', 'true'), +@pytest.mark.parametrize('launch_file, sim, estop_port', [ + ('h12_sim.launch.py', 'true', 'socket://127.0.0.1:15893'), + ('lmg_sim.launch.py', 'true', 'socket://127.0.0.1:15893'), + ('h12_real_robot.launch.py', 'false', ''), + ('lmg_real.launch.py', 'false', ''), + ('h12_safety_grippers.launch.py', 'false', ''), ]) -def test_top_level_files_pass_sim_and_use_estop(launch_file, sim, use_estop, monkeypatch): - """Each top-level H1-2 file passes h12_control its sim and use_estop, which reach the nodes.""" +def test_top_level_files_always_run_the_estop(launch_file, sim, estop_port, monkeypatch): + """ + SAFETY: every top-level H1-2 file runs estop_node and a safety_node that obeys it. + + The sims point estop_node at the box the sim serves on 15700 + ROS_DOMAIN_ID; the real + robot finds its USB box (an empty serial_port). + """ # lmg_sim refuses an unset or 0 ROS_DOMAIN_ID. monkeypatch.setenv('ROS_DOMAIN_ID', '193') arguments = _passed_to(launch_file, 'h12_control.launch.py') assert arguments['sim'] == sim - # Absent: h12_control.launch.py's default, false. - assert arguments.get('use_estop', 'false') == use_estop + assert 'use_estop' not in arguments + assert arguments.get('estop_serial_port', '') == estop_port nodes = _walk('h12_control.launch.py', **arguments) assert nodes['safety_node'].parameters['sim'] == (sim == 'true') - assert nodes['safety_node'].parameters['use_estop'] == (use_estop == 'true') - assert ('estop_node' in nodes) == (use_estop == 'true') + assert nodes['safety_node'].parameters['use_estop'] is True + assert nodes['estop_node'].parameters['serial_port'] == estop_port def test_lmg_real_refuses_to_start_before_any_node(): @@ -177,17 +182,15 @@ def test_control_checks_the_pair_before_any_node(): @pytest.mark.parametrize('safety_config, frame_task_config, use_frame_task, refused', [ ('relax_safety_split.yaml', 'safety_split.yaml', 'true', False), - ('sim_safety_split.yaml', 'sim_safety_split.yaml', 'true', False), ('tight_safety_full', 'safety_full', 'true', False), - ('sim_safety_split.yaml', 'safety_split.yaml', 'true', True), + ('relax_safety_split.yaml', 'safety_full.yaml', 'true', True), ('tight_safety_full.yaml', 'safety_split.yaml', 'true', True), ('tight_safety_full.yaml', 'safety_split.yaml', 'false', False), ]) def test_control_refuses_a_mismatched_pair(safety_config, frame_task_config, use_frame_task, refused): """With frame_task_server, a safety and controller config that do not pair are refused.""" - args = dict(sim=str(safety_config.startswith('sim')).lower(), - safety_config=safety_config, frame_task_config=frame_task_config, + args = dict(sim='false', safety_config=safety_config, frame_task_config=frame_task_config, use_frame_task=use_frame_task) if refused: with pytest.raises(RuntimeError, match='^upper_ik: not starting: safety config '): @@ -201,24 +204,28 @@ def test_control_refuses_a_mismatched_pair(safety_config, frame_task_config, use assert 'frame_task_server' not in nodes -def test_control_starts_safety_and_the_controller_after_their_delays(): - """The real robot staggers estop_node, safety_node and frame_task_server; the sim does not.""" - nodes = _walk('h12_control.launch.py', sim='false', use_estop='true') - assert nodes['estop_node'] == Started('estop', [], 0.0, {'use_sim_time': False}) +def test_control_starts_safety_and_the_controller_after_their_delays(monkeypatch): + """The sim staggers estop_node, safety_node and frame_task_server as the real robot does.""" + nodes = _walk('h12_control.launch.py', sim='false') + assert nodes['estop_node'] == Started('estop', [], 0.0, + {'use_sim_time': False, 'serial_port': ''}) assert nodes['safety_node'] == Started('golem_safety', [], 3.0, { 'robot': 'h12', 'config': 'relax_safety_split.yaml', 'sim': False, 'use_estop': True, 'use_sim_time': False}) assert nodes['frame_task_server'] == Started('golem_upper_ik', [], 5.0, { 'robot': 'h12', 'config': 'safety_split.yaml', 'use_sim_time': False}) - # The sim bringup waits for no estop. + # The sim bringup: the real configs and delays, the sim's estop box. + monkeypatch.setenv('ROS_DOMAIN_ID', '7') sim = _passed_to('h12_sim.launch.py', 'h12_control.launch.py') nodes = _walk('h12_control.launch.py', **sim) - assert set(nodes) == {'safety_node', 'frame_task_server'} - assert nodes['safety_node'] == Started('golem_safety', [], 0.0, { - 'robot': 'h12', 'config': 'sim_safety_split.yaml', 'sim': True, - 'use_estop': False, 'use_sim_time': True}) - assert nodes['frame_task_server'] == Started('golem_upper_ik', [], 0.0, { - 'robot': 'h12', 'config': 'sim_safety_split.yaml', 'use_sim_time': True}) + assert set(nodes) == {'estop_node', 'safety_node', 'frame_task_server'} + assert nodes['estop_node'] == Started('estop', [], 0.0, { + 'use_sim_time': True, 'serial_port': 'socket://127.0.0.1:15707'}) + assert nodes['safety_node'] == Started('golem_safety', [], 3.0, { + 'robot': 'h12', 'config': 'relax_safety_split.yaml', 'sim': True, + 'use_estop': True, 'use_sim_time': True}) + assert nodes['frame_task_server'] == Started('golem_upper_ik', [], 5.0, { + 'robot': 'h12', 'config': 'safety_split.yaml', 'use_sim_time': True}) def test_state_starts_golem_upper_ik_robot_state(): @@ -288,7 +295,7 @@ def test_top_level_files_run_the_leg_node(launch_file, sim, config): def test_leg_block_passes_every_argument(): """An earlier include's config and start_delay (Humble leaks them) do not reach the node.""" nodes = _legs(sim='true', lowerbody_config='sim', start_position_verified='true', - policy='fame', config='sim_safety_split.yaml', start_delay='5.0') + policy='fame', config='relax_safety_split.yaml', start_delay='5.0') assert nodes['lowerbody_controller_node'] == Started('golem_lowerbody_rl', [], 0.0, { 'robot': 'h12', 'config': 'sim', 'policy': 'fame', 'use_sim_time': False}) diff --git a/core_ws/src/golem_bringup/test/test_sim_time.py b/core_ws/src/golem_bringup/test/test_sim_time.py index 95ef9083..23bf43ef 100644 --- a/core_ws/src/golem_bringup/test/test_sim_time.py +++ b/core_ws/src/golem_bringup/test/test_sim_time.py @@ -28,7 +28,7 @@ # (launch file, its arguments, use_sim_time of every node): every top-level file, with the # arguments that start the most nodes. TOP_LEVEL = [ - ('h12_sim.launch.py', {'use_skills': 'true', 'use_nav': 'true', 'use_rviz': 'true'}, True), + ('h12_sim.launch.py', {'use_skills': 'true', 'use_rviz': 'true'}, True), ('lmg_sim.launch.py', {'use_slam': 'true', 'use_skills': 'true'}, True), ('g1_sim.launch.py', {'use_skills': 'true', 'use_nav': 'true', 'use_rviz': 'true'}, True), ('h12_real_robot.launch.py', {'start_position_verified': 'true'}, False), diff --git a/core_ws/src/golem_lowerbody_rl/README.md b/core_ws/src/golem_lowerbody_rl/README.md index e3da0a6f..1f116b31 100644 --- a/core_ws/src/golem_lowerbody_rl/README.md +++ b/core_ws/src/golem_lowerbody_rl/README.md @@ -72,8 +72,9 @@ takes no `/lowstate` before the first `/clock`, and logs that it waits. The node within 1 s of the node's clock. The arm controller's `/safety/lowcmd_upper_in` is fresh within 0.5 s and, with `band.release` (the sims), `frame_task_server`'s first `/left_ee_pose` has come or `band.max_wait` has passed. The node logs - `the policy waits: ` every 5 s. On the real robot it then waits - for `ros2 service call /lowerbody/confirm_engage std_srvs/srv/Trigger`, + `the policy waits: ` every 5 s. With `startup.wait_for_confirm` + (the real robot and the H1-2 sim) it then waits for + `ros2 service call /lowerbody/confirm_engage std_srvs/srv/Trigger`, accepted only now and refused with the reason otherwise. 4. **Running.** `policy started at zero velocity and the default height ...`; the policy resets as its spec's `reset_on` says. `/cmd_vel` goes to the @@ -84,8 +85,8 @@ takes no `/lowstate` before the first `/clock`, and logs that it waits. The node warned about at most every 5 s. Every 10 s the node logs `timing over the last 10 s: ...` (step spacing and tick lag). 5. **Faulted.** Once the legs are commanded, any of these stops the node for - good: a `/lowstate` gap over `faults.max_state_gap` (0.25 s real, 0.5 s - sim), or none for that long; a tick that goes back; a non-finite value or + good: a `/lowstate` gap over `faults.max_state_gap` (0.25 s on the real robots and the H1-2 sim, + 0.5 s in the G1 sim), or none for that long; a tick that goes back; a non-finite value or a non-unit IMU quaternion; a fall (gravity z above -0.5); while running, a tick lag over `faults.max_latency` when it is set (null in every shipped config, and refused in a sim run config: a sim's tick counts sim time, and diff --git a/core_ws/src/golem_lowerbody_rl/config/h12/sim.yaml b/core_ws/src/golem_lowerbody_rl/config/h12/sim.yaml index 55f0d8ab..843f56e1 100644 --- a/core_ws/src/golem_lowerbody_rl/config/h12/sim.yaml +++ b/core_ws/src/golem_lowerbody_rl/config/h12/sim.yaml @@ -1,9 +1,12 @@ # The H1-2's leg node in the simulators; every key is described in ../example.run.yaml. +# Its start-up and faults are real.yaml's; it differs only where the sim does: the +# elastic band it releases and an IMU without the real mounting bias. sim: true startup: - # unattended sim runs start the policy once the pre-pose holds and the timing is ready - wait_for_confirm: false + # SAFETY: as on the real robot, the policy takes the legs only on + # /lowerbody/confirm_engage + wait_for_confirm: true ramp_seconds: 2.0 settle_tolerance: 0.15 settle_ticks: 10 @@ -31,10 +34,9 @@ commands: height_rate: null faults: - # longer than real.yaml's: gaps count sim time, and a delivery delay on a loaded host - # counts as sim time here, more of it at a GOLEM_SIM_RATE above 1 - max_state_gap: 0.5 - # the sim's tick is sim time, so tick lag is not a delivery delay there + # real.yaml's; gaps count sim time here, so a loaded host or a GOLEM_SIM_RATE above 1 + # can trip it + max_state_gap: 0.25 max_latency: null # the sim IMU has no mounting bias diff --git a/core_ws/src/golem_lowerbody_rl/test/test_node.py b/core_ws/src/golem_lowerbody_rl/test/test_node.py index 2ee1aacf..be83572f 100644 --- a/core_ws/src/golem_lowerbody_rl/test/test_node.py +++ b/core_ws/src/golem_lowerbody_rl/test/test_node.py @@ -15,6 +15,7 @@ import threading import time +from ament_index_python.packages import get_package_share_directory from geometry_msgs.msg import PoseStamped, Twist import golem_lowerbody_rl.node as node_module from golem_lowerbody_rl.imu import apply_trim, IDENTITY, trim_quaternion @@ -32,6 +33,7 @@ from std_msgs.msg import Bool, String from std_srvs.srv import Trigger from unitree_hg.msg import LowCmd, LowState +import yaml # the shell's ROS_DOMAIN_ID, but never 0 (the real robot) or 1 (the sim): 97 then _SHELL_DOMAIN = int(os.environ.get('ROS_DOMAIN_ID') or 0) @@ -253,8 +255,22 @@ def _wait(condition, timeout): return bool(condition()) +def _unattended_sim(directory): + """Write the H1-2's sim run config with startup.wait_for_confirm false; return its path.""" + path = os.path.join(get_package_share_directory('golem_lowerbody_rl'), 'config', 'h12', + 'sim.yaml') + with open(path, encoding='utf-8') as source: + raw = yaml.safe_load(source) + raw['startup']['wait_for_confirm'] = False + os.makedirs(os.path.join(str(directory), 'config', 'h12'), exist_ok=True) + target = os.path.join(str(directory), 'config', 'h12', 'sim.yaml') + with open(target, 'w', encoding='utf-8') as sink: + yaml.safe_dump(raw, sink) + return target + + @pytest.fixture -def rig(monkeypatch): +def rig(monkeypatch, tmp_path): """ Return a function that starts the node and the fake robot, both spinning. @@ -271,11 +287,16 @@ def start(config, policy='almi', overrides=(), clock_rate=None): """ Start the node on config and policy (and overrides), and the fake robot beside it. + config 'sim' is the H1-2's sim run config without the operator's confirm, so the + policy starts unattended once ready. + clock_rate: run the node with use_sim_time, and return (node, robot, SimClock) with the sim clock at that rate, not yet publishing. """ if clock_rate is not None: overrides = [*overrides, Parameter('use_sim_time', value=True)] + if config == 'sim': + config = _unattended_sim(tmp_path) node = RecordingNode([], context=context, parameter_overrides=[ Parameter('robot', value='h12'), Parameter('config', value=config), Parameter('policy', value=policy), *overrides]) @@ -347,17 +368,17 @@ def test_sim_time_takes_no_lowstate_before_the_first_clock(rig): def test_sim_time_state_gap_counts_sim_time(rig): - """Under use_sim_time a /lowstate gap is s of sim time: 0.8 s of wall time at 1/4 is 0.2.""" - node, robot, clock = rig('sim', clock_rate=0.25) + """Under use_sim_time a /lowstate gap is s of sim time: 0.6 s of wall time at 1/8 is 0.075.""" + node, robot, clock = rig('sim', clock_rate=0.125) clock.running.set() robot.heartbeat = True - assert _wait(lambda: node.supervisor.phase in (Phase.RAMPING, Phase.HOLDING), 5.0) + assert _wait(lambda: node.supervisor.phase in (Phase.RAMPING, Phase.HOLDING), 10.0) robot.state = False - time.sleep(0.8) + time.sleep(0.6) robot.state = True time.sleep(0.3) assert node.supervisor.fault is None, node.supervisor.fault - # max_state_gap is 0.5 s of sim time: 2 s of wall time + # max_state_gap is 0.25 s of sim time: 2 s of wall time robot.state = False stopped = time.monotonic() assert _wait(lambda: node.supervisor.phase is Phase.FAULTED, 5.0) diff --git a/core_ws/src/golem_lowerbody_rl/test/test_run_config.py b/core_ws/src/golem_lowerbody_rl/test/test_run_config.py index 3451ac6e..ddbd036d 100644 --- a/core_ws/src/golem_lowerbody_rl/test/test_run_config.py +++ b/core_ws/src/golem_lowerbody_rl/test/test_run_config.py @@ -12,9 +12,9 @@ # (robot, run) -> the values that differ between the four run configs EXPECTED = { ('h12', 'sim'): dict( - sim=True, wait_for_confirm=False, band_release=True, max_wait=34.0, + sim=True, wait_for_confirm=True, band_release=True, max_wait=34.0, vx=(-1.0, 1.0), vy=(-1.0, 1.0), yaw=(-1.0, 1.0), accel=None, height_rate=None, - max_state_gap=0.5, max_latency=None, imu_trim_deg=(0.0, 0.0, 0.0)), + max_state_gap=0.25, max_latency=None, imu_trim_deg=(0.0, 0.0, 0.0)), ('h12', 'real'): dict( sim=False, wait_for_confirm=True, band_release=False, max_wait=34.0, vx=(-1.0, 1.0), vy=(-1.0, 1.0), yaw=(-1.0, 1.0), accel=None, height_rate=None, diff --git a/core_ws/src/golem_lowerbody_rl/test/test_supervisor.py b/core_ws/src/golem_lowerbody_rl/test/test_supervisor.py index 00861556..e3d7c0d6 100644 --- a/core_ws/src/golem_lowerbody_rl/test/test_supervisor.py +++ b/core_ws/src/golem_lowerbody_rl/test/test_supervisor.py @@ -333,7 +333,7 @@ def test_alternating_reasons_to_wait_are_logged_at_most_every_5_s(): @pytest.mark.parametrize('ticks', [1, 4], ids=['half', 'double']) def test_tick_not_in_ms_never_becomes_ready(ticks): """Never start the policy on a tick that is not ms of the node's clock, and say why.""" - rig = Rig('h12', 'sim') + rig = Rig('h12', 'sim', startup={'wait_for_confirm': False}) rig.to(Phase.WAITING) rig.feed(round(20.0 / SAMPLE_DT), ticks=ticks) assert rig.sup.phase is Phase.HOLDING and rig.sup.fault is None @@ -393,7 +393,7 @@ def test_ready_needs_fresh_upper_command(): def test_ready_needs_steady_seconds_from_the_ramp_start(): """Start no sooner than steady_seconds of ticks after the ramp began.""" - rig = Rig('h12', 'sim', startup={'ramp_seconds': 0.1}) + rig = Rig('h12', 'sim', startup={'ramp_seconds': 0.1, 'wait_for_confirm': False}) rig.to(Phase.WAITING) rig.until(Phase.RAMPING) ramp_tick = rig.tick @@ -438,7 +438,7 @@ def test_engage_resets_only_when_the_spec_says(): def test_sim_policy_waits_for_ee_pose_or_max_wait(): """In the sim, start on the first /left_ee_pose, or max_wait after holding began.""" - rig = Rig('h12', 'sim') + rig = Rig('h12', 'sim', startup={'wait_for_confirm': False}) rig.feed(round(6.0 / SAMPLE_DT)) assert rig.sup.phase is Phase.HOLDING and not rig.runner.inputs assert 'the policy waits: frame_task_server is not up yet (no /left_ee_pose)' in \ @@ -509,7 +509,7 @@ def test_stale_cmd_is_zero_after_0_5_s(): def test_commands_before_the_start_are_dropped(): """Start the policy at zero velocity and the default height whatever came before.""" - rig = Rig('h12', 'sim') + rig = Rig('h12', 'sim', startup={'wait_for_confirm': False}) rig.sup.on_ee_pose(rig.mono) rig.until(Phase.HOLDING, upper=False) rig.sup.on_cmd(0.5, 0.0, 0.0, rig.mono) diff --git a/core_ws/src/golem_safety/README.md b/core_ws/src/golem_safety/README.md index 51e3601b..c2f07787 100644 --- a/core_ws/src/golem_safety/README.md +++ b/core_ws/src/golem_safety/README.md @@ -21,7 +21,7 @@ robots' bringups include it (`golem_bringup`'s `h12_control.launch.py` and `g1_core.launch.py`). On its own: ```bash -ros2 launch golem_safety safety.launch.py robot:=h12 config:=sim_safety_split sim:=true +ros2 launch golem_safety safety.launch.py robot:=h12 config:=relax_safety_split sim:=true ros2 run golem_safety safety_node --ros-args -p robot:=g1 -p config:=sim_split -p sim:=true ``` @@ -86,9 +86,8 @@ Both robots' `LowCmd`/`LowState` motor arrays hold 35 slots. | Robot | Config | Mode | Run by | |---|---|---|---| -| H1-2 | `relax_safety_split` | split | `h12_real_robot` (the `h12_control.launch.py` default), `lmg_sim`, `lmg_real`, `h12_safety_grippers` (their defaults) | +| H1-2 | `relax_safety_split` | split | `h12_sim`, `h12_real_robot` (the `h12_control.launch.py` default), `lmg_sim`, `lmg_real`, `h12_safety_grippers` (their defaults), `tests/stack` | | H1-2 | `default_safety_split`, `tight_safety_split` | split | by name (`safety_config:=`) | -| H1-2 | `sim_safety_split` | split | `h12_sim`, `tests/stack` | | H1-2 | `default_safety_full`, `relax_safety_full`, `tight_safety_full`, `locomotion_safety_full` | full body | by name | | G1 | `split` | split | `g1_real_robot.launch.py` (default) | | G1 | `sim_split` | split | `g1_sim.launch.py` (default) | diff --git a/core_ws/src/golem_safety/config/h12/sim_safety_split.yaml b/core_ws/src/golem_safety/config/h12/sim_safety_split.yaml deleted file mode 100644 index 298ae67b..00000000 --- a/core_ws/src/golem_safety/config/h12/sim_safety_split.yaml +++ /dev/null @@ -1,39 +0,0 @@ -# Keys, units and allowed values: config/example.yaml. -mode: split_mode - -topics: - low_cmd_lower_in: /safety/lowcmd_lower_in - low_cmd_upper_in: /safety/lowcmd_upper_in - low_cmd_out: /lowcmd - low_state: /lowstate - -control: - publish_hz: 500.0 - state_lag_ms: 100.0 - -stale: - upper_seconds: 0.0 - lower_seconds: 0.2 - state_seconds: 0.1 - -estop_response: - mode: zero - after: burst_then_silence - burst_count: 50 - -limits: - clip: - position_offset: 0.001 - velocity_ratio: 3.0 - torque_ratio: 3.0 - kp_max: 500.0 - kd_max: 30.0 - estop: - position_offset: -0.1 - velocity_ratio: 3.0 - torque_ratio: 3.0 - -logging: - enabled: false - base_dir: /tmp/golem_safety - chunk_prefix: chunk_ diff --git a/core_ws/src/golem_safety/launch/safety.launch.py b/core_ws/src/golem_safety/launch/safety.launch.py index 9f3c28f9..e73dd5a5 100644 --- a/core_ws/src/golem_safety/launch/safety.launch.py +++ b/core_ws/src/golem_safety/launch/safety.launch.py @@ -7,7 +7,7 @@ it, sim:=true on ROS domain 0) by exiting 1 before it publishes. Both robots' bringups include this file (golem_bringup's h12_control.launch.py and g1_core.launch.py); on its own: - ros2 launch golem_safety safety.launch.py robot:=h12 config:=sim_safety_split sim:=true + ros2 launch golem_safety safety.launch.py robot:=h12 config:=relax_safety_split sim:=true """ from launch import LaunchContext, LaunchDescription diff --git a/core_ws/src/golem_safety/test/test_config.cpp b/core_ws/src/golem_safety/test/test_config.cpp index 6acef100..4e335fa8 100644 --- a/core_ws/src/golem_safety/test/test_config.cpp +++ b/core_ws/src/golem_safety/test/test_config.cpp @@ -43,7 +43,7 @@ const RobotProfile & g1() // Every config under config/h12/. const char * const kH12Configs[] = { "default_safety_full", "default_safety_split", "locomotion_safety_full", "relax_safety_full", - "relax_safety_split", "sim_safety_split", "tight_safety_full", "tight_safety_split"}; + "relax_safety_split", "tight_safety_full", "tight_safety_split"}; // Every config under config/g1/. const char * const kG1Configs[] = {"full", "sim_split", "split"}; @@ -337,8 +337,6 @@ TEST(Config, H12UpperStaleSecondsAreTheOldEffectiveValues) .stale.upper_seconds, 2.0); EXPECT_DOUBLE_EQ(golem_safety::load_safety_config("relax_safety_split", h12()) .stale.upper_seconds, 0.0); - EXPECT_DOUBLE_EQ(golem_safety::load_safety_config("sim_safety_split", h12()) - .stale.upper_seconds, 0.0); } // SAFETY: every G1 config writes the zero estop command until restart, logs, and runs @@ -401,7 +399,7 @@ TEST(Config, ConfigNamesBelongToTheirRobot) // of one robot also parse for the other. TEST(Config, PathInAnotherRobotsDirectoryIsRefused) { - // The G1's full.yaml parses for the H1-2, and the H1-2's sim_safety_split.yaml for the G1. + // The G1's full.yaml parses for the H1-2, and the H1-2's relax_safety_split.yaml for the G1. const std::string share = ament_index_cpp::get_package_share_directory("golem_safety"); for (const char * name : kG1Configs) { const std::string path = share + "/config/g1/" + name + ".yaml"; @@ -418,7 +416,7 @@ TEST(Config, PathInAnotherRobotsDirectoryIsRefused) EXPECT_TRUE(contains(message, "robot h12")) << message; } EXPECT_NO_THROW( - golem_safety::load_safety_config(share + "/config/h12/sim_safety_split.yaml", h12())); + golem_safety::load_safety_config(share + "/config/h12/relax_safety_split.yaml", h12())); const TempConfig file(completed("mode: split_mode\n", h12())); EXPECT_NO_THROW(golem_safety::load_safety_config(file.path(), h12())); } diff --git a/core_ws/src/golem_safety/test/test_node_ros.py b/core_ws/src/golem_safety/test/test_node_ros.py index 71228417..fc7cff7e 100644 --- a/core_ws/src/golem_safety/test/test_node_ros.py +++ b/core_ws/src/golem_safety/test/test_node_ros.py @@ -29,7 +29,7 @@ UPPER = '/safety/lowcmd_upper_in' LOWER = '/safety/lowcmd_lower_in' FULL_IN = '/safety/lowcmd_in' -SPLIT = {'h12': 'sim_safety_split', 'g1': 'sim_split'} +SPLIT = {'h12': 'relax_safety_split', 'g1': 'sim_split'} FULL = {'h12': 'default_safety_full', 'g1': 'full'} # s: long enough for every input of the mode to engage and its commands to be relayed. SETTLE = 0.9 @@ -160,8 +160,8 @@ def _refused(robot, config, timeout: float = 30.0, **kwargs) -> tuple[int, str]: ['g1', 'default_safety_split', "no config 'default_safety_split' for robot g1"]), ('h12', 'share:g1/full.yaml', ['h12', 'g1/full.yaml', 'in the config directory of robot g1, not of robot h12']), - ('g1', 'share:h12/sim_safety_split.yaml', - ['g1', 'h12/sim_safety_split.yaml', 'in the config directory of robot h12, not of robot g1']), + ('g1', 'share:h12/relax_safety_split.yaml', + ['g1', 'h12/relax_safety_split.yaml', 'in the config directory of robot h12, not of robot g1']), ], ids=['h12-g1_name', 'g1-h12_name', 'h12-g1_path', 'g1-h12_path']) def test_config_for_other_robot_is_refused(robot, config, words): """SAFETY: another robot's config, by name or by path, is refused before relaying.""" @@ -191,7 +191,7 @@ def test_a_config_that_fails_to_load_is_refused(tmp_path): ]) def test_missing_or_unknown_parameters_are_refused(params, word): """A missing robot or config, or an unknown robot, is refused.""" - node = SafetyNode('h12', 'sim_safety_split', params=params) + node = SafetyNode('h12', 'relax_safety_split', params=params) try: code = node.wait_exit(30.0) finally: @@ -251,7 +251,7 @@ def test_sim_refuses_domain_zero(domain): text=True, stdout=subprocess.PIPE, stderr=subprocess.STDOUT) try: assert watcher.stdout.readline().strip() == 'ready' - node = SafetyNode('h12', 'sim_safety_split', sim=True, env=env) + node = SafetyNode('h12', 'relax_safety_split', sim=True, env=env) try: code = node.wait_exit(30.0) finally: @@ -271,7 +271,7 @@ def test_sim_refuses_domain_zero(domain): def test_use_sim_time_needs_sim(): """SAFETY: use_sim_time:=true without sim:=true is refused: real robots keep the boot clock.""" - node = SafetyNode('h12', 'sim_safety_split', sim=False, params={'use_sim_time': 'true'}) + node = SafetyNode('h12', 'relax_safety_split', sim=False, params={'use_sim_time': 'true'}) try: code = node.wait_exit(30.0) finally: @@ -344,7 +344,7 @@ def test_sim_time_exit_finishes_the_burst_with_clock_stopped(stack, sim_clock): """ clock = sim_clock(rate=0.1) clock.start() - run = stack('h12', 'sim_safety_split', params=SIM_TIME) + run = stack('h12', 'relax_safety_split', params=SIM_TIME) pairs = input_pairs(run) run.robot.stream(*pairs) run.robot.wait_for_output('/lowcmd', lambda msg: carries(msg, run), timeout=15.0) @@ -421,7 +421,7 @@ def walk(entities, delay): def test_the_launch_file_runs_the_node(): """ros2 launch of safety.launch.py runs the node, which exits cleanly on SIGINT.""" command = ['ros2', 'launch', 'golem_safety', 'safety.launch.py', 'robot:=h12', - 'config:=sim_safety_split', 'sim:=true', 'use_sim_time:=false', + 'config:=relax_safety_split', 'sim:=true', 'use_sim_time:=false', 'start_delay:=0.5'] launch = subprocess.Popen(command, env=node_env(), text=True, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, start_new_session=True) @@ -541,7 +541,7 @@ def test_zero_stream_engages_each_input_on_its_own(stack, first): SAFETY: the zero stream runs from boot, and each input engages on its own first command, the other input's slots staying at the zero command. """ - run = stack('h12', 'sim_safety_split') + run = stack('h12', 'relax_safety_split') joints = len(run.robot.profile.joints) time.sleep(0.3) boot = [msg for _, msg in run.outputs()] @@ -636,7 +636,7 @@ def test_full_body_mode_clips_gains_and_holds_a_quiet_input(stack, robot): # (robot, config, the input that goes quiet). QUIET_INPUTS = [ - ('h12', 'sim_safety_split', 'lower'), ('h12', 'sim_safety_split', 'upper'), + ('h12', 'relax_safety_split', 'lower'), ('h12', 'relax_safety_split', 'upper'), ('h12', 'default_safety_full', 'full'), ('g1', 'sim_split', 'lower'), ('g1', 'sim_split', 'upper'), ('g1', 'full', 'full'), ] @@ -678,7 +678,7 @@ def test_quiet_input_is_held_and_reported(stack, robot, config, quiet): assert run.robot.outputs(HEARTBEAT) and all(beat for _, beat in run.robot.outputs(HEARTBEAT)) -QUIET_STATE = [('h12', 'sim_safety_split'), ('g1', 'sim_split'), ('g1', 'full')] +QUIET_STATE = [('h12', 'relax_safety_split'), ('g1', 'sim_split'), ('g1', 'full')] @pytest.mark.parametrize('robot, config', QUIET_STATE, ids=[f'{r}-{c}' for r, c in QUIET_STATE]) @@ -713,7 +713,7 @@ def test_engage_refusals_are_logged_at_most_once_a_second(stack, robot): On another robot's /lowstate no input engages, and engage_refused is logged at most once a second. """ - run = stack(robot, 'split' if robot == 'g1' else 'sim_safety_split') + run = stack(robot, 'split' if robot == 'g1' else 'relax_safety_split') foreign = 6 if robot == 'g1' else 5 run.robot.state.mode_machine = foreign time.sleep(0.1) @@ -738,7 +738,7 @@ def test_an_estop_before_engaging(stack, robot): SAFETY: a non-finite first command latches before engaging: no input engages, the estop command follows, and the heartbeat is false. """ - run = stack(robot, 'split' if robot == 'g1' else 'sim_safety_split') + run = stack(robot, 'split' if robot == 'g1' else 'relax_safety_split') joints = len(run.robot.profile.joints) (lower_pub, lower), (upper_pub, upper) = input_pairs(run) # An input engages on its first valid command, so the NaN comes first. @@ -765,7 +765,7 @@ def test_an_estop_before_engaging(stack, robot): assert run.robot.outputs(HEARTBEAT)[-1][1] is False -DAMP = [('g1', 'split'), ('g1', 'full'), ('h12', 'sim_safety_split')] +DAMP = [('g1', 'split'), ('g1', 'full'), ('h12', 'relax_safety_split')] @pytest.mark.parametrize('robot, config', DAMP, ids=[f'{r}-{c}' for r, c in DAMP]) @@ -798,7 +798,7 @@ def test_damp_estop_keeps_every_motor_on_with_only_kd(stack, robot, config): assert reason.startswith('Command validation failed') -STATE_LIMITS = [('g1', 'split'), ('g1', 'full'), ('h12', 'sim_safety_split')] +STATE_LIMITS = [('g1', 'split'), ('g1', 'full'), ('h12', 'relax_safety_split')] @pytest.mark.parametrize('robot, config', STATE_LIMITS, @@ -894,7 +894,7 @@ def test_a_restarted_writer_is_accepted_after_a_quiet_gap(stack, robot): @pytest.mark.parametrize('robot', ['h12', 'g1']) def test_a_second_writer_on_the_lower_input_before_engaging(stack, robot): """SAFETY: a second writer latches even before the other input engages.""" - run = stack(robot, 'split' if robot == 'g1' else 'sim_safety_split') + run = stack(robot, 'split' if robot == 'g1' else 'relax_safety_split') joints = len(run.robot.profile.joints) (lower_pub, lower), (upper_pub, upper) = input_pairs(run) second = run.robot.publisher(LOWER) @@ -1061,7 +1061,7 @@ def test_estop_silent_after_engaging_latches(stack, robot): def test_estop_topics_are_ignored_without_use_estop(stack): """Without use_estop the estop topics are ignored.""" - run = stack('h12', 'sim_safety_split') + run = stack('h12', 'relax_safety_split') run.robot.estop_values = (True, False) run.robot.stream(*input_pairs(run)) time.sleep(0.5) @@ -1070,7 +1070,7 @@ def test_estop_topics_are_ignored_without_use_estop(stack): assert all(beat for _, beat in run.robot.outputs(HEARTBEAT)) -EXIT_LOWCMD = [('h12', 'sim_safety_split'), ('h12', 'default_safety_full'), ('g1', 'split'), +EXIT_LOWCMD = [('h12', 'relax_safety_split'), ('h12', 'default_safety_full'), ('g1', 'split'), ('g1', 'full')] diff --git a/core_ws/src/golem_safety/test/test_safety_core_relay.cpp b/core_ws/src/golem_safety/test/test_safety_core_relay.cpp index 32662a0d..e247c20b 100644 --- a/core_ws/src/golem_safety/test/test_safety_core_relay.cpp +++ b/core_ws/src/golem_safety/test/test_safety_core_relay.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -221,7 +222,7 @@ LowState past_estop(const RobotProfile & profile, const SafetyConfig & cfg, int // from "A". struct H12Split { - SafetyConfig cfg = config(h12(), "sim_safety_split"); + SafetyConfig cfg = config(h12(), "relax_safety_split"); SafetyCore core{h12(), cfg, false, kDomain}; H12Split() @@ -238,7 +239,7 @@ struct H12Split // whatever header the input carried, on both robots. TEST(Relay, SplitMergesUpperAndLowerSlots) { - SafetyCore core(h12(), config(h12(), "sim_safety_split"), false, kDomain); + SafetyCore core(h12(), config(h12(), "relax_safety_split"), false, kDomain); core.on_state(state_of(h12()), 0.0); LowCmd lower = command(h12()); lower.motor_cmd[5].q = 0.1F; @@ -291,7 +292,7 @@ TEST(Relay, UnownedSlotsAreZeroCommand) const LowCmd zero = golem_safety::zero_command(h12()); // split_mode: Lower owns 0-11 and Upper 12-26; 27-34 are no input's. - SafetyCore split(h12(), config(h12(), "sim_safety_split"), false, kDomain); + SafetyCore split(h12(), config(h12(), "relax_safety_split"), false, kDomain); split.on_state(state_of(h12()), 0.0); LowCmd lower = command(h12()); fill_junk(lower, 27, 34); @@ -345,7 +346,7 @@ TEST(Relay, UnownedSlotsAreZeroCommand) // gain_clipped is raised once per joint and gain. TEST(Clip, PositionVelocityTorqueAndGains) { - const SafetyConfig cfg = config(h12(), "sim_safety_split"); + const SafetyConfig cfg = config(h12(), "relax_safety_split"); const auto & limits = cfg.limits; SafetyCore core(h12(), cfg, false, kDomain); core.on_state(state_of(h12()), 0.0); @@ -461,9 +462,9 @@ TEST(Clip, NonFiniteSampleLatches) int slot; }; const std::vector cases = { - {h12(), "sim_safety_split", Input::Upper, "/safety/lowcmd_upper_in", 13}, - {h12(), "sim_safety_split", Input::Lower, "/safety/lowcmd_lower_in", 4}, - {h12(), "sim_safety_split", Input::Lower, "/safety/lowcmd_lower_in", 26}, + {h12(), "relax_safety_split", Input::Upper, "/safety/lowcmd_upper_in", 13}, + {h12(), "relax_safety_split", Input::Lower, "/safety/lowcmd_lower_in", 4}, + {h12(), "relax_safety_split", Input::Lower, "/safety/lowcmd_lower_in", 26}, {h12(), "default_safety_full", Input::Full, "/safety/lowcmd_in", 26}, {g1(), "sim_split", Input::Upper, "/safety/lowcmd_upper_in", 3}, {g1(), "sim_split", Input::Upper, "/safety/lowcmd_upper_in", 28}, @@ -495,7 +496,7 @@ TEST(Clip, NonFiniteFirstSampleLatches) { // A first command that is not valid latches before any input engages; the burst is the // estop command. - const SafetyConfig cfg = config(h12(), "sim_safety_split"); + const SafetyConfig cfg = config(h12(), "relax_safety_split"); SafetyCore core(h12(), cfg, false, kDomain); core.on_state(state_of(h12()), 0.0); ASSERT_TRUE(core.step(0.0)); @@ -537,15 +538,16 @@ TEST(Clip, NonFiniteFirstSampleLatches) // limit; past the clip range alone does not. TEST(Limits, MeasuredPastEstopLatches) { - const SafetyConfig cfg = config(h12(), "sim_safety_split"); + const SafetyConfig cfg = config(h12(), "relax_safety_split"); const auto & limits = cfg.limits; { SafetyCore core(h12(), cfg, false, kDomain); core.on_state(state_of(h12()), 0.0); - // Past the clip range but inside the estop range: no latch. + // Past the clip range but inside the estop range: no latch. Slot 25, the right wrist + // pitch, whose estop range is 0.1 rad wider than its clip range. LowState inside = state_of(h12()); - inside.motor_state[20].q = static_cast(limits.q_clip_high[20] + 0.05); + inside.motor_state[25].q = static_cast(limits.q_clip_high[25] + 0.05); core.on_state(inside, 0.002); EXPECT_FALSE(core.estopped()); @@ -557,8 +559,9 @@ TEST(Limits, MeasuredPastEstopLatches) EXPECT_TRUE(contains(reason, "right_shoulder_pitch_joint")) << reason; EXPECT_TRUE(contains(reason, "x")) << reason; EXPECT_TRUE(contains(reason, " q ")) << reason; - EXPECT_TRUE(contains(reason, "limit 1.670")) << reason; - EXPECT_TRUE(contains(reason, "1.12x")) << reason; + char limit[32]; + std::snprintf(limit, sizeof(limit), "limit %.3f", limits.q_estop_high[20]); + EXPECT_TRUE(contains(reason, limit)) << reason; } struct Case @@ -596,7 +599,7 @@ TEST(Limits, MeasuredPastEstopLatches) // SAFETY: once engaged, a /lowstate with another robot's mode_machine latches. TEST(Identity, ForeignSampleAfterEngageLatches) { - SafetyCore core(h12(), config(h12(), "sim_safety_split"), false, kDomain); + SafetyCore core(h12(), config(h12(), "relax_safety_split"), false, kDomain); core.on_state(state_of(h12()), 0.0); LowState foreign = state_of(h12()); foreign.mode_machine = 5; @@ -764,7 +767,7 @@ TEST(Estop, KeepWriting) // samples are ignored. TEST(EstopInput, TriggeredOrUnpluggedLatchesWithUseEstop) { - const SafetyConfig cfg = config(h12(), "sim_safety_split"); + const SafetyConfig cfg = config(h12(), "relax_safety_split"); { SafetyCore core(h12(), cfg, true, kDomain); core.on_estop(false, true, 0.0); @@ -837,7 +840,7 @@ TEST(ExitStep, FullAndSplitAreDoneAtOnce) // the last command written. TEST(Log, OneSamplePerLowstateOnTheBodySlots) { - SafetyConfig cfg = config(h12(), "sim_safety_split"); + SafetyConfig cfg = config(h12(), "relax_safety_split"); { SafetyCore core(h12(), cfg, false, kDomain); core.on_state(state_of(h12()), 0.0); @@ -901,10 +904,10 @@ TEST(Events, GainClippedIsTheOldJson) {g1(), "sim_split", Input::Upper, 20, "kp", 300.1F, "{\"commanded\": 300.1000061035156, \"event\": \"gain_clipped\", \"gain\": \"kp\", " "\"joint\": \"left_wrist_pitch_joint\", \"max\": 200.0, \"source\": \"split_mode_upper\"}"}, - {h12(), "sim_safety_split", Input::Lower, 3, "kd", 31.5F, + {h12(), "relax_safety_split", Input::Lower, 3, "kd", 31.5F, "{\"commanded\": 31.5, \"event\": \"gain_clipped\", \"gain\": \"kd\", " "\"joint\": \"left_knee_joint\", \"max\": 30.0, \"source\": \"split_mode_lower\"}"}, - {h12(), "sim_safety_split", Input::Upper, 20, "kd", 1e20F, + {h12(), "relax_safety_split", Input::Upper, 20, "kd", 1e20F, "{\"commanded\": 1.0000000200408773e+20, \"event\": \"gain_clipped\", \"gain\": \"kd\", " "\"joint\": \"right_shoulder_pitch_joint\", \"max\": 30.0, " "\"source\": \"split_mode_upper\"}"}, diff --git a/core_ws/src/golem_safety/test/test_safety_core_startup.cpp b/core_ws/src/golem_safety/test/test_safety_core_startup.cpp index 9c2d8278..12116140 100644 --- a/core_ws/src/golem_safety/test/test_safety_core_startup.cpp +++ b/core_ws/src/golem_safety/test/test_safety_core_startup.cpp @@ -180,7 +180,7 @@ std::optional run( TEST(Startup, ZeroStreamFromFirstStep) { const LowCmd zero = golem_safety::zero_command(h12()); - for (const char * name : {"sim_safety_split", "default_safety_full"}) { + for (const char * name : {"relax_safety_split", "default_safety_full"}) { for (const bool use_estop : {false, true}) { SCOPED_TRACE(std::string(name) + (use_estop ? ", use_estop" : "")); SafetyCore core(h12(), config(h12(), name), use_estop, kDomain); @@ -201,7 +201,7 @@ TEST(Startup, NoLowstateNeverEngages) // H1-2: commands for 5 s and no /lowstate. zero_command on every step, and the reason // at most once a second. const LowCmd zero = golem_safety::zero_command(h12()); - SafetyCore core(h12(), config(h12(), "sim_safety_split"), false, kDomain); + SafetyCore core(h12(), config(h12(), "relax_safety_split"), false, kDomain); const LowCmd cmd = command(h12(), 0.1F); for (int i = 0; i < 500; ++i) { const double t = i * kStep; @@ -294,7 +294,7 @@ TEST(Startup, NoLowstateNeverEngages) TEST(Startup, ZeroStreamEngagesPerInput) { const LowCmd zero = golem_safety::zero_command(h12()); - SafetyCore core(h12(), config(h12(), "sim_safety_split"), false, kDomain); + SafetyCore core(h12(), config(h12(), "relax_safety_split"), false, kDomain); core.on_state(state_of(h12()), 0.0); const LowCmd upper = command(h12(), 0.1F); core.on_command(Input::Upper, upper, "upper", 0.001); @@ -329,7 +329,7 @@ TEST(Startup, ZeroStreamEngagesPerInput) // A command that arrived before the first /lowstate does not engage its input then; the // input's next command does. - SafetyCore early(h12(), config(h12(), "sim_safety_split"), false, kDomain); + SafetyCore early(h12(), config(h12(), "relax_safety_split"), false, kDomain); early.on_command(Input::Upper, upper, "upper", 0.0); early.on_state(state_of(h12()), 0.01); out = early.step(0.01); @@ -464,7 +464,7 @@ TEST(Estop, FirstSampleTriggeredLatches) const RobotProfile & profile; const char * config_name; }; - for (const Case & c : {Case{h12(), "sim_safety_split"}, Case{g1(), "sim_split"}}) { + for (const Case & c : {Case{h12(), "relax_safety_split"}, Case{g1(), "sim_split"}}) { SCOPED_TRACE(c.config_name); const SafetyConfig cfg = config(c.profile, c.config_name); { @@ -491,7 +491,7 @@ TEST(Estop, NoEngageBeforeFirstSample) { // H1-2: /lowstate and both inputs for 1 s, no estop sample: zero_command throughout. const LowCmd zero = golem_safety::zero_command(h12()); - const SafetyConfig cfg = config(h12(), "sim_safety_split"); + const SafetyConfig cfg = config(h12(), "relax_safety_split"); SafetyCore core(h12(), cfg, true, kDomain); const LowCmd cmd = command(h12(), 0.1F); for (int i = 0; i < 100; ++i) { @@ -539,7 +539,7 @@ TEST(Startup, RelayReadyNeedsTheRobotsLowstateAndAnEstopSample) const RobotProfile & profile; const char * config_name; }; - for (const Case & c : {Case{h12(), "sim_safety_split"}, Case{g1(), "sim_split"}}) { + for (const Case & c : {Case{h12(), "relax_safety_split"}, Case{g1(), "sim_split"}}) { SCOPED_TRACE(c.config_name); const SafetyConfig cfg = config(c.profile, c.config_name); LowState foreign = state_of(c.profile); @@ -598,7 +598,7 @@ TEST(Estop, StaleAfterEngageLatches) const RobotProfile & profile; const char * config_name; }; - for (const Case & c : {Case{h12(), "sim_safety_split"}, Case{g1(), "sim_split"}}) { + for (const Case & c : {Case{h12(), "relax_safety_split"}, Case{g1(), "sim_split"}}) { SCOPED_TRACE(c.config_name); const SafetyConfig cfg = config(c.profile, c.config_name); ASSERT_EQ(cfg.estop.timeout_seconds, 0.5); @@ -628,7 +628,7 @@ TEST(Estop, StaleAfterEngageLatches) } // From the first sample on, also before engaging. - SafetyCore early(h12(), config(h12(), "sim_safety_split"), true, kDomain); + SafetyCore early(h12(), config(h12(), "relax_safety_split"), true, kDomain); early.on_estop(false, true, 0.0); ASSERT_TRUE(early.step(0.5)); EXPECT_FALSE(early.estopped()); @@ -639,7 +639,7 @@ TEST(Estop, StaleAfterEngageLatches) // Without use_estop, inputs engage with no estop sample, and silence latches nothing. TEST(Estop, IgnoredWithoutUseEstop) { - const SafetyConfig cfg = config(h12(), "sim_safety_split"); + const SafetyConfig cfg = config(h12(), "relax_safety_split"); SafetyCore core(h12(), cfg, false, kDomain); core.on_estop(true, true, 0.0); core.on_estop(false, false, 0.0); @@ -672,7 +672,7 @@ TEST(Quiet, InputHeldAndReported) int last_before, first_after; // the gap, in 10 ms steps }; const std::vector cases = { - {h12(), "sim_safety_split", Input::Lower, "split_mode_lower", "/safety/lowcmd_lower_in", + {h12(), "relax_safety_split", Input::Lower, "split_mode_lower", "/safety/lowcmd_lower_in", 0.2, 0, 101}, // A gap of 3 s, past stale.upper_seconds (2.0). {h12(), "default_safety_split", Input::Upper, "split_mode_upper", "/safety/lowcmd_upper_in", @@ -782,7 +782,7 @@ TEST(Quiet, LowstateGapHolds) const RobotProfile & profile; const char * config_name; }; - for (const Case & c : {Case{h12(), "sim_safety_split"}, Case{g1(), "split"}}) { + for (const Case & c : {Case{h12(), "relax_safety_split"}, Case{g1(), "split"}}) { SCOPED_TRACE(c.config_name); const SafetyConfig cfg = config(c.profile, c.config_name); ASSERT_EQ(cfg.stale.state_seconds, 0.1); @@ -830,7 +830,7 @@ TEST(Lag, Domain0LateTicksLatch) // /lowstate every 2 ms on time for 1 s, then each sample 1 ms later against the robot's // ms tick than the one before. The baseline may rise 0.5 ms/s, so the lag at the k-th // late sample is 0.9985 k ms: over 100 ms first at k = 101. - const SafetyConfig cfg = config(h12(), "sim_safety_split"); + const SafetyConfig cfg = config(h12(), "relax_safety_split"); ASSERT_EQ(cfg.control.state_lag_ms, 100.0); auto feed = [](SafetyCore & core, LowState state, int late_samples) { for (int i = 0; i <= 500; ++i) { @@ -873,7 +873,7 @@ TEST(Lag, Domain0LateTicksLatch) // A tick wrap and clock drift below 0.5 ms/s are not lag. TEST(Lag, TickWrapAndSlowDriftAreNotLag) { - const SafetyConfig cfg = config(h12(), "sim_safety_split"); + const SafetyConfig cfg = config(h12(), "relax_safety_split"); // The uint32 ms tick wraps 1 s in. { SafetyCore core(h12(), cfg, false, kRealRobotDomain); @@ -906,7 +906,7 @@ TEST(Exit, SplitStopsAtOnce) const RobotProfile & profile; const char * config_name; }; - for (const Case & c : {Case{h12(), "sim_safety_split"}, Case{h12(), "default_safety_full"}, + for (const Case & c : {Case{h12(), "relax_safety_split"}, Case{h12(), "default_safety_full"}, Case{g1(), "sim_split"}, Case{g1(), "full"}}) { SCOPED_TRACE(c.config_name); diff --git a/core_ws/src/golem_upper_ik/README.md b/core_ws/src/golem_upper_ik/README.md index 602ee89b..d6817f80 100644 --- a/core_ws/src/golem_upper_ik/README.md +++ b/core_ws/src/golem_upper_ik/README.md @@ -34,8 +34,8 @@ The package's launch files start both nodes, and both robots' bringups include t On their own: ```bash -ros2 launch golem_upper_ik upper_ik.launch.py robot:=h12 config:=sim_safety_split \ - safety_config:=sim_safety_split sim:=true use_sim_time:=true +ros2 launch golem_upper_ik upper_ik.launch.py robot:=h12 config:=safety_split \ + safety_config:=relax_safety_split sim:=true use_sim_time:=true ros2 launch golem_upper_ik robot_state.launch.py robot:=h12 use_sim_time:=true ros2 run golem_upper_ik frame_task_server --ros-args -p robot:=g1 -p config:=sim_split ros2 run golem_upper_ik joint_state_publisher --ros-args -p robot:=h12 @@ -105,7 +105,6 @@ robot is refused with `ValueError`, naming the known ones. | Robot | Config | Writes | Runs with `golem_safety`'s | |---|---|---|---| | H1-2 | `safety_split` | `/safety/lowcmd_upper_in` | `relax_safety_split`, `default_safety_split`, `tight_safety_split` | -| H1-2 | `sim_safety_split` | `/safety/lowcmd_upper_in` | `sim_safety_split` | | H1-2 | `safety_full` | `/safety/lowcmd_in` | `relax_safety_full`, `default_safety_full`, `tight_safety_full`, `locomotion_safety_full` | | G1 | `g1_split` | `/safety/lowcmd_upper_in` | `split` (a stiff waist, for walking) | | G1 | `sim_split` | `/safety/lowcmd_upper_in` | `sim_split` | diff --git a/core_ws/src/golem_upper_ik/config/h12/example.pairing.yaml b/core_ws/src/golem_upper_ik/config/h12/example.pairing.yaml index 540bd005..9221e430 100644 --- a/core_ws/src/golem_upper_ik/config/h12/example.pairing.yaml +++ b/core_ws/src/golem_upper_ik/config/h12/example.pairing.yaml @@ -1,9 +1,9 @@ # The format of pairing.yaml, read by golem_upper_ik.pairing: one key, pairs, mapping each # golem_safety config name (its config// file name without .yaml) to the one # controller config name (a file of this directory) that may run with it. A config named -# sim* is a sim config: the pairing refuses it on the real robot, and a real config in the -# sim. This file documents the format: setup.py does not install it, and test_pairing.py +# sim* is a sim config: the pairing refuses it on the real robot; a robot that pairs one +# also refuses its real configs in the sim, and a robot with none runs them there. This file documents the format: setup.py does not install it, and test_pairing.py # checks that it loads as a pairing.yaml would. pairs: relax_safety_split: safety_split - sim_safety_split: sim_safety_split + relax_safety_full: safety_full diff --git a/core_ws/src/golem_upper_ik/config/h12/pairing.yaml b/core_ws/src/golem_upper_ik/config/h12/pairing.yaml index 6dd02075..aa9ccdfa 100644 --- a/core_ws/src/golem_upper_ik/config/h12/pairing.yaml +++ b/core_ws/src/golem_upper_ik/config/h12/pairing.yaml @@ -1,12 +1,11 @@ # The controller config each H1-2 safety config runs with (golem_upper_ik.pairing). The # *_split safety configs relay the upper-body input safety_split writes, the *_full ones -# the full-body input safety_full writes, and sim_safety_split's limits suit the sim. +# the full-body input safety_full writes. The sim runs these real pairs too. # Format: example.pairing.yaml. pairs: relax_safety_split: safety_split default_safety_split: safety_split tight_safety_split: safety_split - sim_safety_split: sim_safety_split relax_safety_full: safety_full default_safety_full: safety_full tight_safety_full: safety_full diff --git a/core_ws/src/golem_upper_ik/config/h12/sim_safety_split.yaml b/core_ws/src/golem_upper_ik/config/h12/sim_safety_split.yaml deleted file mode 100644 index dadb868d..00000000 --- a/core_ws/src/golem_upper_ik/config/h12/sim_safety_split.yaml +++ /dev/null @@ -1,65 +0,0 @@ -# The H1-2 sim's controller config, run with golem_safety's sim_safety_split -# (pairing.yaml). Every key: example.controller.yaml. - -topics: - low_cmd: /safety/lowcmd_upper_in - low_state: /lowstate - -controller: - v_lim: 1.0 - w_lim: 2.0 - dq_lim: 1.0 - d_min: 0.02 - timeout: 5.0 - hold_targets: - torso_joint: 0.0 - threshold_ik: 0.0001 - threshold_joint: 0.01 - threshold_linear: 0.001 - threshold_angular: 0.002 - -planner: - planner: RRTConnect - timeout: 1.0 - range: 0.0 - try_direct: true - simplify: true - simplify_time: 1.0 - shortcut: true - moving_speed: 0.30 - min_interpolation_steps: 30 - max_interpolation_steps: 300 - validity_resolution: 0.0025 - constraint_check_steps: 10 - frame_names: - - left_grasp_frame - - right_grasp_frame - frame_x_max: 0.60 - frame_y_min: -0.45 - frame_y_max: 0.45 - frame_z_min: -0.05 - # below the head camera (z 0.688 m) and the lidar (0.680 m), above the shoulder pitch - # joints (0.423 m), all at the zero pose - frame_z_max: 0.60 - frame_limit_margin: 0.01 - frame_z_corridor_margin: 0.05 - start_limit_tolerance: 0.05 - -gains: - # torso, then the left and right arms, in slot order - kp: [160.0, - 240.0, 200.0, 150.0, 150.0, 120.0, 120.0, 120.0, - 240.0, 200.0, 150.0, 150.0, 120.0, 120.0, 120.0] - kd: [10.0, - 12.0, 12.0, 12.0, 12.0, 12.0, 12.0, 12.0, - 12.0, 12.0, 12.0, 12.0, 12.0, 12.0, 12.0] - ki: [60.0, - 600.0, 600.0, 400.0, 400.0, 100.0, 100.0, 100.0, - 600.0, 600.0, 400.0, 400.0, 100.0, 100.0, 100.0] - -limits: - clip: - position_offset: 0.001 - # relaxed for the sim - velocity_ratio: 3.0 - torque_ratio: 3.0 diff --git a/core_ws/src/golem_upper_ik/golem_upper_ik/pairing.py b/core_ws/src/golem_upper_ik/golem_upper_ik/pairing.py index cf2b3310..1e71c415 100644 --- a/core_ws/src/golem_upper_ik/golem_upper_ik/pairing.py +++ b/core_ws/src/golem_upper_ik/golem_upper_ik/pairing.py @@ -6,8 +6,9 @@ - the safety config is one of the robot's pairs; - the controller config is the one paired with it; -- with sim known, neither is a sim config (named sim*) on the real robot or a real config in - the sim. +- with sim known, neither is a sim config (named sim*) on the real robot; and when the robot + pairs a sim config of its own (the G1), neither is a real config in the sim. A robot with + no sim pair (the H1-2) runs its real configs in the sim. upper_ik.launch.py applies the rule before it starts the controller, and golem_bringup's G1 core takes its default controller config from default_controller_config(). @@ -85,9 +86,10 @@ def pairing_refusal(robot: str, controller_config: str, safety_config: str, f'"{controller}": the controller must write the input the safety config ' 'relays, with the limits it was tuned with') if sim is not None: + has_sim_pair = any(is_sim_config(name) for name in pairs) for kind, name in (('safety', safety), ('controller', controller)): - if is_sim_config(name) != sim: - if sim: - return f'{kind} config "{name}" is for the real robot, not the sim' + if is_sim_config(name) and not sim: return f'{kind} config "{name}" is a sim config; the real robot never runs one' + if sim and has_sim_pair and not is_sim_config(name): + return f'{kind} config "{name}" is for the real robot, not the sim' return None diff --git a/core_ws/src/golem_upper_ik/launch/upper_ik.launch.py b/core_ws/src/golem_upper_ik/launch/upper_ik.launch.py index 7ecd6932..6f7e612b 100644 --- a/core_ws/src/golem_upper_ik/launch/upper_ik.launch.py +++ b/core_ws/src/golem_upper_ik/launch/upper_ik.launch.py @@ -3,15 +3,15 @@ In order: 1. The pairing check (golem_upper_ik.pairing): config must be the controller config paired with safety_config in config//pairing.yaml, and with sim neither may be meant for - the other platform. A refused pairing raises before any node starts. + the other platform (golem_upper_ik.pairing). A refused pairing raises before any node starts. 2. frame_task_server, start_delay seconds (wall clock) later. Both robots' bringups include this file first (golem_bringup's h12_control.launch.py and g1_core.launch.py), so a refused pairing stops the launch before any of their nodes starts. On its own: - ros2 launch golem_upper_ik upper_ik.launch.py robot:=h12 config:=sim_safety_split \ - safety_config:=sim_safety_split sim:=true use_sim_time:=true + ros2 launch golem_upper_ik upper_ik.launch.py robot:=h12 config:=safety_split \ + safety_config:=relax_safety_split sim:=true use_sim_time:=true """ from launch import LaunchContext, LaunchDescription @@ -87,7 +87,8 @@ def generate_launch_description() -> LaunchDescription: DeclareLaunchArgument( 'sim', description='true for a simulator, false for the real robot: a sim config ' - '(named sim*) is refused on the real robot and a real one in the sim'), + '(named sim*) is refused on the real robot, and a real one in the sim ' + 'of a robot that pairs sim configs (the G1)'), DeclareLaunchArgument( 'use_sim_time', default_value='false', description="true to stamp and time the published poses with a simulator's " diff --git a/core_ws/src/golem_upper_ik/test/test_config.py b/core_ws/src/golem_upper_ik/test/test_config.py index ac48934a..d7d2096a 100644 --- a/core_ws/src/golem_upper_ik/test/test_config.py +++ b/core_ws/src/golem_upper_ik/test/test_config.py @@ -9,7 +9,7 @@ import yaml CONFIGS = { - 'h12': ['safety_split', 'sim_safety_split', 'safety_full'], + 'h12': ['safety_split', 'safety_full'], 'g1': ['g1_split', 'sim_split'], } CASES = [f'{robot}/{name}' for robot, names in CONFIGS.items() for name in names] diff --git a/core_ws/src/golem_upper_ik/test/test_frame_task_server.py b/core_ws/src/golem_upper_ik/test/test_frame_task_server.py index dfaeff62..c8344e5d 100644 --- a/core_ws/src/golem_upper_ik/test/test_frame_task_server.py +++ b/core_ws/src/golem_upper_ik/test/test_frame_task_server.py @@ -1056,15 +1056,15 @@ def test_upper_ik_launch_starts_only_frame_task_server(): """upper_ik.launch.py checks the pairing, then starts frame_task_server after start_delay.""" from launch.actions import DeclareLaunchArgument, OpaqueFunction declared, started, context = _launched( - 'upper_ik.launch.py', robot='h12', config='sim_safety_split', - safety_config='sim_safety_split.yaml', sim='true', start_delay='1.5') + 'upper_ik.launch.py', robot='h12', config='safety_split', + safety_config='relax_safety_split.yaml', sim='true', start_delay='1.5') assert declared == {'robot', 'config', 'safety_config', 'sim', 'use_sim_time', 'start_delay'} ((delay, node),) = started assert (node.node_package, node.node_executable, node._Node__node_name, delay) == ( 'golem_upper_ik', 'frame_task_server', 'frame_task_server', 1.5) assert _parameters(node, context) == { - 'robot': 'h12', 'config': 'sim_safety_split', 'use_sim_time': False} + 'robot': 'h12', 'config': 'safety_split', 'use_sim_time': False} # the pairing check runs first, before the controller starts entities = [entity for entity in _launch_description('upper_ik.launch.py').entities if not isinstance(entity, DeclareLaunchArgument)] @@ -1072,12 +1072,11 @@ def test_upper_ik_launch_starts_only_frame_task_server(): @pytest.mark.parametrize('args, reason', [ - (dict(robot='h12', config='safety_split', safety_config='sim_safety_split', sim='true'), - 'safety config "sim_safety_split" runs with controller config "sim_safety_split", not ' - '"safety_split"'), - (dict(robot='h12', config='sim_safety_split', safety_config='sim_safety_split', - sim='false'), - 'safety config "sim_safety_split" is a sim config'), + (dict(robot='h12', config='safety_full', safety_config='relax_safety_split', sim='true'), + 'safety config "relax_safety_split" runs with controller config "safety_split", not ' + '"safety_full"'), + (dict(robot='g1', config='sim_split', safety_config='sim_split', sim='false'), + 'safety config "sim_split" is a sim config'), (dict(robot='g1', config='sim_split', safety_config='full', sim='true'), 'safety config "full" is not one of the G1 pairs'), ]) diff --git a/core_ws/src/golem_upper_ik/test/test_pairing.py b/core_ws/src/golem_upper_ik/test/test_pairing.py index 0bd9ddcf..fde804d1 100644 --- a/core_ws/src/golem_upper_ik/test/test_pairing.py +++ b/core_ws/src/golem_upper_ik/test/test_pairing.py @@ -29,7 +29,6 @@ def test_pairing_yaml_is_the_pair_table(): 'relax_safety_split': 'safety_split', 'default_safety_split': 'safety_split', 'tight_safety_split': 'safety_split', - 'sim_safety_split': 'sim_safety_split', 'relax_safety_full': 'safety_full', 'default_safety_full': 'safety_full', 'tight_safety_full': 'safety_full', @@ -60,16 +59,14 @@ def test_the_pairing_functions_are_robot_independent(): 'safety config "sim_split" runs with controller config "sim_split", not "g1_split"'), ('g1', 'split', 'g1_split', True, 'safety config "split" is for the real robot, not the sim'), ('h12', 'relax_safety_split', 'safety_split', False, None), - ('h12', 'sim_safety_split', 'sim_safety_split', True, None), - ('h12', 'relax_safety_split', 'sim_safety_split', None, + ('h12', 'relax_safety_split', 'safety_full', None, 'safety config "relax_safety_split" runs with controller config "safety_split", not ' - '"sim_safety_split"'), + '"safety_full"'), ('h12', 'debug_unknown', 'safety_split', None, 'safety config "debug_unknown" is not one of the H1-2 pairs'), - ('h12', 'relax_safety_split', 'safety_split', True, - 'safety config "relax_safety_split" is for the real robot, not the sim'), - ('h12', 'sim_safety_split', 'sim_safety_split', False, - 'safety config "sim_safety_split" is a sim config; the real robot never runs one'), + # The H1-2 pairs no sim config, so its sim runs the real ones. + ('h12', 'relax_safety_split', 'safety_split', True, None), + ('h12', 'default_safety_full', 'safety_full', True, None), ]) def test_pairing_one_rule(robot, safety, controller, sim, expected): """One rule decides every robot: in the pairs, the paired controller, the right platform.""" diff --git a/docker/RosDockerfile b/docker/RosDockerfile index 1a44fa71..959ebe9f 100644 --- a/docker/RosDockerfile +++ b/docker/RosDockerfile @@ -363,6 +363,15 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ RUN pip install -c /opt/golem-locks/ros-amd64-11.txt --no-cache-dir onnxruntime==1.23.2 \ && python3 -c "import onnxruntime" +# ----------------------------------------------------------------------------- +# pyserial (python3-serial, estop's exec_depend): estop_node reads the H1-2's +# estop box with it, the real USB one and the sims' socket:// one, which every +# H1-2 sim bringup runs. From apt, so the snapshot pins it. +# ----------------------------------------------------------------------------- +RUN apt-get update && apt-get install -y --no-install-recommends python3-serial \ + && rm -rf /var/lib/apt/lists/* \ + && python3 -c "import serial; serial.serial_for_url('loop://')" + # ----------------------------------------------------------------------------- # Clamp setuptools, wheel and numpy again, for the reasons of step 6: the # GraspGenX dependency step (ros-amd64-08) installs setuptools 84 and its diff --git a/docs/RUNNING.md b/docs/RUNNING.md index 11c3d385..1fea5899 100644 --- a/docs/RUNNING.md +++ b/docs/RUNNING.md @@ -31,6 +31,10 @@ docker/scripts/docker_run.sh robocasa # or: isaac # Terminal 2 — ROS workspace. Colcon-builds core_ws if stale, then drops to a shell. docker/scripts/docker_run.sh ros ros2 launch golem_bringup h12_sim.launch.py # bringup is a manual step +# The H1-2 sim runs the real robot's operator gates. The sim serves the estop box +# (ENTER in terminal 1 or in its viewer presses it; golem_safety then latches until +# you restart the bringup), and the legs hold their pre-pose until the confirm: +# ros2 service call /lowerbody/confirm_engage std_srvs/srv/Trigger # Terminal 3 — drive the robot from inside the running ROS container docker exec -it golem_ros bash @@ -103,13 +107,14 @@ reference. For sensors + SLAM/nav on the real robot with no joint commanded (no estop, safety or IK), use `ros2 launch golem_bringup h12_real_slam.launch.py`; record those runs with `ros2 run golem_slam record_slam_bag --name `. -The sim bringup starts the state publishers, the `frame_task_server` IK -solver, `safety_node`, the Gemini/SAM/GraspGen model servers, the RL lower-body -controller, the skills node, nav, and RViz. Common arguments: +The sim bringup starts the state publishers, `estop_node` on the sim's estop box, +`safety_node` and the `frame_task_server` IK solver with the real robot's configs and +delays (3 s, 5 s), the Gemini/SAM/GraspGen model servers (`golem_vision_models.launch.py`), +the RL lower-body controller, the skills node, nav (always), and RViz. Common arguments: ```bash ros2 launch golem_bringup h12_sim.launch.py \ - use_rviz:=false use_nav:=false use_skills:=false policy:=fame + use_rviz:=false use_skills:=false policy:=fame ``` ## Worked example — open the fridge diff --git a/golem_robocasa/estop_server.py b/golem_robocasa/estop_server.py new file mode 100644 index 00000000..48e161da --- /dev/null +++ b/golem_robocasa/estop_server.py @@ -0,0 +1,163 @@ +"""Serve the H1-2's hardware estop box over TCP, for the estop package's estop_node. + +The real box is an Arduino on USB serial (core_ws/src/estop/arduino/estop_serial.ino) +that prints "ESTOP,BOOT" once, then "ESTOP,0" (clear) or "ESTOP,1" (triggered) on every +loop. This server speaks the same lines to every client on 127.0.0.1:, which +estop_node opens as serial_port socket://127.0.0.1: (pyserial's serial_for_url), +so the sim bringup runs the real estop_node and golem_safety's estop path unchanged. + +The port is estop_port() of the sim's ROS_DOMAIN_ID, so a sim on another domain never +answers this domain's estop_node. The ENTER key, on the sim's terminal or in the +viewer, toggles the button between clear and triggered. + +SAFETY: golem_safety latches its estop until restart on a triggered sample, on an +unplugged box and on a box silent for its estop.timeout_seconds; the server streams on +the wall clock from its own thread, so a stalled physics loop still reads as a live box, +and a stopped sim reads as an unplugged one. +""" +import os +import select +import socket +import sys +import threading + +# First port of the estop servers; the sim on ROS domain d listens on BASE_PORT + d. +BASE_PORT = 15700 +# Hz of lines sent to each client: estop_node treats a box silent for its +# line_stale_timeout_sec (0.1 s) as stale. +LINE_HZ = 50.0 +# GLFW's key code for ENTER, as the MuJoCo viewer passes it to a key callback. +GLFW_KEY_ENTER = 257 + + +def estop_port(domain_id: str | None = None) -> int: + """Return the estop TCP port of a ROS domain (ROS_DOMAIN_ID when domain_id is None).""" + raw = os.environ.get("ROS_DOMAIN_ID", "") if domain_id is None else domain_id + return BASE_PORT + int(raw.strip() or "1") + + +class EstopServer: + """Stream the simulated estop box's lines to every connected estop_node.""" + + def __init__(self, port: int, tag: str): + """Bind the port; raise OSError when another process holds it. + + tag prefixes the log lines (e.g. "h12_mujoco"). + """ + self._tag = tag + self._lock = threading.Lock() + self._triggered = False + self._clients: list[socket.socket] = [] + self._stop = threading.Event() + self._listener = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self._listener.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + self._listener.bind(("127.0.0.1", port)) + self._listener.listen(4) + self._listener.settimeout(0.1) + self.port = port + self._threads = [ + threading.Thread(target=self._accept_loop, daemon=True, name="estop_accept"), + threading.Thread(target=self._send_loop, daemon=True, name="estop_send"), + ] + if sys.stdin is not None and sys.stdin.isatty(): + self._threads.append( + threading.Thread(target=self._stdin_loop, daemon=True, name="estop_stdin")) + + def start(self) -> None: + """Start serving, and reading ENTER from the terminal when it is one.""" + for thread in self._threads: + thread.start() + keys = "ENTER (terminal or viewer)" if len(self._threads) == 3 else "ENTER (viewer)" + print(f"[{self._tag}] estop box on socket://127.0.0.1:{self.port}: clear; " + f"{keys} toggles it", flush=True) + + @property + def triggered(self) -> bool: + """Return whether the button is pressed.""" + with self._lock: + return self._triggered + + def toggle(self) -> None: + """Press the button if it is clear, release it if it is pressed.""" + with self._lock: + self._triggered = not self._triggered + state = self._triggered + print(f"[{self._tag}] estop {'TRIGGERED' if state else 'released'}", flush=True) + + def key_callback(self, keycode: int) -> None: + """Toggle the button on ENTER (a MuJoCo viewer key callback).""" + if keycode == GLFW_KEY_ENTER: + self.toggle() + + def shutdown(self) -> None: + """Stop serving and close every connection, which estop_node reads as unplugged.""" + self._stop.set() + for thread in self._threads: + if thread.name != "estop_stdin": + thread.join(timeout=1.0) + with self._lock: + clients, self._clients = self._clients, [] + for client in clients: + client.close() + self._listener.close() + + def _accept_loop(self) -> None: + """Accept estop_node's connections; each gets ESTOP,BOOT first, as at power-up.""" + while not self._stop.is_set(): + try: + client, _ = self._listener.accept() + except (socket.timeout, OSError): + continue + client.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1) + try: + client.sendall(b"ESTOP,BOOT\r\n") + except OSError: + client.close() + continue + with self._lock: + self._clients.append(client) + + def _send_loop(self) -> None: + """Send the button's state to every client at LINE_HZ, on the wall clock.""" + period = 1.0 / LINE_HZ + while not self._stop.wait(period): + with self._lock: + line = b"ESTOP,1\r\n" if self._triggered else b"ESTOP,0\r\n" + clients = list(self._clients) + dead = [] + for client in clients: + try: + client.sendall(line) + except OSError: + dead.append(client) + if dead: + with self._lock: + self._clients = [c for c in self._clients if c not in dead] + for client in dead: + client.close() + + def _stdin_loop(self) -> None: + """Toggle the button on each line read from the terminal.""" + while not self._stop.is_set(): + ready, _, _ = select.select([sys.stdin], [], [], 0.2) + if not ready: + continue + if not sys.stdin.readline(): + return + self.toggle() + + +def start_estop_server(tag: str) -> EstopServer: + """Start the estop server on this domain's port; exit the sim when the port is taken.""" + port = estop_port() + try: + server = EstopServer(port, tag) + except OSError as exc: + raise SystemExit( + f"[{tag}] cannot serve the estop on 127.0.0.1:{port} ({exc}): another sim " + f"on ROS_DOMAIN_ID={os.environ.get('ROS_DOMAIN_ID', '')!r} holds it") from exc + server.start() + return server + + +__all__ = ["BASE_PORT", "EstopServer", "estop_port", "start_estop_server"] diff --git a/golem_robocasa/h12_mujoco.py b/golem_robocasa/h12_mujoco.py index 06b1fecb..b7d76176 100644 --- a/golem_robocasa/h12_mujoco.py +++ b/golem_robocasa/h12_mujoco.py @@ -59,6 +59,7 @@ def _parser() -> argparse.ArgumentParser: from measurement_bridge import MeasurementBridge from mujoco_env import ElasticBand, FallLogger, apply_band from mujoco_ros_bridge import RosSensorBridge +from estop_server import start_estop_server from sim_names import NameResolver from sim_timing import RealtimePacer, LoopTimings from unitree_interface import SimInterface @@ -314,10 +315,15 @@ def sim_loop(task, viewer=True, layout=None, style=None, seed=None, timing=False # The viewer draws its own MjData, refreshed from data by the viewer thread, # with the band while it is enabled; its mouse perturbation forces come back # through view_sync.apply_perturbation(). + # The hardware estop box estop_node reads (estop_server); SAFETY: golem_safety + # latches on its ENTER toggle, and on its silence once the sim stops. + estop = start_estop_server("h12_mujoco") + handle = view_sync = None if viewer: handle, view_sync = open_viewer(model, data, sim_lock, band, band_body_id, "h12_mujoco", - timings=timings, sim_rate=sim_rate) + timings=timings, sim_rate=sim_rate, + key_callbacks=(estop.key_callback,)) # Logs the band's release and the first time the torso tips or drops. fall_logger = FallLogger(band_body_id, "torso", upright_min=0.5, height_min=0.5) @@ -380,6 +386,7 @@ def sim_loop(task, viewer=True, layout=None, style=None, seed=None, timing=False hand_right.stop() hand_left.stop() close_viewer(handle, view_sync) + estop.shutdown() executor.shutdown() ros_bridge.shutdown() for node in (measurement, hand_right, hand_left, ros_bridge): diff --git a/golem_robocasa/viewer_sync.py b/golem_robocasa/viewer_sync.py index cbf13b34..07fa925a 100644 --- a/golem_robocasa/viewer_sync.py +++ b/golem_robocasa/viewer_sync.py @@ -83,14 +83,15 @@ def draw(handle, viewer_data): return draw -def open_viewer(model, data, lock, band, body, tag, timings=None, sim_rate=None): +def open_viewer(model, data, lock, band, body, tag, timings=None, sim_rate=None, key_callbacks=()): """Launch the passive viewer on its own copy of data, framed on body, and start its sync thread. lock is the physics loop's lock (ViewerSync). band, the sim's ElasticBand or None, is drawn while enabled and toggled by SPACE. Collision geoms are - hidden. A framing failure is logged under [tag]. sim_rate, the target - real-time factor (sim_common.sim_rate_from_env), is shown beside the - measured one. Returns (handle, view_sync); close them with close_viewer. + hidden. Each of key_callbacks also gets every key the viewer passes on. + A framing failure is logged under [tag]. sim_rate, the target real-time + factor (sim_common.sim_rate_from_env), is shown beside the measured one. + Returns (handle, view_sync); close them with close_viewer. """ import mujoco.viewer @@ -100,9 +101,14 @@ def open_viewer(model, data, lock, band, body, tag, timings=None, sim_rate=None) with lock: copy_display_state(viewer_data, data) mujoco.mj_forward(model, viewer_data) - if band is not None: - # SPACE in the viewer toggles the band (ElasticBand.key_callback). - handle = mujoco.viewer.launch_passive(model, viewer_data, key_callback=band.key_callback) + # SPACE in the viewer toggles the band (ElasticBand.key_callback). + callbacks = ((band.key_callback,) if band is not None else ()) + tuple(key_callbacks) + if callbacks: + def on_key(keycode): + """Pass a viewer key to every callback.""" + for callback in callbacks: + callback(keycode) + handle = mujoco.viewer.launch_passive(model, viewer_data, key_callback=on_key) else: handle = mujoco.viewer.launch_passive(model, viewer_data) handle.opt.geomgroup[0] = 0 # hide collision geoms diff --git a/tests/g1/test_sim_nav.py b/tests/g1/test_sim_nav.py index bd966552..70441fd3 100644 --- a/tests/g1/test_sim_nav.py +++ b/tests/g1/test_sim_nav.py @@ -517,15 +517,29 @@ def _log_has(self, text): except OSError: return False - def wait_band_released(self, timeout=120.0, refusals=()): + def wait_band_released(self, timeout=120.0, refusals=(), confirm=False): """Wait for the sim to log the band's release; return False on a timeout or at once on a refusal. The robot's leg node asks for the release once its policy runs and frame_task_server is - up; `refusals` are the launch's refusal lines in launch.log. + up; `refusals` are the launch's refusal lines in launch.log. With `confirm`, for a run + config that waits for the operator (the H1-2's), it calls /lowerbody/confirm_engage once + a second until the leg node accepts it (it refuses one before the pre-pose is ready). """ + from std_srvs.srv import Trigger + client = self.node.create_client(Trigger, "/lowerbody/confirm_engage") if confirm else None + future, next_call, confirmed = None, 0.0, False end = time.time() + timeout sim_log = os.path.join(self.out, "sim.log") while time.time() < end: + if client is not None and not confirmed: + if future is None and time.time() >= next_call and client.service_is_ready(): + future = client.call_async(Trigger.Request()) + if future is not None and future.done(): + response = future.result() + confirmed = response is not None and response.success + future, next_call = None, time.time() + 1.0 + if confirmed: + self.mark("confirm_engage") if bringup_test.file_has(sim_log, bringup_test.BAND_RELEASED_LINE): self.mark("band_released") return True diff --git a/tests/sim/common.sh b/tests/sim/common.sh index 7ddd8587..3820b84f 100644 --- a/tests/sim/common.sh +++ b/tests/sim/common.sh @@ -247,7 +247,7 @@ sim_test_run_ros() { local seconds=$1 script=$2 set +e timeout "$seconds" "${COMPOSE[@]}" -f "$OUT/test_compose_ros_in_sim.yml" run --rm -T --no-deps --name "$ROS" \ - -e G1_DDS_INTERFACE=lo -e PYTHONDONTWRITEBYTECODE=1 -e PYTHONUNBUFFERED=1 ros bash -c "set -m + -e "GOLEM_ROBOT=$SIM_TEST_ROBOT" -e G1_DDS_INTERFACE=lo -e PYTHONDONTWRITEBYTECODE=1 -e PYTHONUNBUFFERED=1 ros bash -c "set -m source /opt/ros/humble/setup.bash && source /home/code/core_ws/install/setup.bash || exit 3 $script" 2>&1 | tee "$OUT/drive.log" DRIVE_RC=${PIPESTATUS[0]} diff --git a/tests/sim/test_sim_stack.py b/tests/sim/test_sim_stack.py index b0062c43..f7d867cf 100644 --- a/tests/sim/test_sim_stack.py +++ b/tests/sim/test_sim_stack.py @@ -403,7 +403,9 @@ def run_drive(out): if "error" in goal: d.mark("no_goal", error=goal["error"]) return 1 - if d.wait_band_released(timeout=300.0, refusals=bringup_test.LAUNCH_REFUSALS) and d.wait_nav(): + # The H1-2's sim run config waits for the operator's confirm, as the real one does. + if d.wait_band_released(timeout=300.0, refusals=bringup_test.LAUNCH_REFUSALS, + confirm=robot_from_env() == "h12") and d.wait_nav(): d.spin_for(STAND_S) x, y, yaw = d.goal_in_map(goal) ok = d.navigate_with_arms(x, y, yaw) diff --git a/tests/stack/check_lowerbody.py b/tests/stack/check_lowerbody.py index dfb8d25f..0fb4ad77 100644 --- a/tests/stack/check_lowerbody.py +++ b/tests/stack/check_lowerbody.py @@ -109,7 +109,6 @@ def main(): parser.add_argument('--output', type=Path, required=True) parser.add_argument('--expected-policy', default='almi', help='The policy /lowerbody/active_policy must name in every scored phase') - parser.add_argument('--confirm-engage', action='store_true', help='Call confirm_engage on the isolated simulation domain') parser.add_argument('--wall-timeout', type=float, default=600.) args = parser.parse_args() domain = int(os.environ.get('ROS_DOMAIN_ID', '0')) @@ -147,7 +146,7 @@ def on_state(msg): QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL, reliability=ReliabilityPolicy.RELIABLE)) node.create_subscription(LowState, '/lowstate', on_state, qos_profile_sensor_data) confirm = node.create_client(Trigger, '/lowerbody/confirm_engage') - future = None + future, confirmed, next_confirm = None, False, 0. file_position, pending = 0, '' deadline = time.monotonic()+args.wall_timeout def pump(speed=0.): @@ -179,11 +178,15 @@ def pump(speed=0.): try: while True: pump() - if args.confirm_engage and future is None and confirm.service_is_ready(): + # The sim run config waits for the operator's confirm, as the real one does; the + # node refuses it until the pre-pose is held and ready, so retry once a second. + if not confirmed and future is None and confirm.service_is_ready() and \ + time.monotonic() >= next_confirm: future = confirm.call_async(Trigger.Request()) if future is not None and future.done(): response = future.result() - assert response is not None and response.success, f'Engage rejected: {response}' + future, next_confirm = None, time.monotonic() + 1. + confirmed = response is not None and response.success if (state['clock'] is not None and state['policy'] == args.expected_policy and state['lowstate_at'] is not None and time.monotonic()-state['lowstate_at'] < 2. and rows and abs(rows[-1]['sim_time']-state['clock']) < .25 and not rows[-1]['support_active']): diff --git a/tests/stack/minimal_stack.launch.py b/tests/stack/minimal_stack.launch.py index 71168c97..8861a840 100644 --- a/tests/stack/minimal_stack.launch.py +++ b/tests/stack/minimal_stack.launch.py @@ -3,7 +3,8 @@ Starts, at once and in this order: golem_safety's safety_node, golem_upper_ik's frame_task_server and joint_state_publisher, robot_state_publisher on the H1-2 URDF and, for the locomotion case only, golem_lowerbody_rl's lowerbody_controller_node pinned to ALMI. Every -node runs on sim time with the H1-2's sim configs. If any node exits, the whole launch shuts +node runs on sim time with the real H1-2's configs, as h12_sim does (no estop_node: +safety_node runs without use_estop here). If any node exits, the whole launch shuts down. Reads the environment, not launch arguments: ROS_DOMAIN_ID (1..232), TEST_CASE @@ -33,12 +34,12 @@ def generate_launch_description(): nodes = [ # SAFETY: the only writer of /lowcmd; the controllers below write its /safety/* inputs. Node(package='golem_safety', executable='safety_node', name='safety_node', - parameters=[sim_time, {'robot': 'h12', 'config': 'sim_safety_split.yaml', 'sim': True}], + parameters=[sim_time, {'robot': 'h12', 'config': 'relax_safety_split.yaml', 'sim': True}], output='screen'), # The arm controller under test in the arms case, and the torso and arm hold in locomotion; # its config pairs with the safety config above. Node(package='golem_upper_ik', executable='frame_task_server', name='frame_task_server', - parameters=[sim_time, {'robot': 'h12', 'config': 'sim_safety_split.yaml'}], output='screen'), + parameters=[sim_time, {'robot': 'h12', 'config': 'safety_split.yaml'}], output='screen'), # /joint_states from /lowstate, for robot_state_publisher's TF. Node(package='golem_upper_ik', executable='joint_state_publisher', name='joint_state_publisher', parameters=[sim_time, {'robot': 'h12'}], output='screen'), @@ -48,7 +49,7 @@ def generate_launch_description(): ] if case == 'locomotion': # The leg controller under test: ALMI on the H1-2's sim run config, as the sim bringups - # run it (no operator confirm; it releases the band). Its start needs frame_task_server's + # run it (check_lowerbody.py gives the operator's confirm; it releases the band). Its start needs frame_task_server's # fresh commands and first /left_ee_pose, so the arm controller above runs in this case too. nodes.append(Node( package='golem_lowerbody_rl', executable='lowerbody_controller_node', name='lowerbody_controller_node', diff --git a/tests/stack/test_heartbeat_clock.py b/tests/stack/test_heartbeat_clock.py index 1894d4fe..852c26bb 100644 --- a/tests/stack/test_heartbeat_clock.py +++ b/tests/stack/test_heartbeat_clock.py @@ -64,7 +64,7 @@ def feed(): executor.add_node(observer) with tempfile.TemporaryFile(mode='w+') as log: process = subprocess.Popen( - [safety_node, '--ros-args', '-p', 'robot:=h12', '-p', 'config:=sim_safety_split', + [safety_node, '--ros-args', '-p', 'robot:=h12', '-p', 'config:=relax_safety_split', '-p', 'sim:=true', '-p', 'use_sim_time:=true', '-r', '/lowcmd:=/heartbeat_clock_test/lowcmd', '-r', '/lowstate:=/heartbeat_clock_test/lowstate'],