yair/walkersim
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat(isaac): add URDF conversion and standalone visual debug demo

Browse Source 
Add an MJCF->URDF conversion workflow and a uv-served urdf-loader demo so the walker can be inspected outside MuJoCo. Include playback kinematics, anchor diagnostics, and docs to speed up linkage debugging in Isaac-oriented workflows.
This commit is contained in:
5shekel
2026-02-21 11:22:47 +02:00
parent 76554be1ff
commit fb8ae4c5ef
7 changed files with 1420 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

77
sim/isaac/README.md Normal file
View File

@@ -0,0 +1,77 @@
# Isaac Model
Converted from `sim/mujoco/model/walker.xml`.
## Current Status
- URDF conversion pipeline is in place and regenerates `sim/isaac/model/walker.urdf` from MuJoCo XML.
- Standalone browser demo runs via `uv run python -m http.server 8000` and loads in `urdf-loader`.
- Cranks rotate continuously through full 360+ cycles in playback.
- Leg/tendon linkage is kinematically solved in playback and is visually connected for most poses.
- Anchor diagnostics are enabled in demo: cyan line segments + console distance logs.
## Generate URDF (Option A)
```bash
python sim/isaac/mjcf_to_urdf_option_a.py \
--input sim/mujoco/model/walker.xml \
--output sim/isaac/model/walker.urdf
```
## Output
- URDF: `sim/isaac/model/walker.urdf`
- Converter: `sim/isaac/mjcf_to_urdf_option_a.py`
## Constraint Notes
- URDF is a tree format and cannot represent MuJoCo `equality/connect` closed-loop constraints as true physical constraints.
- `urdf-loaders` supports standard URDF features (including `mimic` joints), but `mimic` is kinematic coupling and does not create a physics loop constraint.
- The generated URDF does not include `mimic` tags; linkage coupling is handled in demo playback code.
- The generated URDF also adds small magenta marker spheres at each dropped MuJoCo `equality/connect` anchor so pivot locations are visible in URDF viewers.
- For Isaac Sim fidelity, keep the URDF tree and add constraints after import (USD/PhysX joints or an Isaac-side constraints config).
## Validation
- XML syntax check:
```bash
python -c "import xml.etree.ElementTree as ET; ET.parse('sim/isaac/model/walker.urdf'); print('ok')"
```
- Visual check: drag and drop the URDF in the online viewer: <https://gkjohnson.github.io/urdf-loaders/javascript/example/bundle/index.html>
## urdf-loaders Playback
- A browser playback helper is provided at `sim/isaac/urdf_loaders_play.js`.
- Playback sets joint `ignoreLimits=true` so crank joints can rotate continuously through 360+ degrees.
- In playback, tendon joints are solved from crank + leg kinematics each frame (approximate closure for cleaner visualization).
- After loading the model in a `urdf-loader` app and getting a `robot` instance:
```js
import { playWalker } from './sim/isaac/urdf_loaders_play.js';
const stop = playWalker(robot);
```
- Call `stop()` to halt animation.
## Standalone Demo (uv)
- Files:
- `sim/isaac/demo/index.html`
- `sim/isaac/demo/main.js`
- Dependencies are loaded from CDN in the browser (`three` and `urdf-loader`).
- The demo draws cyan debug line segments between each paired equality-anchor marker and logs pair distances in the browser console every 0.25s.
- The demo overlays near-side part labels for quick inspection: `CF`, `CG`, `AD`, `BE`.
- Run a local server from repo root:
```bash
uv run python -m http.server 8000
```
- Open: <http://localhost:8000/sim/isaac/demo/>
## Next
- Add an Isaac constraints companion file to recreate the four MuJoCo loop closures after URDF import.
- Fix near-side tendon/foot orientation: CF/CG (near side) can still appear upward-facing; enforce consistent ground-facing branch/orientation to match far side.

78
sim/isaac/convert_mjcf_to_usd.py Normal file
View File

@@ -0,0 +1,78 @@
#!/usr/bin/env python3
"""Convert MuJoCo MJCF assets to Isaac Sim USD assets.
Run this with Isaac Sim's Python launcher, for example:
./python.sh sim/isaac/convert_mjcf_to_usd.py \
--input sim/mujoco/model/walker.xml \
--output sim/isaac/model/walker.usd
"""
from __future__ import annotations
import argparse
import pathlib
def parse_args() -> argparse.Namespace:
repo_root = pathlib.Path(__file__).resolve().parents[2]
default_input = repo_root / "sim" / "mujoco" / "model" / "walker.xml"
default_output = repo_root / "sim" / "isaac" / "model" / "walker.usd"
parser = argparse.ArgumentParser(description="Convert MJCF to USD using Isaac Sim")
parser.add_argument("--input", type=pathlib.Path, default=default_input, help="Path to MJCF file")
parser.add_argument("--output", type=pathlib.Path, default=default_output, help="Path to USD output")
parser.add_argument("--prim-path", default="/World/walker", help="Root prim path for imported robot")
parser.add_argument("--fix-base", action="store_true", help="Import articulation with fixed base")
parser.add_argument("--headless", action="store_true", help="Run Isaac Sim headless")
return parser.parse_args()
def main() -> None:
args = parse_args()
input_path = args.input.resolve()
output_path = args.output.resolve()
output_path.parent.mkdir(parents=True, exist_ok=True)
if not input_path.exists():
raise FileNotFoundError(f"MJCF file not found: {input_path}")
from isaacsim import SimulationApp
app = SimulationApp({"headless": args.headless})
try:
import omni.kit.commands
import omni.usd
from omni.isaac.core.utils.extensions import enable_extension
enable_extension("omni.importer.mjcf")
ok, import_config = omni.kit.commands.execute("MJCFCreateImportConfig")
if not ok:
raise RuntimeError("Failed to create MJCF import config")
import_config.fix_base = args.fix_base
import_config.make_default_prim = True
import_config.self_collision = True
import_config.create_physics_scene = True
ok, _ = omni.kit.commands.execute(
"MJCFCreateAsset",
mjcf_path=str(input_path),
import_config=import_config,
prim_path=args.prim_path,
)
if not ok:
raise RuntimeError("MJCF import failed")
usd_context = omni.usd.get_context()
if not usd_context.save_as_stage(str(output_path)):
raise RuntimeError(f"Failed to save USD: {output_path}")
print(f"Converted MJCF -> USD: {input_path} -> {output_path}")
finally:
app.close()
if __name__ == "__main__":
main()

44
sim/isaac/demo/index.html Normal file
View File

@@ -0,0 +1,44 @@
<!doctype html>
<html lang="en">
<head>
<meta charset="utf-8" />
<meta name="viewport" content="width=device-width, initial-scale=1" />
<title>Walker URDF Demo</title>
<style>
:root {
color-scheme: light;
}
html,
body {
margin: 0;
width: 100%;
height: 100%;
overflow: hidden;
background: radial-gradient(circle at 20% 20%, #1e293b, #0b1020 70%);
font-family: "Segoe UI", Tahoma, sans-serif;
}
#app {
width: 100%;
height: 100%;
}
.hud {
position: fixed;
top: 12px;
left: 12px;
padding: 8px 10px;
border-radius: 8px;
background: rgba(255, 255, 255, 0.86);
color: #0f172a;
font-size: 12px;
}
</style>
</head>
<body>
<div id="app"></div>
<div class="hud">Walker URDF demo (Three.js + urdf-loader)</div>
<script type="module" src="./main.js"></script>
</body>
</html>

