7f39abb613
Add declarative single-leg import, fixed-anchor benchmark, masked shared-policy PPO prototype, regression tests, and consolidated documentation.
72 lines
2.7 KiB
Python
72 lines
2.7 KiB
Python
#!/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()
|