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:
@@ -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))
|
||||
Binary file not shown.
@@ -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'})
|
||||
+98
@@ -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
|
||||
+95
@@ -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,
|
||||
}
|
||||
+1
-1
@@ -38,4 +38,4 @@ class PPORunnerCfg(RslRlOnPolicyRunnerCfg):
|
||||
lam=0.95,
|
||||
desired_kl=0.01,
|
||||
max_grad_norm=1.0,
|
||||
)
|
||||
)
|
||||
|
||||
+1
-1
@@ -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")
|
||||
|
||||
@@ -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" = {}
|
||||
|
||||
@@ -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.
|
||||
|
||||
@@ -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,
|
||||
)
|
||||
)
|
||||
|
||||
Reference in New Issue
Block a user