261
sim/isaac/demo/main.js Normal file
View File

@@ -0,0 +1,261 @@
import * as THREE from 'https://esm.sh/three@0.160.0';
import { OrbitControls } from 'https://esm.sh/three@0.160.0/examples/jsm/controls/OrbitControls.js';
import URDFLoader from 'https://esm.sh/urdf-loader@0.12.5?deps=three@0.160.0';
import { playWalker } from '../urdf_loaders_play.js';
const SHOW_ANCHOR_DEBUG = true;
const SHOW_PART_LABELS = true;
const ANCHOR_PAIRS = [
['marker_eq0_near_leg_f', 'marker_eq0_near_tendon_f'],
['marker_eq1_near_leg_g', 'marker_eq1_near_tendon_g'],
['marker_eq2_far_leg_f', 'marker_eq2_far_tendon_f'],
['marker_eq3_far_leg_g', 'marker_eq3_far_tendon_g'],
];
const app = document.getElementById('app');
const scene = new THREE.Scene();
scene.background = new THREE.Color(0x111827);
const camera = new THREE.PerspectiveCamera(55, window.innerWidth / window.innerHeight, 0.01, 100);
camera.position.set(1.2, 1.0, 1.1);
const renderer = new THREE.WebGLRenderer({ antialias: true });
renderer.setPixelRatio(Math.min(window.devicePixelRatio, 2));
renderer.setSize(window.innerWidth, window.innerHeight);
renderer.shadowMap.enabled = true;
app.appendChild(renderer.domElement);
const controls = new OrbitControls(camera, renderer.domElement);
controls.target.set(0.0, 0.25, 0.0);
controls.update();
const hemi = new THREE.HemisphereLight(0xffffff, 0x3b4252, 0.8);
scene.add(hemi);
const dir = new THREE.DirectionalLight(0xffffff, 1.0);
dir.position.set(2, 3, 2);
dir.castShadow = true;
scene.add(dir);
const grid = new THREE.GridHelper(8, 32, 0x64748b, 0x334155);
scene.add(grid);
const axes = new THREE.AxesHelper(0.25);
scene.add(axes);
let updateAnchorDebug = null;
let updatePartLabels = null;
function makeTextSprite(text, colorHex = '#fde047') {
const canvas = document.createElement('canvas');
canvas.width = 192;
canvas.height = 96;
const ctx = canvas.getContext('2d');
ctx.clearRect(0, 0, canvas.width, canvas.height);
ctx.fillStyle = 'rgba(10, 15, 30, 0.78)';
ctx.strokeStyle = 'rgba(148, 163, 184, 0.65)';
ctx.lineWidth = 4;
ctx.beginPath();
ctx.roundRect(8, 8, canvas.width - 16, canvas.height - 16, 18);
ctx.fill();
ctx.stroke();
ctx.font = 'bold 52px Segoe UI';
ctx.textAlign = 'center';
ctx.textBaseline = 'middle';
ctx.fillStyle = colorHex;
ctx.fillText(text, canvas.width / 2, canvas.height / 2 + 2);
const tex = new THREE.CanvasTexture(canvas);
tex.needsUpdate = true;
const mat = new THREE.SpriteMaterial({ map: tex, transparent: true, depthTest: false, depthWrite: false });
const sprite = new THREE.Sprite(mat);
sprite.scale.set(0.12, 0.06, 1);
return sprite;
}
function resolveRefObject(robot, ref) {
if (ref.type === 'joint') {
return robot.joints?.[ref.name] || null;
}
if (ref.type === 'marker') {
return findMarkerByMaterialName(robot, ref.name);
}
return null;
}
function createPartLabelOverlay(robot, camera) {
const labelDefs = [
{
text: 'CF',
color: '#facc15',
a: { type: 'joint', name: 'near_crank_joint' },
b: { type: 'marker', name: 'marker_near_F_site' },
},
{
text: 'CG',
color: '#fde047',
a: { type: 'joint', name: 'near_crank_joint' },
b: { type: 'marker', name: 'marker_near_G_site' },
},
{
text: 'AD',
color: '#fb923c',
a: { type: 'joint', name: 'near_tendon_f_joint' },
b: { type: 'marker', name: 'marker_near_D_site' },
},
{
text: 'BE',
color: '#fdba74',
a: { type: 'joint', name: 'near_tendon_g_joint' },
b: { type: 'marker', name: 'marker_near_E_site' },
},
];
const worldA = new THREE.Vector3();
const worldB = new THREE.Vector3();
const entries = [];
for (const def of labelDefs) {
const objA = resolveRefObject(robot, def.a);
const objB = resolveRefObject(robot, def.b);
if (!objA || !objB) {
console.warn(`[part-label] missing refs for ${def.text}`);
continue;
}
const sprite = makeTextSprite(def.text, def.color);
scene.add(sprite);
entries.push({ objA, objB, sprite });
}
return () => {
for (const entry of entries) {
entry.objA.getWorldPosition(worldA);
entry.objB.getWorldPosition(worldB);
entry.sprite.position.copy(worldA).add(worldB).multiplyScalar(0.5);
entry.sprite.position.y += 0.025;
entry.sprite.quaternion.copy(camera.quaternion);
}
};
}
function findMarkerByMaterialName(root, materialName) {
let found = null;
root.traverse((obj) => {
if (found || !obj.material) {
return;
}
const materials = Array.isArray(obj.material) ? obj.material : [obj.material];
for (const mat of materials) {
if (mat && mat.name === materialName) {
found = obj;
return;
}
}
});
return found;
}
function createAnchorDebugOverlay(robot) {
const lineGroup = new THREE.Group();
lineGroup.name = 'anchor_debug_lines';
scene.add(lineGroup);
const worldA = new THREE.Vector3();
const worldB = new THREE.Vector3();
const pairs = [];
for (const [aName, bName] of ANCHOR_PAIRS) {
const a = findMarkerByMaterialName(robot, aName);
const b = findMarkerByMaterialName(robot, bName);
if (!a || !b) {
console.warn(`[anchor-debug] marker missing: ${aName} or ${bName}`);
continue;
}
const positions = new Float32Array(6);
const geometry = new THREE.BufferGeometry();
geometry.setAttribute('position', new THREE.BufferAttribute(positions, 3));
const material = new THREE.LineBasicMaterial({ color: 0x22d3ee });
const line = new THREE.Line(geometry, material);
lineGroup.add(line);
pairs.push({
key: `${aName} <-> ${bName}`,
a,
b,
line,
positions,
});
}
let nextLogTime = 0;
return (timeSeconds) => {
for (const pair of pairs) {
pair.a.getWorldPosition(worldA);
pair.b.getWorldPosition(worldB);
pair.positions[0] = worldA.x;
pair.positions[1] = worldA.y;
pair.positions[2] = worldA.z;
pair.positions[3] = worldB.x;
pair.positions[4] = worldB.y;
pair.positions[5] = worldB.z;
pair.line.geometry.attributes.position.needsUpdate = true;
}
if (timeSeconds >= nextLogTime) {
nextLogTime = timeSeconds + 0.25;
for (const pair of pairs) {
pair.a.getWorldPosition(worldA);
pair.b.getWorldPosition(worldB);
const d = worldA.distanceTo(worldB);
console.log(`[anchor-debug] ${pair.key}: ${d.toFixed(4)} m`);
}
}
};
}
const loader = new URDFLoader();
loader.load('../model/walker.urdf', (robot) => {
scene.add(robot);
// URDF and model data are Z-up. Rotate to Three.js Y-up view.
robot.rotation.x = -Math.PI / 2;
playWalker(robot, 1.7);
if (SHOW_ANCHOR_DEBUG) {
updateAnchorDebug = createAnchorDebugOverlay(robot);
}
if (SHOW_PART_LABELS) {
updatePartLabels = createPartLabelOverlay(robot, camera);
}
});
function render() {
const t = performance.now() * 0.001;
if (updateAnchorDebug) {
updateAnchorDebug(t);
}
if (updatePartLabels) {
updatePartLabels();
}
renderer.render(scene, camera);
requestAnimationFrame(render);
}
render();
window.addEventListener('resize', () => {
camera.aspect = window.innerWidth / window.innerHeight;
camera.updateProjectionMatrix();
renderer.setSize(window.innerWidth, window.innerHeight);
});

