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,92 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Convert the extracted Go2 leg with Kit and verify independent physics bodies."""
|
||||
|
||||
import argparse
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[2]
|
||||
|
||||
|
||||
def main():
|
||||
if not __debug__:
|
||||
raise RuntimeError("Asset checks require Python without -O/PYTHONOPTIMIZE")
|
||||
from isaaclab.app import AppLauncher
|
||||
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--asset_dir", type=Path, default=ROOT / "assets/generated/go2_fl")
|
||||
AppLauncher.add_app_launcher_args(parser)
|
||||
args = parser.parse_args()
|
||||
app = AppLauncher(args).app
|
||||
exit_code = 0
|
||||
try:
|
||||
from pxr import PhysxSchema, Usd, UsdPhysics
|
||||
|
||||
from isaaclab.sim.converters import UrdfConverter, UrdfConverterCfg
|
||||
|
||||
directory = args.asset_dir.resolve()
|
||||
cfg = UrdfConverterCfg(
|
||||
asset_path=str(directory / "go2_fl.urdf"),
|
||||
usd_dir=str(directory / "usd"),
|
||||
fix_base=True,
|
||||
merge_fixed_joints=False,
|
||||
force_usd_conversion=True,
|
||||
joint_drive=UrdfConverterCfg.JointDriveCfg(target_type="none"),
|
||||
run_multi_physics_conversion=False,
|
||||
)
|
||||
converter = UrdfConverter(cfg)
|
||||
stage = Usd.Stage.Open(converter.usd_path)
|
||||
if stage is None:
|
||||
raise RuntimeError("Converter produced no readable stage")
|
||||
root = stage.GetDefaultPrim()
|
||||
assert root, "No default prim"
|
||||
bodies = {p.GetName(): str(p.GetPath()) for p in stage.Traverse() if p.HasAPI(UsdPhysics.RigidBodyAPI)}
|
||||
revolute = [p for p in stage.Traverse() if p.IsA(UsdPhysics.RevoluteJoint)]
|
||||
fixed = [p for p in stage.Traverse() if p.IsA(UsdPhysics.FixedJoint)]
|
||||
expected = ["FL_hip_joint", "FL_thigh_joint", "FL_calf_joint"]
|
||||
assert sorted(p.GetName() for p in revolute) == sorted(expected)
|
||||
assert "FL_foot" in bodies and "anchor_base" in bodies
|
||||
|
||||
def is_world_fixed(p):
|
||||
targets = UsdPhysics.Joint(p).GetBody0Rel().GetTargets() + UsdPhysics.Joint(p).GetBody1Rel().GetTargets()
|
||||
return len(targets) < 2 or any(not stage.GetPrimAtPath(t).HasAPI(UsdPhysics.RigidBodyAPI) for t in targets)
|
||||
|
||||
assert any(is_world_fixed(p) for p in fixed), "No fixed-to-world joint"
|
||||
foot = stage.GetPrimAtPath(bodies["FL_foot"])
|
||||
assert any(p.HasAPI(UsdPhysics.CollisionAPI) for p in Usd.PrimRange(foot, Usd.TraverseInstanceProxies())), (
|
||||
"No foot collision"
|
||||
)
|
||||
# Importer 3.0 produces nested rigid bodies. The generic spawner stops
|
||||
# at the first rigid body (anchor), so explicitly enable the foot here.
|
||||
PhysxSchema.PhysxContactReportAPI.Apply(foot).CreateThresholdAttr(0.0)
|
||||
stage.GetRootLayer().Save()
|
||||
assert foot.HasAPI(PhysxSchema.PhysxContactReportAPI)
|
||||
used_layers = stage.GetUsedLayers()
|
||||
missing = [
|
||||
layer.identifier for layer in used_layers if not layer.anonymous and not Path(layer.realPath).is_file()
|
||||
]
|
||||
assert not missing, missing
|
||||
mapping = {
|
||||
"usd_path": str(Path(converter.usd_path).relative_to(directory)),
|
||||
"default_prim": str(root.GetPath()),
|
||||
"body_paths": {k: v.removeprefix(str(root.GetPath())) for k, v in bodies.items()},
|
||||
"joint_paths": {p.GetName(): str(p.GetPath()).removeprefix(str(root.GetPath())) for p in revolute},
|
||||
"used_layers": [
|
||||
str(Path(layer.realPath).relative_to(directory)) for layer in used_layers if layer.realPath
|
||||
],
|
||||
"fixed_to_world": True,
|
||||
"merge_fixed_joints": False,
|
||||
}
|
||||
(directory / "conversion.json").write_text(json.dumps(mapping, indent=2) + "\n")
|
||||
print(json.dumps(mapping, indent=2))
|
||||
except BaseException:
|
||||
import traceback
|
||||
|
||||
traceback.print_exc()
|
||||
exit_code = 1
|
||||
finally:
|
||||
app.close(exit_code=exit_code)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,85 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Convert a PartSpec-derived single-leg bundle and validate its USD mapping."""
|
||||
|
||||
import argparse
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
def main():
|
||||
if not __debug__:
|
||||
raise RuntimeError("Conversion assertions require Python without -O")
|
||||
from isaaclab.app import AppLauncher
|
||||
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--bundle", type=Path, required=True)
|
||||
AppLauncher.add_app_launcher_args(parser)
|
||||
args = parser.parse_args()
|
||||
app = AppLauncher(args).app
|
||||
code = 0
|
||||
try:
|
||||
from SkillFactory.legs.spec import load_bundle
|
||||
from SkillFactory.legs.urdf import digest
|
||||
|
||||
from pxr import PhysxSchema, Usd, UsdPhysics
|
||||
|
||||
from isaaclab.sim.converters import UrdfConverter, UrdfConverterCfg
|
||||
|
||||
directory = args.bundle.resolve()
|
||||
bundle = load_bundle(directory)
|
||||
converter = UrdfConverter(
|
||||
UrdfConverterCfg(
|
||||
asset_path=str(directory / "leg.urdf"),
|
||||
usd_dir=str(directory / "usd"),
|
||||
fix_base=True,
|
||||
merge_fixed_joints=False,
|
||||
force_usd_conversion=True,
|
||||
joint_drive=UrdfConverterCfg.JointDriveCfg(target_type="none"),
|
||||
run_multi_physics_conversion=False,
|
||||
)
|
||||
)
|
||||
stage = Usd.Stage.Open(converter.usd_path)
|
||||
assert stage and stage.GetDefaultPrim()
|
||||
root = stage.GetDefaultPrim()
|
||||
bodies = {
|
||||
prim.GetName(): str(prim.GetPath()) for prim in stage.Traverse() if prim.HasAPI(UsdPhysics.RigidBodyAPI)
|
||||
}
|
||||
revolute = [prim for prim in stage.Traverse() if prim.IsA(UsdPhysics.RevoluteJoint)]
|
||||
assert sorted(prim.GetName() for prim in revolute) == sorted(bundle["joint_order"])
|
||||
assert set(bundle["links"]) | {bundle["anchor_body"]} == set(bodies)
|
||||
fixed_world = False
|
||||
for prim in stage.Traverse():
|
||||
if prim.IsA(UsdPhysics.FixedJoint):
|
||||
joint = UsdPhysics.Joint(prim)
|
||||
targets = joint.GetBody0Rel().GetTargets() + joint.GetBody1Rel().GetTargets()
|
||||
fixed_world |= len(targets) < 2 or any(
|
||||
not stage.GetPrimAtPath(path).HasAPI(UsdPhysics.RigidBodyAPI) for path in targets
|
||||
)
|
||||
assert fixed_world, "Missing fixed-to-world support"
|
||||
foot = stage.GetPrimAtPath(bodies[bundle["foot"]["body"]])
|
||||
assert any(prim.HasAPI(UsdPhysics.CollisionAPI) for prim in Usd.PrimRange(foot, Usd.TraverseInstanceProxies()))
|
||||
PhysxSchema.PhysxContactReportAPI.Apply(foot).CreateThresholdAttr(0.0)
|
||||
stage.GetRootLayer().Save()
|
||||
layers = [Path(layer.realPath).resolve() for layer in stage.GetUsedLayers() if layer.realPath]
|
||||
assert layers and all(layer.is_relative_to(directory) and layer.is_file() for layer in layers)
|
||||
conversion = {
|
||||
"usd_path": str(Path(converter.usd_path).relative_to(directory)),
|
||||
"manifest_sha256": digest(directory / "manifest.json"),
|
||||
"fixed_to_world": True,
|
||||
"body_paths": {name: path.removeprefix(str(root.GetPath())) for name, path in bodies.items()},
|
||||
"joint_paths": {prim.GetName(): str(prim.GetPath()).removeprefix(str(root.GetPath())) for prim in revolute},
|
||||
"used_layers": sorted(str(layer.relative_to(directory)) for layer in layers),
|
||||
}
|
||||
(directory / "conversion.json").write_text(json.dumps(conversion, indent=2) + "\n")
|
||||
print(json.dumps(conversion, indent=2))
|
||||
except BaseException:
|
||||
import traceback
|
||||
|
||||
traceback.print_exc()
|
||||
code = 1
|
||||
finally:
|
||||
app.close(exit_code=code)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,260 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Extract the real Go2 FL subtree without importing Isaac Lab (SI units)."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import copy
|
||||
import hashlib
|
||||
import json
|
||||
import math
|
||||
import shutil
|
||||
import subprocess
|
||||
import xml.etree.ElementTree as ET
|
||||
from pathlib import Path
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[2]
|
||||
DEFAULT_SOURCE = Path("/home/cen/Embodied_Workspace/unitree_ros/robots/go2_description")
|
||||
JOINTS = ("FL_hip_joint", "FL_thigh_joint", "FL_calf_joint")
|
||||
Q_DEFAULT = (0.0, 0.8, -1.5)
|
||||
|
||||
|
||||
def digest(path: Path) -> str:
|
||||
return hashlib.sha256(path.read_bytes()).hexdigest()
|
||||
|
||||
|
||||
def numbers(text: str) -> list[float]:
|
||||
result = [float(x) for x in text.split()]
|
||||
if not all(math.isfinite(x) for x in result):
|
||||
raise ValueError(f"Non-finite numeric field: {text}")
|
||||
return result
|
||||
|
||||
|
||||
def origin(element: ET.Element) -> dict:
|
||||
value = element.find("origin")
|
||||
return {key: numbers(value.get(key, "0 0 0") if value is not None else "0 0 0") for key in ("xyz", "rpy")}
|
||||
|
||||
|
||||
def inertia_record(link: ET.Element) -> dict | None:
|
||||
inertial = link.find("inertial")
|
||||
if inertial is None:
|
||||
return None
|
||||
mass = float(inertial.find("mass").get("value"))
|
||||
values = {k: float(inertial.find("inertia").get(k, "0")) for k in ("ixx", "ixy", "ixz", "iyy", "iyz", "izz")}
|
||||
a, b, c, d, e, f = (values[k] for k in ("ixx", "ixy", "ixz", "iyy", "iyz", "izz"))
|
||||
# Sylvester's criterion for the complete symmetric inertia, not just its diagonal.
|
||||
determinant = a * (d * f - e * e) - b * (b * f - c * e) + c * (b * e - c * d)
|
||||
if (
|
||||
not all(math.isfinite(x) for x in (mass, a, b, c, d, e, f))
|
||||
or mass <= 0
|
||||
or min(a, a * d - b * b, determinant) <= 0
|
||||
):
|
||||
raise ValueError(f"Invalid inertia on {link.get('name')}")
|
||||
return {"mass": mass, "inertia": values, "origin": origin(inertial)}
|
||||
|
||||
|
||||
def matmul(a, b):
|
||||
return [[sum(a[i][k] * b[k][j] for k in range(len(b))) for j in range(len(b[0]))] for i in range(len(a))]
|
||||
|
||||
|
||||
def transform(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, 1],
|
||||
]
|
||||
|
||||
|
||||
def foot_center(robot: ET.Element, q=Q_DEFAULT) -> list[float]:
|
||||
"""Collision sphere center in the anchor frame at the specified joint angles [m]."""
|
||||
t = transform([0, 0, 0], [0, 0, 0])
|
||||
by_name = {j.get("name"): j for j in robot.findall("joint")}
|
||||
for name in (*JOINTS, "FL_foot_joint"):
|
||||
j = by_name[name]
|
||||
o = origin(j)
|
||||
t = matmul(t, transform(o["xyz"], o["rpy"]))
|
||||
if name in JOINTS:
|
||||
angle = q[JOINTS.index(name)]
|
||||
axis = numbers(j.find("axis").get("xyz"))
|
||||
# Go2 uses the canonical x and y axes. Refuse unhandled topologies.
|
||||
if axis not in ([1.0, 0.0, 0.0], [0.0, 1.0, 0.0]):
|
||||
raise ValueError(f"Unsupported axis {axis}")
|
||||
t = matmul(t, transform([0, 0, 0], [angle * axis[0], angle * axis[1], 0]))
|
||||
foot = next(link for link in robot.findall("link") if link.get("name") == "FL_foot")
|
||||
o = origin(foot.find("collision"))
|
||||
t = matmul(t, transform(o["xyz"], o["rpy"]))
|
||||
return [t[i][3] for i in range(3)]
|
||||
|
||||
|
||||
def extract(source: Path, output: Path) -> dict:
|
||||
source = source.resolve()
|
||||
source_urdf = source / "urdf/go2_description.urdf"
|
||||
original = ET.parse(source_urdf).getroot()
|
||||
links = {link.get("name"): link for link in original.findall("link")}
|
||||
joints = {j.get("name"): j for j in original.findall("joint")}
|
||||
for name in (*JOINTS, "FL_foot_joint", "FL_hip_rotor_joint"):
|
||||
if name not in joints:
|
||||
raise ValueError(f"Missing required joint: {name}")
|
||||
keep = {"FL_hip", "FL_hip_rotor"}
|
||||
while True:
|
||||
expanded = keep | {j.find("child").get("link") for j in joints.values() if j.find("parent").get("link") in keep}
|
||||
if expanded == keep:
|
||||
break
|
||||
keep = expanded
|
||||
if "FL_foot" not in keep or any(not n.startswith("FL_") for n in keep):
|
||||
raise ValueError("FL branch must contain only the FL leg and its independent foot")
|
||||
robot = ET.Element("robot", name="go2_fl_anchor")
|
||||
anchor = ET.SubElement(robot, "link", name="anchor_base")
|
||||
anchor.append(
|
||||
ET.fromstring(
|
||||
'<inertial><origin xyz="0 0 0" rpy="0 0 0"/><mass value="1"/>'
|
||||
'<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/></inertial>'
|
||||
)
|
||||
)
|
||||
shift = origin(joints[JOINTS[0]])["xyz"]
|
||||
for link in original.findall("link"):
|
||||
if link.get("name") in keep:
|
||||
inertia_record(link)
|
||||
robot.append(copy.deepcopy(link))
|
||||
for j in original.findall("joint"):
|
||||
if j.find("child").get("link") in keep:
|
||||
j = copy.deepcopy(j)
|
||||
if j.find("parent").get("link") == "base":
|
||||
j.find("parent").set("link", "anchor_base")
|
||||
old_xyz = origin(j)["xyz"]
|
||||
j.find("origin").set("xyz", " ".join(f"{x - s:.12g}" for x, s in zip(old_xyz, shift)))
|
||||
robot.append(j)
|
||||
revolute = [j.get("name") for j in robot.findall("joint") if j.get("type") != "fixed"]
|
||||
if tuple(revolute) != JOINTS:
|
||||
raise ValueError(f"Expected exactly the three ordered joints, got {revolute}")
|
||||
output.mkdir(parents=True, exist_ok=True)
|
||||
resources = {}
|
||||
for mesh in robot.findall(".//mesh"):
|
||||
filename = mesh.get("filename")
|
||||
prefix = "package://go2_description/"
|
||||
if not filename.startswith(prefix):
|
||||
raise ValueError(f"Unexpected mesh URI: {filename}")
|
||||
relative = Path(filename[len(prefix) :])
|
||||
if relative.is_absolute() or ".." in relative.parts:
|
||||
raise ValueError(f"Unsafe mesh path: {relative}")
|
||||
src, dst = source / relative, output / relative
|
||||
dst.parent.mkdir(parents=True, exist_ok=True)
|
||||
shutil.copy2(src, dst)
|
||||
mesh.set("filename", relative.as_posix())
|
||||
resources[relative.as_posix()] = digest(src)
|
||||
if src.suffix.lower() == ".dae":
|
||||
for image in ET.parse(src).getroot().findall(".//{*}image/{*}init_from"):
|
||||
image_relative = Path(image.text.strip())
|
||||
if image_relative.is_absolute() or ".." in image_relative.parts:
|
||||
raise ValueError("Unsafe DAE image reference")
|
||||
image_src, image_dst = src.parent / image_relative, dst.parent / image_relative
|
||||
image_dst.parent.mkdir(parents=True, exist_ok=True)
|
||||
shutil.copy2(image_src, image_dst)
|
||||
resources[image_dst.relative_to(output).as_posix()] = digest(image_src)
|
||||
license_path = source.parents[1] / "LICENSE"
|
||||
shutil.copy2(license_path, output / "LICENSE")
|
||||
records = []
|
||||
for name in JOINTS:
|
||||
j = joints[name]
|
||||
limits = {k: float(j.find("limit").get(k)) for k in ("lower", "upper", "effort", "velocity")}
|
||||
if (
|
||||
not all(math.isfinite(x) for x in limits.values())
|
||||
or limits["lower"] >= limits["upper"]
|
||||
or min(limits["effort"], limits["velocity"]) <= 0
|
||||
):
|
||||
raise ValueError(f"Invalid limits: {name}")
|
||||
midpoint = (limits["lower"] + limits["upper"]) / 2
|
||||
records.append(
|
||||
{
|
||||
"name": name,
|
||||
"axis": numbers(j.find("axis").get("xyz")),
|
||||
"limits": limits,
|
||||
"soft_limits": [midpoint + 0.9 * (limits[k] - midpoint) for k in ("lower", "upper")],
|
||||
}
|
||||
)
|
||||
radius = float(links["FL_foot"].find("collision/geometry/sphere").get("radius"))
|
||||
center = foot_center(robot)
|
||||
manifest = {
|
||||
"schema_version": 1,
|
||||
"asset": "Go2 FL fixed anchor",
|
||||
"source": {
|
||||
"urdf": str(source_urdf),
|
||||
"sha256": digest(source_urdf),
|
||||
"commit": subprocess.check_output(["git", "-C", str(source), "rev-parse", "HEAD"], text=True).strip(),
|
||||
},
|
||||
"resources": dict(sorted(resources.items())),
|
||||
"license": "BSD-3-Clause",
|
||||
"joint_order": list(JOINTS),
|
||||
"joints": records,
|
||||
"links": {link.get("name"): inertia_record(link) for link in robot.findall("link")},
|
||||
"transforms": {
|
||||
j.get("name"): {"parent": j.find("parent").get("link"), "child": j.find("child").get("link"), **origin(j)}
|
||||
for j in robot.findall("joint")
|
||||
},
|
||||
"missing_inertial_fixed_links": [
|
||||
link.get("name") for link in robot.findall("link") if link.find("inertial") is None
|
||||
],
|
||||
"anchor": {
|
||||
"fixed": True,
|
||||
"original_mount_translation": shift,
|
||||
"synthetic_root_mass": 1.0,
|
||||
"default_height": radius - center[2] + 0.01,
|
||||
},
|
||||
"foot": {"frame": "FL_foot", "radius": radius, "collision_origin": origin(links["FL_foot"].find("collision"))},
|
||||
"control": {
|
||||
"q_default": list(Q_DEFAULT),
|
||||
"action_scale": 0.25,
|
||||
"kp": 25.0,
|
||||
"kd": 0.5,
|
||||
"physics_dt": 0.002,
|
||||
"decimation": 5,
|
||||
},
|
||||
"interface": {
|
||||
"action_shape": ["B", 3],
|
||||
"policy_shape": ["B", 19],
|
||||
"device": "cuda",
|
||||
"dtype": "float32",
|
||||
"quaternion_order": "xyzw",
|
||||
"policy_order": [
|
||||
"q_rel",
|
||||
"dq",
|
||||
"foot_pos_anchor",
|
||||
"foot_vel_anchor",
|
||||
"foot_normal_force_anchor",
|
||||
"contact",
|
||||
"last_action",
|
||||
],
|
||||
"force_semantics": "normal contact force, not full friction wrench",
|
||||
},
|
||||
"skill": {"weights": None, "latent": None, "reaction_wrench": None, "valid": False},
|
||||
"modifications": [
|
||||
"Replace whole body with fixed synthetic anchor root",
|
||||
"Translate mount origin to anchor origin",
|
||||
"Preserve FL fixed subtrees and root-side rotor",
|
||||
"Relative mesh URIs; no ROS runtime dependency",
|
||||
],
|
||||
}
|
||||
ET.indent(robot)
|
||||
ET.ElementTree(robot).write(output / "go2_fl.urdf", encoding="utf-8", xml_declaration=True)
|
||||
manifest["derived_urdf_sha256"] = digest(output / "go2_fl.urdf")
|
||||
(output / "manifest.json").write_text(json.dumps(manifest, indent=2, sort_keys=True) + "\n")
|
||||
return manifest
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--source", type=Path, default=DEFAULT_SOURCE)
|
||||
parser.add_argument("--output", type=Path, default=ROOT / "assets/generated/go2_fl")
|
||||
args = parser.parse_args()
|
||||
manifest = extract(args.source, args.output)
|
||||
print(
|
||||
f"Extracted {manifest['joint_order']} to {args.output}; "
|
||||
f"anchor height={manifest['anchor']['default_height']:.6f} m"
|
||||
)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,24 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Extract an explicitly described real single leg; no simulator startup."""
|
||||
|
||||
import argparse
|
||||
from pathlib import Path
|
||||
|
||||
from SkillFactory.legs import LegSpec
|
||||
from SkillFactory.legs.urdf import extract_leg
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--spec", type=Path, required=True)
|
||||
parser.add_argument("--source-root", type=Path)
|
||||
parser.add_argument("--output", type=Path)
|
||||
args = parser.parse_args()
|
||||
spec = LegSpec.load(args.spec, args.source_root)
|
||||
output = args.output or Path("assets/generated/legs") / spec.part_id
|
||||
bundle = extract_leg(spec, output)
|
||||
print(f"Extracted {bundle['joint_order']} to {output}; source {bundle['source']['sha256']}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,71 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Finite single-leg GUI physics preview before registering the RL environment."""
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import math
|
||||
import traceback
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
def main():
|
||||
from isaaclab.app import AppLauncher
|
||||
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--steps", type=int, default=1500)
|
||||
AppLauncher.add_app_launcher_args(parser)
|
||||
args = parser.parse_args()
|
||||
app = AppLauncher(args).app
|
||||
code = 0
|
||||
try:
|
||||
import torch
|
||||
from isaaclab_physx.physics import PhysxCfg
|
||||
from SkillFactory.assets.go2_leg import GO2_LEG_CFG, JOINT_NAMES
|
||||
|
||||
import isaaclab.sim as sim_utils
|
||||
from isaaclab.assets import Articulation
|
||||
|
||||
sim = sim_utils.SimulationContext(sim_utils.SimulationCfg(dt=0.002, device="cuda:0", physics=PhysxCfg()))
|
||||
sim.set_camera_view((1.0, 1.0, 0.8), (0.0, 0.0, 0.35))
|
||||
ground = sim_utils.GroundPlaneCfg()
|
||||
ground.func("/World/ground", ground)
|
||||
light = sim_utils.DomeLightCfg(intensity=2000)
|
||||
light.func("/World/light", light)
|
||||
cfg = GO2_LEG_CFG.replace(prim_path="/World/Robot")
|
||||
cfg.init_state.pos = (0.0, 0.0, 0.60)
|
||||
robot = Articulation(cfg)
|
||||
sim.reset()
|
||||
robot.write_joint_position_to_sim_index(position=robot.data.default_joint_pos.torch)
|
||||
robot.write_joint_velocity_to_sim_index(velocity=robot.data.default_joint_vel.torch)
|
||||
ids, names = robot.find_joints(JOINT_NAMES, preserve_order=True)
|
||||
assert names == JOINT_NAMES and robot.num_joints == 3
|
||||
start = robot.data.root_pos_w.torch.clone()
|
||||
for step in range(args.steps):
|
||||
target = robot.data.default_joint_pos.torch.clone()
|
||||
target[:, ids] += 0.05 * math.sin(2 * math.pi * 0.5 * step * 0.002)
|
||||
robot.set_joint_position_target_index(target=target)
|
||||
robot.write_data_to_sim()
|
||||
sim.step()
|
||||
robot.update(0.002)
|
||||
assert torch.isfinite(robot.data.joint_pos.torch).all()
|
||||
result = {
|
||||
"joint_names": names,
|
||||
"num_joints": robot.num_joints,
|
||||
"body_names": robot.body_names,
|
||||
"q": robot.data.joint_pos.torch.tolist(),
|
||||
"root_drift_m": (robot.data.root_pos_w.torch - start).abs().max().item(),
|
||||
"steps": args.steps,
|
||||
"gui_requested": not args.headless,
|
||||
}
|
||||
assert result["root_drift_m"] <= 1e-4
|
||||
Path("logs/week1_anchor/preview.json").write_text(json.dumps(result, indent=2))
|
||||
print(json.dumps(result, indent=2))
|
||||
except BaseException:
|
||||
traceback.print_exc()
|
||||
code = 1
|
||||
finally:
|
||||
app.close(exit_code=code)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user