wip: checkpoint paused Go2 FL four-skill development

This commit is contained in:
2026-10-16 17:39:37 +08:00
parent 37d1b03b1a
commit 77c2978700
29 changed files with 4071 additions and 51 deletions
@@ -0,0 +1 @@
"""Additive four-skill simulation contract; legacy cycle identities are unchanged."""
@@ -0,0 +1,109 @@
"""Public in-process skill service and feedback-only central composition.
No requested force, support load, external-force metadata or privileged callbacks
cross this seam. The same service is used by training, evaluation and the demo.
"""
import math
SKILLS = ('support', 'retract', 'swing', 'plant')
LEGAL = {'support': ('retract',), 'retract': ('swing', 'plant'),
'swing': ('swing', 'plant'), 'plant': ('support',)}
def seconds(value):
if type(value) not in (int, float) or not math.isfinite(value) or not 0 <= value <= 10:
raise ValueError('hold_seconds must be finite in [0,10]')
return float(value)
def direction(value):
if (not isinstance(value, (list, tuple)) or len(value) != 2
or any(type(x) not in (int, float) or not math.isfinite(x) for x in value)
or not .008 <= math.hypot(*value) <= .04):
raise ValueError('swing direction_m must be finite XY, norm .008..04m')
return tuple(float(x) for x in value)
class SkillService:
def __init__(self, state):
self._state = state
def call(self, skill, *, hold_seconds=0., direction_m=None, env_ids=None):
if skill not in SKILLS:
raise ValueError('Unknown skill')
duration = seconds(hold_seconds)
vector = direction(direction_m) if skill == 'swing' else None
if skill != 'swing' and direction_m is not None:
raise ValueError('Only swing accepts a spatial direction')
# State validates all selected rows before mutating any row.
return self._state.request(skill, duration, vector, env_ids)
switch = call
def hold(self, duration, *, env_ids=None):
return self._state.hold(seconds(duration), env_ids)
def query(self, *, env_ids=None):
return self._state.query(env_ids)
def request_stop(self, *, env_ids=None):
return self._state.request_stop(env_ids)
class CentralComposer:
"""Feedback-driven scalar demo/evaluation program; no physics/state writes.
A/B are fixed *commanded* ground endpoints. Direction reverses every other
loop rather than accumulating root/foot drift or reanchoring measured landings.
"""
def __init__(self, service, sequence):
self.service = service
self.vector = direction(sequence['direction_m'])
holds = sequence['holds_s']
if not isinstance(holds, (list, tuple)) or len(holds) != 4:
raise ValueError('Four skill holds required')
self.holds = tuple(seconds(x) for x in holds)
self.loops = sequence['loops']
if type(self.loops) is not int or not 1 <= self.loops <= 5:
raise ValueError('One to five loops required')
self.completed_loops = 0
self.started = self.finished = self.safely_stopped = False
self.transitions = []
def update(self):
status = self.service.query()
if not self.started:
self.service.hold(self.holds[0])
self.started = True
status = self.service.query()
if status['failed']:
raise RuntimeError('Central feedback failure: ' + ','.join(status['failure_reasons']))
if status['stop_requested'] and self.completed_loops < self.loops:
self.safely_stopped = bool(status['stopped'] and status['skill'] == 'support' and status['ready'])
self.finished = self.safely_stopped
return self.finished
if self.finished or not status['ready'] or not status['hold_complete']:
return self.finished
skill = status['skill']
if skill == 'support':
if self.completed_loops == self.loops:
self.service.request_stop()
self.finished = bool(self.service.query()['stopped'])
return self.finished
next_skill, hold, vector = 'retract', self.holds[1], None
elif skill == 'retract':
sign = 1 if self.completed_loops % 2 == 0 else -1
next_skill, hold = 'swing', self.holds[2]
vector = [sign * x for x in self.vector]
elif skill == 'swing':
next_skill, hold, vector = 'plant', self.holds[3], None
elif skill == 'plant':
self.completed_loops += 1
next_skill, hold, vector = 'support', self.holds[0], None
else:
raise RuntimeError('Invalid public skill feedback')
self.service.call(next_skill, hold_seconds=hold, direction_m=vector)
self.transitions.append({'from': skill, 'to': next_skill,
'at_tick': status['tick'], 'completed_loops': self.completed_loops,
'feedback_ready': True, 'feedback_hold_complete': True})
return False
@@ -0,0 +1,179 @@
"""Pure, precomputed true-force schedules; never policy observations.
Forces are world-frame Newtons applied at the payload COM with zero requested
couple. This module does not apply forces or depend on simulation/Kit imports.
"""
import hashlib
import json
import math
import random
from collections.abc import Sequence
from numbers import Real
from types import MappingProxyType
import torch
_TIER_MAX_N = MappingProxyType({"none": 0.0, "small": 1.5, "medium": 4.0, "full": 8.0})
_SLOT_S = 2.0
_RAMP_S = 0.15
_ACTIVE_PROBABILITY = 0.8
def _integer(value, name):
if type(value) is not int:
raise TypeError(f"{name} must be an exact int (not bool)")
return value
def _integers(values, name, *, unique=False):
if not isinstance(values, Sequence) or isinstance(values, (str, bytes)):
raise TypeError(f"{name} must be a sequence of exact ints")
result = [_integer(value, name) for value in values]
if unique and len(set(result)) != len(result):
raise ValueError(f"{name} must be unique")
return result
def _tier(value):
if not isinstance(value, str) or value not in _TIER_MAX_N:
raise ValueError(f"unknown force tier: {value!r}")
return value
class ForceSchedule:
"""Episode-keyed schedules independent of row ordering and global RNG state.
``env_ids`` are immutable identities. ``reset(ids, ...)`` instead selects row
indices; only those rows change. A tier change affects only selected rows.
Evaluation pairs must use the same seed, environment and episode identities.
The manifest is audit data, not an actor observation.
"""
def __init__(self, seed: int, env_ids: Sequence[int], tier: str, device,
duration_s: float = 300, episode_ids: Sequence[int] | None = None):
self._seed = _integer(seed, "seed")
self._env_ids = _integers(env_ids, "env_ids", unique=True)
tier = _tier(tier)
if (isinstance(duration_s, bool) or not isinstance(duration_s, Real)
or not math.isfinite(duration_s) or duration_s <= 0):
raise ValueError("duration_s must be finite and positive")
self._duration_s = float(duration_s)
episodes = ([0] * len(self._env_ids) if episode_ids is None
else _integers(episode_ids, "episode_ids"))
if len(episodes) != len(self._env_ids):
raise ValueError("episode_ids must match env_ids length")
self._device = torch.device(device)
self._slot_count = max(1, math.ceil(self._duration_s / _SLOT_S))
self._rows = []
batch = len(self._env_ids)
# Float64 timing avoids rounding away short ramps late in an episode.
self._onsets = torch.empty((batch, self._slot_count), dtype=torch.float64,
device=self._device)
self._ends = torch.empty_like(self._onsets)
self._peaks = torch.empty((batch, self._slot_count, 3), dtype=torch.float64,
device=self._device)
for row, episode in enumerate(episodes):
record = self._sample(self._env_ids[row], episode, tier)
self._store(row, record)
self._rows.append(record)
def _sample(self, env_id, episode_id, tier):
key = json.dumps([self._seed, env_id, episode_id], separators=(",", ":"))
digest = hashlib.sha256(key.encode("ascii")).digest()
rng = random.Random(int.from_bytes(digest, "big"))
maximum = _TIER_MAX_N[tier]
events = []
for slot in range(self._slot_count):
active = rng.random() < _ACTIVE_PROBABILITY
onset = slot * _SLOT_S + rng.uniform(0.2, 0.5)
duration = rng.uniform(0.5, 1.3)
amplitude = rng.uniform(0.25 * maximum, maximum)
# Uniform azimuth and uniform z give an isotropic 3-D unit vector.
z = rng.uniform(-1.0, 1.0)
azimuth = rng.uniform(0.0, 2.0 * math.pi)
radial = math.sqrt(max(0.0, 1.0 - z * z))
direction = [radial * math.cos(azimuth), radial * math.sin(azimuth), z]
events.append({
"slot_index": slot, "slot_start_s": slot * _SLOT_S,
"active": active, "onset_s": onset, "duration_s": duration,
"end_s": onset + duration, "amplitude_N": amplitude,
"direction_world": direction,
"peak_force_world_N": [amplitude * v if active else 0.0 for v in direction],
"truncated_by_horizon": onset + duration > self._duration_s,
"onset_beyond_horizon": onset > self._duration_s,
})
return {"env_id": env_id, "episode_id": episode_id, "tier": tier,
"maximum_amplitude_N": maximum, "events": events}
def _store(self, row, record):
events = record["events"]
self._onsets[row] = torch.tensor([e["onset_s"] for e in events],
dtype=torch.float64, device=self._device)
self._ends[row] = torch.tensor([e["end_s"] for e in events],
dtype=torch.float64, device=self._device)
self._peaks[row] = torch.tensor([e["peak_force_world_N"] for e in events],
dtype=torch.float64, device=self._device)
def at(self, time: torch.Tensor) -> torch.Tensor:
"""Return float32 [B, 3] world force; accept only times in [0, horizon]."""
if not isinstance(time, torch.Tensor):
raise TypeError("time must be a torch.Tensor")
if time.ndim != 1 or time.shape[0] != len(self._env_ids):
raise ValueError("time must have shape [number of environments]")
if time.dtype == torch.bool or time.is_complex():
raise TypeError("time must contain real, non-boolean numbers")
time = time.to(device=self._device, dtype=torch.float64)
if not bool(torch.isfinite(time).all()) or bool((time < 0).any()):
raise ValueError("time must be finite and nonnegative")
if bool((time > self._duration_s).any()):
raise ValueError("time exceeds registered duration_s")
slots = (time / _SLOT_S).floor().long().clamp(max=self._slot_count - 1)
rows = torch.arange(len(self._env_ids), device=self._device)
onset = self._onsets[rows, slots]
end = self._ends[rows, slots]
# A raised cosine on each edge has max slope A*pi/(2*0.15),
# or 83.776 N/s at 8 N, below the 100 N/s contract.
edge = torch.minimum(time - onset, end - time).clamp(0.0, _RAMP_S)
envelope = 0.5 * (1.0 - torch.cos(math.pi * edge / _RAMP_S))
return (self._peaks[rows, slots] * envelope[:, None]).to(torch.float32)
def reset(self, ids: Sequence[int], episode_ids: Sequence[int],
tier: str | None = None):
"""Resample selected row indices using explicit episode identities."""
ids = _integers(ids, "ids", unique=True)
episodes = _integers(episode_ids, "episode_ids")
if len(ids) != len(episodes):
raise ValueError("episode_ids must match ids length")
if any(row < 0 or row >= len(self._env_ids) for row in ids):
raise ValueError("ids contains an out-of-range row index")
if tier is not None:
tier = _tier(tier)
records = [self._sample(self._env_ids[row], episode,
self._rows[row]["tier"] if tier is None else tier)
for row, episode in zip(ids, episodes)]
for row, record in zip(ids, records):
self._store(row, record)
self._rows[row] = record
def manifest(self) -> dict:
"""Return detached, JSON-serializable parameters and every sampled slot."""
return json.loads(json.dumps({
"schema": "four_skills_force_schedule_v0.5.0",
"seed": self._seed, "duration_s": self._duration_s,
"rng": "python.Random; SHA256(ASCII compact JSON [seed,env_id,episode_id]) big-endian seed",
"frame": "world", "application_point": "payload_com",
"requested_couple_Nm": [0.0, 0.0, 0.0],
"tier_maxima_N": dict(_TIER_MAX_N),
"slot_duration_s": _SLOT_S, "active_probability": _ACTIVE_PROBABILITY,
"onset_offset_uniform_s": [0.2, 0.5],
"duration_uniform_s": [0.5, 1.3],
"amplitude_uniform_fraction_of_max": [0.25, 1.0],
"direction_distribution": "isotropic 3D: uniform z in [-1,1], azimuth in [0,2*pi]",
"envelope": "raised cosine rise and fall; constant plateau",
"rise_s": _RAMP_S, "fall_s": _RAMP_S,
"maximum_force_derivative_N_per_s": 8.0 * math.pi / (2.0 * _RAMP_S),
"horizon_rule": "sample slots starting before horizon without clipping; at(horizon) allowed",
"rows": self._rows,
}, allow_nan=False))
@@ -0,0 +1,265 @@
"""Pure-Torch 48D four-skills residual policy; no simulator or robot-data imports."""
import hashlib
import io
import json
import math
import os
from pathlib import Path
import tempfile
import torch
from torch import nn
from torch.distributions import Normal
FEATURE_VERSION = "four-skills-48-fixed-si-v1"
FEATURE_DIM = 48
CHECKPOINT_FORMAT = "skillfactory.four-skills-ppo.v1"
def _model_config(hidden, initial_std):
if type(hidden) is not int or hidden < 8:
raise ValueError("hidden must be an integer >= 8")
if type(initial_std) not in (int, float) or not math.isfinite(initial_std) or not 0.02 <= initial_std <= 0.5:
raise ValueError("initial_std must be finite and in [0.02, 0.5]")
return {"hidden": hidden, "initial_std": float(initial_std)}
def features(x):
"""Validate q3,qdot3,wrench6,gravity3,omega3,root-error3,root-vel3,
foot-relative3,foot-vel3,contact3,gap1,skill4,target-relative3,progress1,
analytic-reference3,previous-residual3, in that order. No privileged labels.
"""
if (
not isinstance(x, torch.Tensor)
or x.layout != torch.strided
or x.device.type == "meta"
or x.ndim != 2
or x.shape[0] < 1
or x.shape[1] != FEATURE_DIM
or x.dtype != torch.float32
or not torch.isfinite(x).all()
):
raise ValueError("features must be finite float32 [B,48], B >= 1")
return x
class FourSkillActorCritic(nn.Module):
"""Actor and critic see the same state; the environment maps tanh output to ±0.05 rad."""
feature_version = FEATURE_VERSION
def __init__(self, hidden=128, initial_std=0.08):
super().__init__()
self.model_config = _model_config(hidden, initial_std)
offset = torch.zeros(FEATURE_DIM, dtype=torch.float32)
offset[:3] = torch.tensor([0.0, 0.8, -1.5])
scale = torch.ones(FEATURE_DIM, dtype=torch.float32)
scale[:3] = 0.5
scale[3:6] = 10.0
scale[6:12] = torch.tensor([50.0, 50.0, 50.0, 5.0, 5.0, 5.0])
scale[15:18] = 5.0
scale[18:21] = 0.1
scale[21:24] = 0.5
scale[24:27] = 0.4
scale[30:33] = 50.0
scale[33] = 0.05
scale[38:41] = 0.4
self.register_buffer("offset", offset)
self.register_buffer("scale", scale)
self.actor = nn.Sequential(
nn.Linear(FEATURE_DIM, hidden), nn.Tanh(), nn.Linear(hidden, hidden), nn.Tanh(), nn.Linear(hidden, 3)
)
self.critic = nn.Sequential(
nn.Linear(FEATURE_DIM, hidden), nn.Tanh(), nn.Linear(hidden, hidden), nn.Tanh(), nn.Linear(hidden, 1)
)
self.float()
for module in self.modules():
if isinstance(module, nn.Linear):
nn.init.orthogonal_(module.weight, math.sqrt(2))
nn.init.zeros_(module.bias)
# Exact analytic baseline prior, independent of state and random seed.
nn.init.zeros_(self.actor[-1].weight)
nn.init.zeros_(self.actor[-1].bias)
nn.init.orthogonal_(self.critic[-1].weight, 1.0)
self.log_std = nn.Parameter(torch.full((3,), math.log(initial_std), dtype=torch.float32))
def distribution_value(self, x):
x = features(x)
try:
x = x.to(device=self.offset.device)
except (RuntimeError, TypeError) as exc:
raise ValueError("Cannot move features to model device") from exc
x = ((x - self.offset) / self.scale).clamp(-10, 10)
return Normal(
self.actor(x), self.log_std.clamp(math.log(0.02), math.log(0.5)).exp(), validate_args=False
), self.critic(x).squeeze(-1)
def act(self, x, deterministic=False):
if type(deterministic) is not bool:
raise ValueError("deterministic must be bool")
dist, value = self.distribution_value(x)
latent = dist.mean if deterministic else dist.sample()
# PPO stores fixed Gaussian latents: the tanh Jacobian cancels in ratios.
return latent.tanh(), latent, dist.log_prob(latent).sum(-1), value
def evaluate(self, x, latent):
x = features(x)
if (
not isinstance(latent, torch.Tensor)
or latent.layout != torch.strided
or latent.device.type == "meta"
or latent.shape != (len(x), 3)
or latent.dtype != torch.float32
or not torch.isfinite(latent).all()
):
raise ValueError("latent must be finite float32 [B,3]")
try:
latent = latent.to(device=self.offset.device)
except (RuntimeError, TypeError) as exc:
raise ValueError("Cannot move latent to model device") from exc
dist, value = self.distribution_value(x)
return dist.log_prob(latent).sum(-1), value
def value(self, x):
return self.distribution_value(x)[1]
def _plain_tree(value):
"""Snapshot only weights-only-safe data, rejecting nonfinite tensors or metadata."""
if type(value) is torch.Tensor:
if value.layout != torch.strided or value.device.type == "meta" or not torch.isfinite(value).all():
raise ValueError("Checkpoint contains nonfinite or unsupported tensor")
return value.detach().cpu().clone()
if type(value) is dict:
if any(type(key) not in (str, int) for key in value):
raise ValueError("Checkpoint dictionary keys must be strings or integers")
return {key: _plain_tree(item) for key, item in value.items()}
if type(value) in (list, tuple):
return type(value)(_plain_tree(item) for item in value)
if value is None or type(value) in (str, bool, int):
return value
if type(value) is float and math.isfinite(value):
return value
raise ValueError("Checkpoint must contain only finite tensors and plain data")
def _canonical_identity(identity):
if type(identity) is not dict or not identity:
raise ValueError("identity must be a nonempty plain dictionary")
try:
return json.dumps(identity, sort_keys=True, separators=(",", ":"), allow_nan=False)
except (TypeError, ValueError) as exc:
raise ValueError("identity must contain canonical JSON data") from exc
def _validate_payload(payload):
keys = {"format", "features", "feature_dim", "model_config", "model", "optimizer", "identity", "update", "metrics", "rng"}
if type(payload) is not dict or set(payload) != keys:
raise ValueError("Malformed four-skills checkpoint fields")
if payload["format"] != CHECKPOINT_FORMAT or payload["features"] != FEATURE_VERSION:
raise ValueError("Wrong four-skills checkpoint format or feature version")
if type(payload["feature_dim"]) is not int or payload["feature_dim"] != FEATURE_DIM:
raise ValueError("Wrong four-skills feature dimension; legacy policies are incompatible")
config = payload["model_config"]
if type(config) is not dict or set(config) != {"hidden", "initial_std"}:
raise ValueError("Malformed model_config")
_model_config(**config)
if type(payload["update"]) is not int or payload["update"] < 0:
raise ValueError("update must be a nonnegative integer")
_canonical_identity(payload["identity"])
if payload["metrics"] is not None and type(payload["metrics"]) is not dict:
raise ValueError("metrics must be a dictionary or None")
if type(payload["model"]) is not dict or type(payload["optimizer"]) is not dict:
raise ValueError("Malformed model or optimizer state")
if set(payload["optimizer"]) != {"state", "param_groups"}:
raise ValueError("Malformed optimizer state")
if type(payload["optimizer"]["state"]) is not dict or type(payload["optimizer"]["param_groups"]) is not list:
raise ValueError("Malformed optimizer state")
rng = payload["rng"]
if type(rng) is not dict or set(rng) != {"cpu", "cuda"} or type(rng["cuda"]) is not list:
raise ValueError("Malformed RNG state")
for state in [rng["cpu"], *rng["cuda"]]:
if not isinstance(state, torch.Tensor) or state.dtype != torch.uint8 or state.ndim != 1 or state.numel() == 0:
raise ValueError("RNG state must contain nonempty uint8 vectors")
if rng["cpu"].shape != torch.get_rng_state().shape:
raise ValueError("Invalid CPU RNG state size")
def _load_model_state(model, state):
expected = model.state_dict()
if set(state) != set(expected):
raise ValueError("Wrong four-skills model state fields")
for key, value in state.items():
if not isinstance(value, torch.Tensor) or value.shape != expected[key].shape or value.dtype != expected[key].dtype:
raise ValueError(f"Invalid model tensor: {key}")
for key in ("offset", "scale"):
if not torch.equal(state[key].cpu(), expected[key].cpu()):
raise ValueError("Checkpoint changes fixed SI normalization")
model.load_state_dict(state, strict=True)
def save_checkpoint(path, model, optimizer, identity, update, metrics=None):
"""Atomically publish a finite, plain-data checkpoint (no simulator resume state)."""
if not isinstance(model, FourSkillActorCritic) or not isinstance(optimizer, torch.optim.Optimizer):
raise ValueError("Expected FourSkillActorCritic and Torch optimizer")
payload = {
"format": CHECKPOINT_FORMAT,
"features": FEATURE_VERSION,
"feature_dim": FEATURE_DIM,
"model_config": model.model_config,
"model": dict(model.state_dict()),
"optimizer": optimizer.state_dict(),
"identity": identity,
"update": update,
"metrics": metrics,
"rng": {"cpu": torch.get_rng_state(), "cuda": torch.cuda.get_rng_state_all() if torch.cuda.is_available() else []},
}
payload = _plain_tree(payload)
_validate_payload(payload)
# Validate architecture and fixed buffers before creating/replacing any artifact.
with torch.random.fork_rng(devices=[]):
check = FourSkillActorCritic(**payload["model_config"])
_load_model_state(check, payload["model"])
path = Path(path)
path.parent.mkdir(parents=True, exist_ok=True)
temporary = None
try:
with tempfile.NamedTemporaryFile(dir=path.parent, delete=False) as stream:
temporary = Path(stream.name)
torch.save(payload, stream)
stream.flush()
os.fsync(stream.fileno())
os.replace(temporary, path)
finally:
if temporary is not None:
temporary.unlink(missing_ok=True)
def load_policy(path, device="cpu", expected_sha256=None, expected_identity=None):
"""Verify optional digest before deserialization; return an eval model and payload."""
contents = Path(path).read_bytes()
if expected_sha256 is not None:
if (
type(expected_sha256) is not str
or len(expected_sha256) != 64
or any(c not in "0123456789abcdefABCDEF" for c in expected_sha256)
or hashlib.sha256(contents).hexdigest() != expected_sha256.lower()
):
raise ValueError("Checkpoint SHA256 mismatch or malformed expected digest")
try:
payload = torch.load(io.BytesIO(contents), map_location="cpu", weights_only=True)
except Exception as exc:
raise ValueError("Cannot read a weights-only four-skills checkpoint") from exc
payload = _plain_tree(payload)
_validate_payload(payload)
if expected_identity is not None and _canonical_identity(payload["identity"]) != _canonical_identity(expected_identity):
raise ValueError("Four-skills checkpoint identity mismatch")
with torch.random.fork_rng(devices=[]):
model = FourSkillActorCritic(**payload["model_config"])
_load_model_state(model, payload["model"])
try:
model.to(device=device)
except (RuntimeError, TypeError, ValueError, AssertionError) as exc:
raise ValueError(f"Cannot move four-skills model to device {device!r}") from exc
return model.eval(), payload
@@ -0,0 +1 @@
"""Direct-construction four-skill task. Import only after Isaac AppLauncher."""
@@ -0,0 +1,70 @@
"""Additive physical config: all original asset/auxiliary limits remain unchanged."""
import json
from pathlib import Path
from isaaclab.envs import mdp as base_mdp
from isaaclab.managers import ObservationGroupCfg, ObservationTermCfg, RewardTermCfg, TerminationTermCfg
from isaaclab.utils.configclass import configclass
from SkillFactory.tasks.manager_based.contact_cycle_sim.cfg import SimContactCycleEnvCfg, RewardsCfg as CycleRewardsCfg
from SkillFactory.tasks.manager_based.single_leg_anchor.mdp.actions import BoundedJointPositionActionCfg
from . import mdp
ROOT = Path(__file__).resolve().parents[6]
@configclass
class ObservationsCfg:
@configclass
class Policy(ObservationGroupCfg):
state = ObservationTermCfg(func=mdp.observation)
def __post_init__(self):
self.enable_corruption = False
self.concatenate_terms = True
policy = Policy()
@configclass
class RewardsCfg:
task = RewardTermCfg(func=mdp.reward, weight=1.)
@configclass
class TerminationsCfg:
time_out = TerminationTermCfg(func=base_mdp.time_out, time_out=True)
failed = TerminationTermCfg(func=mdp.failed)
@configclass
class FourSkillsEnvCfg(SimContactCycleEnvCfg):
four_config: str = str(ROOT / 'configs/four_skills/v050.json')
four_parameters: dict | None = None
force_tier: str = 'none'
training_mode: bool = False
observations = ObservationsCfg()
rewards = RewardsCfg()
terminations = TerminationsCfg()
def __post_init__(self):
self.four_parameters = json.loads(Path(self.four_config).read_text())
if self.four_parameters['format'] != 'skillfactory.four-skills.v0.5.0':
raise ValueError('Require independent four-skill configuration')
# The inherited constructor configures its own old reward terms before
# environment construction; use those only during that config phase.
skill_rewards = self.rewards
self.rewards = CycleRewardsCfg()
super().__post_init__()
self.rewards = skill_rewards
# Each environment owns a deterministic independently sampled real force
# schedule; legacy named scenarios are not superimposed.
self.cycle_parameters['events'] = []
self.scene.num_envs = 1
self.sim.dt = .002
self.decimation = 5
self.episode_length_s = 120.
self.actions.joint_position = BoundedJointPositionActionCfg(
class_type=mdp.FourSkillAction, asset_name='robot',
joint_names=self.leg_bundle['joint_order'], scale=.5,
preserve_order=True, use_default_offset=True)
@@ -0,0 +1,139 @@
"""Explicit 500Hz safety stepping. Evaluation never autoresets or writes poses."""
import torch
from isaaclab.envs import ManagerBasedRLEnv
from SkillFactory.legs.four_skills.api import SkillService
from .mdp import state
class FourSkillsEnv(ManagerBasedRLEnv):
def __init__(self, cfg, *, runtime_hooks, guard=lambda: None, record=None, paint=None):
self.runtime_hooks = runtime_hooks
self.guard, self.record, self.paint = guard, record, paint
self._reset_allowed = False
self._initial_reset_done = False
self.total_ticks = 0
super().__init__(cfg)
self.s = state(self)
self.service = SkillService(self.s)
if (self._physics_handles_decimation or self.recorder_manager.active_terms
or self.physics_dt != .002 or self.cfg.decimation != 5
or self.s.robot.is_fixed_base or self.cfg.sim.physics.solver_type != 0
or self.cfg.sim.physics.enable_external_forces_every_iteration
or self.cfg.cycle_parameters['support'] != {'kp_N_m': 1800., 'kd_Ns_m': 120., 'max_N': 140.}):
raise RuntimeError('Require original aided floating-root PGS,500Hz/100Hz,unchanged auxiliary')
self.runtime_hooks.mode = 'locked'
self.runtime_hooks.guard_composer(self.s.robot.permanent_wrench_composer)
# Guard sensor and action resets as well as SDK pose writers.
for obj in [self.s.controller, self.action_manager, self.observation_manager,
*self.scene.sensors.values(), *self.s.robot.actuators.values()]:
self.runtime_hooks.guard_object(obj, ('reset',))
def _reset_idx(self, ids):
if not self._reset_allowed:
raise RuntimeError('No reset/pose overwrite inside held skills or composition')
self.runtime_hooks.require_initialization('four_skills._reset_idx')
ManagerBasedRLEnv._reset_idx(self, ids)
if hasattr(self, '_cycle_state'):
self._cycle_state.reset(ids)
def reset(self, seed=None, env_ids=None, options=None):
if self._initial_reset_done and not self.cfg.training_mode:
raise RuntimeError('Exactly one initial reset for continuous evaluation')
if options is not None:
raise ValueError('No reset state options')
self._reset_allowed = True
try:
with self.runtime_hooks.scope('initialization'):
result = ManagerBasedRLEnv.reset(self, seed=seed, env_ids=env_ids)
self.s.controller.refresh(torch.arange(self.num_envs, device=self.device))
self._initial_reset_done = True
return result
finally:
self._reset_allowed = False
def reset_to(self, *args, **kwargs):
raise RuntimeError('No reset_to/state teleport API')
def _training_reset(self, ids):
if not self.cfg.training_mode:
raise RuntimeError('Training reset prohibited during evaluation')
self._reset_allowed = True
try:
with self.runtime_hooks.scope('initialization'):
self._reset_idx(ids)
self.scene.write_data_to_sim()
self.sim.forward()
self.scene.update(0.)
self.s.controller.refresh(ids)
finally:
self._reset_allowed = False
def step(self, action):
if not self._initial_reset_done:
raise RuntimeError('Initialize once before controls')
if (not isinstance(action, torch.Tensor) or action.shape != (self.num_envs, 3)
or action.dtype != torch.float32 or action.device != torch.device(self.device)
or not bool(torch.isfinite(action).all())):
raise ValueError('Finite float32[B,3] residual action required')
s = self.s
if bool(s.failed.any()) or (not self.cfg.training_mode and bool(s.stopped.any())):
raise RuntimeError('Never advance a failed/stopped session')
self.guard()
self.action_manager.process_action(action)
advanced = 0
for _ in range(self.cfg.decimation):
self.guard()
if self.paint is not None and self.paint.closed:
raise InterruptedError('gui_window_closed_before_completion')
self._sim_step_counter += 1
with self.runtime_hooks.wrench_update(s.robot.permanent_wrench_composer):
self.action_manager.apply_action()
self.scene.write_data_to_sim()
with self.runtime_hooks.scope('rollout'):
self.sim.step(render=False)
self.scene.update(dt=self.physics_dt)
s.after_physics(self.physics_dt)
self.total_ticks += 1
advanced += 1
if self.record is not None:
self.record(s)
# Preserve offending tick before any next physical step, including
# shortened training controls. Only new training episodes may reset.
if bool(s.failed.any()) or (not self.cfg.training_mode and bool(s.stopped.any())):
break
if self.paint is not None:
self.paint.check_health()
self.paint.update(self)
self.sim.render(skip_app_pumping=not self.render_enabled)
self.paint.check_health()
self.paint.after_render()
if self.paint.closed:
raise InterruptedError('gui_window_closed_before_completion')
elif self.sim.is_rendering and self._sim_step_counter % self.cfg.sim.render_interval == 0:
self.sim.render(skip_app_pumping=not self.render_enabled)
self.episode_length_buf += 1
self.common_step_counter += 1
s.controller.refresh(torch.arange(self.num_envs, device=self.device))
reward = s.reward()
terminated = s.failed.clone()
truncated = s.time >= self.cfg.episode_length_s
terminal = s.policy().clone()
info = {'ticks_advanced': advanced, 'terminal_observation': {'policy': terminal},
'terminal_ids': torch.arange(self.num_envs, device=self.device),
'failure_counts': {k: int(v.sum()) for k, v in s.failure_masks.items()}}
if self.cfg.training_mode:
ids = (terminated | truncated).nonzero().flatten()
if len(ids):
self._training_reset(ids)
if not self.cfg.training_mode and not bool(s.failed.any()):
# Route stops AFTER old-command physical evidence/reward and BEFORE
# the next observation/inference. Never apply an old-skill residual
# to a newly switched skill in the same step.
pending = (s.stop_requested & s.ready & ((s.skill == 1) | (s.skill == 2))).nonzero().flatten()
supported = (s.stop_requested & s.ready & (s.skill == 3)).nonzero().flatten()
if len(pending):
self.service.call('plant', hold_seconds=0., env_ids=pending)
if len(supported):
self.service.call('support', hold_seconds=0., env_ids=supported)
self.obs_buf = {'policy': s.policy()}
return self.obs_buf, reward, terminated, truncated, info
@@ -0,0 +1,262 @@
"""Genuine live Kit presentation and owned-X11 original-speed recording.
Only camera/light/UI and nonphysical force-line prims are written. No Robot
poses, rigid-body fields, material/physics parameters, or control state writes.
"""
import ctypes
import json
import os
from pathlib import Path
import re
import signal
import subprocess
import time
from SkillFactory.tasks.manager_based.contact_cycle_sim import skill_gui_v041 as base
FFMPEG = Path('/home/cen/isaacsim-6.0.1/kit/python/lib/python3.12/site-packages/imageio_ffmpeg/binaries/ffmpeg-linux-x86_64-v7.0.2')
def owned_window():
text = subprocess.check_output(['xprop', '-root', '_NET_CLIENT_LIST'], text=True)
for wid in re.findall(r'0x[0-9a-f]+', text):
try:
info = subprocess.check_output(['xprop', '-id', wid, '_NET_WM_PID', '_NET_WM_NAME'],
text=True, stderr=subprocess.DEVNULL)
match = re.search(r'_NET_WM_PID\(CARDINAL\) = (\d+)', info)
if match is None or int(match[1]) != os.getpid():
continue
geo = subprocess.check_output(['xwininfo', '-id', wid], text=True, stderr=subprocess.DEVNULL)
def value(label):
return int(re.search(label + r':\s+(-?\d+)', geo)[1])
if value('Width') >= 800 and value('Height') >= 600 and 'IsViewable' in geo:
return {'id': wid, 'pid': os.getpid(), 'x': value('Absolute upper-left X'),
'y': value('Absolute upper-left Y'), 'width': value('Width'),
'height': value('Height'), 'xprop': info, 'xwininfo': geo}
except subprocess.CalledProcessError:
continue # disappearing window is not interpreted as successful capture
return None
def wm_close(window):
class Data(ctypes.Union):
_fields_ = [('b', ctypes.c_char * 20), ('s', ctypes.c_short * 10), ('l', ctypes.c_long * 5)]
class ClientMessage(ctypes.Structure):
_fields_ = [('type', ctypes.c_int), ('serial', ctypes.c_ulong), ('send_event', ctypes.c_int),
('display', ctypes.c_void_p), ('window', ctypes.c_ulong),
('message_type', ctypes.c_ulong), ('format', ctypes.c_int), ('data', Data)]
class Event(ctypes.Union):
_fields_ = [('xclient', ClientMessage), ('padding', ctypes.c_long * 24)]
lib = ctypes.CDLL('libX11.so.6')
lib.XOpenDisplay.argtypes = [ctypes.c_char_p]
lib.XOpenDisplay.restype = ctypes.c_void_p
display = lib.XOpenDisplay(os.environ['DISPLAY'].encode())
if not display:
raise RuntimeError('No actual X11 display')
lib.XInternAtom.argtypes = [ctypes.c_void_p, ctypes.c_char_p, ctypes.c_int]
lib.XInternAtom.restype = ctypes.c_ulong
event = Event()
event.xclient.type = 33
event.xclient.display = display
event.xclient.window = int(window['id'], 16)
event.xclient.message_type = lib.XInternAtom(display, b'WM_PROTOCOLS', False)
event.xclient.format = 32
event.xclient.data.l[0] = lib.XInternAtom(display, b'WM_DELETE_WINDOW', False)
lib.XSendEvent.argtypes = [ctypes.c_void_p, ctypes.c_ulong, ctypes.c_int, ctypes.c_long, ctypes.POINTER(Event)]
lib.XSendEvent(display, int(window['id'], 16), False, 0, ctypes.byref(event))
lib.XFlush.argtypes = [ctypes.c_void_p]
lib.XFlush(display)
lib.XCloseDisplay.argtypes = [ctypes.c_void_p]
lib.XCloseDisplay(display)
class Presentation(base.LivePresentation):
def __init__(self, app, env, output):
base.prepare_native()
self.output = Path(output)
self.window_info = None
self.video = self.video_log = None
self.video_command = None
self.video_exit = None
self.captures = []
self.seen = set()
self.created = time.monotonic()
self.first_tick_wall = None
self.final_carry_equal = False
super().__init__(app, env.guard, lambda value: None)
self.env = env
self.initial_render_generation = env.sim.render_generation
self.attach(env)
self.expected_visualizer = next(v for v in env.sim.visualizers if v.cfg.visualizer_type == 'kit')
self.window.title = 'Go2 FL | four skills 0.5.0 | LIVE PhysX'
self.window.height = 350
self.message.text = 'Aided single leg | original auxiliary | real force | no resets'
self.arrow = base.UsdGeom.BasisCurves.Define(base.omni.usd.get_context().get_stage(), '/World/ActualExternalForce')
self.arrow.CreateTypeAttr('linear')
self.arrow.CreateCurveVertexCountsAttr([2])
self.arrow.CreateWidthsAttr([.004])
self.arrow.SetWidthsInterpolation('constant')
self.arrow.CreateDisplayColorAttr([(1., .05, .03)])
with self.window.frame:
# Rebuild with a separate explicit safety request; OS close remains
# immediate abort while physics is active, never a safe-stop claim.
with base.ui.VStack(spacing=4):
self.mode = base.ui.Label('', height=24)
self.phase = base.ui.Label('', height=24)
self.cycle = base.ui.Label('', height=24)
self.progress = base.ui.ProgressBar(height=14)
self.contact = base.ui.Label('', height=24)
self.position = base.ui.Label('', height=24)
self.message = base.ui.Label('Aided fixture | truth sensing | no pose reset', height=24)
base.ui.Button('Request SAFE STOP (plant then support)', clicked_fn=env.service.request_stop, height=28)
base.ui.Button('Close window (ABORT while running)', clicked_fn=self.request_close, height=28)
for name in ('Simulation Output Settings', 'Semantics Schema Editor', 'Layer', 'Render Settings'):
panel = base.ui.Workspace.get_window(name)
if panel is not None:
panel.visible = False
def check_health(self):
v = self.expected_visualizer
if (not self.app.is_running() or v not in self.env.sim.visualizers
or not v.is_running() or v.is_training_paused()):
raise RuntimeError('Actual bound Kit visualizer closed/removed/paused')
def update(self, env):
from SkillFactory.legs.four_skills.api import SKILLS
s = env.s
tick = int(s.substeps[0])
self.last_tick = tick
self.render_updates = env.sim.render_generation - self.initial_render_generation
if self.window is not None:
skill = SKILLS[int(s.skill[0])]
self.mode.text = 'Control: ' + getattr(self, 'controller_label', 'analytic') + ' | ' + skill.upper()
self.phase.text = f'Ready: {bool(s.ready[0])} | hold {float(s.held[0]):.2f}/{float(s.hold_required[0]):.2f}s'
central = getattr(env, 'central_progress', {})
loops = f"loop {central.get('completed_loops', 0)}/{central.get('requested_loops', '?')}"
self.cycle.text = f'{loops} | tick {tick} | sim {float(s.time[0]):.2f}s | 500Hz safety'
self.progress.model.set_value(min(float(s.held[0]) / max(.1, float(s.hold_required[0])), 1.))
normal = float(s.contact()[0, 2])
gap = float(s.foot_center()[0, 2] - s.bundle['foot']['radius'])
force = s.actual_external[0].tolist()
auxiliary = float(s.auxiliary[0, s.payload, 2])
self.contact.text = f'Contact Fn {normal:.2f}N | gap {gap*1000:.1f}mm | auxiliaryZ {auxiliary:.1f}N'
self.position.text = f'REAL external world N: {force[0]:+.2f}, {force[1]:+.2f}, {force[2]:+.2f}'
if hasattr(self, 'arrow'):
pos = s.root()[0][0].detach().cpu().tolist()
end = [p + .025 * f for p, f in zip(pos, force)]
self.arrow.GetPointsAttr().Set([base.Gf.Vec3f(*pos), base.Gf.Vec3f(*end)])
if tick and self.window_info is None:
self.window_info = owned_window()
if self.window_info is not None:
self.start_video()
if tick and self.first_tick_wall is None:
self.first_tick_wall = time.monotonic()
def after_render(self):
from SkillFactory.legs.four_skills.api import SKILLS
s = self.env.s
key = (int(s.skill[0]), bool(s.ready[0]))
capture_key = key
label = f'{SKILLS[key[0]]}-' + ('ready' if key[1] else 'enter')
if float(s.actual_external.norm()) > .5 and 'external-force' not in self.seen:
capture_key, label = 'external-force', 'external-force'
if self.window_info is not None and self.last_tick > 5 and capture_key not in self.seen:
import torch
before = torch.cat((s.q(), *s.root()), -1).clone()
counts = (self.env.runtime_hooks.completed_steps, self.env.runtime_hooks.reset_calls,
self.env.runtime_hooks.pose_write_calls)
# UI/compositor is one displayed frame behind after a single paint.
# Synchronize only by rendering the SAME verified physical state.
for _ in range(3):
self.env.sim.render()
self.check_health()
time.sleep(.05)
if (not torch.equal(before, torch.cat((s.q(), *s.root()), -1))
or counts != (self.env.runtime_hooks.completed_steps, self.env.runtime_hooks.reset_calls,
self.env.runtime_hooks.pose_write_calls)):
raise RuntimeError('Presentation synchronization advanced physics')
self.capture(label)
self.seen.add(capture_key)
def start_video(self):
w = self.window_info
self.video_command = [str(FFMPEG), '-hide_banner', '-y', '-f', 'x11grab', '-framerate', '30',
'-video_size', f'{w["width"]//2*2}x{w["height"]//2*2}',
'-i', f'{os.environ["DISPLAY"]}+{w["x"]},{w["y"]}', '-an', '-c:v', 'libx264',
'-preset', 'ultrafast', '-crf', '25', '-pix_fmt', 'yuv420p', '-t', '2400',
str(self.output / 'live-kit-original-speed.mp4')]
self.video_log = (self.output / 'screen-video.log').open('w')
self.video = subprocess.Popen(self.video_command, stdin=subprocess.PIPE,
stdout=self.video_log, stderr=subprocess.STDOUT)
(self.output / 'actual-window.json').write_text(json.dumps(w, indent=2))
def capture(self, label):
from PIL import ImageGrab
w = self.window_info
image = ImageGrab.grab(xdisplay=os.environ['DISPLAY'])
image = image.crop((w['x'], w['y'], w['x'] + w['width'], w['y'] + w['height']))
path = self.output / (label + '.png')
image.save(path)
self.captures.append({'path': path.name, 'tick': self.last_tick, 'wall_time': time.time(),
'state': self.env.service.query()})
def finish(self, complete, *, safely_stopped=False):
import torch
self.protocol_completed = complete
if not complete and not safely_stopped:
return
self.message.text = ('SEQUENCE COMPLETE' if complete else 'SAFE STOP (not full sequence)') + ' | physical state retained | normal close'
before = torch.cat((self.env.s.q(), *self.env.s.root()), -1).clone()
counts = (self.env.runtime_hooks.completed_steps, self.env.runtime_hooks.reset_calls,
self.env.runtime_hooks.pose_write_calls)
end = time.monotonic() + 5
while not self.closed and time.monotonic() < end:
self.guard()
self.check_health()
self.env.sim.render()
time.sleep(1/30)
self.capture('final-retained')
self.final_carry_equal = torch.equal(before, torch.cat((self.env.s.q(), *self.env.s.root()), -1))
self.hold_no_physics = counts == (self.env.runtime_hooks.completed_steps, self.env.runtime_hooks.reset_calls,
self.env.runtime_hooks.pose_write_calls)
if not self.final_carry_equal or not self.hold_no_physics:
raise RuntimeError('Final render-only view changed physical state')
if self.window_info is None:
raise RuntimeError('No verified owned actual Kit window')
wm_close(self.window_info)
close_deadline = time.monotonic() + 10
while not self.closed and time.monotonic() < close_deadline:
self.guard()
self.env.sim.render()
if not self.closed:
raise RuntimeError('Actual WM_DELETE not observed')
self.close_reason = 'actual_WM_DELETE_after_completion'
self.stop_video()
def stop_video(self):
if self.video is not None:
if self.video.poll() is None:
# ffmpeg SIGINT normally returns255 even after finalizing MP4;
# explicit stdin quit is the documented clean exit0 path.
self.video.stdin.write(b'q\n')
self.video.stdin.flush()
self.video.stdin.close()
self.video_exit = self.video.wait(timeout=30)
if self.video_log is not None and not self.video_log.closed:
self.video_log.close()
if self.video_exit != 0:
raise RuntimeError('Real Kit recording did not exit cleanly')
def report(self):
result = {'genuine_Kit_window': self.window_info, 'protocol_completed': self.protocol_completed,
'render_updates': self.render_updates, 'close_reason': self.close_reason,
'final_carry_equal': self.final_carry_equal, 'hold_no_physics': self.hold_no_physics,
'video_command': self.video_command, 'video_waited_exit': self.video_exit,
'captures': self.captures, 'recording_fps_is_not_simulation_rtf': True}
(self.output / 'gui-evidence.json').write_text(json.dumps(result, indent=2))
return result
def release(self):
self.stop_video()
self.report()
super().release()
@@ -0,0 +1,398 @@
"""Skill-local references and real state feedback; no force-sampling actor labels."""
import json
from pathlib import Path
import torch
from isaaclab.utils.math import axis_angle_from_quat
from SkillFactory.legs.contact_cycle.mechanics import inverse_rotate, reduce_to_com
from SkillFactory.legs.contact_cycle.task import phase_failures, phase_objective
from SkillFactory.legs.contact_cycle_sim.config import combine
from SkillFactory.legs.contact_cycle_sim.reference import ReferenceController
from SkillFactory.legs.four_skills.api import LEGAL, SKILLS
from SkillFactory.legs.four_skills.forces import ForceSchedule
from SkillFactory.tasks.manager_based.contact_cycle.mdp import CycleState
from SkillFactory.tasks.manager_based.single_leg_anchor.mdp.actions import BoundedJointPositionAction
def smooth(x):
x = x.clamp(0, 1)
return x.square() * (3 - 2 * x)
def hold_watchdog(ready, held, required, completed, age, deadline):
"""A brief ready edge cannot buy an unlimited unfinished requested hold."""
completed = completed | (ready & (held + 1e-6 >= required))
return completed, ~completed & (age > deadline)
class FourSkillState(CycleState):
def __init__(self, env):
super().__init__(env)
n, d = env.num_envs, env.device
self.config = env.cfg.four_parameters
self.skill = torch.zeros(n, dtype=torch.long, device=d)
self.age = torch.zeros(n, device=d)
self.age_ticks = torch.zeros(n, device=d, dtype=torch.long)
self.dwell = torch.zeros(n, device=d)
self.held = torch.zeros(n, device=d)
self.hold_required = torch.zeros(n, device=d)
self.ready = torch.zeros(n, dtype=torch.bool, device=d)
self.ever_ready = torch.zeros_like(self.ready)
self.hold_completed = torch.zeros_like(self.ready)
self.hold_timed_out = torch.zeros_like(self.ready)
self.hold_deadline = torch.full((n,), 2., device=d)
self.stop_requested = torch.zeros_like(self.ready)
self.stopped = torch.zeros_like(self.ready)
self.failed = torch.zeros_like(self.ready)
self.functional_failed = torch.zeros_like(self.ready)
self.bad_hold = torch.zeros(n, device=d)
self.unload_initial = self.weight.clone()
self.unload_min = self.weight.clone()
self.unload_passed = torch.ones_like(self.ready)
self.ground_target = self.origin.clone()
self.entry_ground = self.origin.clone()
self.swing_start = self.origin.clone()
self.swing_delta = torch.zeros(n, 3, device=d)
self.entry_foot = self.foot_center().clone()
self.episode_ids = [0] * n
self.forces = ForceSchedule(env.cfg.seed, list(range(n)), env.cfg.force_tier, d,
duration_s=300., episode_ids=self.episode_ids)
self.controller = ReferenceController(self, env.cfg.residual_parameters['reference'])
self.residual_request = torch.zeros(n, 3, device=d)
self.reference_used = torch.zeros_like(self.residual_request)
self.effective_residual_rad = torch.zeros_like(self.residual_request)
self.controller.reset(torch.arange(n, device=d))
self.lower = self.time.new_tensor([j['limits']['lower'] for j in self.bundle['joints']])
self.upper = self.time.new_tensor([j['limits']['upper'] for j in self.bundle['joints']])
self.speed = self.time.new_tensor([j['limits']['velocity'] for j in self.bundle['joints']])
self.failure_masks = {}
self.transitions = []
self.force_tier = env.cfg.force_tier
self.actual_external = torch.zeros(n, 3, device=d)
def reset(self, ids):
# CycleState.__init__ calls the virtual reset before additive fields exist.
super().reset(ids)
if not hasattr(self, 'skill'):
return
self.skill[ids] = 0
self.age_ticks[ids] = 0
for v in (self.age, self.dwell, self.held, self.hold_required, self.bad_hold):
v[ids] = 0
for v in (self.ready, self.ever_ready, self.hold_completed, self.hold_timed_out,
self.stop_requested, self.stopped, self.failed, self.functional_failed):
v[ids] = False
self.hold_deadline[ids] = 2.
self.ground_target[ids] = self.origin[ids]
self.entry_ground[ids] = self.origin[ids]
self.entry_foot[ids] = self.foot_center()[ids]
self.swing_delta[ids] = 0
self.unload_initial[ids] = self.weight[ids]
self.unload_min[ids] = self.weight[ids]
self.unload_passed[ids] = True
rows = ids.cpu().tolist()
for i in rows:
self.episode_ids[i] += 1
self.forces.reset(rows, [self.episode_ids[i] for i in rows], tier=self.force_tier)
self.residual_request[ids] = 0
self.reference_used[ids] = 0
self.effective_residual_rad[ids] = 0
self.controller.reset(ids)
def ids(self, ids):
if ids is None:
return torch.arange(self.env.num_envs, device=self.env.device)
if isinstance(ids, torch.Tensor):
x = ids
else:
if not isinstance(ids, (list, tuple)) or any(type(i) is not int for i in ids):
raise ValueError('Exact integer environment indices required')
x = torch.tensor(ids, device=self.env.device, dtype=torch.long)
if (x.ndim != 1 or x.dtype not in (torch.int32, torch.int64) or not len(x)
or bool((x < 0).any() or (x >= self.env.num_envs).any()) or len(x.unique()) != len(x)):
raise ValueError('Invalid/duplicate environment indices')
return x.to(device=self.env.device, dtype=torch.long)
def reference(self):
if not hasattr(self, 'skill'):
return super().reference()
t, k = self.age, self.skill
phase = torch.zeros_like(k)
progress = torch.zeros_like(t)
target = self.ground_target.clone()
force = self.weight.clone()
fraction = torch.zeros_like(t)
# Same six original physical subphases, now owned by the active skill,
# with indefinitely held endpoints, not by a global periodic timer.
retract = k == 1
u, lift = smooth(t / 1.), smooth((t - 1.) / .6)
phase = torch.where(retract, torch.where(t < 1., 1, 2), phase)
progress = torch.where(retract, torch.where(t < 1., t, ((t - 1.) / .6).clamp(0, 1)), progress)
target[:, 2] += torch.where(retract, .05 * lift, 0.)
force = torch.where(retract, self.weight * (1 - u), force)
fraction = torch.where(retract, u, fraction)
swing = k == 2
p = (t / 1.4).clamp(0, 1)
swung = self.swing_start + self.swing_delta * smooth(p)[:, None]
swung[:, 2] = self.origin[:, 2] + .05 + .01 * torch.sin(torch.pi * p).square()
target = torch.where(swing[:, None], swung, target)
phase = torch.where(swing, 3, phase)
progress = torch.where(swing, p, progress)
force = torch.where(swing, 0., force)
fraction = torch.where(swing, 1., fraction)
plant = k == 3
lower, reload = smooth(t / .8), smooth((t - .8) / 2.2)
target[:, 2] += torch.where(plant, .05 * (1 - lower), 0.)
phase = torch.where(plant, torch.where(t < .8, 4, 5), phase)
progress = torch.where(plant, torch.where(t < .8, (t / .8).clamp(0, 1), ((t - .8) / 2.2).clamp(0, 1)), progress)
force = torch.where(plant, self.weight * reload, force)
fraction = torch.where(plant, 1 - reload, fraction)
return phase, progress, target, force, fraction
def apply_loads(self):
# Original guide/auxiliary formula, exactly ONE COM composer submission.
# No old deterministic events or extra cancellation feedforward.
_, _, _, _, fraction = self.reference()
pos, quat, vel, omega = self.root()
target, target_vel = self.port_reference()
identity = torch.zeros_like(quat)
identity[:, 3] = 1
com = self.robot.data.body_com_pos_w.torch
disp = torch.cat((pos - target, axis_angle_from_quat(quat)), -1)
velocity = torch.cat((vel - target_vel, omega), -1)
raw = (-self.stiffness * disp - self.damping * velocity).clamp(-self.wrench_limit, self.wrench_limit)
if not self.env.cfg.guide_enabled:
raw.zero_()
self.guide.zero_()
self.guide[:, self.payload] = reduce_to_com(raw[:, :3], raw[:, 3:], identity, pos, com[:, self.payload])
support = self.cc['support']
fz = fraction * (self.weight + support['kp_N_m'] * (target[:, 2] - pos[:, 2])
+ support['kd_Ns_m'] * (target_vel[:, 2] - vel[:, 2]))
force = torch.zeros_like(pos)
if self.env.cfg.auxiliary_enabled:
force[:, 2] = fz.clamp(0, support['max_N'])
self.auxiliary.zero_()
self.auxiliary[:, self.payload] = reduce_to_com(force, torch.zeros_like(force), identity, pos, com[:, self.payload])
self.actual_external = self.forces.at(self.time)
self.external.zero_()
self.external[:, self.payload, :3] = self.actual_external
total = self.external + self.guide + self.auxiliary
self.robot.permanent_wrench_composer.set_forces_and_torques_index(
forces=total[..., :3], torques=total[..., 3:], body_ids=self.all_bodies,
positions=None, is_global=True)
def after_physics(self, dt):
self.substeps += 1
self.time[:] = self.substeps * dt
self.age_ticks += 1
self.age[:] = self.age_ticks * dt
self.sensed = self.raw_sensors()
normal = self.contact()[:, 2]
gap = self.foot_center()[:, 2] - self.bundle['foot']['radius']
_, self.edge_on = self.latch.update(normal, dt)
self.acquisition.update(self.time, gap, normal, self.weight, dt)
phase = self.reference()[0]
self.edge_off = self.separation.update(gap, self.latch.contact, phase, dt)
self.impact = torch.where(phase == 4, torch.maximum(self.impact, normal), torch.zeros_like(normal))
g = self.config['gate']
support = (normal >= .5 * self.weight) & self.latch.contact & (gap <= .002)
airborne = (gap >= .01) & (normal <= 2.)
final = torch.where(self.skill == 0, self.age >= 0.,
torch.where(self.skill == 1, self.age >= 1.6,
torch.where(self.skill == 2, self.age >= 1.4, self.age >= 3.)))
good = torch.where((self.skill == 0) | (self.skill == 3), support, airborne)
retract_unload = (self.skill == 1) & (self.age <= 1.002)
self.unload_min = torch.where(retract_unload, torch.minimum(self.unload_min, normal), self.unload_min)
self.unload_passed |= (self.skill == 1) & (self.unload_min <= .8 * self.unload_initial)
good &= (self.skill != 1) | self.unload_passed
disp = self.foot_center() - self.entry_foot
requested = self.swing_delta
directed = ((disp[:, :2] * requested[:, :2].sign()) >= .5 * requested[:, :2].abs()) | (requested[:, :2].abs() < 1e-6)
good &= (self.skill != 2) | directed.all(-1)
land_error = (self.foot_center() - self.ground_target).norm(dim=-1)
good &= (self.skill != 3) | (land_error <= .05)
dwell_required = torch.where((self.skill == 0) | (self.skill == 3), .2, .1)
self.dwell = torch.where(good & final, self.dwell + dt, 0.)
self.ready = self.dwell + 1e-6 >= dwell_required
self.ever_ready |= self.ready
self.held = torch.where(self.ready, self.held + dt, 0.)
self.hold_completed, hold_expired = hold_watchdog(
self.ready, self.held, self.hold_required, self.hold_completed, self.age, self.hold_deadline)
self.hold_timed_out |= hold_expired
self.bad_hold = torch.where(self.ever_ready & ~good, self.bad_hold + dt, 0.)
deadline = torch.where(self.skill == 0, 2., torch.where(self.skill == 1, 3.6,
torch.where(self.skill == 2, 3.4, 5.)))
self.functional_failed |= ((~self.ever_ready & (self.age > deadline))
| self.hold_timed_out | (self.bad_hold > .2))
self.stopped |= self.stop_requested & (self.skill == 0) & self.ready
masks = self.safety()
self.failure_masks = {**masks, 'skill_completion_or_hold': self.functional_failed,
'requested_hold_timeout': self.hold_timed_out}
all_failed = torch.stack(list(self.failure_masks.values())).any(0)
self.failed |= all_failed
def safety(self):
pos, quat, velocity, omega = self.root()
gap = self.foot_center()[:, 2] - self.bundle['foot']['radius']
ratio = torch.maximum((self.robot.data.applied_torque.torch[:, self.joints] / self.effort).abs().amax(-1),
(self.robot.data.joint_vel.torch[:, self.joints] / self.speed).abs().amax(-1))
excess = torch.maximum(self.lower - self.q(), self.q() - self.upper).clamp(min=0).amax(-1)
finite = torch.isfinite(torch.cat((self.raw_sensors(), pos, quat, velocity, omega,
self.foot_center(), self.contact(),
self.robot.data.applied_torque.torch[:, self.joints],
ratio[:, None], excess[:, None],
self.controller.q_ik, self.controller.action, self.residual_request,
self.guide.flatten(1), self.auxiliary.flatten(1),
self.external.flatten(1)), -1)).all(-1)
phase, progress, _, _, _ = self.reference()
auxiliary_z = self.auxiliary[..., 2]
original_phase = phase_failures(phase, progress, gap, self.contact()[:, 2], self.weight)
return {**original_phase, 'premature_separation': self.separation.premature,
'nonfinite': ~finite, 'contact_acquisition_timeout': self.acquisition.failed,
'negative_normal': self.contact()[:, 2] < -.5,
'auxiliary_force': auxiliary_z.sum(-1) > 140.,
'auxiliary_geometry': ((self.auxiliary[..., :2].abs().amax(dim=(-1, -2)) > 1e-6)
| (auxiliary_z.amin(-1) < -1e-6)
| (self.auxiliary[..., 5].abs().amax(-1) > 1e-6)
| (self.auxiliary[..., 3:].norm(dim=-1) > .06 * auxiliary_z.clamp_min(0) + 1e-6).any(-1)),
'auxiliary_support_phase': (phase == 0) & (self.auxiliary.abs().amax(dim=(-1, -2)) > 1e-6),
'effort_velocity': ratio > 1.00001,
'joint_limit': excess > .01, 'vertical_guide': self.guide[..., 2].abs().amax(-1) != 0,
'penetration': gap < -.002, 'root_height': (pos[:, 2] - self.base_origin[:, 2]).abs() > .1,
'tilt': self.tilt() > .6}
def request(self, skill, hold, vector, ids=None):
ids = self.ids(ids)
old = self.skill[ids].cpu().tolist()
if any(skill not in LEGAL[SKILLS[i]] for i in old):
raise ValueError('Illegal skill transition')
if bool((~self.ready[ids] | self.failed[ids] | self.stopped[ids]
| (self.held[ids] + 1e-6 < self.hold_required[ids])).any()):
raise ValueError('Transition requires ready, completed hold, live healthy skill')
before = self.substeps[ids].clone()
self.entry_ground[ids] = self.ground_target[ids]
self.entry_foot[ids] = self.foot_center()[ids]
if skill == 'retract':
self.unload_initial[ids] = self.contact()[ids, 2].clamp(min=1.)
self.unload_min[ids] = self.unload_initial[ids]
self.unload_passed[ids] = False
if skill == 'swing':
delta = self.time.new_tensor([*vector, 0.])
self.swing_start[ids] = self.ground_target[ids]
self.swing_start[ids, 2] = self.origin[ids, 2] + .05
self.swing_delta[ids] = delta
self.ground_target[ids] += delta
self.ground_target[ids, 2] = self.origin[ids, 2]
self.skill[ids] = SKILLS.index(skill)
self.age[ids] = self.dwell[ids] = self.held[ids] = self.bad_hold[ids] = 0
self.age_ticks[ids] = 0
self.ready[ids] = self.ever_ready[ids] = self.hold_completed[ids] = self.hold_timed_out[ids] = False
self.hold_required[ids] = hold
self.hold_deadline[ids] = self.time.new_tensor([2., 3.6, 3.4, 5.])[self.skill[ids]] + hold
self.controller.refresh(ids)
if not torch.equal(before, self.substeps[ids]):
raise RuntimeError('Transition advanced physical clock')
if self.env.num_envs == 1:
self.transitions.append({'from': SKILLS[old[0]], 'to': skill, 'tick': int(before[0]),
'hold_s': hold, 'direction_m': vector})
return {'accepted': True, 'skill': skill, 'env_ids': ids.cpu().tolist()}
def hold(self, duration, ids=None):
ids = self.ids(ids)
if bool((self.failed[ids] | self.stopped[ids]).any()):
raise ValueError('Cannot hold failed/stopped skill')
self.hold_required[ids] = duration
self.held[ids] = 0
self.hold_completed[ids] = self.ready[ids] & (duration == 0)
intrinsic = self.time.new_tensor([0., 1.6, 1.4, 3.])[self.skill[ids]]
self.hold_deadline[ids] = torch.maximum(self.age[ids], intrinsic) + duration + 2.
return {'accepted': True, 'hold_seconds': duration}
def request_stop(self, ids=None):
ids = self.ids(ids)
self.stop_requested[ids] = True
self.hold_required[ids] = 0
self.stopped[ids] |= self.ready[ids] & (self.skill[ids] == 0)
return {'accepted': True, 'safe_support_required': True}
def query(self, ids=None):
ids = self.ids(ids)
rows = []
for i in ids.cpu().tolist():
rows.append({'skill': SKILLS[int(self.skill[i])], 'tick': int(self.substeps[i]),
'ready': bool(self.ready[i]), 'hold_complete': bool(self.held[i] + 1e-6 >= self.hold_required[i]),
'held_s': float(self.held[i]), 'age_s': float(self.age[i]),
'failed': bool(self.failed[i]), 'stopped': bool(self.stopped[i]),
'stop_requested': bool(self.stop_requested[i]),
'failure_reasons': [k for k, v in self.failure_masks.items() if bool(v[i])],
'normal_N': float(self.contact()[i, 2]),
'gap_m': float(self.foot_center()[i, 2] - self.bundle['foot']['radius'])})
return rows[0] if len(rows) == 1 else rows
def policy(self):
_, progress, target, _, _ = self.reference()
pos, quat, vel, omega = self.root()
foot = self.foot_center()
gravity = torch.zeros_like(pos)
gravity[:, 2] = -1
# Explicit field allowlist. No forces, desired normal load, fixture
# labels, random seeds/onsets/durations or future disturbance values.
return torch.cat((self.q(), self.robot.data.joint_vel.torch[:, self.joints], self.mount_wrench(),
inverse_rotate(quat, gravity), inverse_rotate(quat, omega),
pos - self.base_origin, vel, inverse_rotate(quat, foot - pos), self.point_velocity(),
self.contact(), (foot[:, 2] - self.bundle['foot']['radius'])[:, None],
torch.nn.functional.one_hot(self.skill, 4).float(),
target - self.base_origin, progress[:, None],
self.controller.action, self.residual_request), -1)
def reward(self):
phase, progress, target, desired_force, _ = self.reference()
center = self.foot_center()
gap = center[:, 2] - self.bundle['foot']['radius']
# Reuse the old phase-correct objective: ground tracking while unloading,
# Fn-aware airborne lift/swing, and load restoration throughout reload.
task, violation = phase_objective(phase, progress, (center - target).norm(dim=-1), gap,
self.contact()[:, 2], desired_force, self.slip_velocity(), self.impact, self.weight,
self.cc['contact_transition']['unload_tracking_scale_m'])
return (task + .5 * self.ready.float() - .02 * self.residual_request.square().mean(-1)
- 10 * (self.failed | violation).float())
def state(env):
if not hasattr(env, '_cycle_state'):
env._cycle_state = FourSkillState(env)
if not isinstance(env._cycle_state, FourSkillState):
raise RuntimeError('Wrong additive four-skill state')
return env._cycle_state
def observation(env):
return state(env).policy()
def reward(env):
return state(env).reward()
def failed(env):
return state(env).failed
class FourSkillAction(BoundedJointPositionAction):
def __init__(self, cfg, env):
super().__init__(cfg, env)
state(env)
def process_actions(self, actions):
s = state(self._env)
full, request, delta = combine(s.controller.action, actions, .05, .5)
super().process_actions(full)
s.reference_used.copy_(s.controller.action)
s.residual_request.copy_(request)
s.effective_residual_rad.copy_(delta)
s.executed.copy_(self.raw_actions)
def apply_actions(self):
state(self._env).apply_loads()
super().apply_actions()