Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 6 additions & 4 deletions core_ws/src/lmg_deploy/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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 |
Expand Down Expand Up @@ -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.
Expand Down
6 changes: 6 additions & 0 deletions core_ws/src/lmg_deploy/config/example.export.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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,
Expand Down
20 changes: 14 additions & 6 deletions core_ws/src/lmg_deploy/lmg_deploy/controller.py
Original file line number Diff line number Diff line change
Expand Up @@ -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"]
Expand Down Expand Up @@ -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, "
Expand Down Expand Up @@ -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}

Expand Down Expand Up @@ -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,
Expand Down
4 changes: 2 additions & 2 deletions core_ws/src/lmg_deploy/policies/h12/arms.pt
Git LFS file not shown
10 changes: 6 additions & 4 deletions core_ws/src/lmg_deploy/policies/h12/export.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand All @@ -237,7 +240,6 @@ legs_obs_terms:
- projected_gravity
- velocity_commands
- arm_goal
- ee_targets
- joint_pos
- joint_vel
- gait_phase
Expand All @@ -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'
2 changes: 1 addition & 1 deletion core_ws/src/lmg_deploy/policies/h12/h12.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
4 changes: 2 additions & 2 deletions core_ws/src/lmg_deploy/policies/h12/legs.pt
Git LFS file not shown
18 changes: 18 additions & 0 deletions core_ws/src/lmg_deploy/test/test_controller.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
Loading