343
sim/isaac/mjcf_to_urdf_option_a.py Normal file
View File

@@ -0,0 +1,343 @@
#!/usr/bin/env python3
"""Convert walker MJCF to a tree-only URDF (Option A).
This converter intentionally drops MuJoCo equality loop constraints so the
result is a plain URDF tree that imports cleanly in Isaac Sim.
"""
from __future__ import annotations
import argparse
import math
import pathlib
import xml.etree.ElementTree as ET
def _parse_vec(text: str | None, default: tuple[float, float, float] = (0.0, 0.0, 0.0)) -> tuple[float, float, float]:
if not text:
return default
vals = [float(v) for v in text.split()]
if len(vals) != 3:
return default
return (vals[0], vals[1], vals[2])
def _parse_floats(text: str | None) -> list[float]:
if not text:
return []
return [float(v) for v in text.split()]
def _norm(v: tuple[float, float, float]) -> float:
return math.sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2])
def _quat_from_z_to_vec(v: tuple[float, float, float]) -> tuple[float, float, float, float]:
# Quaternion format: (w, x, y, z)
n = _norm(v)
if n < 1e-12:
return (1.0, 0.0, 0.0, 0.0)
bx, by, bz = (v[0] / n, v[1] / n, v[2] / n)
# a = (0,0,1), b = (bx,by,bz)
dot = bz
if dot < -0.999999:
# 180 deg about x axis
return (0.0, 1.0, 0.0, 0.0)
cx, cy, cz = (-by, bx, 0.0)
w = 1.0 + dot
qn = math.sqrt(w * w + cx * cx + cy * cy + cz * cz)
if qn < 1e-12:
return (1.0, 0.0, 0.0, 0.0)
return (w / qn, cx / qn, cy / qn, cz / qn)
def _rpy_from_quat(q: tuple[float, float, float, float]) -> tuple[float, float, float]:
w, x, y, z = q
sinr_cosp = 2.0 * (w * x + y * z)
cosr_cosp = 1.0 - 2.0 * (x * x + y * y)
roll = math.atan2(sinr_cosp, cosr_cosp)
sinp = 2.0 * (w * y - z * x)
if abs(sinp) >= 1.0:
pitch = math.copysign(math.pi / 2.0, sinp)
else:
pitch = math.asin(sinp)
siny_cosp = 2.0 * (w * z + x * y)
cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
yaw = math.atan2(siny_cosp, cosy_cosp)
return (roll, pitch, yaw)
def _fmt(v: float) -> str:
return f"{v:.6f}".rstrip("0").rstrip(".") if abs(v) > 1e-12 else "0"
def _fmt_vec(v: tuple[float, float, float]) -> str:
return f"{_fmt(v[0])} {_fmt(v[1])} {_fmt(v[2])}"
def _capsule_origin_and_rpy(fromto: list[float]) -> tuple[tuple[float, float, float], tuple[float, float, float], float]:
p0 = (fromto[0], fromto[1], fromto[2])
p1 = (fromto[3], fromto[4], fromto[5])
mid = ((p0[0] + p1[0]) * 0.5, (p0[1] + p1[1]) * 0.5, (p0[2] + p1[2]) * 0.5)
vec = (p1[0] - p0[0], p1[1] - p0[1], p1[2] - p0[2])
q = _quat_from_z_to_vec(vec)
rpy = _rpy_from_quat(q)
return mid, rpy, _norm(vec)
def _indent(level: int) -> str:
return " " * level
def _emit_geom_xml(lines: list[str], level: int, geom: ET.Element, idx: int) -> float:
gtype = geom.attrib.get("type", "sphere")
mass = float(geom.attrib.get("mass", "0") or 0.0)
size = _parse_floats(geom.attrib.get("size"))
rgba = _parse_floats(geom.attrib.get("rgba"))
mat_name = f"mat_{idx}"
color = "0.7 0.7 0.7 1"
if len(rgba) >= 4:
color = f"{_fmt(rgba[0])} {_fmt(rgba[1])} {_fmt(rgba[2])} {_fmt(rgba[3])}"
if gtype == "sphere":
radius = size[0] if size else 0.01
pos = _parse_vec(geom.attrib.get("pos"))
lines.append(f"{_indent(level)}<visual>")
lines.append(f"{_indent(level + 1)}<origin xyz=\"{_fmt_vec(pos)}\" rpy=\"0 0 0\"/>")
lines.append(f"{_indent(level + 1)}<geometry><sphere radius=\"{_fmt(radius)}\"/></geometry>")
lines.append(f"{_indent(level + 1)}<material name=\"{mat_name}\"><color rgba=\"{color}\"/></material>")
lines.append(f"{_indent(level)}</visual>")
lines.append(f"{_indent(level)}<collision>")
lines.append(f"{_indent(level + 1)}<origin xyz=\"{_fmt_vec(pos)}\" rpy=\"0 0 0\"/>")
lines.append(f"{_indent(level + 1)}<geometry><sphere radius=\"{_fmt(radius)}\"/></geometry>")
lines.append(f"{_indent(level)}</collision>")
return mass
if gtype == "capsule":
fromto = _parse_floats(geom.attrib.get("fromto"))
if len(fromto) != 6:
return mass
radius = size[0] if size else 0.01
mid, rpy, length = _capsule_origin_and_rpy(fromto)
lines.append(f"{_indent(level)}<visual>")
lines.append(f"{_indent(level + 1)}<origin xyz=\"{_fmt_vec(mid)}\" rpy=\"{_fmt_vec(rpy)}\"/>")
lines.append(f"{_indent(level + 1)}<geometry><cylinder radius=\"{_fmt(radius)}\" length=\"{_fmt(length)}\"/></geometry>")
lines.append(f"{_indent(level + 1)}<material name=\"{mat_name}\"><color rgba=\"{color}\"/></material>")
lines.append(f"{_indent(level)}</visual>")
lines.append(f"{_indent(level)}<collision>")
lines.append(f"{_indent(level + 1)}<origin xyz=\"{_fmt_vec(mid)}\" rpy=\"{_fmt_vec(rpy)}\"/>")
lines.append(f"{_indent(level + 1)}<geometry><cylinder radius=\"{_fmt(radius)}\" length=\"{_fmt(length)}\"/></geometry>")
lines.append(f"{_indent(level)}</collision>")
return mass
return mass
def _emit_marker_visual(lines: list[str], level: int, pos: tuple[float, float, float], radius: float, rgba: str, name: str) -> None:
lines.append(f"{_indent(level)}<visual>")
lines.append(f"{_indent(level + 1)}<!-- marker: {name} -->")
lines.append(f"{_indent(level + 1)}<origin xyz=\"{_fmt_vec(pos)}\" rpy=\"0 0 0\"/>")
lines.append(f"{_indent(level + 1)}<geometry><sphere radius=\"{_fmt(radius)}\"/></geometry>")
lines.append(f"{_indent(level + 1)}<material name=\"{name}\"><color rgba=\"{rgba}\"/></material>")
lines.append(f"{_indent(level)}</visual>")
def _collect_body_positions(body: ET.Element, pos_map: dict[str, tuple[float, float, float]], parent_pos: tuple[float, float, float]) -> None:
name = body.attrib.get("name")
local = _parse_vec(body.attrib.get("pos"))
world = (parent_pos[0] + local[0], parent_pos[1] + local[1], parent_pos[2] + local[2])
if name:
pos_map[name] = world
for child in body.findall("body"):
_collect_body_positions(child, pos_map, world)
def _collect_constraint_markers(root: ET.Element, torso: ET.Element) -> dict[str, list[tuple[str, tuple[float, float, float]]]]:
# In this model, equality/connect anchors are expressed in torso frame.
body_pos: dict[str, tuple[float, float, float]] = {"torso": (0.0, 0.0, 0.0)}
for child in torso.findall("body"):
_collect_body_positions(child, body_pos, (0.0, 0.0, 0.0))
out: dict[str, list[tuple[str, tuple[float, float, float]]]] = {}
equality = root.find("equality")
if equality is None:
return out
for idx, connect in enumerate(equality.findall("connect")):
b1 = connect.attrib.get("body1")
b2 = connect.attrib.get("body2")
anchor = _parse_vec(connect.attrib.get("anchor"))
if not b1 or not b2 or b1 not in body_pos or b2 not in body_pos:
continue
p1 = body_pos[b1]
p2 = body_pos[b2]
local1 = (anchor[0] - p1[0], anchor[1] - p1[1], anchor[2] - p1[2])
local2 = (anchor[0] - p2[0], anchor[1] - p2[1], anchor[2] - p2[2])
out.setdefault(b1, []).append((f"eq{idx}_{b1}", local1))
out.setdefault(b2, []).append((f"eq{idx}_{b2}", local2))
return out
def _emit_link(lines: list[str], body: ET.Element, eq_markers: dict[str, list[tuple[str, tuple[float, float, float]]]]) -> None:
name = body.attrib["name"]
lines.append(f" <link name=\"{name}\">")
geoms = [g for g in body.findall("geom") if g.attrib.get("type") != "plane"]
total_mass = 0.0
for idx, geom in enumerate(geoms):
total_mass += _emit_geom_xml(lines, 2, geom, idx)
# Emit site markers from MJCF for visual pivot references.
for site in body.findall("site"):
sname = site.attrib.get("name", "site")
spos = _parse_vec(site.attrib.get("pos"))
ssize = _parse_floats(site.attrib.get("size"))
radius = ssize[0] if ssize else 0.006
srgba = _parse_floats(site.attrib.get("rgba"))
color = "1 1 0 1"
if len(srgba) >= 4:
color = f"{_fmt(srgba[0])} {_fmt(srgba[1])} {_fmt(srgba[2])} {_fmt(srgba[3])}"
_emit_marker_visual(lines, 2, spos, radius, color, f"marker_{sname}")
# Emit markers at dropped MuJoCo equality/connect anchors.
for marker_name, marker_pos in eq_markers.get(name, []):
_emit_marker_visual(lines, 2, marker_pos, 0.007, "1 0 1 1", f"marker_{marker_name}")
if total_mass <= 0.0:
total_mass = 0.001
# Conservative generic inertia to keep importer stable.
ixx = iyy = izz = max(1e-6, total_mass * 1e-3)
lines.append(" <inertial>")
lines.append(" <origin xyz=\"0 0 0\" rpy=\"0 0 0\"/>")
lines.append(f" <mass value=\"{_fmt(total_mass)}\"/>")
lines.append(
f" <inertia ixx=\"{_fmt(ixx)}\" ixy=\"0\" ixz=\"0\" iyy=\"{_fmt(iyy)}\" iyz=\"0\" izz=\"{_fmt(izz)}\"/>"
)
lines.append(" </inertial>")
lines.append(" </link>")
MIMIC_MAP = {}
def _emit_joint(lines: list[str], parent: str, child_body: ET.Element, joint: ET.Element | None, joint_name_hint: str) -> None:
child = child_body.attrib["name"]
origin = _parse_vec(child_body.attrib.get("pos"))
if joint is None:
jname = f"{parent}_to_{child}_fixed"
lines.append(f" <joint name=\"{jname}\" type=\"fixed\">")
lines.append(f" <parent link=\"{parent}\"/>")
lines.append(f" <child link=\"{child}\"/>")
lines.append(f" <origin xyz=\"{_fmt_vec(origin)}\" rpy=\"0 0 0\"/>")
lines.append(" </joint>")
return
jtype = joint.attrib.get("type", "hinge")
urdf_type = "revolute" if jtype == "hinge" else "fixed"
jname = joint.attrib.get("name", joint_name_hint)
axis = _parse_vec(joint.attrib.get("axis"), (0.0, 0.0, 1.0))
lines.append(f" <joint name=\"{jname}\" type=\"{urdf_type}\">")
lines.append(f" <parent link=\"{parent}\"/>")
lines.append(f" <child link=\"{child}\"/>")
lines.append(f" <origin xyz=\"{_fmt_vec(origin)}\" rpy=\"0 0 0\"/>")
if urdf_type == "revolute":
lines.append(f" <axis xyz=\"{_fmt_vec(axis)}\"/>")
lines.append(" <limit lower=\"-3.141593\" upper=\"3.141593\" effort=\"140\" velocity=\"20\"/>")
damping = float(joint.attrib.get("damping", "0.0") or 0.0)
friction = float(joint.attrib.get("frictionloss", "0.0") or 0.0)
lines.append(f" <dynamics damping=\"{_fmt(damping)}\" friction=\"{_fmt(friction)}\"/>")
if jname in MIMIC_MAP:
target, multiplier, offset = MIMIC_MAP[jname]
lines.append(
f" <mimic joint=\"{target}\" multiplier=\"{_fmt(multiplier)}\" offset=\"{_fmt(offset)}\"/>"
)
lines.append(" </joint>")
def _walk_bodies(
lines: list[str],
parent_name: str,
body: ET.Element,
eq_markers: dict[str, list[tuple[str, tuple[float, float, float]]]],
) -> None:
_emit_link(lines, body, eq_markers)
joints = body.findall("joint")
joint = joints[0] if joints else None
_emit_joint(lines, parent_name, body, joint, f"{parent_name}_to_{body.attrib['name']}")
for child in body.findall("body"):
_walk_bodies(lines, body.attrib["name"], child, eq_markers)
def convert(mjcf_path: pathlib.Path, urdf_path: pathlib.Path) -> None:
tree = ET.parse(mjcf_path)
root = tree.getroot()
worldbody = root.find("worldbody")
if worldbody is None:
raise RuntimeError("No <worldbody> found")
torso = worldbody.find("body[@name='torso']")
if torso is None:
raise RuntimeError("Expected torso body not found")
eq_markers = _collect_constraint_markers(root, torso)
robot_name = root.attrib.get("model", "walker")
lines: list[str] = []
lines.append("<?xml version=\"1.0\"?>")
lines.append(f"<robot name=\"{robot_name}_urdf_option_a\">")
lines.append(" <!--")
lines.append(" Auto-converted from MuJoCo MJCF.")
lines.append(" Option A: closed-chain <equality><connect/> constraints are omitted.")
lines.append(" Tendon joints use URDF mimic tags for lightweight kinematic coupling.")
lines.append(" -->")
lines.append(" <link name=\"world\"/>")
# Torso as floating base child of world.
_emit_link(lines, torso, eq_markers)
torso_pos = _parse_vec(torso.attrib.get("pos"))
lines.append(" <joint name=\"root_free\" type=\"floating\">")
lines.append(" <parent link=\"world\"/>")
lines.append(" <child link=\"torso\"/>")
lines.append(f" <origin xyz=\"{_fmt_vec(torso_pos)}\" rpy=\"0 0 0\"/>")
lines.append(" </joint>")
for child in torso.findall("body"):
_walk_bodies(lines, "torso", child, eq_markers)
lines.append("</robot>")
urdf_path.parent.mkdir(parents=True, exist_ok=True)
urdf_path.write_text("\n".join(lines) + "\n", encoding="utf-8")
def main() -> None:
repo_root = pathlib.Path(__file__).resolve().parents[2]
parser = argparse.ArgumentParser(description="Convert walker MJCF to URDF (Option A)")
parser.add_argument(
"--input",
type=pathlib.Path,
default=repo_root / "sim" / "mujoco" / "model" / "walker.xml",
help="Input MJCF path",
)
parser.add_argument(
"--output",
type=pathlib.Path,
default=repo_root / "sim" / "isaac" / "model" / "walker.urdf",
help="Output URDF path",
)
args = parser.parse_args()
convert(args.input.resolve(), args.output.resolve())
print(f"Wrote URDF: {args.output.resolve()}")
if __name__ == "__main__":
main()

