diff --git a/core_ws/src/lmg_deploy/README.md b/core_ws/src/lmg_deploy/README.md index 73c26c9..110033d 100644 --- a/core_ws/src/lmg_deploy/README.md +++ b/core_ws/src/lmg_deploy/README.md @@ -24,8 +24,8 @@ 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`; 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/controller.py` | ROS-free env step: SLAM odometry → mode → observations (each actor's terms as the export's `legs_obs_terms` / `arms_obs_terms` list them, in the controller's order, any left out; all of them without the lists: legs 95, arms 123, 97 and 125 with `gait_phase_cos`) → joint targets | +| `lmg_deploy/kinematics.py` | Pinocchio wrist FK, Jacobian and IK step, in the pelvis frame; the arm joints' gravity torques and `arm_ik`'s seed solve | | `lmg_deploy/rotations.py` | numpy versions of the isaaclab.utils.math functions training used | | `lmg_deploy/networks.py` | TorchScript loader for the exported actors | | `lmg_deploy/livox_imu.py` | `/livox/imu` into the policy's IMU frame: the gyro low-pass, the specific force, and a complementary-filter attitude (the fallback, or `livox_attitude:=filter`) | @@ -35,7 +35,7 @@ the first latches it silent. | `config/policy_template.yaml` | the deploy config a new policy starts from: joint order, defaults, gains, scales, bounds, command ranges (each from the training env), and the SLAM body frame | | `policies//.yaml`, `{legs,arms}.pt`, `export.yaml` | the robot's one policy (`h12/`; `g1/` empty): its deploy config and what the export writes, below; the `.pt` files in Git LFS (`policies/.gitattributes`). One iteration at a time, old ones deleted (`policies/README.md`) | | `bags//` | rosbag2 recordings of Explicit500 on the real robot (`policy_bag_walk`: walking to nav2 goals; `reach`: an arm goal, ended by the hardware e-stop), the `.db3` files in Git LFS | -| `test/test_controller.py` | unit tests (rotations vs Isaac Lab, Jacobian, IK, observation layout, SLAM odometry, modes) on `test/data/explicit500.yaml`, Explicit500's deploy config | +| `test/test_controller.py` | unit tests (rotations vs Isaac Lab, Jacobian, IK, observation layout, SLAM odometry, modes, `arm_ik`) on `test/data/explicit500.yaml`, Explicit500's deploy config | | `test/sim_drive.py` | a scripted drive in the Robocasa sim (stand, walk, turn, side-step, arm goal), measured on FAST-LIO's `/Odometry` | | `test/test_heartbeat_gate.py` | a real node with the shipped policy on a non-zero DDS domain: silent, in `waiting_heartbeat`, until the first beat, then it pre-poses from the pose at the beat | | `test/test_livox_imu.py`, `test/test_livox_attitude.py` | unit tests of the two `/livox/imu` attitude paths, the gyro low-pass and the URDF mount | @@ -71,8 +71,25 @@ It writes: 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. + loads it over the policy's yaml and runs at 1 / `control_dt`. A new policy's yaml is the template with + `legs.num_obs` / `arms.num_obs` set to the networks' sizes (export_marl.py writes them from 2026-11-20). +- `imu.source: lowstate` (runs from 2026-11-20, trained on the torso IMU as /lowstate reports it): the node reads + `LowState.imu_state` (quaternion, accelerometer; the gyroscope through `imu.gyro_lowpass_hz`), untrimmed, and needs + neither `/livox/imu` nor FAST-LIO's attitude (marl_deploy_node.py's IMU section). Those runs' actors drop + `base_lin_acc` (`imu.acc: dropped`). +- `arm_goal_frame: pelvis` and `arm_goal_keeps_walking: true` (LocoManip-Marl-Direct-v0 from 2026-11-19): an arm goal + rides on the pelvis (the SLAM odometry doesn't move it) and the velocity command stays live through it. +- With an `arm_ik` block in `export.yaml` (runs whose arms trained with `mdp.IKPostureArmAction`: + LocoManip-ArmsOnly-v0, LocoManip-Marl-Direct-v0 from 2026-11-19), the arms run as that action term does, not by the + IK step above: with `trajectory`, the arm target moves from its own last value to a posture (each arm's joints + together, at `trajectory_max_vel` / `trajectory_max_acc`), the posture being the default angles or, with + `posture_from_command`, the joint angles of `arm_seed_table.npz`'s row nearest the arm goal (solved for the goal + itself with `seed_solve`; set once per goal); plus the arms' gravity torque at the target over kp + (`gravity_offset`; gravity in the pelvis frame from the IMU attitude), the pose-error integral (`integral`) and the + residual (integrated with `residual_integrate`, latched on the goal with `freeze`). `reset` (engage, band release) + starts the target at the measured arm angles. `config/example.export.yaml` documents every key; + `arm_seed_table` names a file in the export's folder (`policies//`, copied with the networks). Locomanipulation_game's + `mujoco_evals/mujoco_sim.py` (`MarlMujoco.process_actions`) is the same arm path in numpy. The export also writes `estimator.pt` and an `odometry` block in `export.yaml`; the deploy doesn't use them (its odometry is the SLAM pipeline's, below), so leave them out of `policies/`. diff --git a/core_ws/src/lmg_deploy/config/example.export.yaml b/core_ws/src/lmg_deploy/config/example.export.yaml index 0469bd2..f200105 100644 --- a/core_ws/src/lmg_deploy/config/example.export.yaml +++ b/core_ws/src/lmg_deploy/config/example.export.yaml @@ -52,6 +52,46 @@ legs_obs_terms: [base_lin_acc, base_ang_vel, projected_gravity, velocity_command arms_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, wrist_poses, wrist_errors] +# Runs whose arms trained with mdp.IKPostureArmAction (LocoManip-ArmsOnly-v0, LocoManip-Marl-Direct-v0 from 2026-11-19) +# also carry its settings and, with posture_from_command, arm_seed_table; the controller then runs the arms as that +# action term does (controller.py's module doc). The shipped policy has none, so they are documented here as comments +# (test_example_configs.py compares this file's keys with the shipped export.yaml's). Coexist200k_s42's: +# arm_ik: +# rot_weight: 0.3 # the rotation error's weight against position's (1 per m), the IK's and the integral's +# null_gain: 0.05 # without trajectory: share of the posture error the null-space pull asks per step +# posture_scale: 0.5 # rad of posture offset per action unit (posture_actions) +# posture_gain: 1.0 # times posture_scale +# limit_margin: 0.05 # rad: the posture (and the IK's joint locks) stay this far inside the target bounds +# ik_damping: 0.05 # the weighted DLS damping (integral, and the IK without trajectory) +# ik_passes: 3 # without trajectory: solves per step, locking the joints that would pass their bounds +# posture_from_command: true # the posture is the seed table's row nearest the arm command (else the default angles) +# trajectory: true # the target moves from its own last value to the posture (else one IK step from q) +# seed_rot_weight: 0.3 # m per rad of rotation in the seed lookup's distance +# posture_actions: false # true: the arms' action is 28 wide, the residual then the posture offset +# trajectory_max_vel: 2.0 # rad/s, each arm's fastest joint (null: max_ik_step per joint per step) +# trajectory_max_acc: 10.0 # rad/s^2, its velocity change (null: no limit) +# gravity_offset: true # the target carries minus each arm joint's gravity torque at it over kp +# residual_integrate: # the residual integrated, not low-passed (absent: residual_cutoff_hz's low-pass) +# gain: 0.02 # action units added per step per action unit +# leak_s: 5.0 # s, its leak toward 0 +# clamp: 1.0 # action units +# integral: # the measured wrists' pose error integrated onto the target once it has stopped +# gain: 0.01 # share of the weighted DLS step on the error added per step +# leak_s: 60.0 # s +# clamp: 0.3 # rad per joint +# freeze: # the residual latched once the wrists sit on the command +# pos_tol: 0.001 # m, each wrist +# rot_tol: 0.0873 # rad, each wrist +# settle_s: 0.5 # s on the goal, the target stopped, before the latch +# seed_solve: # the nearest row's angles solved for the commanded poses themselves, once per goal +# iters: 20 +# damping: 0.05 +# rot_weight: 0.5 +# joint_range_margin: 0.1 # rad inside the soft limits +# soft_limit_factor: 0.9 # not written by export_marl.py; default 0.9, the training asset's soft-limit factor +# arm_seed_table: arm_seed_table.npz # per arm the goal table's poses (poses_, pelvis frame, x y z qw qx qy qz) +# # and joint angles (joints_), next to export.yaml + # Where the export came from; marl_deploy_node logs it at start. source: checkpoint: /checkpoints/agent_.pt # the skrl checkpoint the actors came from diff --git a/core_ws/src/lmg_deploy/lmg_deploy/controller.py b/core_ws/src/lmg_deploy/lmg_deploy/controller.py index 14610b9..b8982dd 100644 --- a/core_ws/src/lmg_deploy/lmg_deploy/controller.py +++ b/core_ws/src/lmg_deploy/lmg_deploy/controller.py @@ -6,8 +6,10 @@ 1. pelvis odometry: move the arm command by how the pelvis moved since the last SLAM pose (ArmTargetsCommand.apply_pelvis_motion); 2. commands: advance the settle timer, switch mode; - 3. observations: the legs' (95) and the arms' (123), or 97 and 125 with - gait_phase_cos (each leg's phase cos after its sin); + 3. observations: each agent's terms in export.yaml's legs_obs_terms / + arms_obs_terms (OBS_TERMS' order, any of them left out), or all of them + (legs 95, arms 123; 97 and 125 with gait_phase_cos, each leg's phase cos + after its sin); 4. actions: clamp to +-clip, legs = scale * a + default, arms = q + IK step + residual_scale * r (each scale one number, or per joint from export.yaml's leg_action_scales / arm_residual_scales: runs that matched their scales to @@ -19,6 +21,37 @@ action. With torque_headroom set, each target is then kept where the PD torque it asks for at the measured state, kp (target - q) - kd qd, is within torque_headroom x effort_limits (mdp.actions.torque_bounds). + With export.yaml's arm_ik block the arms are mdp.IKPostureArmAction's + instead (below). + +arm_ik (mdp.IKPostureArmAction, LocoManip-ArmsOnly-v0 / LocoManip-Marl-Direct-v0 +runs; Locomanipulation_game's mujoco_evals/mujoco_sim.py MarlMujoco is the +numpy twin). Per step, from the arms' action a (14 wide, 28 with +posture_actions: the residual, then the posture offset): + - residual: integrated (residual_integrate: leak exp(-dt / leak_s), plus + gain x a, clamped to +-clamp) or low-passed as above; held while frozen; + - posture: per arm seed_q (posture_from_command: during an arm goal the + joint angles of arm_seed_table's row nearest the arm command, position and + rotation at seed_rot_weight m per rad, with seed_solve solved for the + command itself; set once per goal, as training draws it; a rest-pose goal + and navigation: the default angles), plus posture_scale x posture_gain x + the offset, inside the target bounds narrowed by limit_margin; + - trajectory: the target ik_q moves from its own last value (not the + measured angles) to the posture, each arm's joints together at + trajectory_max_vel / trajectory_max_acc (without them: max_ik_step per + joint); without trajectory, one limit-aware, posture-seeking IK step from + the measured angles (_posture_step); + - target: ik_q + gravity offset (gravity_offset: minus each arm joint's + gravity torque at ik_q over its kp, gravity in the pelvis frame from the + IMU attitude carried through imu.frame's pose) + integral + scale x + residual, then the bounds above; + - integral: leaks (integral.leak_s); once ik_q has arrived at the posture, + integral.gain x each arm's rot_weight-weighted DLS step on the measured + wrist's error is added; clamped to +-integral.clamp; + - freeze: the residual is latched once ik_q has arrived and both wrists have + been within freeze.pos_tol / rot_tol of the arm command for settle_s; + released when the posture moves or the arm goal ends. +reset() starts ik_q at the measured arm angles with everything else zero. Odometry is the SLAM pipeline's: set_slam_pose() takes FAST-LIO's body pose (camera_init -> body; the body is cfg["slam_frame"], the IMU frame FAST-LIO @@ -64,13 +97,16 @@ NUM_LEGS = 12 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; 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). +# The observation terms step() can build, in order (gait_phase_cos only with cfg["gait_phase_cos"]). An export's +# legs_obs_terms / arms_obs_terms name the ones its actors saw, in this order, any left out: LocoManip-LegsOnly-v0's +# legs from 2026-11-16 drop ee_targets, from 2026-11-19 arm_goal, and every actor from 2026-11-20 base_lin_acc (the +# torso IMU's runs); LocoManip-ArmsOnly-v0's arms drop the IMU, the velocity command and the gait clock. 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"] ARMS_OBS_TERMS = COMMON_OBS_TERMS + ["actions", "wrist_poses", "wrist_errors"] +GRAVITY = 9.81 # m/s^2, the training sim's +SOFT_LIMIT_FACTOR = 0.9 # the training asset's soft_joint_pos_limit_factor (H1_2_MAGPIE_CFG): the seed solve's range @dataclass @@ -131,14 +167,8 @@ 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) - 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, " - f"{expected}") + self.obs_terms = {agent: self._agent_terms(cfg.get(f"{agent}_obs_terms"), terms) + for agent, terms in (("legs", LEGS_OBS_TERMS), ("arms", ARMS_OBS_TERMS))} ranges = cfg["command_ranges"] self.cmd_low = np.array([ranges["lin_vel_x"][0], ranges["lin_vel_y"][0], ranges["ang_vel_z"][0]]) self.cmd_high = np.array([ranges["lin_vel_x"][1], ranges["lin_vel_y"][1], ranges["ang_vel_z"][1]]) @@ -149,9 +179,19 @@ def __init__(self, cfg: dict, kinematics: ArmKinematics, legs, arms): self.torque_headroom = cfg.get("torque_headroom") if self.torque_headroom is not None: self.effort_limits = np.asarray(cfg["effort_limits"], dtype=np.float64) - cutoff = cfg["arms"].get("residual_cutoff_hz") + # export.yaml's residual_cutoff_hz (the run's) over the template's + cutoff = cfg.get("residual_cutoff_hz", cfg["arms"].get("residual_cutoff_hz")) + # LocoManip-Marl-Direct-v0's arm goals (export.yaml, from its ArmTargetsCommandCfg): arm_goal_frame "pelvis" + # rides the goal on the pelvis (pelvis_fixed_goals: no odometry moves it), arm_goal_keeps_walking keeps the + # operator's velocity command through an arm goal (independent_of_legs) + frame = cfg.get("arm_goal_frame", "world") + if frame not in ("world", "pelvis"): + raise ValueError(f"arm_goal_frame must be 'world' or 'pelvis', not {frame!r}") + self.arm_goal_on_pelvis = frame == "pelvis" + self.arm_goal_keeps_walking = bool(cfg.get("arm_goal_keeps_walking", False)) self.filter_alpha = 1.0 if cutoff is None else 1.0 - np.exp(-2.0 * np.pi * float(cutoff) * self.dt) self.slam_frame = cfg["slam_frame"] + self._init_arm_ik(cfg) self._slam_pose: tuple[np.ndarray, np.ndarray] | None = None # latest body pose, SLAM world frame if len(self.default) != NUM_JOINTS or len(self.kp) != NUM_JOINTS or len(self.kd) != NUM_JOINTS: raise ValueError("default_joint_pos, kp and kd need one entry per policy joint (27)") @@ -177,8 +217,15 @@ def reset(self, state: RobotState | None) -> None: # The action terms' state after a reset: the legs see the applied target in action units, the arms zero. q_legs = state.q[:NUM_LEGS] if state is not None else self.default[:NUM_LEGS] self.legs_last = (q_legs - self.default[:NUM_LEGS]) / self.legs_scale - self.arms_last = np.zeros(NUM_ARM_JOINTS) - self.residual = np.zeros(NUM_ARM_JOINTS) # the arms' action after the low-pass + self.arms_last = np.zeros(self.n_arm_actions) + self.residual = np.zeros(self.n_arm_actions) # the arms' action after the low-pass (or integration) + # arm_ik's state, as IKPostureArmAction.reset leaves it: the target from the measured arm angles, at rest + self.ik_q = (state.q if state is not None else self.default)[ARM_START:].copy() + self.traj_v = np.zeros(NUM_ARM_JOINTS) # the trajectory's velocity (rad/s) + self.integral = np.zeros(NUM_ARM_JOINTS) # the pose-error integral (rad) + self.frozen, self.settled_steps = False, 0 # the residual latched (arm_ik.freeze) + self.posture = self.default[ARM_START:].copy() + self.seed_q = self.default[ARM_START:].copy() # (14,) the posture's base: navigation's, the default angles self._prev_pelvis: tuple[np.ndarray, np.ndarray] | None = None # pelvis pose at the last SLAM pose used self.last_obs: dict[str, np.ndarray] = {} for network in (self.legs, self.arms): @@ -209,6 +256,7 @@ def request_arm_goal(self, poses: np.ndarray) -> None: goal[arm, 3:] = quat_unique(goal[arm, 3:] / np.linalg.norm(goal[arm, 3:])) if self.mode == Mode.ARM: self.believed = goal # a goal replacing a goal has no settle, as in training + self._update_seeds() elif self.settle_to_arm_s > 0.0: self.pending_goal = goal self.mode, self.timer = Mode.SETTLE_TO_ARM, self.settle_to_arm_s @@ -227,6 +275,7 @@ def set_arm_goals(self, poses: np.ndarray) -> None: self.arm_goals = goal if self.mode == Mode.ARM: self.believed = goal.copy() + self._update_seeds() elif self.mode == Mode.SETTLE_TO_ARM: self.pending_goal = goal.copy() @@ -250,6 +299,7 @@ def end_arm_goal(self) -> None: """Return to navigation, through settle_to_nav_s from an arm goal; a pending goal is dropped.""" if self.mode == Mode.ARM: self.believed = self.rest_pose.copy() + self._update_seeds() if self.settle_to_nav_s > 0.0: self.mode, self.timer = Mode.SETTLE_TO_NAV, self.settle_to_nav_s else: @@ -265,11 +315,8 @@ def step(self, state: RobotState) -> JointCommand: self._update_mode() self._filter_command() - common = self._common_obs(state) - 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} + self.last_obs = self.observations(state) + legs_obs, arms_obs = self.last_obs["legs"], self.last_obs["arms"] legs_action = np.clip(self.legs(legs_obs), -self.clip, self.clip) arms_action = np.clip(self.arms(arms_obs), -self.clip, self.clip) @@ -277,14 +324,17 @@ def step(self, state: RobotState) -> JointCommand: target = self.default.copy() legs_target = legs_action * self.legs_scale + self.default[:NUM_LEGS] target[:NUM_LEGS] = self._torque_bound(self._bound(legs_target, slice(0, NUM_LEGS)), state, slice(0, NUM_LEGS)) - ik = state.q[ARM_START:].copy() - for arm in range(self.num_arms): - cols = self.kin.arm_cols[arm] - ARM_START - ik[cols] += self.kin.ik_step(arm, self.believed[arm, :3], self.believed[arm, 3:], - self.ik_damping, self.max_ik_step) - self.residual = self.residual + self.filter_alpha * (arms_action - self.residual) arms = slice(ARM_START, NUM_JOINTS) - target[arms] = self._torque_bound(self._bound(ik + self.residual * self.arms_scale, arms), state, arms) + if self.arm_ik is not None: + target[arms] = self._posture_arms(state, arms_action) + else: + ik = state.q[ARM_START:].copy() + for arm in range(self.num_arms): + cols = self.kin.arm_cols[arm] - ARM_START + ik[cols] += self.kin.ik_step(arm, self.believed[arm, :3], self.believed[arm, 3:], + self.ik_damping, self.max_ik_step) + self.residual = self.residual + self.filter_alpha * (arms_action - self.residual) + target[arms] = self._torque_bound(self._bound(ik + self.residual * self.arms_scale, arms), state, arms) # What each agent observes of its action next step (mdp.applied_action). self.legs_last = (target[:NUM_LEGS] - self.default[:NUM_LEGS]) / self.legs_scale @@ -292,6 +342,221 @@ def step(self, state: RobotState) -> JointCommand: self.steps += 1 return JointCommand(q=target, kp=self.kp.copy(), kd=self.kd.copy()) + # -- arm_ik: mdp.IKPostureArmAction ------------------------------------------ + + def _init_arm_ik(self, cfg: dict) -> None: + """Read export.yaml's arm_ik block (mujoco_evals/common.py read_deploy_settings writes it), if any.""" + self.arm_ik = cfg.get("arm_ik") or None + c = self.arm_ik or {} + self.n_arm_actions = 2 * NUM_ARM_JOINTS if self.arm_ik and c.get("posture_actions", True) else NUM_ARM_JOINTS + num_actions = cfg["arms"].get("num_actions") + if num_actions is not None and int(num_actions) != self.n_arm_actions: + raise ValueError(f"arms.num_actions {num_actions}, but arm_ik's action is {self.n_arm_actions} wide") + if self.arm_ik is None: + return + if self.num_arms != 2: + raise ValueError("arm_ik needs two arms") + if c.get("trajectory") and c.get("trajectory_max_acc") is not None and c.get("trajectory_max_vel") is None: + raise ValueError("arm_ik.trajectory_max_acc needs trajectory_max_vel") + if (c.get("integral") or c.get("freeze")) and not c.get("trajectory"): + raise ValueError("arm_ik.integral and arm_ik.freeze need trajectory") + arms = slice(ARM_START, NUM_JOINTS) + self.ik_lower = self.lower[arms] + float(c["limit_margin"]) + self.ik_upper = self.upper[arms] - float(c["limit_margin"]) + self.row_weight = np.array([1.0, 1.0, 1.0, c["rot_weight"], c["rot_weight"], c["rot_weight"]], dtype=np.float64) + self.settle_steps = max(1, int(round(float(c["freeze"]["settle_s"]) / self.dt))) if c.get("freeze") else 0 + # the goal table's poses and joint angles per arm (export.yaml arm_seed_table; marl_deploy_node makes the path + # absolute) + self.seed_table = None + if c.get("posture_from_command"): + table = np.load(cfg["arm_seed_table"]) + self.seed_table = [(table[f"poses_{arm}"].astype(np.float64), table[f"joints_{arm}"].astype(np.float64)) + for arm in range(self.num_arms)] + if c.get("seed_solve") and self.seed_table is None: + raise ValueError("arm_ik.seed_solve needs posture_from_command (the seed table)") + if c.get("seed_solve"): + # the solve's range: training's soft limits (SOFT_LIMIT_FACTOR of the hard range) narrowed by the margin + factor = float(c["seed_solve"].get("soft_limit_factor", SOFT_LIMIT_FACTOR)) + hard = self.kin.joint_limits + mid, half = hard.mean(axis=1), 0.5 * (hard[:, 1] - hard[:, 0]) + margin = float(c["seed_solve"].get("joint_range_margin", 0.0)) + self.solve_low, self.solve_high = mid - factor * half + margin, mid + factor * half - margin + # the IMU frame the state's attitude is in, for the gravity offset's pelvis-frame gravity + self.imu_frame = (cfg.get("imu") or {}).get("frame", "imu_link") + if c.get("gravity_offset") and self.imu_frame not in self.kin.extra_frames: + raise ValueError(f"arm_ik.gravity_offset needs the IMU frame {self.imu_frame!r} among the kinematics' frames") + + def _update_seeds(self) -> None: + """Set seed_q for the arm command just set (ArmTargetsCommand draws it once per goal): during an arm goal the + seed table's nearest rows, with seed_solve solved for the command; the rest pose and navigation: default.""" + default = self.default[ARM_START:].copy() + if (self.arm_ik is None or self.seed_table is None or self.mode != Mode.ARM + or np.allclose(self.believed, self.rest_pose, atol=1e-9)): + self.seed_q = default + return + seed = self.default.copy() + for arm in range(self.num_arms): + seed[self.kin.arm_cols[arm]] = self._nearest_row(arm) + if self.arm_ik.get("seed_solve"): + c = self.arm_ik["seed_solve"] + seed = self.kin.solve_arms(self.believed, seed, self.solve_low, self.solve_high, iters=int(c["iters"]), + damping=float(c["damping"]), rot_weight=float(c["rot_weight"])) + self.seed_q = seed[ARM_START:].copy() + + def _nearest_row(self, arm: int) -> np.ndarray: + """Return the joint angles of the arm's seed-table row nearest its command: squared position distance plus + (seed_rot_weight x rotation angle)^2, the angle from 8 (1 - |q . q_row|) (ArmTargetsCommand.nearest_joints).""" + poses, joints = self.seed_table[arm] + b = self.believed[arm] + w2 = float(self.arm_ik["seed_rot_weight"]) ** 2 + d = ((poses[:, :3] - b[:3]) ** 2).sum(axis=1) + w2 * 8.0 * (1.0 - np.abs(poses[:, 3:] @ b[3:])) + return joints[int(np.argmin(d))] + + def _posture_arms(self, state: RobotState, action: np.ndarray) -> np.ndarray: + """Return the arms' 14 targets from their action, IKPostureArmAction.process_actions (module doc).""" + c, n = self.arm_ik, NUM_ARM_JOINTS + if action.shape != (self.n_arm_actions,): + raise ValueError(f"arms action of {action.shape}, arm_ik expects ({self.n_arm_actions},)") + if not self.frozen: + ri = c.get("residual_integrate") + if ri: + leak = np.exp(-self.dt / float(ri["leak_s"])) + self.residual = np.clip(leak * self.residual + float(ri["gain"]) * action, + -float(ri["clamp"]), float(ri["clamp"])) + else: + self.residual = self.residual + self.filter_alpha * (action - self.residual) + offset = self.residual[n:] if self.n_arm_actions == 2 * n else 0.0 + self.posture = np.clip(self.seed_q + float(c["posture_scale"]) * float(c["posture_gain"]) * offset, + self.ik_lower, self.ik_upper) + arms = slice(ARM_START, NUM_JOINTS) + if c.get("trajectory"): + if c.get("trajectory_max_vel") is None: + ik = self.ik_q + np.clip(self.posture - self.ik_q, -self.max_ik_step, self.max_ik_step) + else: + ik = self._synchronized_step() + self.ik_q = self._bound(ik, arms) + if c.get("integral"): + self._update_integral() + target = self.ik_q + self._gravity_offset(state, self.ik_q) + self.integral + self.arms_scale * self.residual[:n] + target = self._torque_bound(self._bound(target, arms), state, arms) + if c.get("freeze"): + self._update_freeze() + return target + ik = state.q[arms].copy() + for arm in range(self.num_arms): + cols = self.kin.arm_cols[arm] - ARM_START + ik[cols] += self._posture_step(arm, ik[cols], cols) + self.ik_q = self._bound(ik, arms) # where a trajectory would start from + target = ik + self._gravity_offset(state, ik) + self.arms_scale * self.residual[:n] + return self._torque_bound(self._bound(target, arms), state, arms) + + def _synchronized_step(self) -> np.ndarray: + """Return the trajectory's next target: per arm, all joints along the straight line to the posture, the fastest + at trajectory_max_vel, slowed to stop there, its velocity changed by at most trajectory_max_acc x dt.""" + c, dt = self.arm_ik, self.dt + vmax = float(c["trajectory_max_vel"]) + amax = None if c.get("trajectory_max_acc") is None else float(c["trajectory_max_acc"]) + ik = self.ik_q.copy() + for arm in range(self.num_arms): + cols = self.kin.arm_cols[arm] - ARM_START + delta = self.posture[cols] - self.ik_q[cols] + dist = max(float(np.abs(delta).max()), 1e-9) + cap = min(vmax, np.sqrt(2.0 * amax * dist)) if amax is not None else vmax + v = delta / dt * min(1.0, cap * dt / dist) + if amax is not None: + dv = v - self.traj_v[cols] + v = self.traj_v[cols] + dv * min(1.0, amax * dt / max(float(np.abs(dv).max()), 1e-12)) + self.traj_v[cols] = v + ik[cols] = self.ik_q[cols] + v * dt + return ik + + def _arrived(self) -> bool: + """Return whether the trajectory's target has stopped at the posture.""" + return bool(np.abs(self.ik_q - self.posture).max() < 1e-4 and np.abs(self.traj_v).max() < 1e-3) + + def _wrist_error(self, arm: int) -> np.ndarray: + """Return the arm's measured wrist (position, axis-angle) error to the arm command, pelvis frame.""" + pos, quat = self.kin.wrist_pose(arm) + return np.concatenate(compute_pose_error(pos, quat, self.believed[arm, :3], self.believed[arm, 3:])) + + def _update_integral(self) -> None: + """Leak the pose-error integral; once arrived, add gain x each arm's weighted DLS step on its wrist's error.""" + c, ik = self.arm_ik["integral"], self.arm_ik + self.integral *= np.exp(-self.dt / float(c["leak_s"])) + if self._arrived(): + damp = float(ik["ik_damping"]) ** 2 * np.eye(6) + for arm in range(self.num_arms): + jac = self.kin.jacobian(arm) * self.row_weight[:, None] + err = self._wrist_error(arm) * self.row_weight + cols = self.kin.arm_cols[arm] - ARM_START + self.integral[cols] += float(c["gain"]) * (jac.T @ np.linalg.solve(jac @ jac.T + damp, err)) + self.integral = np.clip(self.integral, -float(c["clamp"]), float(c["clamp"])) + + def _update_freeze(self) -> None: + """Latch the residual once arrived with both wrists on the command for settle_s; release when the posture + moves or the arm goal ends.""" + f = self.arm_ik["freeze"] + arrived, arm_mode = self._arrived(), self.mode == Mode.ARM + self.frozen = self.frozen and arrived and arm_mode + on_goal = arm_mode + for arm in range(self.num_arms): + err = self._wrist_error(arm) + on_goal = on_goal and np.linalg.norm(err[:3]) < float(f["pos_tol"]) and np.linalg.norm(err[3:]) < float(f["rot_tol"]) + self.settled_steps = self.settled_steps + 1 if (arrived and on_goal and not self.frozen) else 0 + self.frozen = self.frozen or self.settled_steps >= self.settle_steps + + def gravity_b(self, state: RobotState) -> np.ndarray: + """Return gravity (m/s^2) in the pelvis frame: the IMU's projected gravity carried through imu_frame's pose.""" + _, q_pelvis_imu = self.kin.frame_pose(self.imu_frame) + return GRAVITY * quat_apply(q_pelvis_imu, projected_gravity(state.quat)) + + def _gravity_offset(self, state: RobotState, arm_q: np.ndarray) -> np.ndarray | float: + """Return minus each arm joint's gravity torque at arm_q over its kp (arm_ik.gravity_offset), else 0.""" + if not self.arm_ik.get("gravity_offset"): + return 0.0 + q = self.default.copy() # training's ArmKinematics holds the torso at its default angle + q[ARM_START:] = arm_q + tau = self.kin.gravity_torques(q, self.gravity_b(state))[ARM_START:] + return -tau / self.kp[ARM_START:] + + def _posture_step(self, arm: int, q: np.ndarray, cols: np.ndarray) -> np.ndarray: + """Return one arm's IKPostureArmAction._step from its measured angles q: the weighted DLS task step and the + null-space pull toward the posture, joints that would pass limit_margin short of their bounds locked and solved + out, ik_passes times, capped at max_ik_step.""" + c = self.arm_ik + jac = self.kin.jacobian(arm) * self.row_weight[:, None] + err = self._wrist_error(arm) * self.row_weight + pull = float(c["null_gain"]) * (self.posture[cols] - q) + low, high = self.ik_lower[cols], self.ik_upper[cols] + damp = float(c["ik_damping"]) ** 2 * np.eye(6) + free = np.ones(len(q), dtype=bool) + for _ in range(int(c["ik_passes"])): + f = free.astype(np.float64) + jf = jac * f[None, :] + pinv = jf.T @ np.linalg.inv(jf @ jf.T + damp) + step = (pinv @ err + (np.eye(len(q)) - pinv @ jf) @ (pull * f)) * f + step = np.clip(step, -self.max_ik_step, self.max_ik_step) + new = q + step + hit = ((new < low) & (step < 0)) | ((new > high) & (step > 0)) + if not (hit & free).any(): + break + free &= ~hit + return step + + # -- observations and bounds ----------------------------------------------- + + def _agent_terms(self, trained, terms: list[str]) -> list[str]: + """Return the terms an actor observes: trained (an export's list), checked to be terms, in order, or all.""" + full = [t for t in terms if t != "gait_phase_cos" or self.gait_phase_cos] + if trained is None: + return full + trained = list(trained) + unknown = [t for t in trained if t not in full] + order = [full.index(t) for t in trained if t in full] + if unknown or order != sorted(set(order)): + raise ValueError(f"obs terms {trained} are not observations this controller builds in its order {full}") + return trained + def _position_bounds(self, hard: np.ndarray, margin: float, target_limits: dict | None): """Return (lower, upper) per joint (rad): the hard limits widened by margin, narrowed to target_limits.""" lower, upper = hard[:, 0] - margin, hard[:, 1] + margin @@ -323,6 +588,7 @@ def _start_arm_goal(self, goal: np.ndarray) -> None: self.believed = goal self.pending_goal = None self.mode, self.timer = Mode.ARM, 0.0 + self._update_seeds() def _update_mode(self) -> None: """Count down a settle by one step and finish it when it runs out.""" @@ -337,8 +603,10 @@ def _update_mode(self) -> None: self.mode = Mode.NAV def velocity_command(self) -> np.ndarray: - """Return the velocity command the policy sees: the operator's while navigating, else zero.""" - return self.cmd_vel.copy() if self.mode == Mode.NAV else np.zeros(3) + """Return the velocity command the policy sees: the operator's while navigating (and through an arm goal with + arm_goal_keeps_walking), else zero.""" + walking = self.mode == Mode.NAV or self.arm_goal_keeps_walking + return self.cmd_vel.copy() if walking else np.zeros(3) def observed_command(self) -> np.ndarray: """Return the command the policies see and the gait clock reads: velocity_command low-passed (command_filter_tau).""" @@ -348,7 +616,7 @@ def _filter_command(self) -> None: """Step the command low-pass once toward velocity_command; zero it during an arm goal.""" if self.cmd_alpha is None: return - if self.mode == Mode.ARM: + if self.mode == Mode.ARM and not self.arm_goal_keeps_walking: self.cmd_filtered[:] = 0.0 # training zeroes it with the command when an arm goal starts else: self.cmd_filtered += self.cmd_alpha * (self.velocity_command() - self.cmd_filtered) @@ -371,25 +639,27 @@ 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, 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. - """ + def observations(self, state: RobotState) -> dict[str, np.ndarray]: + """Return each agent's observation vector at the measured state: its terms (obs_terms) in the env's order.""" cmd = self.observed_command() - 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, - projected_gravity(state.quat), - cmd, - [1.0 if self.mode == Mode.ARM else 0.0], - ee_targets, - state.q - self.default, - state.dq, - self._gait_phase(cmd), - ]) + phase = self._gait_phase(cmd) + terms = { + "base_lin_acc": state.acc, + "base_ang_vel": state.gyro, + "projected_gravity": projected_gravity(state.quat), + "velocity_commands": cmd, + "arm_goal": np.array([1.0 if self.mode == Mode.ARM else 0.0]), + "ee_targets": np.concatenate([np.concatenate([p[:3], wxyz_to_xyzw(p[3:])]) for p in self.believed]), + "joint_pos": state.q - self.default, + "joint_vel": state.dq, + "gait_phase": phase[:2], + "gait_phase_cos": phase[2:], + } + legs = {**terms, "actions": self.legs_last} + arms = {**terms, "actions": self.arms_last, "wrist_poses": self._wrist_poses(), + "wrist_errors": self._wrist_errors()} + return {"legs": np.concatenate([legs[t] for t in self.obs_terms["legs"]]), + "arms": np.concatenate([arms[t] for t in self.obs_terms["arms"]])} def _wrist_poses(self) -> np.ndarray: """Return each wrist's pelvis-frame pose as position and xyzw quaternion, concatenated.""" @@ -429,7 +699,7 @@ def _update_odometry(self) -> None: def _move_goal(self, delta_pos: np.ndarray, delta_quat: np.ndarray) -> None: """Keep the arm goal fixed in the world through a pelvis motion delta (previous pelvis frame).""" - if self.mode not in (Mode.ARM, Mode.SETTLE_TO_ARM): + if self.mode not in (Mode.ARM, Mode.SETTLE_TO_ARM) or self.arm_goal_on_pelvis: return # A world-fixed target: T_prev o c_prev = T_new o c_new, so c_new = delta^-1 o c_prev. poses = self.believed if self.mode == Mode.ARM else self.pending_goal diff --git a/core_ws/src/lmg_deploy/lmg_deploy/kinematics.py b/core_ws/src/lmg_deploy/lmg_deploy/kinematics.py index 8f21369..61e2fbd 100644 --- a/core_ws/src/lmg_deploy/lmg_deploy/kinematics.py +++ b/core_ws/src/lmg_deploy/lmg_deploy/kinematics.py @@ -14,6 +14,12 @@ The model is the H1-2 URDF with a fixed base at the pelvis: every pose and Jacobian here is relative to the pelvis, as in training (PhysX world-frame Jacobian rotated into the root frame). + +For mdp.IKPostureArmAction (export.yaml arm_ik) it also gives the arm joints' +gravity torques (ArmKinematics.gravity_torques in Locomanipulation_game's +mdp/arm_kinematics.py) and the seed solve (its ArmKinematics.solve, the wrist +frame's origin, not the COM), on their own Pinocchio data so the measured +state's poses and Jacobians stay as update() left them. """ import xml.etree.ElementTree as ET @@ -60,6 +66,12 @@ def __init__(self, urdf_path: str, joint_names: list[str], ee_frames: list[str], self.joint_limits = np.stack([lower, upper], axis=1) # (num joints, 2), policy order self._jacobians: list[np.ndarray] = [] self._poses: list[tuple[np.ndarray, np.ndarray]] = [] + self.extra_frames = list(self._extra_ids) + # gravity_torques sets its model's gravity per call: a model of its own; solve_arms works on its own data + self._gmodel = pin.buildModelFromUrdf(urdf_path) + self._gdata = self._gmodel.createData() + self._sdata = self.model.createData() + self._v_idx = np.array([self.model.joints[self.model.getJointId(n)].idx_v for n in joint_names]) def update(self, q: np.ndarray) -> None: """Compute the poses and wrist Jacobians at joint angles q (rad, policy order).""" @@ -91,6 +103,53 @@ def frame_pose(self, name: str) -> tuple[np.ndarray, np.ndarray]: """Return an extra frame's (position m, quaternion wxyz) in the pelvis frame at the last update().""" return self._extra_poses[name] + def jacobian(self, arm: int) -> np.ndarray: + """Return the arm's (6, 7) wrist Jacobian at the last update(): pelvis frame, linear rows at the wrist COM.""" + return self._jacobians[arm] + + def gravity_torques(self, q: np.ndarray, gravity_b: np.ndarray) -> np.ndarray: + """Return the torque gravity puts on each joint (Nm, policy order) at joint angles q (rad, policy order; the + joints the policy leaves out, the grippers', at 0), gravity_b the gravity vector (m/s^2) in the pelvis frame. + + Training's ArmKinematics.gravity_torques for the arm joints (a PD holds q with its target at q - tau / kp). + """ + q_pin = pin.neutral(self._gmodel) + q_pin[self._q_idx] = q + pin.framesForwardKinematics(self._gmodel, self._gdata, q_pin) + r_root_pelvis = self._gdata.oMf[self._pelvis_id].rotation + self._gmodel.gravity.linear = r_root_pelvis @ np.asarray(gravity_b, dtype=np.float64) + # Pinocchio's generalized gravity is the torque that holds q: minus the torque gravity puts on each joint + return -pin.computeGeneralizedGravity(self._gmodel, self._gdata, q_pin)[self._v_idx] + + def solve_arms(self, goals: np.ndarray, q0: np.ndarray, low: np.ndarray, high: np.ndarray, iters: int, + damping: float, rot_weight: float, max_step: float = 0.2) -> np.ndarray: + """Return q0 (policy order) with each arm's joints solved for its wrist goal (arms, 7: x y z qw qx qy qz, + pelvis frame), from q0's: training's ArmKinematics.solve, damped least squares on the pose error [position, + rot_weight x axis-angle] at the wrist frame's origin, each step capped at max_step per joint and clamped into + [low, high] (policy order); the other joints held at q0's.""" + q = np.asarray(q0, dtype=np.float64).copy() + w = np.array([1.0, 1.0, 1.0, rot_weight, rot_weight, rot_weight]) + damp = damping ** 2 * np.eye(6) + q_pin = pin.neutral(self.model) + data = self._sdata + for _ in range(iters): + q_pin[self._q_idx] = q + pin.computeJointJacobians(self.model, data, q_pin) + pin.updateFramePlacements(self.model, data) + o_m_pelvis = data.oMf[self._pelvis_id] + r_pelvis_t = o_m_pelvis.rotation.T + for arm, fid in enumerate(self._ee_ids): + pelvis_m_ee = o_m_pelvis.actInv(data.oMf[fid]) + pos_err, rot_err = compute_pose_error(pelvis_m_ee.translation, quat_from_matrix(pelvis_m_ee.rotation), + goals[arm, :3], goals[arm, 3:]) + jac = pin.getFrameJacobian(self.model, data, fid, pin.LOCAL_WORLD_ALIGNED)[:, self._v_cols[arm]] + jac = np.vstack([r_pelvis_t @ jac[:3], r_pelvis_t @ jac[3:]]) * w[:, None] + err = np.concatenate([pos_err, rot_err]) * w + step = jac.T @ np.linalg.solve(jac @ jac.T + damp, err) + cols = self.arm_cols[arm] + q[cols] = np.clip(q[cols] + np.clip(step, -max_step, max_step), low[cols], high[cols]) + return q + def ik_step(self, arm: int, target_pos: np.ndarray, target_quat: np.ndarray, damping: float, max_step: float) -> np.ndarray: """Return one arm's capped DLS joint step (rad) toward a pelvis-frame wrist pose, in arm_cols order.""" diff --git a/core_ws/src/lmg_deploy/lmg_deploy/marl_deploy_node.py b/core_ws/src/lmg_deploy/lmg_deploy/marl_deploy_node.py index 51094a0..3fd69b0 100644 --- a/core_ws/src/lmg_deploy/lmg_deploy/marl_deploy_node.py +++ b/core_ws/src/lmg_deploy/lmg_deploy/marl_deploy_node.py @@ -59,9 +59,19 @@ IMU --- -The policy's IMU observations (angular velocity, specific force, projected -gravity) come from the MID360's /livox/imu, never LowState.imu_state, whose -attitude carries a calibration error of about 2.5 deg. /livox/imu is in the +An export whose ``imu.source`` is ``lowstate`` (Locomanipulation_game runs from +2026-11-20, export.yaml imu: {source: lowstate, frame: imu_link, ...}) trained +on the torso IMU as /lowstate reports it, its quaternion's bias drawn around the +robot's measured one: the node takes LowState.imu_state's quaternion (wxyz) and +accelerometer as reported and its gyroscope through the GyroLowpass the run +trained with (``imu.gyro_lowpass_hz``, sampled at every /lowstate by its tick), +with no trim (``imu_offset_*`` are ignored), and needs neither /livox/imu nor +FAST-LIO's attitude. + +Otherwise the policy's IMU observations (angular velocity, specific force, +projected gravity) come from the MID360's /livox/imu, never LowState.imu_state, +whose attitude carries a calibration error of about 2.5 deg (which those +policies did not train with). /livox/imu is in the URDF's livox_imu_link (the IMU chip), the MID-360's raw frame, upside down on the H1-2's head. @@ -119,7 +129,7 @@ ) from lmg_deploy.kinematics import ArmKinematics from lmg_deploy.livox_attitude import LivoxAttitude, in_imu_frame -from lmg_deploy.livox_imu import LIVOX_IMU_FRAME, LivoxImu, livox_to_imu_rotation +from lmg_deploy.livox_imu import LIVOX_IMU_FRAME, GyroLowpass, LivoxImu, livox_to_imu_rotation from lmg_deploy.networks import load_network from lmg_deploy.rotations import ( imu_mount_quat, @@ -183,6 +193,9 @@ def __init__(self): with open(export_path) as f: cfg.update(yaml.safe_load(f) or {}) self.get_logger().info(f"export: {cfg.get('source')}") + # arm_ik's goal table (arm_seed_table.npz), named relative to the export's folder + if cfg.get("arm_seed_table"): + cfg["arm_seed_table"] = os.path.join(cfg_dir, cfg["arm_seed_table"]) control_hz = float(self.get_parameter("control_hz").value) or 1.0 / float(cfg["control_dt"]) if abs(1.0 / control_hz - float(cfg["control_dt"])) > 1e-6: raise ValueError(f"control_hz {control_hz} doesn't match the policy's control_dt {cfg['control_dt']}") @@ -204,6 +217,13 @@ def __init__(self): self._livox = LivoxImu(r_imu_livox, tau_s=float(self.get_parameter("livox_gravity_tau_s").value), gyro_lowpass_hz=gyro_lowpass_hz) self._gyro_lowpass_hz = gyro_lowpass_hz + # /lowstate's torso IMU (imu.source lowstate): its gyroscope through the run's low-pass, per /lowstate sample + self._lowstate_imu = (cfg.get("imu") or {}).get("source") == "lowstate" + if self._lowstate_imu and imu_frame != "imu_link": + raise ValueError(f"imu.source lowstate needs imu.frame imu_link (the torso IMU's), not {imu_frame!r}") + self._lowstate_lowpass = GyroLowpass(gyro_lowpass_hz) if self._lowstate_imu and gyro_lowpass_hz > 0 else None + self._lowstate_gyro: np.ndarray | None = None + self._lowstate_tick: int | None = None self._livox_time: float | None = None # monotonic receipt time of the last /livox/imu # The attitude: FAST-LIO's, of the IMU body (/livox/imu's frame), carried into the policy's IMU frame. source = self.get_parameter("livox_attitude").value @@ -270,7 +290,8 @@ def __init__(self): self._set_state("idle") self.get_logger().info( f"marl_deploy_node ready: {cfg_path}, {control_hz:.0f} Hz, " - f"confirm={'required' if self._confirm_wait else 'auto'}, IMU /livox/imu in {self._imu_frame}, " + f"confirm={'required' if self._confirm_wait else 'auto'}, " + f"IMU {'/lowstate imu_state (untrimmed)' if self._lowstate_imu else '/livox/imu'} in {self._imu_frame}, " f"gyro low-pass {f'{self._gyro_lowpass_hz:g} Hz' if self._gyro_lowpass_hz > 0 else 'off'}") def _build_imu_mount(self, deg: dict) -> np.ndarray | None: @@ -294,9 +315,17 @@ def _on_set_parameters(self, params) -> SetParametersResult: return SetParametersResult(successful=True) def _on_lowstate(self, msg: LowState) -> None: - """Keep the latest /lowstate and its receipt time.""" + """Keep the latest /lowstate and its receipt time; with imu.source lowstate, filter its gyroscope.""" self._lowstate = msg self._lowstate_time = time.monotonic() + if self._lowstate_imu: + gyro = np.array(msg.imu_state.gyroscope, dtype=np.float64) + if self._lowstate_lowpass is not None: + # the sample's dt from LowState.tick (ms); a first sample or a gap restarts the filter + dt = 0.0 if self._lowstate_tick is None else 1e-3 * ((msg.tick - self._lowstate_tick) & 0xFFFFFFFF) + gyro = self._lowstate_lowpass(gyro, dt) + self._lowstate_tick = msg.tick + self._lowstate_gyro = gyro def _on_livox_imu(self, msg: Imu) -> None: """Feed a /livox/imu sample to the filter and the FAST-LIO attitude.""" @@ -432,6 +461,14 @@ def _set_state(self, name: str) -> None: def _robot_state(self, msg: LowState) -> RobotState: """Return the controller's state from /lowstate and the IMU, in the policy's IMU frame, trimmed.""" motors = msg.motor_state + q = np.array([motors[i].q for i in range(NUM_JOINTS)], dtype=np.float64) + dq = np.array([motors[i].dq for i in range(NUM_JOINTS)], dtype=np.float64) + if self._lowstate_imu: + # the torso IMU as reported, untrimmed: the policy trained on its bias + imu = msg.imu_state + gyro = self._lowstate_gyro if self._lowstate_gyro is not None else np.array(imu.gyroscope, dtype=np.float64) + return RobotState(q=q, dq=dq, quat=np.array(imu.quaternion, dtype=np.float64), gyro=gyro.copy(), + acc=np.array(imu.accelerometer, dtype=np.float64)) if self._livox.ready: # Gyro (low-passed) and specific force from LivoxImu; the attitude from # FAST-LIO when levelled and current, else LivoxImu's filter. @@ -449,18 +486,17 @@ def _robot_state(self, msg: LowState) -> RobotState: if self._imu_mount is not None: quat = quat_mul(quat, quat_conjugate(self._imu_mount)) gyro, acc = quat_apply(self._imu_mount, gyro), quat_apply(self._imu_mount, acc) - return RobotState( - q=np.array([motors[i].q for i in range(NUM_JOINTS)], dtype=np.float64), - dq=np.array([motors[i].dq for i in range(NUM_JOINTS)], dtype=np.float64), - quat=quat, gyro=gyro, acc=acc, - ) + return RobotState(q=q, dq=dq, quat=quat, gyro=gyro, acc=acc) def _odom_fresh(self, now: float) -> bool: """Return whether /Odometry arrived within livox_odom_timeout of now (monotonic s).""" return self._slam_time is not None and now - self._slam_time <= self._odom_timeout def _livox_fresh(self, now: float) -> bool: - """Return whether the IMU attitude is up and /livox/imu arrived within livox_imu_timeout of now.""" + """Return whether the IMU attitude is up and /livox/imu arrived within livox_imu_timeout of now (always, + with imu.source lowstate: /lowstate's freshness is checked on its own).""" + if self._lowstate_imu: + return True return (self._livox.ready and self._livox_time is not None and now - self._livox_time <= float(self.get_parameter("livox_imu_timeout").value)) @@ -587,7 +623,7 @@ def _prepose(self, state: RobotState, mode_machine: int) -> None: self.get_logger().info("pre-posed; waiting for /livox/imu (h12_real_drivers use_lidar?)", throttle_duration_sec=5.0) return - if self._attitude is not None and not self._attitude.calibrated: + if self._attitude is not None and not self._lowstate_imu and not self._attitude.calibrated: why = self._attitude.calibrate() if why is not None: self._set_state("waiting_imu") diff --git a/core_ws/src/lmg_deploy/test/test_controller.py b/core_ws/src/lmg_deploy/test/test_controller.py index 5be8f50..a267556 100644 --- a/core_ws/src/lmg_deploy/test/test_controller.py +++ b/core_ws/src/lmg_deploy/test/test_controller.py @@ -387,6 +387,27 @@ def test_slam_odometry_moves_goal_against_pelvis_motion(): np.testing.assert_allclose(R.quat_unique(ctrl.believed[arm, 3:]), R.quat_unique(quat), atol=1e-9) +def test_pelvis_frame_goal_ignores_odometry_and_walking_goes_on(): + """arm_goal_frame pelvis (LocoManip-Marl-Direct-v0's goals ride on the pelvis): odometry leaves the goal alone; + arm_goal_keeps_walking: the operator's command stays live through an arm goal.""" + ctrl, cfg = _controller(arm_goal_frame="pelvis", arm_goal_keeps_walking=True) + state = _state(cfg) + ctrl.reset(state) + ctrl.set_velocity_command(np.array([0.3, 0.0, 0.2])) + goal = ctrl.rest_pose.copy() + ctrl.request_arm_goal(goal) + for pelvis_pos, pelvis_quat in ((np.zeros(3), np.array([1.0, 0, 0, 0])), + (np.array([0.1, 0.0, 0.0]), R.quat_from_rotvec(np.array([0.0, 0.0, 0.2])))): + ctrl.set_slam_pose(*_slam_body_pose(ctrl, state, pelvis_pos, pelvis_quat)) + ctrl.step(state) + assert ctrl.mode == Mode.ARM + np.testing.assert_allclose(ctrl.believed, goal, atol=1e-12) + np.testing.assert_allclose(ctrl.velocity_command(), [0.3, 0.0, 0.2]) + assert np.linalg.norm(ctrl.observed_command()) > 0.0 + with pytest.raises(ValueError): + _controller(arm_goal_frame="map") + + def test_slam_odometry_follows_the_torso_joint(): """A torso turn moves the lidar but not the pelvis, so the body pose it reports leaves the goal in place.""" ctrl, cfg = _controller() @@ -612,33 +633,43 @@ def test_command_filter_low_passes_the_observed_command(): np.testing.assert_allclose(ctrl.observed_command(), 0.0) # and from zero after a reset -def test_obs_terms_must_match_the_layout(): - """An export's observation term lists must be the controller's own; matching lists and none both build.""" +def test_obs_terms_must_be_known_and_in_order(): + """An export's observation term lists must be terms the controller builds, in its order; any may be left out, + and no list means all of them.""" legs = [t for t in LEGS_OBS_TERMS if t != "gait_phase_cos"] # explicit500 has no gait_phase_cos arms = [t for t in ARMS_OBS_TERMS if t != "gait_phase_cos"] _controller(legs_obs_terms=legs, arms_obs_terms=arms) - with pytest.raises(ValueError): + _controller(legs_obs_terms=legs[1:], arms_obs_terms=arms[4:]) + with pytest.raises(ValueError): # unknown _controller(legs_obs_terms=legs[:-1] + ["height_scan", "actions"], arms_obs_terms=arms) - with pytest.raises(ValueError): + with pytest.raises(ValueError): # gait_phase_cos without gait_phase_cos in the config _controller(legs_obs_terms=legs, arms_obs_terms=LEGS_OBS_TERMS) + with pytest.raises(ValueError): # out of order + _controller(legs_obs_terms=legs, arms_obs_terms=[arms[1], arms[0]] + arms[2:]) -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.""" +def test_obs_left_out_terms_are_cut_from_the_full_vector(): + """Trimmed term lists build each agent's full vector with the left-out terms' slices removed, the rest in order: + the legs without base_lin_acc, arm_goal and ee_targets (LocoManip-LegsOnly-v0, the torso IMU's runs), the arms + without the IMU, the command and the gait clock (LocoManip-ArmsOnly-v0).""" 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) + trimmed, _ = _controller(legs_obs_terms=[t for t in legs if t not in ("base_lin_acc", "arm_goal", "ee_targets")], + arms_obs_terms=[t for t in arms if t not in ("base_lin_acc", "base_ang_vel", + "projected_gravity", "velocity_commands", + "gait_phase")]) state = _state(cfg) for ctrl in (full, trimmed): ctrl.reset(state) + ctrl.set_velocity_command(np.array([0.3, 0.0, 0.1])) 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"]) + # full legs: acc 0:3, gyro 3:6, gravity 6:9, cmd 9:12, arm_goal 12, ee_targets 13:27, then the rest + np.testing.assert_allclose(b, np.concatenate([a[3:12], a[27:]])) + a, b = full.last_obs["arms"], trimmed.last_obs["arms"] + gait = slice(27 + 54, 27 + 54 + 2) # after joint_pos and joint_vel + np.testing.assert_allclose(b, np.concatenate([a[12:gait.start], a[gait.stop:]])) def test_arm_goals_refused_for_a_policy_that_never_trained_one(): @@ -651,3 +682,323 @@ def test_arm_goals_refused_for_a_policy_that_never_trained_one(): ctrl.request_arm_goal(ctrl.rest_pose) assert ctrl.mode == Mode.NAV ctrl.set_mode("walk") # walking stays allowed + + +# -- arm_ik: mdp.IKPostureArmAction ---------------------------------------------------------- + +# Coexist200k_s42's arm_ik block (export.yaml), without the seed table, the gravity offset, the integral or the freeze. +ARM_IK = {"rot_weight": 0.3, "null_gain": 0.05, "posture_scale": 0.5, "posture_gain": 1.0, "limit_margin": 0.05, + "ik_damping": 0.05, "ik_passes": 3, "posture_from_command": False, "trajectory": True, + "seed_rot_weight": 0.3, "posture_actions": False, "trajectory_max_vel": 2.0, "trajectory_max_acc": 10.0, + "gravity_offset": False} + + +def _ik_controller(arms_net=None, arm_ik=None, **overrides): + """Return a controller with ARM_IK (updated by arm_ik) on the test config, the torso IMU among its frames.""" + cfg = _cfg() + cfg.update(overrides) + cfg["arm_ik"] = {**ARM_IK, **(arm_ik or {})} + names = cfg["joint_names"] + kin = ArmKinematics(URDF, names, cfg["arms"]["ee_frames"], [names[13:20], names[20:]], + extra_frames=[cfg["slam_frame"], "imu_link"]) + return MarlController(cfg, kin, ZeroNetwork(12), arms_net or ZeroNetwork(14)), cfg + + +def _arm_q(cfg, offsets): + """Return the default pose with the arm joints moved by offsets (14,).""" + q = np.array(cfg["default_joint_pos"], dtype=np.float64) + q[13:] += offsets + return q + + +def test_arm_ik_reset_starts_the_trajectory_at_the_measured_arms(): + """reset(state): the target's trajectory starts at the measured arm angles, at rest, with no residual or integral.""" + ctrl, cfg = _ik_controller(arms_net=ConstNetwork(np.full(14, 0.5))) + state = _state(cfg, _arm_q(cfg, np.linspace(-0.2, 0.2, 14))) + ctrl.reset(state) + for _ in range(5): + ctrl.step(state) + assert np.abs(ctrl.residual).max() > 0 and np.abs(ctrl.traj_v).max() > 0 + moved = _state(cfg, _arm_q(cfg, np.linspace(0.1, -0.1, 14))) + ctrl.reset(moved) + np.testing.assert_allclose(ctrl.ik_q, moved.q[13:]) + for value in (ctrl.traj_v, ctrl.integral, ctrl.residual, ctrl.arms_last): + np.testing.assert_allclose(value, 0.0) + assert not ctrl.frozen and ctrl.residual.shape == (14,) + np.testing.assert_allclose(ctrl.seed_q, ctrl.default[13:]) + + +def test_arm_ik_trajectory_is_synchronized_and_rate_limited(): + """The target moves from its own last value (not the measured angles) to the posture, each arm's joints along one + straight line, within trajectory_max_vel and trajectory_max_acc, and stops there.""" + ctrl, cfg = _ik_controller(torque_headroom=None) + offsets = np.array([0.3, 0.1, -0.2, 0.25, 0.0, -0.1, 0.05] * 2) + start = _state(cfg, _arm_q(cfg, offsets)) + ctrl.reset(start) + held = _state(cfg, _arm_q(cfg, 0.5 * offsets)) # the measured arms never move: the target ignores them + dt, prev_v = ctrl.dt, np.zeros(14) + for _ in range(100): + before = ctrl.ik_q.copy() + cmd = ctrl.step(held) + v = (ctrl.ik_q - before) / dt + np.testing.assert_allclose(v, ctrl.traj_v, atol=1e-9) + for cols in (slice(0, 7), slice(7, 14)): + assert np.abs(v[cols]).max() <= 2.0 + 1e-9 + assert np.abs(v[cols] - prev_v[cols]).max() <= 10.0 * dt + 1e-9 + # synchronized: the velocity is along the line from the start to the posture + line = -offsets[cols] / np.linalg.norm(offsets[cols]) + np.testing.assert_allclose(v[cols] - (v[cols] @ line) * line, 0.0, atol=1e-9) + np.testing.assert_allclose(cmd.q[13:], ctrl.ik_q) # no residual, no gravity offset, no integral + prev_v = v + np.testing.assert_allclose(ctrl.ik_q, ctrl.default[13:], atol=1e-9) + assert np.abs(ctrl.traj_v).max() < 1e-9 + + +def test_arm_ik_trajectory_without_max_vel_caps_each_joint(): + """Without trajectory_max_vel each joint moves at most max_ik_step per step toward the posture.""" + ctrl, cfg = _ik_controller(torque_headroom=None, arm_ik={"trajectory_max_vel": None, "trajectory_max_acc": None}) + ctrl.reset(_state(cfg, _arm_q(cfg, np.full(14, 0.15)))) + ctrl.step(_state(cfg)) + np.testing.assert_allclose(ctrl.ik_q, 0.15 - ctrl.max_ik_step) + + +def test_arm_ik_residual_integrates_with_leak_and_clamp(): + """residual_integrate: r <- clip(exp(-dt / leak_s) r + gain a, +-clamp), added times the residual scale.""" + ri = {"gain": 0.02, "leak_s": 5.0, "clamp": 1.0} + a = np.linspace(-3.0, 3.0, 14) + ctrl, cfg = _ik_controller(arms_net=ConstNetwork(a), arm_ik={"residual_integrate": ri}, torque_headroom=None) + state = _state(cfg) + ctrl.reset(state) + leak, r = np.exp(-ctrl.dt / 5.0), np.zeros(14) + for _ in range(10): + cmd = ctrl.step(state) + r = np.clip(leak * r + 0.02 * a, -1.0, 1.0) + np.testing.assert_allclose(ctrl.residual, r, atol=1e-12) + np.testing.assert_allclose(ctrl.arms_last, r, atol=1e-12) + np.testing.assert_allclose(cmd.q[13:], np.clip(ctrl.ik_q + ctrl.arms_scale * r, ctrl.lower[13:], ctrl.upper[13:])) + for _ in range(200): + ctrl.step(state) + assert np.abs(ctrl.residual).max() <= 1.0 + 1e-12 and np.isclose(np.abs(ctrl.residual).max(), 1.0) + + +def test_arm_ik_posture_actions_widen_the_action(): + """posture_actions: the action is 28 wide, the residual then the posture offset, which moves the posture.""" + with pytest.raises(ValueError): + _ik_controller(arm_ik={"posture_actions": True}) # the config's arms.num_actions is 14 + cfg = _cfg() + a = np.concatenate([np.zeros(14), np.full(14, 0.2)]) + ctrl, cfg = _ik_controller(arms_net=ConstNetwork(a), arm_ik={"posture_actions": True}, + arms={**cfg["arms"], "num_actions": 28}) + state = _state(cfg) + ctrl.reset(state) + ctrl.step(state) + assert ctrl.arms_last.shape == (28,) + offset = ctrl.residual[14:] + np.testing.assert_allclose(ctrl.posture, np.clip(ctrl.default[13:] + 0.5 * offset, ctrl.ik_lower, ctrl.ik_upper)) + assert np.abs(offset).max() > 0 + + +def _seed_table(ctrl, joints_per_arm, path): + """Write a seed table whose rows are the wrist poses (forward kinematics) at the given arm angles.""" + rows = {} + for arm in range(2): + poses = [] + for joints in joints_per_arm: + q = ctrl.default.copy() + q[ctrl.kin.arm_cols[arm]] = joints[arm] + ctrl.kin.update(q) + poses.append(np.concatenate(ctrl.kin.wrist_pose(arm))) + rows[f"poses_{arm}"] = np.array(poses, dtype=np.float32) + rows[f"joints_{arm}"] = np.array([j[arm] for j in joints_per_arm], dtype=np.float32) + np.savez(path, **rows) + return str(path) + + +def _wrist_goal(ctrl, arm_q): + """Return both wrists' poses (2, 7) at the default pose with the arms at arm_q (14,).""" + q = ctrl.default.copy() + q[13:] = arm_q + ctrl.kin.update(q) + return np.stack([np.concatenate(ctrl.kin.wrist_pose(arm)) for arm in range(2)]) + + +SEED_ROWS = [np.zeros((2, 7)), + np.array([[-0.5, 0.3, 0.0, 0.8, 0.0, 0.0, 0.0], [-0.5, -0.3, 0.0, 0.8, 0.0, 0.0, 0.0]]), + np.array([[-1.0, 0.5, 0.3, 1.2, 0.2, 0.1, 0.0], [-1.0, -0.5, -0.3, 1.2, -0.2, 0.1, 0.0]])] + + +def test_arm_ik_seed_is_the_nearest_rows_angles_once_per_goal(tmp_path): + """posture_from_command: during an arm goal the posture is the seed table's row nearest the command, set when the + goal is; a rest-pose goal and navigation use the default angles; odometry moving the goal keeps the seed.""" + ctrl, cfg = _ik_controller() + table = _seed_table(ctrl, SEED_ROWS, tmp_path / "seeds.npz") + ctrl, cfg = _ik_controller(arm_ik={"posture_from_command": True}, arm_seed_table=table) + state = _state(cfg) + ctrl.reset(state) + goal = _wrist_goal(ctrl, SEED_ROWS[1].reshape(14) + 0.02) + ctrl.request_arm_goal(goal) + ctrl.step(state) + np.testing.assert_allclose(ctrl.seed_q, SEED_ROWS[1].reshape(14), atol=1e-6) + ctrl._move_goal(np.array([0.3, 0.0, 0.0]), np.array([1.0, 0, 0, 0])) # nearer row 2 now: the seed stays + np.testing.assert_allclose(ctrl.seed_q, SEED_ROWS[1].reshape(14), atol=1e-6) + ctrl.set_arm_goals(_wrist_goal(ctrl, SEED_ROWS[2].reshape(14))) # a new goal in arm mode: a new seed + np.testing.assert_allclose(ctrl.seed_q, SEED_ROWS[2].reshape(14), atol=1e-6) + ctrl.set_arm_goals(ctrl.rest_pose) + np.testing.assert_allclose(ctrl.seed_q, ctrl.default[13:]) + ctrl.set_arm_goals(goal) + ctrl.end_arm_goal() + np.testing.assert_allclose(ctrl.seed_q, ctrl.default[13:]) + + +def test_arm_ik_seed_solve_reaches_the_command(tmp_path): + """seed_solve: from the nearest row's angles, the seed is solved for the commanded wrist poses themselves.""" + ctrl, cfg = _ik_controller() + table = _seed_table(ctrl, SEED_ROWS, tmp_path / "seeds.npz") + solve = {"iters": 60, "damping": 0.05, "rot_weight": 0.5, "joint_range_margin": 0.0, "soft_limit_factor": 1.0} + ctrl, cfg = _ik_controller(arm_ik={"posture_from_command": True, "seed_solve": solve}, arm_seed_table=table) + ctrl.reset(_state(cfg)) + target_q = SEED_ROWS[1].reshape(14) + np.array([0.1, -0.05, 0.1, -0.1, 0.1, 0.05, -0.1] * 2) + goal = _wrist_goal(ctrl, target_q) + ctrl.request_arm_goal(goal) + solved = _wrist_goal(ctrl, ctrl.seed_q) + np.testing.assert_allclose(solved[:, :3], goal[:, :3], atol=1e-4) + for arm in range(2): + _, rot = R.compute_pose_error(solved[arm, :3], solved[arm, 3:], goal[arm, :3], goal[arm, 3:]) + assert np.linalg.norm(rot) < 1e-3 + + +def test_gravity_torques_match_the_potential_energy(): + """kinematics.gravity_torques is minus the gradient of the potential energy, for any gravity direction.""" + import pinocchio as pin + + cfg = _cfg() + kin = _kin(cfg) + model = pin.buildModelFromUrdf(URDF) + data = model.createData() + q = np.array(cfg["default_joint_pos"], dtype=np.float64) + q[13:] = RNG.uniform(-0.5, 0.5, 14) + g = RNG.normal(size=3) + g *= 9.81 / np.linalg.norm(g) + model.gravity.linear = g + tau = kin.gravity_torques(q, g) + + def energy(qp): + q_pin = pin.neutral(model) + q_pin[kin._q_idx] = qp + return pin.computePotentialEnergy(model, data, q_pin) + + for j in range(13, 27): + dq = np.zeros(27) + dq[j] = 1e-6 + np.testing.assert_allclose(tau[j], -(energy(q + dq) - energy(q - dq)) / 2e-6, atol=1e-5) + + +def test_arm_ik_gravity_offset_uses_the_pelvis_frame_gravity(): + """gravity_offset: the target carries minus the arm joints' gravity torque at the target over kp, with gravity in + the pelvis frame from the IMU's attitude carried through the IMU frame's pose (a tilted pelvis, a turned torso).""" + ctrl, cfg = _ik_controller(arm_ik={"gravity_offset": True}, torque_headroom=None) + q = _arm_q(cfg, np.zeros(14)) + q[12] = 0.3 # the torso turned: the IMU frame with it + q_wp = R.quat_mul(R.quat_from_rotvec(np.array([0.2, 0.0, 0.0])), R.quat_from_rotvec(np.array([0.0, -0.3, 0.1]))) + ctrl.kin.update(q) + _, q_pi = ctrl.kin.frame_pose("imu_link") + state = RobotState(q=q, dq=np.zeros(27), quat=R.quat_mul(q_wp, q_pi), gyro=np.zeros(3), acc=np.zeros(3)) + ctrl.reset(state) + cmd = ctrl.step(state) + g_b = R.quat_apply_inverse(q_wp, np.array([0.0, 0.0, -9.81])) + np.testing.assert_allclose(ctrl.gravity_b(state), g_b, atol=1e-9) + held = ctrl.default.copy() # training's kinematics: the torso at its default + held[13:] = ctrl.ik_q + tau = ctrl.kin.gravity_torques(held, g_b)[13:] + np.testing.assert_allclose(cmd.q[13:], np.clip(ctrl.ik_q - tau / ctrl.kp[13:], ctrl.lower[13:], ctrl.upper[13:])) + assert np.abs(tau).max() > 0.5 # the arms' weight shows + + +def test_arm_ik_integral_adds_the_weighted_dls_step_once_arrived(): + """integral: leaks, and once the target has arrived adds gain x each arm's rot_weight-weighted DLS step on its + measured wrist's error; clamped.""" + integral = {"gain": 0.01, "leak_s": 60.0, "clamp": 0.3} + ctrl, cfg = _ik_controller(arm_ik={"integral": integral}, torque_headroom=None) + state = _state(cfg) + ctrl.reset(state) + goal = ctrl.rest_pose.copy() + goal[:, 2] -= 0.005 + ctrl.request_arm_goal(goal) + ctrl.step(state) + w = np.array([1, 1, 1, 0.3, 0.3, 0.3]) + expected = np.zeros(14) + for arm in range(2): + jac = ctrl.kin.jacobian(arm) * w[:, None] + pos, quat = ctrl.kin.wrist_pose(arm) + err = np.concatenate(R.compute_pose_error(pos, quat, goal[arm, :3], goal[arm, 3:])) * w + expected[7 * arm:7 * arm + 7] = 0.01 * jac.T @ np.linalg.solve(jac @ jac.T + 0.05 ** 2 * np.eye(6), err) + np.testing.assert_allclose(ctrl.integral, expected, atol=1e-12) + cmd = ctrl.step(state) + np.testing.assert_allclose(ctrl.integral, np.clip(expected * np.exp(-ctrl.dt / 60.0) + expected, -0.3, 0.3), + atol=1e-12) + np.testing.assert_allclose(cmd.q[13:], np.clip(ctrl.ik_q + ctrl.integral, ctrl.lower[13:], ctrl.upper[13:])) + # not arrived (the target still moving): the integral only leaks + ctrl.reset(_state(cfg, _arm_q(cfg, np.full(14, 0.1)))) + ctrl.integral[:] = 0.2 + ctrl.step(state) + np.testing.assert_allclose(ctrl.integral, 0.2 * np.exp(-ctrl.dt / 60.0)) + + +def test_arm_ik_freeze_latches_the_residual_on_the_goal_and_releases_it(): + """freeze: once arrived with both wrists on the command for settle_s the residual is held whatever the action; + ending the arm goal releases it.""" + arms = ConstNetwork(np.full(14, 0.5)) + freeze = {"pos_tol": 0.001, "rot_tol": 0.0873, "settle_s": 0.1} + ctrl, cfg = _ik_controller(arms_net=arms, arm_ik={"freeze": freeze}, torque_headroom=None) + state = _state(cfg) + ctrl.reset(state) + for _ in range(3): + ctrl.step(state) + assert not ctrl.frozen # navigation: never frozen + ctrl.request_arm_goal(ctrl.rest_pose) # the wrists are on it at the default pose + for _ in range(4): + ctrl.step(state) + assert not ctrl.frozen + ctrl.step(state) # the fifth step on the goal: 0.1 s + assert ctrl.frozen + held = ctrl.residual.copy() + arms.out = np.full(14, -1.0) + for _ in range(5): + ctrl.step(state) + np.testing.assert_allclose(ctrl.residual, held) + ctrl.end_arm_goal() + ctrl.step(state) + assert not ctrl.frozen + ctrl.step(state) + assert np.all(ctrl.residual < held) + + +def test_arm_ik_without_trajectory_steps_toward_the_goal(): + """Without trajectory, one limit-aware, posture-seeking IK step from the measured angles per step walks the wrists + toward the goal under perfect tracking, capped per joint; a joint at limit_margin from its bound locks (the wrist + pitch here, which leaves 2 cm of this goal: why training moved to the trajectory).""" + ctrl, cfg = _ik_controller(arm_ik={"trajectory": False}, torque_headroom=None) + q = np.array(cfg["default_joint_pos"], dtype=np.float64) + ctrl.reset(_state(cfg, q)) + goal = ctrl.rest_pose.copy() + goal[0, :3] += [0.05, 0.05, -0.1] + goal[1, :3] += [0.05, -0.05, -0.1] + ctrl.request_arm_goal(goal) + for _ in range(60): + cmd = ctrl.step(_state(cfg, q)) + assert np.abs(cmd.q[13:] - q[13:]).max() <= ctrl.max_ik_step + 1e-12 + q[13:] = cmd.q[13:] + assert np.all(ctrl.wrist_errors() < 0.025) # from 0.122 m + assert np.all(q[13:] >= ctrl.ik_lower - ctrl.max_ik_step) and np.all(q[13:] <= ctrl.ik_upper + ctrl.max_ik_step) + + +def test_arm_ik_config_checks(): + """integral and freeze need trajectory; seed_solve needs the seed table; gravity_offset needs the IMU frame.""" + with pytest.raises(ValueError): + _ik_controller(arm_ik={"trajectory": False, "integral": {"gain": 0.01, "leak_s": 60.0, "clamp": 0.3}}) + with pytest.raises(ValueError): + _ik_controller(arm_ik={"seed_solve": {"iters": 5, "damping": 0.05, "rot_weight": 0.5}}) + cfg = _cfg() + cfg["arm_ik"] = {**ARM_IK, "gravity_offset": True} + with pytest.raises(ValueError): + MarlController(cfg, _kin(cfg), ZeroNetwork(12), ZeroNetwork(14)) # no imu_link among the frames