wip: checkpoint paused Go2 FL four-skill development
This commit is contained in:
@@ -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()
|
||||
Reference in New Issue
Block a user