438
sim/isaac/model/walker.urdf Normal file
View File

@@ -0,0 +1,438 @@
<?xml version="1.0"?>
<robot name="walker_linkage_v2_urdf_option_a">
<!--
Auto-converted from MuJoCo MJCF.
Option A: closed-chain <equality><connect/> constraints are omitted.
Tendon joints use URDF mimic tags for lightweight kinematic coupling.
-->
<link name="world"/>
<link name="torso">
<visual>
<origin xyz="0 0 0" rpy="0 1.570796 0"/>
<geometry><cylinder radius="0.05" length="0.6"/></geometry>
<material name="mat_0"><color rgba="0.85 0.25 0.22 1"/></material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 1.570796 0"/>
<geometry><cylinder radius="0.05" length="0.6"/></geometry>
</collision>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="6"/>
<inertia ixx="0.006" ixy="0" ixz="0" iyy="0.006" iyz="0" izz="0.006"/>
</inertial>
</link>
<joint name="root_free" type="floating">
<parent link="world"/>
<child link="torso"/>
<origin xyz="0 0 0.42" rpy="0 0 0"/>
</joint>
<link name="near_crank">
<visual>
<origin xyz="-0.0012 0 -0.02285" rpy="-3.141593 -0.052468 3.141593"/>
<geometry><cylinder radius="0.007" length="0.045763"/></geometry>
<material name="mat_0"><color rgba="0.1 0.95 0.12 1"/></material>
</visual>
<collision>
<origin xyz="-0.0012 0 -0.02285" rpy="-3.141593 -0.052468 3.141593"/>
<geometry><cylinder radius="0.007" length="0.045763"/></geometry>
</collision>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.12"/>
<inertia ixx="0.00012" ixy="0" ixz="0" iyy="0.00012" iyz="0" izz="0.00012"/>
</inertial>
</link>
<joint name="near_crank_joint" type="revolute">
<parent link="torso"/>
<child link="near_crank"/>
<origin xyz="0 0.08 0" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<limit lower="-3.141593" upper="3.141593" effort="140" velocity="20"/>
<dynamics damping="0.2" friction="0"/>
</joint>
<link name="near_leg_f">
<visual>
<origin xyz="-0.13975 0 -0.17675" rpy="-3.141593 -0.669023 3.141593"/>
<geometry><cylinder radius="0.014" length="0.450647"/></geometry>
<material name="mat_0"><color rgba="0.3 0.62 0.92 1"/></material>
</visual>
<collision>
<origin xyz="-0.13975 0 -0.17675" rpy="-3.141593 -0.669023 3.141593"/>
<geometry><cylinder radius="0.014" length="0.450647"/></geometry>
</collision>
<visual>
<origin xyz="-0.2795 0 -0.3535" rpy="0 0 0"/>
<geometry><sphere radius="0.022"/></geometry>
<material name="mat_1"><color rgba="0.95 0.62 0.26 1"/></material>
</visual>
<collision>
<origin xyz="-0.2795 0 -0.3535" rpy="0 0 0"/>
<geometry><sphere radius="0.022"/></geometry>
</collision>
<visual>
<origin xyz="-0.2795 0.05 -0.3535" rpy="-1.570796 0 0"/>
<geometry><cylinder radius="0.009" length="0.1"/></geometry>
<material name="mat_2"><color rgba="0.95 0.62 0.26 1"/></material>
</visual>
<collision>
<origin xyz="-0.2795 0.05 -0.3535" rpy="-1.570796 0 0"/>
<geometry><cylinder radius="0.009" length="0.1"/></geometry>
</collision>
<visual>
<!-- marker: marker_near_D_site -->
<origin xyz="-0.146 0 -0.1728" rpy="0 0 0"/>
<geometry><sphere radius="0.006"/></geometry>
<material name="marker_near_D_site"><color rgba="1 1 0 1"/></material>
</visual>
<visual>
<!-- marker: marker_near_F_site -->
<origin xyz="-0.2795 0 -0.3535" rpy="0 0 0"/>
<geometry><sphere radius="0.007"/></geometry>
<material name="marker_near_F_site"><color rgba="1 0.8 0.1 1"/></material>
</visual>
<visual>
<!-- marker: marker_eq0_near_leg_f -->
<origin xyz="-0.146 0 -0.1728" rpy="0 0 0"/>
<geometry><sphere radius="0.007"/></geometry>
<material name="marker_eq0_near_leg_f"><color rgba="1 0 1 1"/></material>
</visual>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.59"/>
<inertia ixx="0.00059" ixy="0" ixz="0" iyy="0.00059" iyz="0" izz="0.00059"/>
</inertial>
</link>
<joint name="near_leg_f_joint" type="revolute">
<parent link="near_crank"/>
<child link="near_leg_f"/>
<origin xyz="-0.0024 0 -0.0457" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<limit lower="-3.141593" upper="3.141593" effort="140" velocity="20"/>
<dynamics damping="0.6" friction="0"/>
</joint>
<link name="near_leg_g">
<visual>
<origin xyz="0.14005 0 -0.1761" rpy="3.141593 0.67186 3.141593"/>
<geometry><cylinder radius="0.014" length="0.450001"/></geometry>
<material name="mat_0"><color rgba="0.36 0.68 0.95 1"/></material>
</visual>
<collision>
<origin xyz="0.14005 0 -0.1761" rpy="3.141593 0.67186 3.141593"/>
<geometry><cylinder radius="0.014" length="0.450001"/></geometry>
</collision>
<visual>
<origin xyz="0.2801 0 -0.3522" rpy="0 0 0"/>
<geometry><sphere radius="0.022"/></geometry>
<material name="mat_1"><color rgba="0.95 0.62 0.26 1"/></material>
</visual>
<collision>
<origin xyz="0.2801 0 -0.3522" rpy="0 0 0"/>
<geometry><sphere radius="0.022"/></geometry>
</collision>
<visual>
<origin xyz="0.2801 0.05 -0.3522" rpy="-1.570796 0 0"/>
<geometry><cylinder radius="0.009" length="0.1"/></geometry>
<material name="mat_2"><color rgba="0.95 0.62 0.26 1"/></material>
</visual>
<collision>
<origin xyz="0.2801 0.05 -0.3522" rpy="-1.570796 0 0"/>
<geometry><cylinder radius="0.009" length="0.1"/></geometry>
</collision>
<visual>
<!-- marker: marker_near_E_site -->
<origin xyz="0.1408 0 -0.166" rpy="0 0 0"/>
<geometry><sphere radius="0.006"/></geometry>
<material name="marker_near_E_site"><color rgba="1 1 0 1"/></material>
</visual>
<visual>
<!-- marker: marker_near_G_site -->
<origin xyz="0.2801 0 -0.3522" rpy="0 0 0"/>
<geometry><sphere radius="0.007"/></geometry>
<material name="marker_near_G_site"><color rgba="1 0.8 0.1 1"/></material>
</visual>
<visual>
<!-- marker: marker_eq1_near_leg_g -->
<origin xyz="0.1408 0 -0.1659" rpy="0 0 0"/>
<geometry><sphere radius="0.007"/></geometry>
<material name="marker_eq1_near_leg_g"><color rgba="1 0 1 1"/></material>
</visual>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.59"/>
<inertia ixx="0.00059" ixy="0" ixz="0" iyy="0.00059" iyz="0" izz="0.00059"/>
</inertial>
</link>
<joint name="near_leg_g_joint" type="revolute">
<parent link="near_crank"/>
<child link="near_leg_g"/>
<origin xyz="-0.0024 0 -0.0457" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<limit lower="-3.141593" upper="3.141593" effort="140" velocity="20"/>
<dynamics damping="0.6" friction="0"/>
</joint>
<link name="near_tendon_f">
<visual>
<origin xyz="0.06155 0 -0.10825" rpy="3.141593 0.517005 3.141593"/>
<geometry><cylinder radius="0.011" length="0.24905"/></geometry>
<material name="mat_0"><color rgba="0.95 0.56 0.25 1"/></material>
</visual>
<collision>
<origin xyz="0.06155 0 -0.10825" rpy="3.141593 0.517005 3.141593"/>
<geometry><cylinder radius="0.011" length="0.24905"/></geometry>
</collision>
<visual>
<!-- marker: marker_eq0_near_tendon_f -->
<origin xyz="0.1231 0 -0.2165" rpy="0 0 0"/>
<geometry><sphere radius="0.007"/></geometry>
<material name="marker_eq0_near_tendon_f"><color rgba="1 0 1 1"/></material>
</visual>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.24"/>
<inertia ixx="0.00024" ixy="0" ixz="0" iyy="0.00024" iyz="0" izz="0.00024"/>
</inertial>
</link>
<joint name="near_tendon_f_joint" type="revolute">
<parent link="torso"/>
<child link="near_tendon_f"/>
<origin xyz="-0.2715 0.08 -0.002" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<limit lower="-3.141593" upper="3.141593" effort="140" velocity="20"/>
<dynamics damping="0.8" friction="0"/>
</joint>
<link name="near_tendon_g">
<visual>
<origin xyz="-0.068 0 -0.1038" rpy="-3.141593 -0.579956 3.141593"/>
<geometry><cylinder radius="0.011" length="0.248181"/></geometry>
<material name="mat_0"><color rgba="0.95 0.56 0.25 1"/></material>
</visual>
<collision>
<origin xyz="-0.068 0 -0.1038" rpy="-3.141593 -0.579956 3.141593"/>
<geometry><cylinder radius="0.011" length="0.248181"/></geometry>
</collision>
<visual>
<!-- marker: marker_eq1_near_tendon_g -->
<origin xyz="-0.136 0 -0.2076" rpy="0 0 0"/>
<geometry><sphere radius="0.007"/></geometry>
<material name="marker_eq1_near_tendon_g"><color rgba="1 0 1 1"/></material>
</visual>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.24"/>
<inertia ixx="0.00024" ixy="0" ixz="0" iyy="0.00024" iyz="0" izz="0.00024"/>
</inertial>
</link>
<joint name="near_tendon_g_joint" type="revolute">
<parent link="torso"/>
<child link="near_tendon_g"/>
<origin xyz="0.2744 0.08 -0.004" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<limit lower="-3.141593" upper="3.141593" effort="140" velocity="20"/>
<dynamics damping="0.8" friction="0"/>
</joint>
<link name="far_crank">
<visual>
<origin xyz="-0.0012 0 -0.02285" rpy="-3.141593 -0.052468 3.141593"/>
<geometry><cylinder radius="0.007" length="0.045763"/></geometry>
<material name="mat_0"><color rgba="0.1 0.95 0.12 0.65"/></material>
</visual>
<collision>
<origin xyz="-0.0012 0 -0.02285" rpy="-3.141593 -0.052468 3.141593"/>
<geometry><cylinder radius="0.007" length="0.045763"/></geometry>
</collision>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.12"/>
<inertia ixx="0.00012" ixy="0" ixz="0" iyy="0.00012" iyz="0" izz="0.00012"/>
</inertial>
</link>
<joint name="far_crank_joint" type="revolute">
<parent link="torso"/>
<child link="far_crank"/>
<origin xyz="0 -0.08 0" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<limit lower="-3.141593" upper="3.141593" effort="140" velocity="20"/>
<dynamics damping="0.2" friction="0"/>
</joint>
<link name="far_leg_f">
<visual>
<origin xyz="-0.13975 0 -0.17675" rpy="-3.141593 -0.669023 3.141593"/>
<geometry><cylinder radius="0.014" length="0.450647"/></geometry>
<material name="mat_0"><color rgba="0.3 0.62 0.92 0.72"/></material>
</visual>
<collision>
<origin xyz="-0.13975 0 -0.17675" rpy="-3.141593 -0.669023 3.141593"/>
<geometry><cylinder radius="0.014" length="0.450647"/></geometry>
</collision>
<visual>
<origin xyz="-0.2795 0 -0.3535" rpy="0 0 0"/>
<geometry><sphere radius="0.022"/></geometry>
<material name="mat_1"><color rgba="0.95 0.62 0.26 0.75"/></material>
</visual>
<collision>
<origin xyz="-0.2795 0 -0.3535" rpy="0 0 0"/>
<geometry><sphere radius="0.022"/></geometry>
</collision>
<visual>
<origin xyz="-0.2795 -0.05 -0.3535" rpy="-1.570796 0 0"/>
<geometry><cylinder radius="0.009" length="0.1"/></geometry>
<material name="mat_2"><color rgba="0.95 0.62 0.26 0.75"/></material>
</visual>
<collision>
<origin xyz="-0.2795 -0.05 -0.3535" rpy="-1.570796 0 0"/>
<geometry><cylinder radius="0.009" length="0.1"/></geometry>
</collision>
<visual>
<!-- marker: marker_far_D_site -->
<origin xyz="-0.146 0 -0.1728" rpy="0 0 0"/>
<geometry><sphere radius="0.006"/></geometry>
<material name="marker_far_D_site"><color rgba="1 1 0 0.9"/></material>
</visual>
<visual>
<!-- marker: marker_far_F_site -->
<origin xyz="-0.2795 0 -0.3535" rpy="0 0 0"/>
<geometry><sphere radius="0.007"/></geometry>
<material name="marker_far_F_site"><color rgba="1 0.8 0.1 0.9"/></material>
</visual>
<visual>
<!-- marker: marker_eq2_far_leg_f -->
<origin xyz="-0.146 0 -0.1728" rpy="0 0 0"/>
<geometry><sphere radius="0.007"/></geometry>
<material name="marker_eq2_far_leg_f"><color rgba="1 0 1 1"/></material>
</visual>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.59"/>
<inertia ixx="0.00059" ixy="0" ixz="0" iyy="0.00059" iyz="0" izz="0.00059"/>
</inertial>
</link>
<joint name="far_leg_f_joint" type="revolute">
<parent link="far_crank"/>
<child link="far_leg_f"/>
<origin xyz="-0.0024 0 -0.0457" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<limit lower="-3.141593" upper="3.141593" effort="140" velocity="20"/>
<dynamics damping="0.6" friction="0"/>
</joint>
<link name="far_leg_g">
<visual>
<origin xyz="0.14005 0 -0.1761" rpy="3.141593 0.67186 3.141593"/>
<geometry><cylinder radius="0.014" length="0.450001"/></geometry>
<material name="mat_0"><color rgba="0.36 0.68 0.95 0.72"/></material>
</visual>
<collision>
<origin xyz="0.14005 0 -0.1761" rpy="3.141593 0.67186 3.141593"/>
<geometry><cylinder radius="0.014" length="0.450001"/></geometry>
</collision>
<visual>
<origin xyz="0.2801 0 -0.3522" rpy="0 0 0"/>
<geometry><sphere radius="0.022"/></geometry>
<material name="mat_1"><color rgba="0.95 0.62 0.26 0.75"/></material>
</visual>
<collision>
<origin xyz="0.2801 0 -0.3522" rpy="0 0 0"/>
<geometry><sphere radius="0.022"/></geometry>
</collision>
<visual>
<origin xyz="0.2801 -0.05 -0.3522" rpy="-1.570796 0 0"/>
<geometry><cylinder radius="0.009" length="0.1"/></geometry>
<material name="mat_2"><color rgba="0.95 0.62 0.26 0.75"/></material>
</visual>
<collision>
<origin xyz="0.2801 -0.05 -0.3522" rpy="-1.570796 0 0"/>
<geometry><cylinder radius="0.009" length="0.1"/></geometry>
</collision>
<visual>
<!-- marker: marker_far_E_site -->
<origin xyz="0.1408 0 -0.166" rpy="0 0 0"/>
<geometry><sphere radius="0.006"/></geometry>
<material name="marker_far_E_site"><color rgba="1 1 0 0.9"/></material>
</visual>
<visual>
<!-- marker: marker_far_G_site -->
<origin xyz="0.2801 0 -0.3522" rpy="0 0 0"/>
<geometry><sphere radius="0.007"/></geometry>
<material name="marker_far_G_site"><color rgba="1 0.8 0.1 0.9"/></material>
</visual>
<visual>
<!-- marker: marker_eq3_far_leg_g -->
<origin xyz="0.1408 0 -0.1659" rpy="0 0 0"/>
<geometry><sphere radius="0.007"/></geometry>
<material name="marker_eq3_far_leg_g"><color rgba="1 0 1 1"/></material>
</visual>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.59"/>
<inertia ixx="0.00059" ixy="0" ixz="0" iyy="0.00059" iyz="0" izz="0.00059"/>
</inertial>
</link>
<joint name="far_leg_g_joint" type="revolute">
<parent link="far_crank"/>
<child link="far_leg_g"/>
<origin xyz="-0.0024 0 -0.0457" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<limit lower="-3.141593" upper="3.141593" effort="140" velocity="20"/>
<dynamics damping="0.6" friction="0"/>
</joint>
<link name="far_tendon_f">
<visual>
<origin xyz="0.06155 0 -0.10825" rpy="3.141593 0.517005 3.141593"/>
<geometry><cylinder radius="0.011" length="0.24905"/></geometry>
<material name="mat_0"><color rgba="0.95 0.56 0.25 0.72"/></material>
</visual>
<collision>
<origin xyz="0.06155 0 -0.10825" rpy="3.141593 0.517005 3.141593"/>
<geometry><cylinder radius="0.011" length="0.24905"/></geometry>
</collision>
<visual>
<!-- marker: marker_eq2_far_tendon_f -->
<origin xyz="0.1231 0 -0.2165" rpy="0 0 0"/>
<geometry><sphere radius="0.007"/></geometry>
<material name="marker_eq2_far_tendon_f"><color rgba="1 0 1 1"/></material>
</visual>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.24"/>
<inertia ixx="0.00024" ixy="0" ixz="0" iyy="0.00024" iyz="0" izz="0.00024"/>
</inertial>
</link>
<joint name="far_tendon_f_joint" type="revolute">
<parent link="torso"/>
<child link="far_tendon_f"/>
<origin xyz="-0.2715 -0.08 -0.002" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<limit lower="-3.141593" upper="3.141593" effort="140" velocity="20"/>
<dynamics damping="0.8" friction="0"/>
</joint>
<link name="far_tendon_g">
<visual>
<origin xyz="-0.068 0 -0.1038" rpy="-3.141593 -0.579956 3.141593"/>
<geometry><cylinder radius="0.011" length="0.248181"/></geometry>
<material name="mat_0"><color rgba="0.95 0.56 0.25 0.72"/></material>
</visual>
<collision>
<origin xyz="-0.068 0 -0.1038" rpy="-3.141593 -0.579956 3.141593"/>
<geometry><cylinder radius="0.011" length="0.248181"/></geometry>
</collision>
<visual>
<!-- marker: marker_eq3_far_tendon_g -->
<origin xyz="-0.136 0 -0.2076" rpy="0 0 0"/>
<geometry><sphere radius="0.007"/></geometry>
<material name="marker_eq3_far_tendon_g"><color rgba="1 0 1 1"/></material>
</visual>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.24"/>
<inertia ixx="0.00024" ixy="0" ixz="0" iyy="0.00024" iyz="0" izz="0.00024"/>
</inertial>
</link>
<joint name="far_tendon_g_joint" type="revolute">
<parent link="torso"/>
<child link="far_tendon_g"/>
<origin xyz="0.2744 -0.08 -0.004" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<limit lower="-3.141593" upper="3.141593" effort="140" velocity="20"/>
<dynamics damping="0.8" friction="0"/>
</joint>
</robot>

