Files
cad-world/frontend/scripts/generate_position_mjcf.py
T
2026-09-22 10:36:28 +08:00

631 lines
22 KiB
Python
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
from __future__ import annotations
"""
把“用户上传的默认 MJCF(常见是 motor 力矩控制)”转换成一个:
- **默认打开更可能站住**(自动补齐/生成 `home` keyframe)
- **支持实时调 ctrl 影响机器人**(统一改成 `<position>` 伺服:ctrl=目标关节角)
你可以把它理解为:给 URDF→MJCF 的输出做一次“站立化(standable)+ 交互化(interactive)”后处理。
### 最终输出的“参数种类固定”
脚本只会产生/使用这些固定类型的参数:
- `<position ... kp="...">`:伺服刚度(可用 --kp-scale 统一缩放)
- `<position ... forcerange="lo hi">`:力矩上限(默认从 motor ctrlrange 迁移)
- `home` keyframe:`qpos="..."`、`ctrl="..."`
### 支持的 MJCF 写法
- 直接在 `<actuator>` 里写 `<motor name=".." joint=".."/>`(toddlerbot_2xc 风格)
- 用 class 继承写 motor(fourier_n1 风格):在 `<default class="motor_XXX">` 里定义 `<motor ctrlrange=".."/>`,
并在 `<actuator>` 里用 `<motor class="motor_XXX" .../>`
### 为什么一定要生成 keyframe
只把 motor 改成 position 不等于一定会站住:尤其是双足/人形,默认初始 qpos 往往不是“可站立姿态”。
用户上传模型也经常没有 keyframe,因此我们用 MuJoCo 载入转换后的模型,自动算一套启发式站姿,并写成 `home`。
### 仅提供 1~2 个数值参数给用户调
- `--pose-scale`:站姿启发式幅度缩放(默认 1.0;负号只用于翻转 humanoid 的左右 roll)
- `--kp-scale`:统一缩放所有 position kp(默认 1.0)
"""
import argparse
import re
from pathlib import Path
from typing import Iterable
def guess_kp(act_name: str) -> float:
"""
根据 actuator 名字猜一个 position kp 初值。
设计目标:
- 腿部(hip/knee/ankle_pitch)需要更大 kp 才能抗重力维持站姿
- 手臂(shoulder/elbow/wrist)kp 小一些,避免过硬导致抖动/自碰撞
- 颈/腰/偏航等保持中等
这个启发式是“toddlerbot_2xc 这类小型人形”的经验值,不保证对所有模型最优。
"""
n = act_name.lower()
# arms
if any(k in n for k in ("shoulder", "elbow", "wrist", "upper_arm", "lower_arm", "hand")):
return 4.0
# legs(小型人形默认 14;全尺寸人形请用 --kp-scale 提高,如 Fourier N1 约 15~16)
if any(k in n for k in ("hip", "thigh", "knee", "shank", "ankle", "calf")):
return 14.0
# neck, waist, yaw drives, ankle roll, etc.
return 10.0
def guess_kv(act_name: str, kp: float) -> float:
"""腿部 position 伺服增加少量 kv,减轻振荡、便于静态站立。"""
n = act_name.lower()
if any(k in n for k in ("hip", "thigh", "knee", "shank", "ankle", "calf")):
return min(6.0, max(1.5, 0.08 * float(kp)))
return 0.0
def _replace_default_motor_with_position(text: str, *, default_forcerange: str) -> str:
"""
在 `<default class="toddlerbot">` 块内,把第一个 `<motor .../>` 换成 `<position .../>`。
为什么只换第一个:
- toddlerbot_2xc 的默认里只有一个 `<motor ctrlrange="..."/>` 用作“默认 actuator”设置。
- 为了避免误伤其他 motor(例如 actuator block 里的 motor),我们限定在 default 块内且只替换一次。
"""
# 先定位 default class="toddlerbot" 的文本范围,再在其中替换一次 motor。
m = re.search(
r'(<default\s+class="toddlerbot"\b[^>]*>\s*)([\s\S]*?)(\s*</default>)',
text,
)
if not m:
# 找不到就原样返回;不强行报错,方便你先生成再手动检查。
return text
head, body, tail = m.group(1), m.group(2), m.group(3)
body2 = re.sub(
r'(\s*)<motor\b[^>]*/>\s*',
rf'\1<position inheritrange="1" forcerange="{default_forcerange}"/>\n',
body,
count=1,
)
return text[: m.start()] + head + body2 + tail + text[m.end() :]
_CTRL_RANGE_RE = re.compile(r'\bctrlrange\s*=\s*"([^"]+)"')
def _motor_ctrlrange_to_forcerange(attrs: str) -> tuple[str, str | None]:
"""
从 motor attrs 里提取 ctrlrange="lo hi" 并返回:
- 删除 ctrlrange 后的 attrs
- 提取出的 forcerange 值(字符串 "lo hi"),若不存在返回 None
设计动机:
- `<motor ctrlrange>` 的语义通常是“力矩输入范围”
- `<position>` 更需要 `forcerange` 作为“力矩上限”,因此把 motor 的 ctrlrange 迁移为 position 的 forcerange
"""
m = _CTRL_RANGE_RE.search(attrs)
if not m:
return attrs, None
fr = m.group(1).strip()
attrs2 = _CTRL_RANGE_RE.sub("", attrs)
return attrs2, fr
def _convert_default_motor_classes(text: str) -> str:
"""
支持 class 继承写法:把 `<default class="motor_XXX"> <motor .../> </default>`
中的 motor 换成 position。
规则:
- 若 motor 有 ctrlrange="lo hi",则改为 `<position inheritrange="1" forcerange="lo hi"/>`
- 否则改为 `<position inheritrange="1"/>`
"""
def repl_motor_line(m: re.Match[str]) -> str:
indent, attrs = m.group(1), m.group(2) or ""
attrs2, fr = _motor_ctrlrange_to_forcerange(attrs)
attrs2 = " ".join(attrs2.split())
extra = 'inheritrange="1"'
if fr:
extra += f' forcerange="{fr}"'
if attrs2:
extra = f"{attrs2} {extra}"
return f'{indent}<position {extra}/>\n'
def repl_default_block(bm: re.Match[str]) -> str:
head, body, tail = bm.group(1), bm.group(2), bm.group(3)
body2 = re.sub(
r'(^[ \t]*)<motor\b([^>]*)/>\s*$',
repl_motor_line,
body,
flags=re.MULTILINE,
)
return head + body2 + tail
# Restrict conversion to <default>...</default> blocks so we do NOT
# accidentally convert actuator motors before _convert_actuator_motors adds kp.
return re.sub(r'(<default\b[^>]*>\s*)([\s\S]*?)(\s*</default>)', repl_default_block, text)
def _convert_actuator_motors(text: str) -> str:
"""
把 `<actuator>` 里的 `<motor .../>` 换成 `<position .../>`,并补上 kp。
支持:`<motor class="..." name="..." joint="..."/>` 这类写法。
"""
def repl(m: re.Match[str]) -> str:
indent, attrs = m.group(1), m.group(2) or ""
# Extract name for kp guessing.
mname = re.search(r'\bname\s*=\s*"([^"]+)"', attrs)
name = mname.group(1) if mname else ""
kp_val = float(guess_kp(name))
# If kp already exists, don't override.
if re.search(r'\bkp\s*=\s*"', attrs):
kp_attr = ""
m_kp = re.search(r'\bkp\s*=\s*"([0-9eE+\-\.]+)"', attrs)
if m_kp:
kp_val = float(m_kp.group(1))
else:
kp_attr = f' kp="{kp_val:g}"'
kv = guess_kv(name, kp_val)
if re.search(r'\bkv\s*=\s*"', attrs):
kv_attr = ""
elif kv > 0.0:
kv_attr = f' kv="{kv:g}"'
else:
kv_attr = ""
# Convert motor ctrlrange->position forcerange if present on the actuator itself.
attrs2, fr = _motor_ctrlrange_to_forcerange(attrs)
extra = ""
if fr and not re.search(r'\bforcerange\s*=\s*"', attrs2):
extra += f' forcerange="{fr}"'
# Ensure inheritrange=1 unless user already set it.
if not re.search(r'\binheritrange\s*=\s*"', attrs2):
extra += ' inheritrange="1"'
# Clean up spacing.
attrs2 = " ".join(attrs2.split()).strip()
if attrs2:
attrs2 = " " + attrs2
return f"{indent}<position{attrs2}{kp_attr}{kv_attr}{extra}/>\n"
# Only convert within <actuator> ... </actuator>
def block_repl(bm: re.Match[str]) -> str:
head, body, tail = bm.group(1), bm.group(2), bm.group(3)
body2 = re.sub(r'(^[ \t]*)<motor\b([^>]*)/>\s*$', repl, body, flags=re.MULTILINE)
return head + body2 + tail
return re.sub(r'(<actuator\b[^>]*>\s*)([\s\S]*?)(\s*</actuator>)', block_repl, text, count=1)
_HOME_KEY_RE = re.compile(r'<key\b[^>]*\bname\s*=\s*"home"', re.IGNORECASE)
def _has_home_keyframe(text: str) -> bool:
return _HOME_KEY_RE.search(text) is not None
def _format_floats(xs: Iterable[float]) -> str:
return " ".join(f"{float(x):.6g}" for x in xs)
def _infer_pose_mode_from_joint_names(names: list[str]) -> str:
ns = [n.lower() for n in names]
if any(("thigh" in n) or ("calf" in n) for n in ns):
return "quadruped"
if any(("ankle" in n) or ("knee" in n) for n in ns):
return "humanoid"
return "quadruped"
def _apply_stand_pose_heuristic(model, data, *, mode: str, pose_scale: float) -> None:
import mujoco # type: ignore
mode_l = str(mode).lower().strip()
if mode_l not in ("quadruped", "humanoid"):
mode_l = "quadruped"
ps = float(pose_scale)
if ps == 0.0:
ps = 1.0
sabs = abs(ps)
roll_sign = 1.0 if ps > 0 else -1.0
for j in range(model.njnt):
jtype = int(model.jnt_type[j])
if jtype not in (mujoco.mjtJoint.mjJNT_SLIDE, mujoco.mjtJoint.mjJNT_HINGE):
continue
qadr = int(model.jnt_qposadr[j])
jname = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, j) or ""
n = jname.lower()
lo = float(model.jnt_range[j, 0])
hi = float(model.jnt_range[j, 1])
mid = 0.5 * (lo + hi) if hi > lo else float(data.qpos[qadr])
if mode_l == "quadruped":
if "calf" in n:
target = -1.8 * sabs
elif "thigh" in n:
target = 0.9 * sabs
elif "hip" in n:
target = 0.0
else:
target = mid
else:
# 略下蹲 + 稍宽站:双足在纯 PD 下更易在支撑面内稳住质心
if ("knee" in n or "shank" in n) and ("pitch" in n or "joint" in n):
target = 0.78 * sabs
elif "ankle" in n and "pitch" in n:
target = -0.4 * sabs
elif "hip" in n and "pitch" in n:
target = -0.45 * sabs
elif "hip" in n and "roll" in n:
r = 0.18 * sabs * roll_sign
target = r if "left_" in n else -r
elif "ankle" in n and "roll" in n:
r = 0.18 * sabs * roll_sign
target = -r if "left_" in n else r
elif "shoulder" in n:
# 不对称 range 的 shoulder_roll 若用 mid 会变成「大 T 字」,质心外移导致易倒
if "pitch" in n:
target = 0.15 * sabs
elif "roll" in n:
target = 0.0
elif "yaw" in n:
target = 0.0
else:
target = mid
elif "elbow" in n:
target = 0.5 * sabs
elif "wrist" in n:
target = 0.0
else:
target = mid
if hi > lo:
target = max(min(float(target), hi), lo)
data.qpos[qadr] = float(target)
def _place_freejoint_on_floor(model, data, *, clearance: float = 0.01) -> None:
import mujoco # type: ignore
free_j = None
for j in range(model.njnt):
if int(model.jnt_type[j]) == int(mujoco.mjtJoint.mjJNT_FREE):
free_j = j
break
if free_j is None:
return
qadr = int(model.jnt_qposadr[free_j])
mujoco.mj_forward(model, data)
contype = getattr(model, "geom_contype", None)
conaff = getattr(model, "geom_conaffinity", None)
if contype is None or conaff is None:
return
zmin = None
for g in range(model.ngeom):
if int(contype[g]) == 0 and int(conaff[g]) == 0:
continue
z = float(data.geom_xpos[g, 2])
zmin = z if zmin is None else min(zmin, z)
if zmin is None:
return
dz = (0.0 + float(clearance)) - float(zmin)
data.qpos[qadr + 2] = float(data.qpos[qadr + 2]) + float(dz)
mujoco.mj_forward(model, data)
def _init_ctrl_to_qpos(model, data) -> None:
import mujoco # type: ignore
if model.nu <= 0:
return
data.ctrl[:] = 0.0
for a in range(model.nu):
j = int(model.actuator_trnid[a, 0])
if j < 0:
continue
jtype = int(model.jnt_type[j])
if jtype not in (mujoco.mjtJoint.mjJNT_SLIDE, mujoco.mjtJoint.mjJNT_HINGE):
continue
qadr = int(model.jnt_qposadr[j])
data.ctrl[a] = float(data.qpos[qadr])
def _extract_home_qpos_from_src(text: str) -> list[float] | None:
"""
若源文件已有 `key name="home"`,尝试提取其中的 qpos 作为更靠谱的站姿种子。
设计动机:
- 一些模型(例如 toddlerbot_2xc、a1)作者已经给了稳定的 home 姿态
- 我们把 motor->position 后,仍然想尽量沿用这个姿态,只是把 ctrl 改成 position 语义(ctrl=qpos)
"""
m = re.search(
r'<key\b[^>]*\bname\s*=\s*"home"[^>]*\bqpos\s*=\s*"([^"]*)"',
text,
flags=re.IGNORECASE | re.DOTALL,
)
if not m:
return None
raw = m.group(1)
parts = raw.replace("\n", " ").split()
out: list[float] = []
for p in parts:
try:
out.append(float(p))
except ValueError:
return None
return out or None
def _generate_home_keyframe(
dst_xml: Path,
*,
pose_scale: float,
seed_qpos: list[float] | None = None,
) -> tuple[list[float], list[float]]:
import mujoco # type: ignore
model = mujoco.MjModel.from_xml_path(str(dst_xml))
# --- 方案 1:先试默认姿态(全 0),很多机器人全 0 就能站 ---
data0 = mujoco.MjData(model)
_place_freejoint_on_floor(model, data0, clearance=0.01)
_init_ctrl_to_qpos(model, data0)
mujoco.mj_forward(model, data0)
# 简单稳定性检查:重心 XY 在支撑面内
import numpy as np
com = np.zeros(3)
total_mass = 0.0
for i in range(model.nbody):
m = float(model.body_mass[i])
com += m * data0.xipos[i]
total_mass += m
if total_mass > 0:
com /= total_mass
foot_xy = []
zmin0 = None
for g in range(model.ngeom):
if int(model.geom_contype[g]) == 0 and int(model.geom_conaffinity[g]) == 0:
continue
z = float(data0.geom_xpos[g, 2])
if zmin0 is None:
zmin0 = z
if z < (zmin0 + 0.05):
zmin0 = min(zmin0, z)
foot_xy.append(data0.geom_xpos[g, :2].copy())
stable0 = True
if foot_xy:
fps = np.array(foot_xy)
xr = [fps[:, 0].min() - 0.03, fps[:, 0].max() + 0.03]
yr = [fps[:, 1].min() - 0.03, fps[:, 1].max() + 0.03]
stable0 = (xr[0] <= com[0] <= xr[1]) and (yr[0] <= com[1] <= yr[1])
if stable0:
qpos = [float(x) for x in data0.qpos]
ctrl = [float(x) for x in (data0.ctrl if model.nu > 0 else [])]
return qpos, ctrl
# --- 方案 2:弯膝启发式(仅当全 0 不稳定时) ---
data = mujoco.MjData(model)
if seed_qpos is not None and len(seed_qpos) == int(model.nq):
for i, v in enumerate(seed_qpos):
data.qpos[i] = float(v)
else:
joint_names = []
for j in range(model.njnt):
joint_names.append(mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, j) or "")
mode = _infer_pose_mode_from_joint_names(joint_names)
_apply_stand_pose_heuristic(model, data, mode=mode, pose_scale=float(pose_scale))
_place_freejoint_on_floor(model, data, clearance=0.01)
_init_ctrl_to_qpos(model, data)
mujoco.mj_forward(model, data)
qpos = [float(x) for x in data.qpos]
ctrl = [float(x) for x in (data.ctrl if model.nu > 0 else [])]
return qpos, ctrl
def _insert_home_keyframe(text: str, *, qpos: list[float], ctrl: list[float]) -> str:
key = f' <keyframe>\n <key name="home" qpos="{_format_floats(qpos)}"'
if ctrl:
key += f' ctrl="{_format_floats(ctrl)}"'
key += "/>\n </keyframe>\n"
m = re.search(r"\s*</mujoco>\s*$", text)
if not m:
return text + "\n" + key
return text[: m.start()] + "\n" + key + text[m.start() :]
def _cap_arm_actuator_kp(text: str, *, cap: float) -> str:
"""把手臂相关 position actuator 的 kp 钳到 cap 以下(按 name 关键字)。"""
arm_keys = ("shoulder", "elbow", "wrist", "upper_arm", "lower_arm", "hand")
def repl_line(line: str) -> str:
if "<position" not in line or "name=" not in line:
return line
m = re.search(r'\bname\s*=\s*"([^"]+)"', line)
if not m:
return line
n = m.group(1).lower()
if not any(k in n for k in arm_keys):
return line
return re.sub(r'kp\s*=\s*"([0-9eE+\-\.]+)"', f'kp="{cap:g}"', line)
return "\n".join(repl_line(ln) for ln in text.splitlines())
def _inject_joint_damping_in_joint_defaults(
text: str, *, damping: float, frictionloss: float
) -> str:
"""
在 `<default class="joint_...">` 里的 `<joint .../>` 上追加 damping / frictionloss。
用于双足等模型:纯 PD 站姿时关节粘性阻尼可耗散小幅摆动,减轻“慢慢歪倒”。
仅在用户显式传入 >0 时启用。
"""
if damping <= 0.0 and frictionloss <= 0.0:
return text
def patch_joint_block(block: str) -> str:
def fix_one(m: re.Match[str]) -> str:
tag = m.group(0)
if "damping=" in tag:
return tag
parts: list[str] = []
if damping > 0.0:
parts.append(f'damping="{damping:g}"')
if frictionloss > 0.0:
parts.append(f'frictionloss="{frictionloss:g}"')
if not parts:
return tag
return tag.replace("/>", " " + " ".join(parts) + "/>")
return re.sub(r"<joint\b[^/]*/>", fix_one, block)
return re.sub(
r'<default class="joint_[^"]+"[^>]*>[\s\S]*?</default>',
lambda m: patch_joint_block(m.group(0)),
text,
)
def _remove_home_keyframe_blocks(text: str) -> str:
"""
删除所有包含 `<key name="home" ...>` 的 `<keyframe>...</keyframe>` 块。
这样我们可以“重新生成 home”,避免重复名字导致 MuJoCo 报错。
"""
def block_has_home(block: str) -> bool:
return _HOME_KEY_RE.search(block) is not None
out: list[str] = []
last = 0
for m in re.finditer(r"<keyframe\b[^>]*>[\s\S]*?</keyframe>", text):
block = m.group(0)
if block_has_home(block):
out.append(text[last : m.start()])
last = m.end()
out.append(text[last:])
return "".join(out)
def main() -> int:
ap = argparse.ArgumentParser(
description="把 MJCF 的 <motor> actuator 自动改成 <position>(用于默认站立/交互调参)。",
)
# 用户要求:只要 2 个参数:转换前 xml、转换后 xml。
ap.add_argument("src", type=str, help="转换前 MJCF(motor 模式,例如 toddlerbot_2xc.xml)。")
ap.add_argument("dst", type=str, help="转换后 MJCF(position 模式,例如 toddlerbot_2xc_fic.xml)。")
ap.add_argument(
"--default-forcerange",
type=str,
default="-10 10",
help='写入到 `<default class="toddlerbot">` 的 `<position forcerange="...">`,建议先用 "-10 10"。',
)
ap.add_argument(
"--pose-scale",
type=float,
default=1.0,
help="自动生成 home 站姿的幅度缩放(绝对值控制幅度;负号仅用于翻转 humanoid 的左右 roll)。",
)
ap.add_argument(
"--kp-scale",
type=float,
default=1.0,
help="统一缩放 position kp(默认 1.0)。",
)
ap.add_argument(
"--arm-kp-max",
type=float,
default=None,
help="若指定:shoulder/elbow/wrist 等手臂关节的 kp 钳制到不超过该值(大 --kp-scale 时避免手臂过硬)。",
)
ap.add_argument(
"--joint-damping",
type=float,
default=0.0,
help='写入 joint 默认的 damping(例如 0.4~0.6),双足纯 PD 站姿时建议试 0.45;默认 0 不改。',
)
ap.add_argument(
"--joint-frictionloss",
type=float,
default=0.0,
help="写入 joint 默认的 frictionloss(可选,与 damping 配合;默认 0 不改。",
)
args = ap.parse_args()
src = Path(args.src).expanduser().resolve()
dst = Path(args.dst).expanduser().resolve()
text = src.read_text(encoding="utf-8")
text = _inject_joint_damping_in_joint_defaults(
text,
damping=float(args.joint_damping),
frictionloss=float(args.joint_frictionloss),
)
seed_qpos = _extract_home_qpos_from_src(text)
# (1) default:motor -> position(限定在 default class="toddlerbot" 块内)
text = _replace_default_motor_with_position(text, default_forcerange=str(args.default_forcerange))
# (1b) 支持 class 继承写法:把 default 里的 motor 换成 position(例如 motor_8029E)
text = _convert_default_motor_classes(text)
# (2) actuator:把 actuator block 里的 motor 换成 position(支持 class="..."),并补 kp
text = _convert_actuator_motors(text)
# (2b) 全局缩放 kp(只改 position actuator 上的 kp="...")
kp_scale = float(args.kp_scale)
if kp_scale != 1.0:
def kp_repl(m: re.Match[str]) -> str:
v = float(m.group(1))
return f'kp="{v * kp_scale:.6g}"'
text = re.sub(r'kp\s*=\s*"([0-9eE+\-\.]+)"', kp_repl, text)
arm_kp_max = getattr(args, "arm_kp_max", None)
if arm_kp_max is not None:
text = _cap_arm_actuator_kp(text, cap=float(arm_kp_max))
dst.parent.mkdir(parents=True, exist_ok=True)
dst.write_text(text, encoding="utf-8")
# (3) 生成/更新 home keyframe(有则替换,无则插入)
qpos, ctrl = _generate_home_keyframe(dst, pose_scale=float(args.pose_scale), seed_qpos=seed_qpos)
text2 = dst.read_text(encoding="utf-8")
text2 = _remove_home_keyframe_blocks(text2)
text2 = _insert_home_keyframe(text2, qpos=qpos, ctrl=ctrl)
dst.write_text(text2, encoding="utf-8")
return 0
if __name__ == "__main__":
raise SystemExit(main())