diff --git a/core_ws/src/lmg_deploy/README.md b/core_ws/src/lmg_deploy/README.md index dd4a8a8c..73c26c9f 100644 --- a/core_ws/src/lmg_deploy/README.md +++ b/core_ws/src/lmg_deploy/README.md @@ -5,8 +5,9 @@ Runs a two-agent MARL policy from Locomanipulation_game (branch `marl-direct`, delay in training) on the H1-2. The policies live one per robot, in `policies/h12/` and `policies/g1/` (empty so far), **one iteration at a time: a new export replaces the robot's folder, and old ones are deleted, not kept beside it** (`policies/README.md`). `policies/h12/` holds -LegsBalanceAmp_s42 (LocoManip-LegsBalanceAmp-v0: the sport-mode recipe at 100 Hz, legs only, so it -refuses arm goals); its `export.yaml`'s `source` names the checkpoint. A +LegsEnergyDefault200k_s43 (LocoManip-LegsOnly-v0: the sport-mode recipe at 100 Hz, legs only, so it +refuses arm goals; trained with a mechanical-power penalty and no pelvis-height reward, and its legs +actor is not shown the wrist targets); its `export.yaml`'s `source` names the checkpoint. A **legs** agent that walks on `/cmd_vel`, and an **arms** agent that reaches wrist-pose goals through a damped-least-squares IK step plus a learned residual. One node, `marl_deploy_node`, owns all 27 joints. It publishes @@ -23,7 +24,7 @@ the first latches it silent. | File | What it is | |---|---| -| `lmg_deploy/controller.py` | ROS-free env step: SLAM odometry → mode → observations (legs 95, arms 123; 97 and 125 with `gait_phase_cos`) → joint targets | +| `lmg_deploy/controller.py` | ROS-free env step: SLAM odometry → mode → observations (legs 95, arms 123; 97 and 125 with `gait_phase_cos`; the legs' 14 fewer when the export's `legs_obs_terms` leaves out `ee_targets`) → joint targets | | `lmg_deploy/kinematics.py` | Pinocchio wrist FK, Jacobian and IK step, in the pelvis frame | | `lmg_deploy/rotations.py` | numpy versions of the isaaclab.utils.math functions training used | | `lmg_deploy/networks.py` | TorchScript loader for the exported actors | @@ -67,7 +68,8 @@ It writes: phase cos after its sin), `imu` (the frame the run's IMU sat in, its offset, and `gyro_lowpass_hz` when the gyro was low-passed) and the source files. The sport-mode runs' exports also carry their control rate (`control_dt` 0.01), gains, stance, per-joint action scales, `max_ik_step`, gait period, standing threshold, command ranges and - low-pass (`command_filter_tau`), their observation term lists, which the controller checks against its own, and + low-pass (`command_filter_tau`), their observation term lists, which the controller checks against its own (the +legs' list may leave out `ee_targets`: LocoManip-LegsOnly-v0 runs from 2026-11-16, 83 inputs), and `arm_goals` (false: the run never trained an arm goal, so `set_mode arm` and `~/arm_goal` are refused). The node loads it over the policy's yaml and runs at 1 / `control_dt`. A new policy's yaml is the last one's with `legs.num_obs` / `arms.num_obs` set to the export's printed sizes. diff --git a/core_ws/src/lmg_deploy/config/example.export.yaml b/core_ws/src/lmg_deploy/config/example.export.yaml index c2cd0f00..0469bd23 100644 --- a/core_ws/src/lmg_deploy/config/example.export.yaml +++ b/core_ws/src/lmg_deploy/config/example.export.yaml @@ -33,6 +33,11 @@ default_joint_pos: [0.0, -0.375, 0.0, 0.697, -0.322, 0.0, 0.0, -0.375, 0.0, 0.69 leg_action_scales: [0.2, 0.2, 0.2, 0.1875, 0.125, 0.25, 0.2, 0.2, 0.2, 0.1875, 0.125, 0.25] # per leg joint arm_residual_scales: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # 0: IK alone max_ik_step: 0.05 # rad per policy step +# recorded by exports from 2026-11-16. The controller reads the first two from the deploy config's arms block +# (h12.yaml: the same values), so a run that trains with others needs them set there; clip_actions overrides it. +residual_cutoff_hz: 3.0 # Hz, the arms' residual low-pass +ik_damping: 0.05 # the IK's damping +clip_actions: 10.0 # the env clamps every raw action to +-this gait_period: 0.7 # s, sport mode's gait cycle zero_cmd_eps: 0.03 # a command norm under this is standing command_ranges: # the run's command ranges; /cmd_vel is clipped to them @@ -41,6 +46,7 @@ command_ranges: # the run's command ranges; /cmd_vel is clipped to t ang_vel_z: [-0.97, 0.97] command_filter_tau: 0.5 # s, the velocity command's low-pass arm_goals: false # the run never trained an arm goal (arm_goal_prob 0); the deploy refuses one +# the legs' list may leave out ee_targets (LocoManip-LegsOnly-v0 runs from 2026-11-16; legs.num_obs is then 83) legs_obs_terms: [base_lin_acc, base_ang_vel, projected_gravity, velocity_commands, arm_goal, ee_targets, joint_pos, joint_vel, gait_phase, gait_phase_cos, actions] arms_obs_terms: [base_lin_acc, base_ang_vel, projected_gravity, velocity_commands, arm_goal, ee_targets, joint_pos, diff --git a/core_ws/src/lmg_deploy/lmg_deploy/controller.py b/core_ws/src/lmg_deploy/lmg_deploy/controller.py index 44310003..14610b98 100644 --- a/core_ws/src/lmg_deploy/lmg_deploy/controller.py +++ b/core_ws/src/lmg_deploy/lmg_deploy/controller.py @@ -65,7 +65,8 @@ ARM_START = 13 NUM_ARM_JOINTS = 14 # The observation terms step() builds, in order (gait_phase_cos only with cfg["gait_phase_cos"]). An export's -# legs_obs_terms / arms_obs_terms must equal them. +# legs_obs_terms / arms_obs_terms must equal them; the legs' may leave out ee_targets (LocoManip-LegsOnly-v0 runs +# from 2026-11-16: no arm goals, so the legs' actor is not shown the wrist targets). COMMON_OBS_TERMS = ["base_lin_acc", "base_ang_vel", "projected_gravity", "velocity_commands", "arm_goal", "ee_targets", "joint_pos", "joint_vel", "gait_phase", "gait_phase_cos"] LEGS_OBS_TERMS = COMMON_OBS_TERMS + ["actions"] @@ -130,8 +131,10 @@ def __init__(self, cfg: dict, kinematics: ArmKinematics, legs, arms): self.gait_period = float(cfg["gait_period"]) self.zero_cmd_eps = float(cfg["zero_cmd_eps"]) self.gait_phase_cos = bool(cfg.get("gait_phase_cos", False)) + self.legs_see_ee_targets = "ee_targets" in (cfg.get("legs_obs_terms") or LEGS_OBS_TERMS) for agent, terms in (("legs", LEGS_OBS_TERMS), ("arms", ARMS_OBS_TERMS)): - expected = [t for t in terms if t != "gait_phase_cos" or self.gait_phase_cos] + expected = [t for t in terms if (t != "gait_phase_cos" or self.gait_phase_cos) + and (t != "ee_targets" or agent == "arms" or self.legs_see_ee_targets)] trained = cfg.get(f"{agent}_obs_terms") if trained is not None and list(trained) != expected: raise ValueError(f"{agent}_obs_terms {list(trained)} are not the observations this controller builds, " @@ -263,7 +266,8 @@ def step(self, state: RobotState) -> JointCommand: self._filter_command() common = self._common_obs(state) - legs_obs = np.concatenate([common, self.legs_last]) + legs_common = common if self.legs_see_ee_targets else self._common_obs(state, ee_targets=False) + legs_obs = np.concatenate([legs_common, self.legs_last]) arms_obs = np.concatenate([common, self.arms_last, self._wrist_poses(), self._wrist_errors()]) self.last_obs = {"legs": legs_obs, "arms": arms_obs} @@ -367,10 +371,14 @@ def _gait_phase(self, cmd: np.ndarray) -> np.ndarray: angle = 2.0 * np.pi * phase return np.concatenate([np.sin(angle), np.cos(angle)]) if self.gait_phase_cos else np.sin(angle) - def _common_obs(self, state: RobotState) -> np.ndarray: - """Return the observation terms both agents share (ProprioCfg), in the env's order.""" + def _common_obs(self, state: RobotState, ee_targets: bool = True) -> np.ndarray: + """Return the observation terms both agents share (ProprioCfg), in the env's order. + + ee_targets False leaves the wrist targets out, for a legs actor trained without them. + """ cmd = self.observed_command() - ee_targets = np.concatenate([np.concatenate([p[:3], wxyz_to_xyzw(p[3:])]) for p in self.believed]) + ee_targets = (np.concatenate([np.concatenate([p[:3], wxyz_to_xyzw(p[3:])]) for p in self.believed]) + if ee_targets else np.zeros(0)) return np.concatenate([ state.acc, state.gyro, diff --git a/core_ws/src/lmg_deploy/policies/h12/arms.pt b/core_ws/src/lmg_deploy/policies/h12/arms.pt index 662c308d..586663a1 100644 --- a/core_ws/src/lmg_deploy/policies/h12/arms.pt +++ b/core_ws/src/lmg_deploy/policies/h12/arms.pt @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:12bcb69c589e5fd1c96dfa6e28025c8f34a4248eed953c32a8a5a22dc4a12df6 -size 582792 +oid sha256:20dd372616ca6b0ac3a83babf0189510fc275d8cb4c842b693b2af16c8f2878c +size 582984 diff --git a/core_ws/src/lmg_deploy/policies/h12/export.yaml b/core_ws/src/lmg_deploy/policies/h12/export.yaml index 3c18871e..5f236d4b 100644 --- a/core_ws/src/lmg_deploy/policies/h12/export.yaml +++ b/core_ws/src/lmg_deploy/policies/h12/export.yaml @@ -217,6 +217,9 @@ arm_residual_scales: - 0.0 - 0.0 max_ik_step: 0.05 +residual_cutoff_hz: 3.0 +ik_damping: 0.05 +clip_actions: 10.0 gait_period: 0.7 zero_cmd_eps: 0.03 command_ranges: @@ -237,7 +240,6 @@ legs_obs_terms: - projected_gravity - velocity_commands - arm_goal -- ee_targets - joint_pos - joint_vel - gait_phase @@ -258,7 +260,7 @@ arms_obs_terms: - wrist_poses - wrist_errors source: - checkpoint: /home/max/Locomanipulation_game/logs/skrl/locomanip_marl/2026-11-10_06-17-22_LegsBalanceAmp_s42/checkpoints/agent_200160.pt + checkpoint: /home/max/Locomanipulation_game/logs/skrl/locomanip_marl/2026-11-19_10-32-00_LegsEnergyDefault200k_s43/checkpoints/agent_200160.pt target_tables: /home/max/Locomanipulation_game/source/locomanipulation_game/locomanipulation_game/data/arm_target_tables_v3.pt - task: LocoManip-LegsBalanceAmp-v0 - exported: '2026-11-10T14:16:58' + task: LocoManip-LegsOnly-v0 + exported: '2026-11-19T21:49:51' diff --git a/core_ws/src/lmg_deploy/policies/h12/h12.yaml b/core_ws/src/lmg_deploy/policies/h12/h12.yaml index 208af996..f3acc7e4 100644 --- a/core_ws/src/lmg_deploy/policies/h12/h12.yaml +++ b/core_ws/src/lmg_deploy/policies/h12/h12.yaml @@ -59,7 +59,7 @@ torque_headroom: 0.85 # then: kp (target - q) - kd dq within this share of legs: policy: legs.pt - num_obs: 97 # 95 + each leg's gait phase cos + num_obs: 83 # 81 + each leg's gait phase cos; no wrist targets (export.yaml legs_obs_terms) num_actions: 12 action_scale: 0.25 # target = scale * action + default diff --git a/core_ws/src/lmg_deploy/policies/h12/legs.pt b/core_ws/src/lmg_deploy/policies/h12/legs.pt index 3e958bac..681a1620 100644 --- a/core_ws/src/lmg_deploy/policies/h12/legs.pt +++ b/core_ws/src/lmg_deploy/policies/h12/legs.pt @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:0b3de1bd77b8d5e429f856a5763784acbba7712b70352d07fe93de53bb20bf11 -size 523580 +oid sha256:79292a370883c09f031fb05cbac39c86f1f8fa3478ba821387272af2b7270207 +size 494972 diff --git a/core_ws/src/lmg_deploy/test/test_controller.py b/core_ws/src/lmg_deploy/test/test_controller.py index c3b1e214..5be8f509 100644 --- a/core_ws/src/lmg_deploy/test/test_controller.py +++ b/core_ws/src/lmg_deploy/test/test_controller.py @@ -623,6 +623,24 @@ def test_obs_terms_must_match_the_layout(): _controller(legs_obs_terms=legs, arms_obs_terms=LEGS_OBS_TERMS) +def test_legs_obs_may_leave_out_the_wrist_targets(): + """A legs term list without ee_targets builds the legs' observation 14 shorter, the rest in order; the arms' not.""" + legs = [t for t in LEGS_OBS_TERMS if t != "gait_phase_cos"] + arms = [t for t in ARMS_OBS_TERMS if t != "gait_phase_cos"] + full, cfg = _controller(legs_obs_terms=legs, arms_obs_terms=arms) + trimmed, _ = _controller(legs_obs_terms=[t for t in legs if t != "ee_targets"], arms_obs_terms=arms) + state = _state(cfg) + for ctrl in (full, trimmed): + ctrl.reset(state) + ctrl.step(state) + a, b = full.last_obs["legs"], trimmed.last_obs["legs"] + assert b.shape == (a.shape[0] - 14,) + np.testing.assert_allclose(b, np.concatenate([a[:13], a[27:]])) + np.testing.assert_allclose(trimmed.last_obs["arms"], full.last_obs["arms"]) + with pytest.raises(ValueError): + _controller(legs_obs_terms=legs, arms_obs_terms=[t for t in arms if t != "ee_targets"]) + + def test_arm_goals_refused_for_a_policy_that_never_trained_one(): """With arm_goals false, set_mode arm and request_arm_goal raise and the controller keeps navigating.""" ctrl, cfg = _controller(arm_goals=False)