Files
SkillFactory/scripts/four_skills_runtime.py
T

475 lines
26 KiB
Python

"""Authenticated Isaac worker for the additive four-skill stage.
Imported by four_skills.py only after private socket handoff. No old run grant,
checkpoint identity, accepted flag or legacy task is reinterpreted.
"""
import importlib
import json
import math
import os
from pathlib import Path
import sys
import time
import traceback
ROOT = Path(__file__).resolve().parents[1]
TASK = 'SkillFactory.tasks.manager_based.four_skills.'
LEG = 'SkillFactory.legs.four_skills.'
def verify_project_imports():
expected = ROOT / 'source/SkillFactory/SkillFactory'
paths = {}
for name, module in tuple(sys.modules.items()):
if (name == 'SkillFactory' or name.startswith('SkillFactory.')) and getattr(module, '__file__', None):
path = Path(module.__file__).resolve()
if not path.is_relative_to(expected):
raise RuntimeError('Imported project source outside current copy: ' + name)
paths[name] = str(path)
return paths
def normalize_scene(cfg, binding):
from SkillFactory.tasks.manager_based.four_skills.cfg import FourSkillsEnvCfg
if type(cfg) is not FourSkillsEnvCfg or cfg.scene.num_envs not in (1, 128):
raise ValueError('Exact new stage config, scalar or128envs required')
if cfg.scene.clone_in_fabric is not False:
raise ValueError('Non-fabric scene required')
# Same pinned SDK normalization mechanics; independent new config identity,
# not a fake RootXY instance or expanded old evaluator allowlist.
for name in binding.NORMALIZATION_SOURCES:
rel, expected = binding.ISAAC_PINS[name]
p = binding._module_file(name)
if p != binding.ISAAC_ROOT / rel or binding.sha256(p.read_bytes()) != expected:
raise ValueError('Installed scene normalizer drift: ' + name)
regex = '/World/envs/env_.*'
for name in ('ground', 'light', 'robot', 'foot_contact', 'mount_wrench'):
entity = getattr(cfg.scene, name)
entity.prim_path = entity.prim_path.format(ENV_REGEX_NS=regex)
if name in ('ground', 'light', 'robot'):
entity.spawn.spawn_path = entity.prim_path.replace(regex, '/World/envs/env_0')
elif name == 'foot_contact':
for field in ('filter_prim_paths_expr', 'sensor_shape_prim_expr', 'filter_shape_prim_expr'):
if hasattr(entity, field) and getattr(entity, field):
setattr(entity, field, [p.format(ENV_REGEX_NS=regex) for p in getattr(entity, field)])
for obj, name, value in binding._reference_updates(cfg):
setattr(obj, name, value)
binding.canonical_config(cfg)
class Trace:
def __init__(self, output, guard, *, full=True):
self.output, self.guard, self.full = Path(output), guard, full
self.frames = []
self.initial = None
self.maximum = {'limit_ratio': 0., 'joint_excess_rad': 0., 'height_error_m': 0.,
'tilt_rad': 0., 'external_N': 0., 'guideZ_N': 0.}
self.ticks = 0
self.failure_frames = []
def snapshot(self, s):
import torch
phase, progress, target, _, fraction = s.reference()
ratio = torch.maximum((s.robot.data.applied_torque.torch[:, s.joints] / s.effort).abs().amax(-1),
(s.robot.data.joint_vel.torch[:, s.joints] / s.speed).abs().amax(-1))
values = {'tick': s.substeps, 'time_s': s.time, 'skill': s.skill, 'age_s': s.age,
'weight': s.weight, 'episode_id': torch.tensor(s.episode_ids, device=s.env.device),
'phase': phase,
'ready': s.ready, 'held_s': s.held, 'hold_required_s': s.hold_required,
'hold_deadline_s': s.hold_deadline, 'hold_completed': s.hold_completed,
'dwell_s': s.dwell, 'bad_hold_s': s.bad_hold,
'unload_passed': s.unload_passed, 'unload_min_N': s.unload_min,
'unload_initial_N': s.unload_initial, 'failed': s.failed,
'root': torch.cat(s.root(), -1), 'joint_pos': s.q(),
'joint_vel': s.robot.data.joint_vel.torch[:, s.joints],
'applied_torque': s.robot.data.applied_torque.torch[:, s.joints],
'foot': s.foot_center(), 'normal': s.contact(),
'gap': s.foot_center()[:, 2] - s.bundle['foot']['radius'],
'target': target, 'progress': progress, 'fixture_fraction': fraction,
'external': s.external, 'guide': s.guide, 'auxiliary': s.auxiliary,
'reference': s.controller.action, 'q_ik': s.controller.q_ik,
'residual': s.residual_request, 'effective_residual_rad': s.effective_residual_rad,
'actor_observation': s.policy(), 'limit_ratio': ratio,
'joint_excess': torch.maximum(s.lower - s.q(), s.q() - s.upper).clamp(min=0).amax(-1),
'height_error': (s.root()[0][:, 2] - s.base_origin[:, 2]).abs(),
'tilt': s.tilt()}
return {k: v.detach().cpu().clone() for k, v in values.items()}
def __call__(self, s):
import torch
# Training keeps aggregates and complete first offending snapshots,
# evaluation retains every500Hz original tensor with no thinning.
if self.full or bool(s.failed.any()):
frame = self.snapshot(s)
if self.full:
self.frames.append(frame)
elif len(self.failure_frames) < 256:
self.failure_frames.append(frame)
self.ticks += 1
if self.full:
values = self.frames[-1]
for dest, src in [('limit_ratio', 'limit_ratio'), ('joint_excess_rad', 'joint_excess'),
('height_error_m', 'height_error'), ('tilt_rad', 'tilt')]:
self.maximum[dest] = max(self.maximum[dest], float(values[src].max()))
self.maximum['external_N'] = max(self.maximum['external_N'], float(values['external'][..., :3].norm(dim=-1).max()))
self.maximum['guideZ_N'] = max(self.maximum['guideZ_N'], float(values['guide'][..., 2].abs().max()))
def save(self):
import torch
self.guard(cleanup=True)
if self.initial is not None:
torch.save(self.initial, self.output / 'initial-state.pt')
if self.frames:
torch.save({k: torch.stack([f[k] for f in self.frames]) for k in self.frames[0]}, self.output / 'trace.pt')
if self.failure_frames:
torch.save(self.failure_frames, self.output / 'training-failure-frames.pt')
class TrainingComposer:
def __init__(self, env):
import torch
self.env = env
self.loops = torch.zeros(env.num_envs, dtype=torch.long, device=env.device)
self.episodes = list(env.s.episode_ids)
self.skill_counts = [0] * 4
self.transition_count = 0
def update(self):
import torch
s, service = self.env.s, self.env.service
for i, episode in enumerate(s.episode_ids):
if episode != self.episodes[i]:
self.loops[i] = 0
self.episodes[i] = episode
allowed = s.ready & (s.held + 1e-6 >= s.hold_required) & ~s.failed & ~s.stopped
kinds = s.skill.clone() # one decision per row, even after service mutation
# Only public skill calls mutate reference goals. No force arguments.
for skill in range(4):
ids = (allowed & (kinds == skill)).nonzero().flatten()
self.skill_counts[skill] += int((kinds == skill).sum())
if not len(ids):
continue
next_skill = ('retract', 'swing', 'plant', 'support')[skill]
self.transition_count += len(ids)
if skill == 1:
for variant, vector in enumerate(((.024, .010), (.020, .024), (.010, .024))):
for sign in (1, -1):
selected = ids[(ids % 3 == variant) & (torch.where(self.loops[ids] % 2 == 0, 1, -1) == sign)]
if len(selected):
service.call('swing', hold_seconds=(.3, .6, 1.)[variant],
direction_m=[sign*x for x in vector], env_ids=selected)
else:
hold = (.4, 0., .5, .8)[skill]
service.call(next_skill, hold_seconds=hold, env_ids=ids)
if skill == 3:
self.loops[ids] += 1
def reject_probe(env):
import torch
def snapshot():
s = env.s
values = [s.q(), *s.root(), s.controller.q_ik, s.controller.action, s.skill, s.substeps,
s.age, s.held, s.hold_required, s.ground_target, s.controller.refreshes,
torch.get_rng_state(), *torch.cuda.get_rng_state_all()]
return [v.detach().cpu().clone() for v in values]
before = snapshot()
counts = (env.runtime_hooks.completed_steps, env.runtime_hooks.reset_calls, env.runtime_hooks.pose_write_calls)
rejected = False
reason = None
try:
env.service.call('plant') # support->plant is illegal, even if ready.
except ValueError as error:
rejected, reason = True, str(error)
after = snapshot()
unchanged = all(torch.equal(a, b) for a, b in zip(before, after)) and counts == (
env.runtime_hooks.completed_steps, env.runtime_hooks.reset_calls, env.runtime_hooks.pose_write_calls)
if not rejected or not unchanged:
raise RuntimeError('Illegal public switch was not rejected atomically')
return {'rejected': rejected, 'reason': reason, 'carried_state_and_rng_equal': unchanged,
'physical_calls_added': 0}
def execute_single(env, payload, output, guard, trace, model=None):
import torch
from SkillFactory.legs.four_skills.api import CentralComposer
config = env.cfg.four_parameters
sequence = dict(config['sequences'][payload['sequence']])
if payload['mode'] == 'smoke':
sequence['loops'] = 1
if payload.get('loops') is not None:
sequence['loops'] = payload['loops']
composer = CentralComposer(env.service, sequence)
observation = env.reset(seed=payload['seed'])[0]['policy']
trace.initial = trace.snapshot(env.s)
if payload['mode'] == 'probe':
(output / 'illegal-switch-probe.json').write_text(json.dumps(reject_probe(env), indent=2))
initial_counts = {'resets': env.runtime_hooks.reset_calls, 'pose_writes': env.runtime_hooks.pose_write_calls}
(output / 'force-schedule.json').write_text(json.dumps(env.s.forces.manifest(), indent=2))
(output / 'initial-counters.json').write_text(json.dumps(initial_counts))
outcome = {'functional_success': False, 'requested_loops': sequence['loops'], 'completed_loops': 0}
try:
while env.s.time[0] < 240:
guard()
status = env.service.query()
if (payload.get('stop_skill') == status['skill'] and status['ready'] and not status['stop_requested']):
env.service.request_stop()
finished = composer.update()
env.central_progress = {'completed_loops': composer.completed_loops, 'requested_loops': composer.loops,
'stop_requested': env.service.query()['stop_requested']}
if finished:
outcome['functional_success'] = not composer.safely_stopped
outcome['safe_stop_success'] = composer.safely_stopped
break
# The public switch may have changed skill/reference at this same
# physical clock; actor must consume the post-command observation.
observation = env.s.policy()
with torch.no_grad():
action = torch.zeros(1, 3, device=env.device) if model is None else model.act(observation, deterministic=True)[0]
observation, _, terminated, _, _ = env.step(action)
observation = observation['policy']
if bool(terminated.any()):
break
outcome.update(completed_loops=composer.completed_loops, status=env.service.query(),
transitions=composer.transitions,
actual_ticks=env.total_ticks, maximum=trace.maximum,
initialization_counters=initial_counts,
final_counters={'resets': env.runtime_hooks.reset_calls,
'pose_writes': env.runtime_hooks.pose_write_calls})
outcome['no_rollout_reset_or_pose_write'] = initial_counts == outcome['final_counters']
outcome['functional_success'] &= outcome['no_rollout_reset_or_pose_write']
if outcome.get('safe_stop_success'):
outcome['safe_stop_success'] &= (outcome['no_rollout_reset_or_pose_write']
and outcome['status']['skill'] == 'support'
and outcome['status']['ready'] and outcome['status']['stopped'])
return outcome
finally:
trace.save()
(output / 'transitions.json').write_text(json.dumps(composer.transitions, indent=2))
(output / 'public-call-receipts.json').write_text(json.dumps(env.s.transitions, indent=2))
def train(env, payload, output, guard, trace):
import torch
from SkillFactory.legs.four_skills.model import FourSkillActorCritic, save_checkpoint
from SkillFactory.legs.standing_ppo import ppo_step
torch.set_num_threads(4)
cfg = env.cfg.four_parameters['training']
torch.manual_seed(cfg['seed'])
torch.cuda.manual_seed_all(cfg['seed'])
model = FourSkillActorCritic().to(env.device)
initial_actor = {k: v.detach().clone() for k, v in model.actor.named_parameters()}
opt = torch.optim.Adam(model.parameters(), lr=cfg['algorithm']['learning_rate'], eps=cfg['algorithm']['adam_eps'])
identity = {'task': env.cfg.four_parameters['task_id'], 'config_sha256': payload['config_sha256'],
'source_sha256': payload['sources'], 'train_seed': cfg['seed'],
'observation': 'four-skills-48-fixed-si-v1', 'device': torch.cuda.get_device_name(0)}
save_checkpoint(output / 'untrained.pt', model, opt, identity, 0)
x = env.reset(seed=cfg['seed'])[0]['policy']
trace.initial = trace.snapshot(env.s)
driver = TrainingComposer(env)
driver.update()
failures = {k: 0 for k in env.s.failure_masks}
history = []
total_transitions = 0
skill_control_samples = [0] * 4
update_limit = payload.get('updates') or cfg['updates']
for update in range(1, update_limit + 1):
guard()
tier = next(t for start, end, t in cfg['tier_updates'] if start <= update <= end)
if tier != env.s.force_tier:
env.s.force_tier = tier
# New training episodes at explicit curriculum boundary, not a hold
# reset, and not a paired independent evaluation.
x = env.reset()[0]['policy']
data = {key: torch.empty(cfg['horizon'], env.num_envs, *shape, device=env.device, dtype=dtype)
for key, shape, dtype in [('features', (48,), torch.float32), ('latent', (3,), torch.float32),
('log_prob', (), torch.float32), ('value', (), torch.float32), ('reward', (), torch.float32),
('next_value', (), torch.float32), ('done', (), torch.bool)]}
update_failures = 0
with torch.no_grad():
for step in range(cfg['horizon']):
guard()
x = env.s.policy() # already post-command from previous boundary.
for k, n in enumerate(torch.bincount(env.s.skill, minlength=4).cpu().tolist()):
skill_control_samples[k] += n
action, latent, logp, value = model.act(x)
obs, reward, terminated, truncated, info = env.step(action)
# Advance exactly once, then bootstrap the very state that the
# next actor will see. Terminal snapshots remain pre-reset.
driver.update()
nx = env.s.policy()
nv = model.value(nx)
timeout = truncated & ~terminated
if bool(timeout.any()):
nv[timeout] = model.value(info['terminal_observation']['policy'][timeout])
nv[terminated] = 0
for key, v in [('features', x), ('latent', latent), ('log_prob', logp), ('value', value),
('reward', reward), ('next_value', nv), ('done', terminated | truncated)]:
data[key][step].copy_(v)
for reason, count in info['failure_counts'].items():
failures[reason] = failures.get(reason, 0) + count
update_failures += int(terminated.sum())
x = nx
guard()
# Physical rollout only, GPU optimizer only. No saved robot trace replay.
metrics = ppo_step(model, opt, data, cfg['algorithm'])
total_transitions += cfg['horizon'] * env.num_envs
row = {'update': update, 'tier': tier, 'environment_transitions': total_transitions,
'actual_physics_calls': env.total_ticks, 'episode_failures': update_failures,
'mean_reward': float(data['reward'].mean()), 'learner': metrics,
'skill_control_samples': skill_control_samples[:],
'feedback_skill_samples': driver.skill_counts[:], 'transitions': driver.transition_count,
'state_diagnostics': {'current_skill_counts': torch.bincount(env.s.skill, minlength=4).cpu().tolist(),
'ready_count': int(env.s.ready.sum()), 'max_age_s': float(env.s.age.max()),
'max_held_s': float(env.s.held.max()), 'min_gap_m': float((env.s.foot_center()[:, 2]-env.s.bundle['foot']['radius']).min())}}
history.append(row)
with (output / 'progress.jsonl').open('a') as stream:
stream.write(json.dumps(row, allow_nan=False) + '\n')
save_checkpoint(output / 'latest.pt', model, opt, identity, update, row)
if update % cfg['checkpoint_interval'] == 0 or update == update_limit:
save_checkpoint(output / f'checkpoint_u{update:04d}.pt', model, opt, identity, update, row)
torch.save(trace.snapshot(env.s), output / f'state_u{update:04d}.pt')
print(json.dumps({'training': row}), flush=True)
changed = {k: float((v.detach() - initial_actor[k]).abs().max()) for k, v in model.actor.named_parameters()}
trace.save()
return {'training_complete': True, 'updates': len(history), 'optimizer_device': env.device,
'environment_transitions': total_transitions, 'actual_ticks': env.total_ticks,
'failures': failures, 'actor_parameter_max_changes': changed,
'skill_control_samples': skill_control_samples, 'feedback_skill_samples': driver.skill_counts,
'transition_count': driver.transition_count,
'candidate_selection_pending_development_validation': True, 'functional_success': False}
def run(payload, guard, write):
import torch
from isaaclab.app import AppLauncher
output = Path(payload['output'])
gui = payload['mode'] == 'gui'
launcher = app = env = hooks = paint = trace = None
result = {'success': False, 'functional_success': False, 'mode': payload['mode']}
code = 1
try:
options = {'visualizer': 'kit'} if gui else {}
launcher = AppLauncher(headless=not gui, device='cuda:0', fast_shutdown=True,
kit_args=f'--/log/file={output / "native" / "kit.log"}', **options)
app = launcher.app
import evaluate_cycle_sim_residual as owner
from SkillFactory.legs.contact_cycle_sim import evaluation_binding as binding
from SkillFactory.tasks.manager_based.contact_cycle_sim.evaluation_env import RuntimeHooks
from SkillFactory.tasks.manager_based.four_skills.cfg import FourSkillsEnvCfg
from SkillFactory.tasks.manager_based.four_skills.env import FourSkillsEnv
for name in dict.fromkeys((*binding.REQUIRED_RUNTIME, *binding.ISAAC_PINS)):
importlib.import_module(name)
verify_project_imports()
owner.preimport_replicator()
owner.preimport_constructor_modules(binding)
owner.physx_constructor_export(binding, resolve=True)
if torch.cuda.get_device_name(0) != 'NVIDIA GeForce RTX 5080':
raise RuntimeError('Only approved RTX5080, no CPU training fallback')
cfg = FourSkillsEnvCfg()
cfg.scene.num_envs = 128 if payload['mode'] == 'train' else 1
cfg.training_mode = payload['mode'] == 'train'
cfg.force_tier = payload['tier']
cfg.seed = payload['seed']
cfg.sim.device = 'cuda:0'
if gui:
from isaaclab_visualizers.kit import KitVisualizerCfg
cfg.sim.visualizer_cfgs = [KitVisualizerCfg()]
owner.preforward_video_recorder(cfg, importlib.import_module(owner.TASK + 'evaluation_env'))
owner.preimport_config_references(cfg, binding)
normalize_scene(cfg, binding)
before = binding.canonical_config(cfg)
(output / 'effective-config-before.json').write_bytes(before)
trace = Trace(output, guard, full=not cfg.training_mode)
def before_step(h):
guard()
if h.attempted_steps > payload['max_physics_calls']:
raise RuntimeError('Physical call cap exhausted before call')
with RuntimeHooks.isaac(before_step=before_step) as hooks:
env = FourSkillsEnv(cfg, runtime_hooks=hooks, guard=guard, record=trace)
write(output / 'runtime-identity.json', {
'task': cfg.four_parameters['task_id'], 'torch': str(torch.__version__),
'device': torch.cuda.get_device_name(0), 'num_envs': env.num_envs,
'physics_dt_s': env.physics_dt, 'decimation': cfg.decimation,
'payload_body_index': env.s.payload, 'body_names': list(env.s.robot.body_names),
'joint_order': list(env.s.bundle['joint_order']), 'fixed_base': bool(env.s.robot.is_fixed_base),
'effective_weight_N': env.s.weight.cpu().tolist(),
'auxiliary_parameters': cfg.cycle_parameters['support'],
'guide_stiffness': cfg.support_parameters['guide_stiffness'],
'guide_damping': cfg.support_parameters['guide_damping'],
'guide_wrench_limit': cfg.support_parameters['guide_wrench_limit']})
actual = binding.canonical_config(cfg)
(output / 'effective-config-actual.json').write_bytes(actual)
if before != actual:
raise RuntimeError('Constructor changed effective bound config')
if gui:
from SkillFactory.tasks.manager_based.four_skills.gui import Presentation
paint = Presentation(app, env, output)
paint.controller_label = payload['controller']
env.paint = paint
model = None
if payload.get('checkpoint'):
from SkillFactory.legs.four_skills.model import load_policy
model, ckpt = load_policy(payload['checkpoint'], 'cuda:0', payload['checkpoint_sha256'])
if (ckpt['identity'].get('config_sha256') != payload['config_sha256']
or ckpt['identity'].get('task') != cfg.four_parameters['task_id']):
raise RuntimeError('Frozen new-stage checkpoint/config/task identity mismatch')
write(output / 'checkpoint-identity.json', {'sha256': payload['checkpoint_sha256'],
'update': ckpt['update'], 'identity': ckpt['identity']})
if cfg.training_mode:
result.update(train(env, payload, output, guard, trace))
else:
result.update(execute_single(env, payload, output, guard, trace, model))
guard()
write(output / 'runtime-project-imports.json', verify_project_imports())
if binding.canonical_config(env.cfg) != actual or env.cfg is not cfg:
raise RuntimeError('Actual config changed during rollout')
if gui:
paint.finish(result['functional_success'], safely_stopped=result.get('safe_stop_success', False))
result['gui'] = paint.report()
result['physics_counts'] = {'attempted': hooks.attempted_steps, 'completed': hooks.completed_steps,
'rollout': hooks.rollout_steps, 'initialization': hooks.initialization_steps,
'resets': hooks.reset_calls, 'pose_writes': hooks.pose_write_calls,
'violations': hooks.violations}
if hooks.violations:
raise RuntimeError('Runtime reset/pose/ownership violation')
torch.save(trace.snapshot(env.s), output / 'terminal-state.pt')
env.close()
env = None
result['success'] = True # integrity only; task function is separate.
result['postclose_identity_verified'] = True
code = 0
except BaseException:
result.update(success=False, error=traceback.format_exc())
traceback.print_exc()
finally:
if hooks is not None:
result['physics_counts'] = {'attempted': hooks.attempted_steps, 'completed': hooks.completed_steps,
'rollout': hooks.rollout_steps, 'initialization': hooks.initialization_steps,
'resets': hooks.reset_calls, 'pose_writes': hooks.pose_write_calls,
'violations': hooks.violations}
if trace is not None:
try:
trace.save()
except BaseException:
result.update(success=False, trace_save_error=traceback.format_exc())
code = 1
if env is not None:
try:
if trace is not None:
torch.save(trace.snapshot(env.s), output / 'terminal-state.pt')
env.close()
except BaseException:
result.update(success=False, close_error=traceback.format_exc())
code = 1
if paint is not None:
try:
paint.release()
except BaseException:
result.update(success=False, gui_release_error=traceback.format_exc())
code = 1
write(output / 'worker-result.json', result)
if app is not None:
app.close(exit_code=code)
return code