release: SkillFactory v0.0.1

Add declarative single-leg import, fixed-anchor benchmark, masked shared-policy PPO prototype, regression tests, and consolidated documentation.
This commit is contained in:
2026-10-10 09:58:30 +08:00
parent 180f8f7f97
commit 7f39abb613
52 changed files with 4370 additions and 147 deletions
@@ -0,0 +1 @@
"""Local, reproducibly generated simulation assets."""
@@ -0,0 +1,55 @@
"""Go2 FL fixed-anchor configuration; asset conversion is always explicit."""
import json
import os
from pathlib import Path
import isaaclab.sim as sim_utils
from isaaclab.actuators import IdealPDActuatorCfg
from isaaclab.assets import ArticulationCfg
ROOT = Path(__file__).resolve().parents[4]
ASSET_DIR = Path(os.environ.get("SKILLFACTORY_GO2_ASSET_DIR", ROOT / "assets/generated/go2_fl"))
JOINT_NAMES = ["FL_hip_joint", "FL_thigh_joint", "FL_calf_joint"]
if not (ASSET_DIR / "conversion.json").is_file():
raise FileNotFoundError(
"Build Go2 asset first: scripts/assets/extract_go2_leg.py then scripts/assets/convert_go2_leg.py"
)
MANIFEST = json.loads((ASSET_DIR / "manifest.json").read_text())
CONVERSION = json.loads((ASSET_DIR / "conversion.json").read_text())
USD_PATH = ASSET_DIR / CONVERSION["usd_path"]
if not USD_PATH.is_file():
raise FileNotFoundError(f"Missing generated USD: {USD_PATH}; rerun converter")
FOOT_PATH = CONVERSION["body_paths"]["FL_foot"]
ANCHOR_PATH = CONVERSION["body_paths"]["anchor_base"]
EFFORT_LIMITS = {j["name"]: j["limits"]["effort"] for j in MANIFEST["joints"]}
VELOCITY_LIMITS = {j["name"]: j["limits"]["velocity"] for j in MANIFEST["joints"]}
GO2_LEG_CFG = ArticulationCfg(
prim_path="{ENV_REGEX_NS}/Robot",
spawn=sim_utils.UsdFileCfg(
usd_path=str(USD_PATH),
activate_contact_sensors=True,
rigid_props=sim_utils.RigidBodyPropertiesCfg(disable_gravity=False, max_depenetration_velocity=1.0),
articulation_props=sim_utils.ArticulationRootPropertiesCfg(
enabled_self_collisions=False, solver_position_iteration_count=8, solver_velocity_iteration_count=4
),
),
init_state=ArticulationCfg.InitialStateCfg(
pos=(0.0, 0.0, MANIFEST["anchor"]["default_height"]),
joint_pos=dict(zip(JOINT_NAMES, MANIFEST["control"]["q_default"])),
joint_vel={".*": 0.0},
),
soft_joint_pos_limit_factor=0.9,
actuators={
"leg": IdealPDActuatorCfg(
joint_names_expr=JOINT_NAMES,
stiffness=MANIFEST["control"]["kp"],
damping=MANIFEST["control"]["kd"],
effort_limit=EFFORT_LIMITS,
effort_limit_sim=EFFORT_LIMITS,
velocity_limit=VELOCITY_LIMITS,
velocity_limit_sim=VELOCITY_LIMITS,
)
},
)
@@ -0,0 +1,5 @@
"""Single-leg asset and learning contracts; no Kit imports at module load."""
from .spec import JointSpec, LegSpec
__all__ = ["JointSpec", "LegSpec"]
@@ -0,0 +1,180 @@
"""URMA-inspired shared actor/critic; no claim of a pretrained modular Skill."""
from __future__ import annotations
import math
import torch
from torch import nn
from torch.distributions import Normal
from .tokens import FEATURE_SCHEMA
def mlp(inputs, hidden, outputs):
return nn.Sequential(nn.Linear(inputs, hidden), nn.LayerNorm(hidden), nn.ELU(), nn.Linear(hidden, outputs))
class DescriptionRouter(nn.Module):
"""Per-latent-dimension softmax over valid entities, not full Transformer attention."""
def __init__(self, descriptions, observations, hidden=64, latent=32):
super().__init__()
self.description = mlp(descriptions, hidden, latent)
self.observation = mlp(observations, hidden, latent)
self.temperature = nn.Parameter(torch.tensor(1.0))
def forward(self, description, observation, mask):
mask = mask.bool()
description = torch.where(mask[..., None], description, 0.0)
observation = torch.where(mask[..., None], observation, 0.0)
encoded = self.description(description)
logits = encoded / (self.temperature.clamp(min=0.0) + 0.015)
logits = logits.masked_fill(~mask[..., None], -torch.inf)
logits = torch.where(mask.any(dim=1)[:, None, None], logits, 0.0)
weights = logits.softmax(dim=1) * mask[..., None]
local = weights * self.observation(observation)
return local.sum(dim=1), local, encoded
class LegActorCritic(nn.Module):
def __init__(self, hidden=64, latent=32):
super().__init__()
self.config = {"hidden": hidden, "latent": latent}
self.joints = DescriptionRouter(len(FEATURE_SCHEMA["joint_description"]), 3, hidden, latent)
self.feet = DescriptionRouter(
len(FEATURE_SCHEMA["effector_description"]), len(FEATURE_SCHEMA["effector_observation"]), hidden, latent
)
general = len(FEATURE_SCHEMA["context"]) + len(FEATURE_SCHEMA["goal"])
self.core = mlp(latent * 2 + general, hidden, latent)
self.mean = mlp(latent * 3, hidden, 1)
self.log_std = mlp(latent, hidden, 1)
nn.init.zeros_(self.log_std[-1].weight)
nn.init.constant_(self.log_std[-1].bias, math.log(0.3))
self.critic = mlp(latent * 2 + general, hidden, 1)
self.float() # feature contract is float32, independent of torch's global default
def _features(self, batch):
pooled, local, descriptions = self.joints(
batch["joint_description"], batch["joint_observation"], batch["joint_mask"]
)
feet, _, _ = self.feet(batch["effector_description"], batch["effector_observation"], batch["effector_mask"])
shared = torch.cat((pooled, feet, batch["context"], batch["goal"]), dim=-1)
return shared, local, descriptions
def distribution_value(self, batch):
shared, local, description = self._features(batch)
intent = self.core(shared)[:, None, :].expand_as(local)
mean = self.mean(torch.cat((description, intent, local), dim=-1)).squeeze(-1)
mask = batch["joint_mask"].bool()
mean = torch.where(mask, mean, 0.0)
std = self.log_std(description).squeeze(-1).clamp(-5.0, 1.0).exp()
std = torch.where(mask, std, 1.0)
return Normal(mean, std), self.critic(shared).squeeze(-1)
def act(self, batch, deterministic=False):
distribution, value = self.distribution_value(batch)
mask = batch["joint_mask"].bool()
action = torch.where(mask, distribution.mean if deterministic else distribution.sample(), 0.0)
log_prob = torch.where(mask, distribution.log_prob(action), 0.0).sum(dim=-1)
return action, log_prob, value
def evaluate(self, batch, actions):
distribution, value = self.distribution_value(batch)
mask = batch["joint_mask"].bool()
actions = torch.where(mask, actions, 0.0)
return (
torch.where(mask, distribution.log_prob(actions), 0.0).sum(dim=-1),
torch.where(mask, distribution.entropy(), 0.0).sum(dim=-1),
value,
)
def value(self, batch):
return self.critic(self._features(batch)[0]).squeeze(-1)
def save_policy(model, path, metadata=None):
torch.save(
{
"model_config": model.config,
"feature_schema": FEATURE_SCHEMA,
"state_dict": {k: v.detach().cpu() for k, v in model.state_dict().items()},
"metadata": metadata or {},
"trained_skill": False,
},
path,
)
def load_policy(path, device="cpu"):
checkpoint = torch.load(path, map_location=device, weights_only=True)
if checkpoint["feature_schema"] != FEATURE_SCHEMA:
raise ValueError("Incompatible feature schema")
model = LegActorCritic(**checkpoint["model_config"]).to(device)
model.load_state_dict(checkpoint["state_dict"])
return model
@torch.no_grad()
def compute_gae(rewards, values, next_values, done, gamma=0.99, lam=0.95):
"""next_values includes terminal-state bootstrap for timeouts, zero for true termination."""
if (
not (rewards.shape == values.shape == next_values.shape == done.shape)
or rewards.ndim != 2
or min(rewards.shape) < 1
or done.dtype != torch.bool
):
raise ValueError("GAE requires nonempty matching [T,B] tensors and bool done")
if not 0 <= gamma <= 1 or not 0 <= lam <= 1:
raise ValueError("GAE discounts must lie in [0,1]")
if any(not torch.isfinite(value).all() for value in (rewards, values, next_values)):
raise ValueError("Non-finite GAE input")
advantage = torch.zeros_like(rewards)
carry = torch.zeros_like(rewards[0])
for t in reversed(range(len(rewards))):
delta = rewards[t] + gamma * next_values[t] - values[t]
carry = delta + gamma * lam * (~done[t].bool()) * carry
advantage[t] = carry
return advantage, advantage + values
def ppo_update(model, optimizer, data, epochs=2, minibatch=128):
if type(epochs) is not int or type(minibatch) is not int or min(epochs, minibatch) < 1:
raise ValueError("PPO epochs/minibatch must be positive integers")
advantage, returns = compute_gae(data["rewards"], data["values"], data["next_values"], data["done"])
tensors = [*data["observation"].values(), data["actions"], data["log_prob"]]
if (
data["log_prob"].shape != data["rewards"].shape
or any(value.shape[:2] != data["rewards"].shape for value in tensors)
or any(not torch.isfinite(value).all() for value in tensors)
):
raise ValueError("PPO requires finite, consistently shaped [T,B,...] rollout tensors")
batch = {k: v.detach().flatten(0, 1) for k, v in data["observation"].items()}
actions = data["actions"].detach().flatten(0, 1)
old_log = data["log_prob"].detach().flatten(0, 1)
advantage = advantage.flatten()
advantage = (advantage - advantage.mean()) / (advantage.std(unbiased=False) + 1e-8)
returns = returns.flatten()
rows = []
for _ in range(epochs):
indices = torch.randperm(len(actions), device=actions.device)
for start in range(0, len(actions), minibatch):
ids = indices[start : start + minibatch]
current = {k: v[ids] for k, v in batch.items()}
log_prob, entropy, value = model.evaluate(current, actions[ids])
ratio = (log_prob - old_log[ids]).exp()
if not torch.isfinite(ratio).all():
raise RuntimeError("Non-finite PPO probability ratio")
surrogate = torch.minimum(ratio * advantage[ids], ratio.clamp(0.8, 1.2) * advantage[ids])
actor = -surrogate.mean()
critic = (value - returns[ids]).square().mean()
loss = actor + 0.5 * critic - 0.01 * entropy.mean()
if not torch.isfinite(loss):
raise RuntimeError("Non-finite PPO loss")
optimizer.zero_grad(set_to_none=True)
loss.backward()
norm = nn.utils.clip_grad_norm_(model.parameters(), 1.0, error_if_nonfinite=True)
optimizer.step()
rows.append(torch.stack((actor.detach(), critic.detach(), entropy.detach().mean(), norm.detach())))
means = torch.stack(rows).mean(dim=0).cpu().tolist()
return dict(zip(("actor_loss", "value_loss", "entropy", "gradient_norm"), means))
@@ -0,0 +1,144 @@
"""Versioned declarative single-leg import contract (SI units)."""
from __future__ import annotations
import json
import math
import re
from dataclasses import asdict, dataclass
from pathlib import Path
@dataclass(frozen=True)
class JointSpec:
name: str
q_default: float
action_scale: float
kp: float
kd: float
def __post_init__(self):
values = (self.q_default, self.action_scale, self.kp, self.kd)
if not isinstance(self.name, str) or not self.name.strip() or not all(math.isfinite(v) for v in values):
raise ValueError("Joint names and finite control parameters are required")
if self.action_scale <= 0 or self.kp <= 0 or self.kd < 0:
raise ValueError("Require action_scale>0, kp>0, kd>=0")
@dataclass(frozen=True)
class LegSpec:
schema_version: int
part_id: str
source_root: Path
urdf: str
package_name: str
license_file: str
license_id: str
mount_joint: str
auxiliary_root_joints: tuple[str, ...]
joints: tuple[JointSpec, ...]
foot_body: str
foot_collision_index: int = 0
soft_limit_factor: float = 0.9
physics_dt: float = 0.002
decimation: int = 5
def __post_init__(self):
if (
type(self.schema_version) is not int
or self.schema_version != 1
or not re.fullmatch(r"[a-z][a-z0-9_]*", self.part_id)
):
raise ValueError("Expected schema_version=1 and a safe lowercase part_id")
names = [joint.name for joint in self.joints]
if not names or len(set(names)) != len(names) or self.mount_joint not in names:
raise ValueError("Require unique actuated joints including mount_joint")
if len(set(self.auxiliary_root_joints)) != len(self.auxiliary_root_joints) or set(
self.auxiliary_root_joints
) & set(names):
raise ValueError("Duplicate/actuated auxiliary roots")
if not self.foot_body or not self.package_name or not self.license_id or not self.license_file:
raise ValueError("Foot, source package, and license provenance are required")
if type(self.foot_collision_index) is not int or self.foot_collision_index < 0:
raise ValueError("foot_collision_index must be a nonnegative integer")
if not math.isfinite(self.soft_limit_factor) or not 0 < self.soft_limit_factor <= 1:
raise ValueError("Invalid soft_limit_factor")
if not math.isfinite(self.physics_dt) or self.physics_dt <= 0:
raise ValueError("Invalid physics_dt")
if type(self.decimation) is not int or self.decimation < 1:
raise ValueError("decimation must be a positive integer")
path = (self.source_root / self.urdf).resolve()
if not path.is_relative_to(self.source_root.resolve()):
raise ValueError("URDF must be inside source_root")
@classmethod
def load(cls, path: str | Path, source_root: str | Path | None = None) -> LegSpec:
return cls.from_dict(json.loads(Path(path).read_text()), source_root)
@classmethod
def from_dict(cls, data: dict, source_root: str | Path | None = None) -> LegSpec:
values = dict(data)
values["source_root"] = Path(source_root or values["source_root"]).expanduser().resolve()
values["joints"] = tuple(JointSpec(**item) for item in values["joints"])
values["auxiliary_root_joints"] = tuple(values.get("auxiliary_root_joints", []))
return cls(**values)
def to_dict(self) -> dict:
data = asdict(self)
data["source_root"] = str(self.source_root)
return data
def bundle_file(directory: Path, relative: str) -> Path:
"""Resolve a bundle-owned file, rejecting traversal and escaping symlinks."""
path = (directory / relative).resolve()
if not path.is_relative_to(directory):
raise ValueError(f"Unsafe bundle path: {relative}")
if not path.is_file():
raise FileNotFoundError(path)
return path
def load_bundle(directory: str | Path, require_usd: bool = False) -> dict:
"""Refuse stale URDF/manifest/converted USD combinations, before using physics."""
import hashlib
directory = Path(directory).resolve()
manifest_path = bundle_file(directory, "manifest.json")
bundle = json.loads(manifest_path.read_text())
if bundle.get("schema_version") != 2 or bundle.get("kind") != "single_leg":
raise ValueError("Expected single_leg asset manifest v2")
LegSpec.from_dict(bundle["spec"])
if any(not math.isfinite(bundle[key]) or bundle[key] <= 0 for key in ("length_scale_m", "known_mass_scale_kg")):
raise ValueError("Require positive finite bundle scales")
if hashlib.sha256(bundle_file(directory, "leg.urdf").read_bytes()).hexdigest() != bundle["derived_urdf_sha256"]:
raise ValueError("Derived URDF hash mismatch; rebuild the leg")
if hashlib.sha256(bundle_file(directory, "LICENSE").read_bytes()).hexdigest() != bundle["source"]["license_sha256"]:
raise ValueError("License provenance hash mismatch")
declared = [joint["name"] for joint in bundle["spec"]["joints"]]
if bundle["joint_order"] != declared or [joint["name"] for joint in bundle["joints"]] != declared:
raise ValueError("Manifest actuator order mismatch")
for relative, expected in bundle["resources"].items():
resource = bundle_file(directory, relative)
if hashlib.sha256(resource.read_bytes()).hexdigest() != expected:
raise ValueError(f"Resource hash/path mismatch: {relative}")
if require_usd:
conversion = json.loads(bundle_file(directory, "conversion.json").read_text())
if conversion["manifest_sha256"] != hashlib.sha256(manifest_path.read_bytes()).hexdigest():
raise ValueError("Stale USD conversion; rerun scripts/assets/convert_leg.py")
usd = bundle_file(directory, conversion["usd_path"])
layers = {bundle_file(directory, relative) for relative in conversion["used_layers"]}
if usd not in layers or conversion.get("fixed_to_world") is not True:
raise ValueError("USD entry layer/fixed support contract mismatch")
expected_bodies = set(bundle["links"]) | {bundle["anchor_body"]}
for key, expected in (("body_paths", expected_bodies), ("joint_paths", set(declared))):
mapping = conversion[key]
if set(mapping) != expected or len(set(mapping.values())) != len(mapping):
raise ValueError(f"USD {key} mapping mismatch")
if any(
not path.startswith("/") or any(p in ("", ".", "..") for p in path[1:].split("/"))
for path in mapping.values()
):
raise ValueError(f"Unsafe USD {key} prim path")
bundle["conversion"] = conversion
return bundle
@@ -0,0 +1,232 @@
"""Simulator-independent token schema v1 and batched arbitrary-axis FK."""
from __future__ import annotations
import math
import torch
from .urdf import forward, multiply, pose
FEATURE_SCHEMA = {
"version": 1,
"joint_description": [
"pivot_x/L",
"pivot_y/L",
"pivot_z/L",
"axis_x",
"axis_y",
"axis_z",
"q_nominal/pi",
"control_min/pi",
"control_max/pi",
"effort/Emax",
"velocity/Vmax",
"kp*pi/Emax",
"kd*Vmax/Emax",
"action_scale/pi",
"child_mass/Mknown",
"child_Ixx/ML2",
"child_Ixy/ML2",
"child_Ixz/ML2",
"child_Iyy/ML2",
"child_Iyz/ML2",
"child_Izz/ML2",
"child_com_x/L",
"child_com_y/L",
"child_com_z/L",
"inertial_valid",
],
"joint_observation": ["(q-q_nominal)/action_scale", "dq/velocity_limit", "last_applied_action"],
"effector_description": [
"nominal_x/L",
"nominal_y/L",
"nominal_z/L",
"collision_x/L",
"collision_y/L",
"collision_z/L",
"collision_qx",
"collision_qy",
"collision_qz",
"collision_qw",
"sphere_radius/L",
"box_x/L",
"box_y/L",
"box_z/L",
"is_sphere",
"is_box",
],
"effector_observation": [
"link_x/L",
"link_y/L",
"link_z/L",
"link_vx/(L/s)",
"link_vy/(L/s)",
"link_vz/(L/s)",
"normal_Fx/(Mknown*g)",
"normal_Fy/(Mknown*g)",
"normal_Fz/(Mknown*g)",
"contact",
"selected_collision_gap/L",
],
"context": [
"gravity_x",
"gravity_y",
"gravity_z",
"control_dt_s",
"ground_z_relative_to_anchor/L",
"length_scale_m",
"known_mass_scale_kg",
"max_effort/(Mknown*g*L)",
"max_joint_velocity_rad_per_s",
"has_missing_source_inertia",
],
"goal": ["target_link_x/L", "target_link_y/L", "target_link_z/L"],
"force_semantics": "normal contact force only; no friction wrench",
"inertia_semantics": (
"Source child COM inertia and relative COM offset rotated into anchor nominal frame; "
"not fixed-subtree aggregated"
),
"mass_semantics": "Known source mass excludes synthetic anchor; not a prediction of support force",
}
class LegKinematics:
def __init__(self, bundle, device="cpu"):
self.bundle = bundle
self.device = torch.empty(0, device=device, dtype=torch.float32).device
self.names = bundle["joint_order"]
self.indices = {name: i for i, name in enumerate(self.names)}
self.eye = torch.eye(4, device=self.device, dtype=torch.float32)
self.nodes = []
for node in bundle["tree"]:
static = self.eye.new_tensor(pose(node["xyz"], node["rpy"]))
axis = self.eye.new_tensor(node["axis"])
x, y, z = node["axis"]
skew = self.eye.new_tensor([[0, -z, y], [z, 0, -x], [-y, x, 0]])
self.nodes.append((node, static, axis[:, None] * axis[None, :], skew))
def forward(self, q):
if q.ndim != 2 or q.shape[1] != len(self.names) or q.device != self.device or q.dtype != torch.float32:
raise ValueError("FK requires float32 [B, declared_joint_count] on the configured device")
b = len(q)
poses = {self.bundle["anchor_body"]: self.eye.expand(b, -1, -1)}
for node, static, outer, skew in self.nodes:
frame = poses[node["parent"]] @ static
if node["type"] == "revolute":
angle = q[:, self.indices[node["name"]]]
c, s = angle.cos()[:, None, None], angle.sin()[:, None, None]
rot = c * self.eye[:3, :3] + (1 - c) * outer + s * skew
local = self.eye.expand(b, -1, -1).clone()
local[:, :3, :3] = rot
frame = frame @ local
poses[node["child"]] = frame
return poses
def foot_position(self, q):
return self.forward(q)[self.bundle["foot"]["body"]][:, :3, 3]
def static_tokens(bundle, max_joints, device="cpu"):
n = len(bundle["joints"])
if type(max_joints) is not int or max_joints < n:
raise ValueError(f"max_joints must be >= {n}")
length, mass = bundle["length_scale_m"], bundle["known_mass_scale_kg"]
effort = max(j["limits"]["effort"] for j in bundle["joints"])
velocity = max(j["limits"]["velocity"] for j in bundle["joints"])
rows = []
nominal_poses, _ = forward(bundle["tree"], {j["name"]: j["q_default"] for j in bundle["spec"]["joints"]})
for record, control in zip(bundle["joints"], bundle["spec"]["joints"]):
child = bundle["links"][record["child"]]
nominal, scale = control["q_default"], control["action_scale"]
inertia, com = [0.0] * 6, [0.0] * 3
if child is not None:
frame = multiply(nominal_poses[record["child"]], pose(child["xyz"], child["rpy"]))
a, b, c, d, e, f = child["tensor6"]
tensor = [[a, b, c], [b, d, e], [c, e, f]]
rotated = [
[
sum(frame[i][k] * tensor[k][column] * frame[j][column] for k in range(3) for column in range(3))
for j in range(3)
]
for i in range(3)
]
inertia = [rotated[i][j] for i, j in ((0, 0), (0, 1), (0, 2), (1, 1), (1, 2), (2, 2))]
com = [frame[i][3] - record["nominal_position"][i] for i in range(3)]
low = max(record["soft_limits"][0], nominal - scale)
high = min(record["soft_limits"][1], nominal + scale)
rows.append(
[
*(v / length for v in record["nominal_position"]),
*record["nominal_axis"],
nominal / math.pi,
low / math.pi,
high / math.pi,
record["limits"]["effort"] / effort,
record["limits"]["velocity"] / velocity,
control["kp"] * math.pi / effort,
control["kd"] * velocity / effort,
scale / math.pi,
child["mass"] / mass if child else 0.0,
*(v / (mass * length**2) for v in inertia),
*(v / length for v in com),
float(child is not None),
]
)
joint = torch.zeros(1, max_joints, len(FEATURE_SCHEMA["joint_description"]), device=device, dtype=torch.float32)
joint[0, :n] = torch.tensor(rows, device=device, dtype=torch.float32)
mask = torch.arange(max_joints, device=device)[None, :] < n
foot = bundle["foot"]
r, p, y = (v / 2 for v in foot["rpy"])
cr, sr, cp, sp, cy, sy = math.cos(r), math.sin(r), math.cos(p), math.sin(p), math.cos(y), math.sin(y)
quat = [
sr * cp * cy - cr * sp * sy,
cr * sp * cy + sr * cp * sy,
cr * cp * sy - sr * sp * cy,
cr * cp * cy + sr * sp * sy,
]
values = [
*(v / length for v in foot["nominal_position"]),
*(v / length for v in foot["xyz"]),
*quat,
foot.get("radius", 0.0) / length,
*(v / length for v in foot.get("size", [0.0] * 3)),
float(foot["type"] == "sphere"),
float(foot["type"] == "box"),
]
return {
"joint_description": joint,
"joint_mask": mask,
"effector_description": torch.tensor(values, device=device, dtype=torch.float32)[None, None, :],
"effector_mask": torch.ones(1, 1, device=device, dtype=torch.bool),
}
def pad_joint_observations(q, dq, previous_action, bundle, max_joints):
n = len(bundle["joint_order"])
if any(value.dtype != torch.float32 or value.device != q.device for value in (q, dq, previous_action)):
raise ValueError("Joint states/actions require float32 on one device")
if q.shape != dq.shape or q.shape != previous_action.shape or q.ndim != 2 or q.shape[1] != n or max_joints < n:
raise ValueError("Joint state/action shape mismatch")
controls = bundle["spec"]["joints"]
nominal = q.new_tensor([j["q_default"] for j in controls])
scale = q.new_tensor([j["action_scale"] for j in controls])
limit = q.new_tensor([j["limits"]["velocity"] for j in bundle["joints"]])
result = q.new_zeros(len(q), max_joints, 3)
result[:, :n] = torch.stack(((q - nominal) / scale, dq / limit, previous_action), dim=-1)
return result
def native_actions(actions, num_envs, num_joints, max_joints, device):
if not 0 < num_joints <= max_joints or num_envs < 1:
raise ValueError("Require B>=1 and 1<=native_DoF<=Jmax")
if (
actions.shape != (num_envs, max_joints)
or actions.dtype != torch.float32
or actions.device != torch.device(device)
):
raise ValueError("Expected float32 [B,Jmax] actions on the environment device")
result = actions[:, :num_joints]
if not torch.isfinite(result).all():
raise ValueError("Non-finite active action")
return result
@@ -0,0 +1,411 @@
"""Pure-stdlib tree extraction and arbitrary-axis FK; never loads a simulator."""
from __future__ import annotations
import copy
import hashlib
import json
import math
import shutil
import subprocess
import xml.etree.ElementTree as ET
from pathlib import Path
from .spec import LegSpec
ANCHOR = "anchor_base"
def digest(path):
return hashlib.sha256(Path(path).read_bytes()).hexdigest()
def vector(text, length=3):
values = [float(v) for v in text.split()]
if len(values) != length or not all(math.isfinite(v) for v in values):
raise ValueError(f"Expected {length} finite values: {text}")
return values
def origin(element):
child = element.find("origin")
return {key: vector(child.get(key, "0 0 0") if child is not None else "0 0 0") for key in ("xyz", "rpy")}
def identity():
return [[float(i == j) for j in range(4)] for i in range(4)]
def multiply(a, b):
return [[sum(a[i][k] * b[k][j] for k in range(4)) for j in range(4)] for i in range(4)]
def pose(xyz, rpy):
r, p, y = rpy
cr, sr, cp, sp, cy, sy = math.cos(r), math.sin(r), math.cos(p), math.sin(p), math.cos(y), math.sin(y)
return [
[cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr, xyz[0]],
[sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr, xyz[1]],
[-sp, cp * sr, cp * cr, xyz[2]],
[0.0, 0.0, 0.0, 1.0],
]
def inverse(t):
result = identity()
for i in range(3):
for j in range(3):
result[i][j] = t[j][i]
result[i][3] = -sum(t[k][i] * t[k][3] for k in range(3))
return result
def rotation(axis, angle):
x, y, z = axis
cross = [[0, -z, y], [z, 0, -x], [-y, x, 0]]
result = identity()
c, s = math.cos(angle), math.sin(angle)
for i in range(3):
for j in range(3):
result[i][j] = c * float(i == j) + (1 - c) * axis[i] * axis[j] + s * cross[i][j]
return result
def inertial(link):
item = link.find("inertial")
if item is None:
return None
mass = float(item.find("mass").get("value"))
values = [float(item.find("inertia").get(k, "0")) for k in ("ixx", "ixy", "ixz", "iyy", "iyz", "izz")]
a, b, c, d, e, f = values
det = a * (d * f - e * e) - b * (b * f - c * e) + c * (b * e - c * d)
if not all(math.isfinite(v) for v in [mass, *values]) or mass <= 0 or min(a, a * d - b * b, det) <= 0:
raise ValueError(f"Invalid complete inertia: {link.get('name')}")
return {"mass": mass, "tensor6": values, **origin(item)}
def forward(tree, q, anchor=ANCHOR):
links = {anchor: identity()}
pivots = {}
for node in tree:
pivot = multiply(links[node["parent"]], pose(node["xyz"], node["rpy"]))
pivots[node["name"]] = pivot
links[node["child"]] = (
multiply(pivot, rotation(node["axis"], q[node["name"]])) if node["type"] == "revolute" else pivot
)
return links, pivots
def _inside(path, root):
path = path.resolve()
if not path.is_relative_to(root):
raise ValueError(f"Resource escapes source_root: {path}")
if not path.is_file():
raise FileNotFoundError(path)
return path
def _source_tree(original):
link_nodes = original.findall("link")
joint_nodes = original.findall("joint")
links = {item.get("name"): item for item in link_nodes}
joints = {item.get("name"): item for item in joint_nodes}
if (
not all(links)
or not all(joints)
or len(links) != len(link_nodes)
or len(joints) != len(joint_nodes)
or ANCHOR in links
):
raise ValueError("Duplicate names or reserved anchor_base link")
parent_of = {}
for joint in joint_nodes:
if joint.find("parent") is None or joint.find("child") is None:
raise ValueError(f"Missing parent/child: {joint.get('name')}")
parent, child = joint.find("parent").get("link"), joint.find("child").get("link")
if parent not in links or child not in links or child in parent_of:
raise ValueError("Source must be a valid tree (missing link/multiple parents)")
parent_of[child] = parent
for name in links:
seen = set()
while name in parent_of:
if name in seen:
raise ValueError("Cyclic URDF")
seen.add(name)
name = parent_of[name]
return link_nodes, joint_nodes, links, joints
def _foot_geometry(spec, links, nominal_links):
collisions = links[spec.foot_body].findall("collision")
if spec.foot_collision_index >= len(collisions):
raise ValueError("Missing selected foot collision")
collision = collisions[spec.foot_collision_index]
geometry = collision.find("geometry")
if geometry is None:
raise ValueError("Missing foot collision geometry")
sphere, box = geometry.find("sphere"), geometry.find("box")
if sphere is not None:
shape = {"type": "sphere", "radius": float(sphere.get("radius"))}
extent = shape["radius"]
elif box is not None:
shape = {"type": "box", "size": vector(box.get("size"))}
extent = math.sqrt(sum(v * v for v in shape["size"])) / 2
else:
raise ValueError("v1 foot gap requires a sphere or box collision (no mesh approximation)")
if not math.isfinite(extent) or extent <= 0 or (box is not None and min(shape["size"]) <= 0):
raise ValueError("Invalid foot collision dimensions")
foot = {
"body": spec.foot_body,
"collision_index": spec.foot_collision_index,
**shape,
**origin(collision),
"nominal_position": [nominal_links[spec.foot_body][i][3] for i in range(3)],
}
return foot, extent
def _resources(robot, spec, root, source):
resources = {}
for item in [*robot.findall(".//mesh"), *robot.findall(".//texture")]:
uri = item.get("filename")
prefix = f"package://{spec.package_name}/"
if uri.startswith("package://"):
if not uri.startswith(prefix):
raise ValueError(f"Undeclared resource package: {uri}")
src = root / uri[len(prefix) :]
elif "://" in uri:
raise ValueError(f"Unsupported resource URI: {uri}")
else:
src = source.parent / uri
src = _inside(src, root)
relative = src.relative_to(root)
resources[relative.as_posix()] = src
item.set("filename", relative.as_posix())
if src.suffix.lower() == ".dae":
for image in ET.parse(src).getroot().findall(".//{*}image/{*}init_from"):
image_src = _inside(src.parent / image.text.strip(), root)
resources[image_src.relative_to(root).as_posix()] = image_src
if src.suffix.lower() == ".obj" and "mtllib " in src.read_text(errors="replace"):
raise ValueError("Textured OBJ dependencies not yet supported; use DAE/STL")
return resources
def _output_directory(output, source_root, resources, protected):
output = Path(output).resolve()
if source_root.is_relative_to(output):
raise ValueError("Output must not overwrite the source root or its ancestors")
reserved = {"leg.urdf", "manifest.json", "conversion.json", "LICENSE"}
if reserved & resources.keys():
raise ValueError("Resource collides with a reserved bundle file")
for relative in (*resources, *reserved):
target = (output / relative).resolve()
if not target.is_relative_to(output):
raise ValueError(f"Output symlink escapes bundle: {relative}")
if target in protected:
raise ValueError(f"Output would overwrite source file: {target}")
return output
def _joint_limits(item, control, soft_factor):
name = item.get("name")
limit = item.find("limit")
keys = ("lower", "upper", "effort", "velocity")
if limit is None or any(limit.get(key) is None for key in keys):
raise ValueError(f"Missing bounded joint limits: {name}")
limits = {key: float(limit.get(key)) for key in keys}
if (
not all(math.isfinite(v) for v in limits.values())
or limits["lower"] >= limits["upper"]
or min(limits["effort"], limits["velocity"]) <= 0
):
raise ValueError(f"Invalid joint limits: {name}")
mid = (limits["lower"] + limits["upper"]) / 2
soft = [mid + soft_factor * (limits[key] - mid) for key in ("lower", "upper")]
if not soft[0] <= control.q_default <= soft[1]:
raise ValueError(f"Default outside soft joint range: {name}")
return limits, soft
def extract_leg(spec: LegSpec, output: str | Path) -> dict:
root = spec.source_root.resolve()
source = _inside(root / spec.urdf, root)
original = ET.parse(source).getroot()
link_nodes, joint_nodes, links, joints = _source_tree(original)
required = (spec.mount_joint, *spec.auxiliary_root_joints)
if any(name not in joints for name in required):
raise ValueError("Missing mount/auxiliary root joint")
source_parent = joints[spec.mount_joint].find("parent").get("link")
if any(joints[name].get("type") != "fixed" for name in spec.auxiliary_root_joints):
raise ValueError("Auxiliary roots must be fixed")
if any(joints[name].find("parent").get("link") != source_parent for name in required):
raise ValueError("Auxiliary roots must share the mount's parent frame")
keep = {joints[name].find("child").get("link") for name in required}
while True:
expanded = keep | {
item.find("child").get("link") for item in joint_nodes if item.find("parent").get("link") in keep
}
if expanded == keep:
break
keep = expanded
if spec.foot_body not in keep or source_parent in keep:
raise ValueError("Foot must belong to the isolated leg")
selected = [copy.deepcopy(item) for item in joint_nodes if item.find("child").get("link") in keep]
names = [item.name for item in spec.joints]
moving = [item.get("name") for item in selected if item.get("type") != "fixed"]
if set(moving) != set(names) or len(moving) != len(names):
raise ValueError(f"Uncontrolled/missing moving joints: {moving}, declared: {names}")
mount = origin(joints[spec.mount_joint])
mounting = pose(mount["xyz"], mount["rpy"])
records = {}
tree = []
for item in selected:
name = item.get("name")
if item.get("type") not in ("fixed", "revolute") or item.find("mimic") is not None:
raise ValueError(f"Only independent bounded revolute trees supported: {name}")
if item.find("parent").get("link") not in keep:
if name not in required:
raise ValueError(f"Unexpected external connection: {name}")
transformed = multiply(inverse(mounting), pose(**origin(item)))
xyz = [transformed[i][3] for i in range(3)]
pitch = math.asin(max(-1.0, min(1.0, -transformed[2][0])))
if abs(math.cos(pitch)) > 1e-8:
roll, yaw = (
math.atan2(transformed[2][1], transformed[2][2]),
math.atan2(transformed[1][0], transformed[0][0]),
)
else:
roll, yaw = math.atan2(-transformed[1][2], transformed[1][1]), 0.0
child = item.find("origin")
if child is None:
child = ET.SubElement(item, "origin")
child.set("xyz", " ".join(f"{v:.12g}" for v in xyz))
child.set("rpy", " ".join(f"{v:.12g}" for v in [roll, pitch, yaw]))
item.find("parent").set("link", ANCHOR)
axis = [1.0, 0.0, 0.0]
if item.get("type") == "revolute":
axis_node = item.find("axis")
axis = vector(axis_node.get("xyz") if axis_node is not None else "1 0 0")
norm = math.sqrt(sum(v * v for v in axis))
if norm < 1e-8:
raise ValueError("Zero joint axis")
axis = [v / norm for v in axis]
if axis_node is None:
axis_node = ET.SubElement(item, "axis")
axis_node.set("xyz", " ".join(f"{v:.12g}" for v in axis))
control = next(value for value in spec.joints if value.name == name)
limits, soft = _joint_limits(item, control, spec.soft_limit_factor)
records[name] = {
"name": name,
"child": item.find("child").get("link"),
"axis": axis,
"limits": limits,
"soft_limits": soft,
}
else:
item.set("dont_collapse", "true")
tree.append(
{
"name": name,
"type": item.get("type"),
"parent": item.find("parent").get("link"),
"child": item.find("child").get("link"),
"axis": axis,
**origin(item),
}
)
# Topological order is separate from declared actuator order.
ordered, available = [], {ANCHOR}
pending = list(tree)
while pending:
ready = [node for node in pending if node["parent"] in available]
if not ready:
raise ValueError("Disconnected/cyclic extracted leg")
ordered.extend(ready)
available.update(node["child"] for node in ready)
pending = [node for node in pending if node not in ready]
tree = ordered
link_records = {name: inertial(links[name]) for name in sorted(keep)}
nominal = {joint.name: joint.q_default for joint in spec.joints}
nominal_links, pivots = forward(tree, nominal)
for name, record in records.items():
frame = pivots[name]
record["nominal_position"] = [frame[i][3] for i in range(3)]
record["nominal_axis"] = [sum(frame[i][k] * record["axis"][k] for k in range(3)) for i in range(3)]
foot, extent = _foot_geometry(spec, links, nominal_links)
# Conservative path length; guarantees air-mode ground clearance for any joint pose.
by_child = {node["child"]: node for node in tree}
cursor = spec.foot_body
length = extent + math.sqrt(sum(v * v for v in foot["xyz"]))
while cursor != ANCHOR:
node = by_child[cursor]
length += math.sqrt(sum(v * v for v in node["xyz"]))
cursor = node["parent"]
mass = sum(value["mass"] for value in link_records.values() if value is not None)
if mass <= 0:
raise ValueError("Require known positive physical link mass")
robot = ET.Element("robot", name=spec.part_id + "_anchor")
for material in original.findall("material"):
robot.append(copy.deepcopy(material))
anchor = ET.SubElement(robot, "link", name=ANCHOR)
anchor.append(
ET.fromstring(
'<inertial><mass value="1"/>'
'<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/></inertial>'
)
)
for item in link_nodes:
if item.get("name") in keep:
robot.append(copy.deepcopy(item))
robot.extend(selected)
resources = _resources(robot, spec, root, source)
license_path = (root / spec.license_file).resolve()
if not license_path.is_file():
raise FileNotFoundError(license_path)
try:
commit = subprocess.check_output(
["git", "-C", str(root), "rev-parse", "HEAD"], text=True, stderr=subprocess.DEVNULL
).strip()
except (OSError, subprocess.CalledProcessError):
commit = None
output = _output_directory(output, root, resources, {source, license_path, *resources.values()})
output.mkdir(parents=True, exist_ok=True)
for relative, src in sorted(resources.items()):
dst = output / relative
dst.parent.mkdir(parents=True, exist_ok=True)
shutil.copy2(src, dst)
shutil.copy2(license_path, output / "LICENSE")
ET.indent(robot)
ET.ElementTree(robot).write(output / "leg.urdf", encoding="utf-8", xml_declaration=True)
bundle = {
"schema_version": 2,
"kind": "single_leg",
"spec": spec.to_dict(),
"source": {
"urdf": str(source),
"sha256": digest(source),
"commit": commit,
"license_id": spec.license_id,
"license_sha256": digest(license_path),
},
"derived_urdf_sha256": digest(output / "leg.urdf"),
"resources": {relative: digest(src) for relative, src in sorted(resources.items())},
"joint_order": names,
"joints": [records[name] for name in names],
"tree": tree,
"links": link_records,
"missing_inertial_links": [name for name, value in link_records.items() if value is None],
"anchor_body": ANCHOR,
"foot": foot,
"length_scale_m": length,
"known_mass_scale_kg": mass,
"mount": {
"source_parent": source_parent,
"source_parent_T_anchor": mount,
"anchor_frame": "Mount-joint zero frame, not the source parent/body frame",
},
"mass_semantics": "Sum of known source inertials, excludes synthetic anchor; not a foot force prediction",
"skill": {"weights": None, "latent": None, "trained": False, "reaction_wrench": None},
}
(output / "manifest.json").write_text(json.dumps(bundle, indent=2, sort_keys=True) + "\n")
return bundle
@@ -0,0 +1,11 @@
"""Generic, descriptor-driven single-leg reach training task."""
import gymnasium as gym
gym.register(
id="SkillFactory-LegSkill-v0",
entry_point=f"{__name__}.leg_env:LegSkillEnv",
disable_env_checker=True,
kwargs={"env_cfg_entry_point":f"{__name__}.leg_env_cfg:LegEnvCfg"},
)
@@ -0,0 +1,61 @@
"""Expose pre-reset terminal tokens for correct PPO timeout bootstrapping."""
import gymnasium as gym
import numpy as np
import torch
from isaaclab.envs import ManagerBasedRLEnv
from .mdp import state
class LegSkillEnv(ManagerBasedRLEnv):
def _configure_gym_env_spaces(self):
super()._configure_gym_env_spaces()
# The standard manager assumes float observations; masks are boolean.
policy = self.single_observation_space["policy"]
for name in ("joint_mask", "effector_mask"):
policy[name] = gym.spaces.Box(0, 1, shape=policy[name].shape, dtype=np.bool_)
self.observation_space = gym.vector.utils.batch_space(self.single_observation_space, self.num_envs)
def reset(self, seed=None, env_ids=None, options=None):
if env_ids is not None:
ids = torch.as_tensor(env_ids, device=self.device)
if ids.ndim != 1:
raise ValueError("env_ids must be one-dimensional")
if ids.numel() == 0:
if seed is not None:
raise ValueError("An empty reset cannot reseed the environment")
return self.observation_manager.compute(update_history=False), {}
if ids.dtype not in (torch.int32, torch.int64):
raise ValueError("env_ids must be integer indices")
if (ids < 0).any() or (ids >= self.num_envs).any() or len(ids.unique()) != len(ids):
raise ValueError("Invalid/duplicate env_ids")
env_ids = ids
return super().reset(seed=seed, env_ids=env_ids, options=options)
def step(self, action):
# Validate before ActionManager overwrites current/previous actions.
if (
not isinstance(action, torch.Tensor)
or action.shape != (self.num_envs, self.action_manager.total_action_dim)
or action.dtype != torch.float32
or not torch.isfinite(action).all()
):
raise ValueError("Expected finite float32 [B, native_DoF] actions")
self.extras["terminal_tokens"] = None
self.extras["terminal_ids"] = None
self._in_leg_step = True
try:
return super().step(action)
finally:
self._in_leg_step = False
def _reset_idx(self, env_ids):
if getattr(self, "_in_leg_step", False):
ids = torch.as_tensor(env_ids, device=self.device, dtype=torch.long)
self.extras["terminal_ids"] = ids.clone()
self.extras["terminal_tokens"] = {
name: value[ids].clone() for name, value in state(self).snapshot().items()
}
super()._reset_idx(env_ids)
@@ -0,0 +1,162 @@
"""One task config for any validated LegSpec bundle; Kit must already be launched."""
import os
import re
from pathlib import Path
from isaaclab_physx.physics import PhysxCfg
import isaaclab.sim as sim_utils
from isaaclab.actuators import IdealPDActuatorCfg
from isaaclab.assets import ArticulationCfg, AssetBaseCfg
from isaaclab.envs import ManagerBasedRLEnvCfg
from isaaclab.envs import mdp as base_mdp
from isaaclab.managers import EventTermCfg, ObservationGroupCfg, ObservationTermCfg, RewardTermCfg, TerminationTermCfg
from isaaclab.scene import InteractiveSceneCfg
from isaaclab.sensors import ContactSensorCfg
from isaaclab.utils.configclass import configclass
from SkillFactory.legs.spec import load_bundle
from SkillFactory.tasks.manager_based.single_leg_anchor.mdp.actions import BoundedJointPositionActionCfg
from . import mdp
ROOT = Path(__file__).resolve().parents[6]
@configclass
class LegSceneCfg(InteractiveSceneCfg):
ground = AssetBaseCfg(
prim_path="/World/ground",
spawn=sim_utils.UsdFileCfg(usd_path=str(Path(__file__).resolve().parents[3] / "legs/resources/ground.usda")),
)
light = AssetBaseCfg(prim_path="/World/light", spawn=sim_utils.DomeLightCfg(intensity=2000.0))
robot: ArticulationCfg = None
foot_contact: ContactSensorCfg = None
@configclass
class ActionsCfg:
joint_position: BoundedJointPositionActionCfg = None
@configclass
class CommandsCfg:
reach = mdp.ReachCommandCfg()
@configclass
class ObservationsCfg:
@configclass
class PolicyCfg(ObservationGroupCfg):
joint_description = ObservationTermCfg(func=mdp.observation, params={"name": "joint_description"})
joint_observation = ObservationTermCfg(func=mdp.observation, params={"name": "joint_observation"})
joint_mask = ObservationTermCfg(func=mdp.observation, params={"name": "joint_mask"})
effector_description = ObservationTermCfg(func=mdp.observation, params={"name": "effector_description"})
effector_observation = ObservationTermCfg(func=mdp.observation, params={"name": "effector_observation"})
effector_mask = ObservationTermCfg(func=mdp.observation, params={"name": "effector_mask"})
context = ObservationTermCfg(func=mdp.observation, params={"name": "context"})
goal = ObservationTermCfg(func=mdp.observation, params={"name": "goal"})
def __post_init__(self):
self.enable_corruption = False
self.concatenate_terms = False
policy = PolicyCfg()
@configclass
class EventsCfg:
reset_state = EventTermCfg(func=base_mdp.reset_scene_to_default, mode="reset", params={"reset_joint_targets": True})
@configclass
class RewardsCfg:
reach = RewardTermCfg(func=mdp.tracking_reward, weight=1.0)
effort = RewardTermCfg(func=mdp.normalized_effort, weight=-0.01)
action_rate = RewardTermCfg(func=mdp.action_rate, weight=-0.005)
@configclass
class TerminationsCfg:
time_out = TerminationTermCfg(func=base_mdp.time_out, time_out=True)
invalid = TerminationTermCfg(func=mdp.invalid_state)
@configclass
class LegEnvCfg(ManagerBasedRLEnvCfg):
bundle_dir: str = os.environ.get("SKILLFACTORY_LEG_BUNDLE", str(ROOT / "assets/generated/legs/go2_fl"))
max_joints: int = 8
anchor_height: float | None = None # default suspended, geometrically conservative
leg_bundle: dict | None = None
scene = LegSceneCfg(
num_envs=64, env_spacing=2.0, replicate_physics=True, filter_collisions=True, lazy_sensor_update=False
)
actions = ActionsCfg()
commands = CommandsCfg()
observations = ObservationsCfg()
events = EventsCfg()
rewards = RewardsCfg()
terminations = TerminationsCfg()
def __post_init__(self):
bundle = load_bundle(self.bundle_dir, require_usd=True)
self.leg_bundle = bundle
controls = bundle["spec"]["joints"]
names = [re.escape(value["name"]) for value in controls]
if type(self.max_joints) is not int or self.max_joints < len(names):
raise ValueError("max_joints smaller than asset DoF")
conversion = bundle["conversion"]
effort = {re.escape(value["name"]): value["limits"]["effort"] for value in bundle["joints"]}
velocity = {re.escape(value["name"]): value["limits"]["velocity"] for value in bundle["joints"]}
height = bundle["length_scale_m"] + 0.1 if self.anchor_height is None else self.anchor_height
self.scene.robot = ArticulationCfg(
prim_path="{ENV_REGEX_NS}/Robot",
spawn=sim_utils.UsdFileCfg(
usd_path=str(Path(self.bundle_dir).resolve() / conversion["usd_path"]),
activate_contact_sensors=True,
rigid_props=sim_utils.RigidBodyPropertiesCfg(disable_gravity=False, max_depenetration_velocity=1.0),
articulation_props=sim_utils.ArticulationRootPropertiesCfg(
enabled_self_collisions=False, solver_position_iteration_count=8, solver_velocity_iteration_count=4
),
),
init_state=ArticulationCfg.InitialStateCfg(
pos=(0.0, 0.0, height),
joint_pos={re.escape(value["name"]): value["q_default"] for value in controls},
joint_vel={".*": 0.0},
),
soft_joint_pos_limit_factor=bundle["spec"]["soft_limit_factor"],
actuators={
"leg": IdealPDActuatorCfg(
joint_names_expr=names,
stiffness={re.escape(value["name"]): value["kp"] for value in controls},
damping={re.escape(value["name"]): value["kd"] for value in controls},
effort_limit=effort,
effort_limit_sim=effort,
velocity_limit=velocity,
velocity_limit_sim=velocity,
)
},
)
self.scene.foot_contact = ContactSensorCfg(
prim_path="{ENV_REGEX_NS}/Robot" + conversion["body_paths"][bundle["foot"]["body"]],
update_period=0.0,
history_length=bundle["spec"]["decimation"],
filter_prim_paths_expr=["/World/ground/GroundPlane/CollisionPlane"],
track_air_time=True,
force_threshold=1.0,
)
self.actions.joint_position = BoundedJointPositionActionCfg(
asset_name="robot",
joint_names=names,
scale={re.escape(value["name"]): value["action_scale"] for value in controls},
preserve_order=True,
use_default_offset=True,
)
self.decimation = bundle["spec"]["decimation"]
self.sim.dt = bundle["spec"]["physics_dt"]
self.sim.render_interval = 20
self.sim.physics = PhysxCfg(enable_external_forces_every_iteration=True)
self.episode_length_s = 5.0
self.viewer.eye = (1.2, 1.2, 1.0)
self.viewer.lookat = (0.0, 0.0, height / 2)
@@ -0,0 +1,214 @@
"""Runtime adapter, reachable FK goals, and normalized single-leg rewards."""
import re
import torch
from isaaclab.managers import CommandTerm, CommandTermCfg
from isaaclab.utils.configclass import configclass
from isaaclab.utils.math import quat_apply, quat_apply_inverse, quat_mul
from SkillFactory.legs.tokens import LegKinematics, native_actions, pad_joint_observations, static_tokens
class LegState:
def __init__(self, env):
self.env = env
self.bundle = env.cfg.leg_bundle
self.robot = env.scene["robot"]
self.joint_ids, _ = self.robot.find_joints(
[re.escape(n) for n in self.bundle["joint_order"]], preserve_order=True
)
if len(self.joint_ids) != len(self.bundle["joint_order"]):
raise RuntimeError("Actuator mapping mismatch")
foot, _ = self.robot.find_bodies(re.escape(self.bundle["foot"]["body"]))
anchor, _ = self.robot.find_bodies(re.escape(self.bundle["anchor_body"]))
if len(foot) != 1 or len(anchor) != 1:
raise RuntimeError("Foot/anchor mapping must be unique")
self.foot, self.anchor = foot[0], anchor[0]
self.static = static_tokens(self.bundle, env.cfg.max_joints, env.device)
self.fk = LegKinematics(self.bundle, env.device)
self.offset = torch.tensor(self.bundle["foot"]["xyz"], device=env.device, dtype=torch.float32)
self.collision_quat = self.static["effector_description"][0, 0, 6:10]
self.half_size = (
torch.tensor(self.bundle["foot"].get("size", [0.0, 0.0, 0.0]), device=env.device, dtype=torch.float32) / 2
)
controls = self.bundle["spec"]["joints"]
self.nominal = torch.tensor([v["q_default"] for v in controls], device=env.device, dtype=torch.float32)
self.scale = torch.tensor([v["action_scale"] for v in controls], device=env.device, dtype=torch.float32)
self.soft = torch.tensor(
[v["soft_limits"] for v in self.bundle["joints"]], device=env.device, dtype=torch.float32
)
self.effort = torch.tensor(
[v["limits"]["effort"] for v in self.bundle["joints"]], device=env.device, dtype=torch.float32
)
self.length = self.bundle["length_scale_m"]
self.mass = self.bundle["known_mass_scale_kg"]
# Keep absolute scales and actuator/gravity authority: per-asset ratios
# alone would erase physically important differences between legs.
maximum_effort = max(j["limits"]["effort"] for j in self.bundle["joints"])
maximum_velocity = max(j["limits"]["velocity"] for j in self.bundle["joints"])
self.up = torch.tensor([0.0, 0.0, 1.0], device=env.device, dtype=torch.float32).expand(env.num_envs, -1)
self.control_dt = torch.full((env.num_envs, 1), env.step_dt, device=env.device, dtype=torch.float32)
self.physical_context = torch.tensor(
[
self.length,
self.mass,
maximum_effort / (self.mass * 9.81 * self.length),
maximum_velocity,
float(bool(self.bundle["missing_inertial_links"])),
],
device=env.device,
dtype=torch.float32,
)[None, :]
def q(self):
return self.robot.data.joint_pos.torch[:, self.joint_ids]
def foot_position(self):
data = self.robot.data
return quat_apply_inverse(
data.body_link_quat_w.torch[:, self.anchor],
data.body_link_pos_w.torch[:, self.foot] - data.body_link_pos_w.torch[:, self.anchor],
)
def gap(self):
data = self.robot.data
quat = data.body_link_quat_w.torch[:, self.foot]
center = data.body_link_pos_w.torch[:, self.foot] + quat_apply(quat, self.offset.expand(self.env.num_envs, -1))
if self.bundle["foot"]["type"] == "sphere":
extent = self.bundle["foot"]["radius"]
else:
collision = quat_mul(quat, self.collision_quat.expand(self.env.num_envs, -1))
extent = (quat_apply_inverse(collision, self.up).abs() * self.half_size).sum(dim=-1)
return center[:, 2] - extent # ground plane world z=0
def joint_observation(self):
return pad_joint_observations(
self.q(),
self.robot.data.joint_vel.torch[:, self.joint_ids],
self.env.action_manager.action,
self.bundle,
self.env.cfg.max_joints,
)
def effector_observation(self):
env, data = self.env, self.robot.data
anchor_quat = data.body_link_quat_w.torch[:, self.anchor]
velocity = quat_apply_inverse(
anchor_quat, data.body_link_lin_vel_w.torch[:, self.foot] - data.body_link_lin_vel_w.torch[:, self.anchor]
)
force_w = env.scene["foot_contact"].data.force_matrix_w.torch[:, 0].sum(dim=1)
force = quat_apply_inverse(anchor_quat, force_w)
return torch.cat(
(
self.foot_position() / self.length,
velocity / self.length,
force / (self.mass * 9.81),
(force.norm(dim=-1, keepdim=True) > 1.0).float(),
self.gap()[:, None] / self.length,
),
dim=-1,
)[:, None, :]
def context(self):
data = self.robot.data
gravity = quat_apply_inverse(data.body_link_quat_w.torch[:, self.anchor], -self.up)
return torch.cat(
(
gravity,
self.control_dt,
-data.body_link_pos_w.torch[:, self.anchor, 2, None] / self.length,
self.physical_context.expand(self.env.num_envs, -1),
),
dim=-1,
)
def goal(self):
return self.env.command_manager.get_command("reach") / self.length
def token(self, name):
if name in self.static:
value = self.static[name]
return value.expand(self.env.num_envs, *value.shape[1:])
if name not in ("joint_observation", "effector_observation", "context", "goal"):
raise KeyError(name)
return getattr(self, name)()
def snapshot(self):
"""Fresh tokens: no step/reset cache that could retain terminal or old goals."""
return {
name: self.token(name)
for name in (*self.static, "joint_observation", "effector_observation", "context", "goal")
}
def actions(self, actions):
return native_actions(actions, self.env.num_envs, len(self.joint_ids), self.env.cfg.max_joints, self.env.device)
def state(env):
if not hasattr(env, "_leg_state"):
env._leg_state = LegState(env)
return env._leg_state
def observation(env, name):
# ObservationManager calls each term separately. Compute only the requested
# component, rather than rebuilding the entire snapshot eight times.
return state(env).token(name)
class ReachCommand(CommandTerm):
def __init__(self, cfg, env):
super().__init__(cfg, env)
self._command = torch.zeros(self.num_envs, 3, device=self.device, dtype=torch.float32)
self.goal_q = torch.zeros(
self.num_envs, len(env.cfg.leg_bundle["joints"]), device=self.device, dtype=torch.float32
)
self.metrics["position_error_m"] = torch.zeros(self.num_envs, device=self.device, dtype=torch.float32)
@property
def command(self):
return self._command
def _resample_command(self, env_ids):
adapter = state(self._env)
actions = adapter.nominal.new_empty((len(env_ids), len(adapter.joint_ids))).uniform_(-0.75, 0.75)
q = (adapter.nominal + actions * adapter.scale).clamp(min=adapter.soft[:, 0], max=adapter.soft[:, 1])
self.goal_q[env_ids] = q
self._command[env_ids] = adapter.fk.foot_position(q)
def _update_metrics(self):
self.metrics["position_error_m"] += (state(self._env).foot_position() - self.command).norm(
dim=-1
) / self._env.max_episode_length
def _update_command(self):
pass
@configclass
class ReachCommandCfg(CommandTermCfg):
class_type = ReachCommand
resampling_time_range = (1.0, 2.0)
debug_vis = False
def tracking_reward(env):
adapter = state(env)
error = (adapter.foot_position() - env.command_manager.get_command("reach")) / adapter.length
return torch.exp(-error.square().sum(dim=-1) / 0.06**2)
def normalized_effort(env):
adapter = state(env)
return (adapter.robot.data.applied_torque.torch[:, adapter.joint_ids] / adapter.effort).square().mean(dim=-1)
def action_rate(env):
return (env.action_manager.action - env.action_manager.prev_action).square().mean(dim=-1)
def invalid_state(env):
adapter = state(env)
return ~(torch.isfinite(adapter.q()).all(dim=-1) & torch.isfinite(adapter.robot.data.joint_vel.torch).all(dim=-1))
@@ -0,0 +1,5 @@
"""Register the isolated Go2 single-leg benchmark independently of Cartpole."""
import gymnasium as gym
gym.register(id='SkillFactory-SingleLegAnchor-Go2-v0', entry_point='isaaclab.envs:ManagerBasedRLEnv',
disable_env_checker=True, kwargs={'env_cfg_entry_point':f'{__name__}.anchor_env_cfg:AnchorEnvCfg'})
@@ -0,0 +1,98 @@
"""Week-one deterministic Go2 FL anchor benchmark; no RL training reward design."""
from isaaclab_physx.physics import PhysxCfg
import isaaclab.sim as sim_utils
from isaaclab.assets import AssetBaseCfg
from isaaclab.envs import ManagerBasedRLEnvCfg
from isaaclab.managers import EventTermCfg, ObservationGroupCfg, ObservationTermCfg, RewardTermCfg, TerminationTermCfg
from isaaclab.scene import InteractiveSceneCfg
from isaaclab.sensors import ContactSensorCfg
from isaaclab.utils.configclass import configclass
from SkillFactory.assets.go2_leg import FOOT_PATH, GO2_LEG_CFG, JOINT_NAMES
from . import mdp
@configclass
class AnchorSceneCfg(InteractiveSceneCfg):
ground = AssetBaseCfg(
prim_path="/World/ground",
spawn=sim_utils.GroundPlaneCfg(
physics_material=sim_utils.RigidBodyMaterialCfg(static_friction=0.8, dynamic_friction=0.6, restitution=0.0)
),
)
robot = GO2_LEG_CFG.copy()
foot_contact = ContactSensorCfg(
prim_path="{ENV_REGEX_NS}/Robot" + FOOT_PATH,
update_period=0.0,
history_length=5,
filter_prim_paths_expr=["/World/ground/GroundPlane/CollisionPlane"],
track_air_time=True,
force_threshold=1.0,
)
light = AssetBaseCfg(prim_path="/World/light", spawn=sim_utils.DomeLightCfg(intensity=2000.0))
@configclass
class ActionsCfg:
joint_position = mdp.BoundedJointPositionActionCfg(
asset_name="robot", joint_names=JOINT_NAMES, scale=0.25, preserve_order=True, use_default_offset=True
)
@configclass
class ObservationsCfg:
@configclass
class PolicyCfg(ObservationGroupCfg):
q_rel = ObservationTermCfg(func=mdp.joint_pos_rel)
dq = ObservationTermCfg(func=mdp.joint_vel)
foot_position = ObservationTermCfg(func=mdp.foot_pos_anchor)
foot_velocity = ObservationTermCfg(func=mdp.foot_vel_anchor)
normal_force = ObservationTermCfg(func=mdp.normal_force_anchor)
contact = ObservationTermCfg(func=mdp.contact)
last_action = ObservationTermCfg(func=mdp.previous_action)
def __post_init__(self):
self.enable_corruption = False
self.concatenate_terms = True
policy = PolicyCfg()
@configclass
class EventsCfg:
reset_state = EventTermCfg(func=mdp.reset_scene_to_default, mode="reset", params={"reset_joint_targets": True})
@configclass
class RewardsCfg:
# Placeholder required by RL environment; not a pretrained skill reward.
alive = RewardTermCfg(func=mdp.is_alive, weight=1.0)
@configclass
class TerminationsCfg:
time_out = TerminationTermCfg(func=mdp.time_out, time_out=True)
@configclass
class AnchorEnvCfg(ManagerBasedRLEnvCfg):
scene = AnchorSceneCfg(
num_envs=64, env_spacing=2.0, replicate_physics=True, filter_collisions=True, lazy_sensor_update=False
)
actions = ActionsCfg()
observations = ObservationsCfg()
events = EventsCfg()
rewards = RewardsCfg()
terminations = TerminationsCfg()
def __post_init__(self):
self.decimation = 5
self.episode_length_s = 10.0
self.sim.dt = 0.002
self.sim.render_interval = 20
self.sim.physics = PhysxCfg(enable_external_forces_every_iteration=True)
self.viewer.eye = (1.0, 1.0, 0.8)
self.viewer.lookat = (0.0, 0.0, 0.3)
@@ -0,0 +1,3 @@
from isaaclab.envs.mdp import * # noqa: F403
from .actions import BoundedJointPositionActionCfg
from .observations import * # noqa: F403
@@ -0,0 +1,40 @@
"""Thin normalized-input guard over Isaac Lab's position action term."""
import torch
from isaaclab.envs.mdp.actions import JointPositionAction, JointPositionActionCfg
from isaaclab.utils.configclass import configclass
class BoundedJointPositionAction(JointPositionAction):
"""Finite normalized actions; absolute position targets respect soft limits [rad]."""
def process_actions(self, actions: torch.Tensor):
if actions.shape != (self.num_envs, self.action_dim) or actions.dtype != torch.float32:
self._env.action_manager.action[:] = self.raw_actions
raise ValueError(f"Expected float32 [num_envs, {self.action_dim}] actions")
if not torch.isfinite(actions).all():
# ActionManager stores input before invoking terms; restore its
# current command so a rejected input cannot poison the next step.
self._env.action_manager.action[:] = self.raw_actions
raise ValueError("Non-finite action rejected before physical stepping")
bounded = actions.clamp(-1.0, 1.0)
super().process_actions(bounded)
limits = self._asset.data.soft_joint_pos_limits.torch[:, self._joint_ids]
self._processed_actions.clamp_(min=limits[:, :, 0], max=limits[:, :, 1])
# The standard manager buffers feed last-action observations. Store the
# effective normalized command, not out-of-range caller input.
self._env.action_manager.action[:] = bounded
def reset(self, env_ids=None):
ids = slice(None) if env_ids is None else env_ids
super().reset(ids)
if isinstance(self._offset, torch.Tensor):
self._processed_actions[ids] = self._offset[ids]
else:
self._processed_actions[ids] = self._offset
@configclass
class BoundedJointPositionActionCfg(JointPositionActionCfg):
class_type = BoundedJointPositionAction
@@ -0,0 +1,95 @@
"""Local observations and diagnostic views. SI units, xyzw quaternions."""
import torch
from isaaclab.utils.math import quat_apply, quat_apply_inverse
def _ids(env):
robot = env.scene["robot"]
if not hasattr(env, "_anchor_body_ids"):
foot, _ = robot.find_bodies("FL_foot")
anchor, _ = robot.find_bodies("anchor_base")
if len(foot) != 1 or len(anchor) != 1:
raise RuntimeError("Expected unique foot and anchor body mapping")
env._anchor_body_ids = foot[0], anchor[0]
return env._anchor_body_ids
def joint_pos_rel(env):
data = env.scene["robot"].data
return data.joint_pos.torch - data.default_joint_pos.torch
def joint_vel(env):
return env.scene["robot"].data.joint_vel.torch
def foot_pos_anchor(env):
foot, anchor = _ids(env)
data = env.scene["robot"].data
p = data.body_link_pos_w.torch
q = data.body_link_quat_w.torch[:, anchor]
return quat_apply_inverse(q, p[:, foot] - p[:, anchor])
def foot_vel_anchor(env):
foot, anchor = _ids(env)
data = env.scene["robot"].data
# Anchor is fixed, but subtract its velocity for explicit frame semantics.
v = data.body_link_lin_vel_w.torch
return quat_apply_inverse(data.body_link_quat_w.torch[:, anchor], v[:, foot] - v[:, anchor])
def normal_force_w(env):
return env.scene["foot_contact"].data.force_matrix_w.torch[:, 0].sum(dim=1)
def normal_force_anchor(env):
_, anchor = _ids(env)
return quat_apply_inverse(env.scene["robot"].data.body_link_quat_w.torch[:, anchor], normal_force_w(env))
def contact(env):
return (normal_force_w(env).norm(dim=-1, keepdim=True) > 1.0).to(torch.float32)
def previous_action(env):
return env.action_manager.prev_action
def foot_gap(env):
"""Sphere bottom height relative to z=0 ground [m], not foot-frame height."""
foot, _ = _ids(env)
data = env.scene["robot"].data
offset = torch.tensor([-0.002, 0.0, 0.0], device=env.device).expand(env.num_envs, -1)
center = data.body_link_pos_w.torch[:, foot] + quat_apply(data.body_link_quat_w.torch[:, foot], offset)
return center[:, 2] - 0.022
def diagnostics(env):
"""Diagnostic tensor views; clone if retaining values across stepping/reset."""
robot = env.scene["robot"]
data = robot.data
foot, anchor = _ids(env)
return {
"q": data.joint_pos.torch,
"dq": data.joint_vel.torch,
"q_target": data.joint_pos_target.torch,
"computed_torque": data.computed_torque.torch,
"applied_torque": data.applied_torque.torch,
"foot_pos_w": data.body_link_pos_w.torch[:, foot],
"foot_vel_w": data.body_link_lin_vel_w.torch[:, foot],
"anchor_pose_w": data.body_link_pose_w.torch[:, anchor],
"foot_pos_anchor": foot_pos_anchor(env),
"foot_vel_anchor": foot_vel_anchor(env),
"normal_force_w": normal_force_w(env),
"gap": foot_gap(env),
"force_history_w": env.scene["foot_contact"].data.net_forces_w_history.torch,
"episode_step": env.episode_length_buf,
"env_ids": torch.arange(env.num_envs, device=env.device),
"joint_mask": torch.ones((env.num_envs, 3), dtype=torch.bool, device=env.device),
"sim_time_s": env._sim_step_counter * env.physics_dt,
"reaction_wrench": None,
"reaction_wrench_valid": False,
}
@@ -38,4 +38,4 @@ class PPORunnerCfg(RslRlOnPolicyRunnerCfg):
lam=0.95,
desired_kl=0.01,
max_grad_norm=1.0,
)
)
@@ -177,4 +177,4 @@ class SkillfactoryEnvCfg(ManagerBasedRLEnvCfg):
self.viewer.eye = (8.0, 0.0, 5.0)
# simulation settings
self.sim.dt = 1 / 120
self.sim.render_interval = self.decimation
self.sim.render_interval = self.decimation
@@ -44,4 +44,4 @@ class ExampleExtension(omni.ext.IExt):
omni.ui.Button("Reset", clicked_fn=on_reset)
def on_shutdown(self):
print("[SkillFactory] shutdown")
print("[SkillFactory] shutdown")
+5 -5
View File
@@ -1,18 +1,18 @@
[package]
# Semantic Versioning is used: https://semver.org/
version = "0.1.0"
version = "0.0.1"
# Description
category = "isaaclab"
readme = "README.md"
title = "Extension Template"
title = "SkillFactory"
author = "Isaac Lab Project Developers"
maintainer = "Isaac Lab Project Developers"
description="Extension Template for Isaac Lab"
repository = "https://github.com/isaac-sim/IsaacLab.git"
keywords = ["extension", "template", "isaaclab"]
description="Single-leg asset import, fixed-anchor simulation, and shared-policy training for Isaac Lab"
repository = "https://gitea.robotquan.com/chenlin/SkillFactory"
keywords = ["isaaclab", "robotics", "single-leg", "shared-policy"]
[dependencies]
"isaaclab" = {}
+9 -5
View File
@@ -1,10 +1,14 @@
Changelog
---------
0.1.0 (2026-10-08)
~~~~~~~~~~~~~~~~~~
0.0.1
~~~~~
Added
^^^^^
* Initial SkillFactory release based on the Isaac Lab project template.
* Verified Go2 fixed-anchor benchmark with contact sensing and indexed reset.
* Declarative Go2/Go1 leg import, variable-DoF tokens and shared-policy PPO smoke runner.
* Mask, kinematics, reset, bootstrap, checkpoint and failure-report regression tests.
* Consolidated run commands and interface/acceptance documentation.
* Created an initial template for building an extension or project based on Isaac Lab
Limitations: no converged or frozen Skill, learned modular latent, held-out morphology
validation, whole-body transfer or production-throughput sampler.
+4 -3
View File
@@ -8,7 +8,7 @@
import os
import toml
from setuptools import setup
from setuptools import find_packages, setup
# Obtain the extension data from the extension.toml file
EXTENSION_PATH = os.path.dirname(os.path.realpath(__file__))
@@ -24,7 +24,7 @@ INSTALL_REQUIRES = [
# Installation operation
setup(
name="SkillFactory",
packages=["SkillFactory"],
packages=find_packages(),
author=EXTENSION_TOML_DATA["package"]["author"],
maintainer=EXTENSION_TOML_DATA["package"]["maintainer"],
url=EXTENSION_TOML_DATA["package"]["repository"],
@@ -34,6 +34,7 @@ setup(
install_requires=INSTALL_REQUIRES,
license="Apache-2.0",
include_package_data=True,
package_data={"SkillFactory.legs": ["resources/*.usda"]},
python_requires=">=3.12",
classifiers=[
"Natural Language :: English",
@@ -41,4 +42,4 @@ setup(
"Isaac Sim :: 6.0.0",
],
zip_safe=False,
)
)