631 lines
22 KiB
Python
631 lines
22 KiB
Python
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())
|
||
|