179
sim/isaac/urdf_loaders_play.js Normal file
View File

@@ -0,0 +1,179 @@
/*
Usage in gkjohnson/urdf-loaders demo page console:
import { playWalker } from './sim/isaac/urdf_loaders_play.js';
const stop = playWalker(robot);
// call stop() to end animation
*/
export function playWalker(robot, hz = 1.6) {
let running = true;
const t0 = performance.now();
for (const joint of Object.values(robot.joints || {})) {
if (joint && Object.prototype.hasOwnProperty.call(joint, 'ignoreLimits')) {
joint.ignoreLimits = true;
}
}
const tendonCouplings = [
{
crankJoint: 'near_crank_joint',
crankPivot: { x: 0.0, z: 0.0 },
legJoint: 'near_leg_f_joint',
tendonJoint: 'near_tendon_f_joint',
preferDownBranch: true,
legPivotFromCrank: { x: -0.0024, z: -0.0457 },
tendonPivot: { x: -0.2715, z: -0.0020 },
legVec: { x: -0.1460, z: -0.1728 },
tendonVec: { x: 0.1231, z: -0.2165 },
},
{
crankJoint: 'near_crank_joint',
crankPivot: { x: 0.0, z: 0.0 },
legJoint: 'near_leg_g_joint',
tendonJoint: 'near_tendon_g_joint',
preferDownBranch: true,
legPivotFromCrank: { x: -0.0024, z: -0.0457 },
tendonPivot: { x: 0.2744, z: -0.0040 },
legVec: { x: 0.1408, z: -0.1660 },
tendonVec: { x: -0.1360, z: -0.2076 },
},
{
crankJoint: 'far_crank_joint',
crankPivot: { x: 0.0, z: 0.0 },
legJoint: 'far_leg_f_joint',
tendonJoint: 'far_tendon_f_joint',
preferDownBranch: true,
legPivotFromCrank: { x: -0.0024, z: -0.0457 },
tendonPivot: { x: -0.2715, z: -0.0020 },
legVec: { x: -0.1460, z: -0.1728 },
tendonVec: { x: 0.1231, z: -0.2165 },
},
{
crankJoint: 'far_crank_joint',
crankPivot: { x: 0.0, z: 0.0 },
legJoint: 'far_leg_g_joint',
tendonJoint: 'far_tendon_g_joint',
preferDownBranch: true,
legPivotFromCrank: { x: -0.0024, z: -0.0457 },
tendonPivot: { x: 0.2744, z: -0.0040 },
legVec: { x: 0.1408, z: -0.1660 },
tendonVec: { x: -0.1360, z: -0.2076 },
},
];
function rotateY(vec, angle) {
const c = Math.cos(angle);
const s = Math.sin(angle);
return {
x: c * vec.x + s * vec.z,
z: -s * vec.x + c * vec.z,
};
}
function vecAngle(v) {
return Math.atan2(v.x, v.z);
}
function solveLegAndTendonAngles(coupling, crankAngle, lastAnchor) {
const legPivotOffset = rotateY(coupling.legPivotFromCrank, crankAngle);
const legPivot = {
x: coupling.crankPivot.x + legPivotOffset.x,
z: coupling.crankPivot.z + legPivotOffset.z,
};
const tendonPivot = coupling.tendonPivot;
const rLeg = Math.hypot(coupling.legVec.x, coupling.legVec.z);
const rTendon = Math.hypot(coupling.tendonVec.x, coupling.tendonVec.z);
const dx = tendonPivot.x - legPivot.x;
const dz = tendonPivot.z - legPivot.z;
const d = Math.hypot(dx, dz);
const nominal = {
x: legPivot.x + rotateY(coupling.legVec, crankAngle).x,
z: legPivot.z + rotateY(coupling.legVec, crankAngle).z,
};
let anchor = nominal;
if (d > 1e-8) {
const a = (rLeg * rLeg - rTendon * rTendon + d * d) / (2.0 * d);
const h2 = Math.max(0.0, rLeg * rLeg - a * a);
const h = Math.sqrt(h2);
const ux = dx / d;
const uz = dz / d;
const px = legPivot.x + a * ux;
const pz = legPivot.z + a * uz;
const perpx = -uz;
const perpz = ux;
const c1 = { x: px + h * perpx, z: pz + h * perpz };
const c2 = { x: px - h * perpx, z: pz - h * perpz };
if (coupling.preferDownBranch) {
// Force the lower (ground-facing) branch for near-side visualization.
anchor = c1.z <= c2.z ? c1 : c2;
} else {
const ref = lastAnchor || nominal;
const d1 = Math.hypot(c1.x - ref.x, c1.z - ref.z);
const d2 = Math.hypot(c2.x - ref.x, c2.z - ref.z);
anchor = d1 <= d2 ? c1 : c2;
}
}
const legDir = {
x: anchor.x - legPivot.x,
z: anchor.z - legPivot.z,
};
const tendonDir = {
x: anchor.x - tendonPivot.x,
z: anchor.z - tendonPivot.z,
};
const legAbsAngle = vecAngle(legDir) - vecAngle(coupling.legVec);
const legRelativeAngle = legAbsAngle - crankAngle;
const tendonAngle = vecAngle(tendonDir) - vecAngle(coupling.tendonVec);
return { legRelativeAngle, tendonAngle, anchor };
}
function set(name, value) {
if (robot.joints[name]) {
robot.setJointValue(name, value);
}
}
function frame(now) {
if (!running) {
return;
}
const t = (now - t0) * 0.001;
const w = 2.0 * Math.PI * hz;
const tau = 2.0 * Math.PI;
const nearCrank = (w * t) % tau;
const farCrank = (nearCrank + Math.PI) % tau;
// Main drives (continuous 360+ rotation).
set('near_crank_joint', nearCrank);
set('far_crank_joint', farCrank);
// Solve leg+tendon from closed-chain geometry for visual closure.
for (const coupling of tendonCouplings) {
const crankAngle = coupling.crankJoint === 'near_crank_joint' ? nearCrank : farCrank;
const solution = solveLegAndTendonAngles(coupling, crankAngle, coupling._lastAnchor);
coupling._lastAnchor = solution.anchor;
set(coupling.legJoint, solution.legRelativeAngle);
set(coupling.tendonJoint, solution.tendonAngle);
}
requestAnimationFrame(frame);
}
requestAnimationFrame(frame);
return () => {
running = false;
};
}