Measure probe-tip positioning accuracy in MuJoCo
Tested by BSVModel a small two-link arm, send its tip to target points and log how fast and how precisely it arrives — a first simulation check for a robot-assisted positioning idea.
- Input
- Target points in millimetres (in reach: 20–220 mm from the base), controller gain kp, tolerance in mm.目標点（ミリ単位。根元から20〜220 mmの範囲）、制御ゲイン kp、許容誤差（mm）。
- Output
- probe_reach.csv — one row per target: time to first reach the tolerance, time it settles inside it, final error, how far the tip went past the target, reached yes/no.probe_reach.csv（目標ごとに1行）：許容誤差に初めて入るまでの時間、その中に収まった時刻、最終誤差、先端が目標を行き過ぎた距離、到達の可否。
- Prerequisites
- Python 3.10 or later and
pip install mujoco numpy. No GPU and no robot hardware needed.Python 3.10以上とpip install mujoco numpy。GPUもロボット実機も不要です。 - Steps
-
- Save the code below as mujoco_probe_reach.py (it contains the arm model, closed-form inverse kinematics and the logger).
- Run
python mujoco_probe_reach.pyto use the four default targets. - Open probe_reach.csv and read time_to_tol_s, settled_at_s, final_err_mm and overshoot_mm side by side.
- Run again with a softer controller:
python mujoco_probe_reach.py "150,60;120,-80;180,0;90,120" 20, and compare the two files. - Replace the targets with points that matter for your idea and keep each run's CSV, named by its settings.
- 下のコードを mujoco_probe_reach.py として保存します（アームのモデル、逆運動学の解析解、記録処理が入っています）。
python mujoco_probe_reach.pyを実行すると、既定の4つの目標点で動きます。- probe_reach.csv を開き、time_to_tol_s・settled_at_s・final_err_mm・overshoot_mm を並べて見ます。
- 制御を弱めてもう一度実行します：
python mujoco_probe_reach.py "150,60;120,-80;180,0;90,120" 20。2つのファイルを比べてください。 - 目標点を、ご自身のアイデアで意味のある位置に置き換えます。実行ごとのCSVは、設定値がわかる名前で残しておきます。
- Expected result
- Every target is reached and the final error is close to zero. A lower gain arrives later. Because the arm moves in joint space, the tip path curves, so it can travel well past the target along the straight line before settling — exactly the kind of behaviour worth logging before anyone trusts a positioning idea.すべての目標に到達し、最終誤差はほぼゼロになります。ゲインを下げると到達は遅くなります。アームは関節空間で動くため先端の軌跡は曲がり、直線上で見ると目標をかなり行き過ぎてから収まることがあります。位置決めのアイデアを信用する前に、まさに記録しておきたい挙動です。
- Next step
- Swap in your own robot's MJCF/URDF model and add a third joint, then practise teleoperation in BSV's Robot Pilot Academy (simulation first).ご自身のロボットのMJCF／URDFモデルに差し替え、関節を1つ増やしてみてください。そのうえで、BSVのRobot Pilot Academyで遠隔操作を練習できます（まずはシミュレーションから）。 Open
Code (BSV original, MIT licence)
Download .py · mujoco_probe_reach.py
#!/usr/bin/env python3
"""BSV recipe: probe-tip positioning test for a 2-link arm in MuJoCo, with logged accuracy metrics.
Research / education / simulation only. Not a medical device, not clinical guidance.
Input : target points in mm (default four points in the arm's workspace), controller gain kp (default 60),
tolerance in mm (default 1.0).
Output: probe_reach.csv with, per target: time to first enter the tolerance, time it stays inside, final error, peak overshoot past
the target and whether the target was reached within 3 s of simulated time.
Original BSV code (model and controller), MIT. Library: MuJoCo (Apache-2.0).
"""
import csv, sys
import mujoco
import numpy as np
XML = """
<mujoco model="bsv_probe_arm">
<option timestep="0.002" gravity="0 0 0"/>
<worldbody>
<body name="link1" pos="0 0 0">
<joint name="j1" type="hinge" axis="0 0 1" damping="2.0"/>
<geom type="capsule" fromto="0 0 0 0.12 0 0" size="0.008" mass="0.15"/>
<body name="link2" pos="0.12 0 0">
<joint name="j2" type="hinge" axis="0 0 1" damping="1.2"/>
<geom type="capsule" fromto="0 0 0 0.10 0 0" size="0.006" mass="0.08"/>
<site name="tip" pos="0.10 0 0" size="0.003"/>
</body>
</body>
</worldbody>
<actuator>
<position name="a1" joint="j1" kp="KP"/>
<position name="a2" joint="j2" kp="KP"/>
</actuator>
</mujoco>
"""
L1, L2 = 0.12, 0.10
def ik(x, y):
"""Closed-form inverse kinematics of a planar 2-link arm (elbow-down)."""
c2 = (x * x + y * y - L1 * L1 - L2 * L2) / (2 * L1 * L2)
if abs(c2) > 1:
raise ValueError(f"target ({x:.3f}, {y:.3f}) is outside the workspace")
q2 = np.arccos(c2)
q1 = np.arctan2(y, x) - np.arctan2(L2 * np.sin(q2), L1 + L2 * np.cos(q2))
return q1, q2
def main(targets="150,60;120,-80;180,0;90,120", kp="60", tol_mm="1.0"):
model = mujoco.MjModel.from_xml_string(XML.replace("KP", str(float(kp))))
data = mujoco.MjData(model)
tip = model.site("tip").id
rows = []
for t in targets.split(";"):
tx, ty = (float(v) / 1000 for v in t.split(","))
q1, q2 = ik(tx, ty)
start = data.site_xpos[tip][:2].copy()
data.ctrl[:] = [q1, q2]
t_in, t_settle, peak_over = None, None, 0.0
direction = np.array([tx, ty]) - start
direction = direction / (np.linalg.norm(direction) or 1)
for step in range(int(3.0 / model.opt.timestep)):
mujoco.mj_step(model, data)
p = data.site_xpos[tip][:2]
err = np.linalg.norm(p - [tx, ty]) * 1000
peak_over = max(peak_over, float(np.dot(p - [tx, ty], direction)) * 1000)
now = (step + 1) * model.opt.timestep
if t_in is None and err <= float(tol_mm):
t_in = now
if err > float(tol_mm):
t_settle = None
elif t_settle is None:
t_settle = now
rows.append({"target_mm": t, "time_to_tol_s": round(t_in, 3) if t_in else "",
"settled_at_s": round(t_settle, 3) if t_settle else "", "final_err_mm": round(float(err), 3),
"overshoot_mm": round(peak_over, 3), "reached": t_in is not None})
with open("probe_reach.csv", "w", newline="") as f:
w = csv.DictWriter(f, fieldnames=list(rows[0])); w.writeheader(); w.writerows(rows)
for r in rows:
print(r)
if __name__ == "__main__":
main(*sys.argv[1:])
Output of BSV's own test run
Run on 2026-10-06 23:37 JST · Python 3.13.5, mujoco 3.15.0, numpy 2.5.3
{'target_mm': '150,60', 'time_to_tol_s': 0.122, 'settled_at_s': 0.122, 'final_err_mm': 0.0, 'overshoot_mm': 47.004, 'reached': True}
{'target_mm': '120,-80', 'time_to_tol_s': 0.16, 'settled_at_s': 0.16, 'final_err_mm': 0.0, 'overshoot_mm': 0.0, 'reached': True}
{'target_mm': '180,0', 'time_to_tol_s': 0.158, 'settled_at_s': 0.158, 'final_err_mm': 0.0, 'overshoot_mm': 0.0, 'reached': True}
{'target_mm': '90,120', 'time_to_tol_s': 0.152, 'settled_at_s': 0.152, 'final_err_mm': 0.0, 'overshoot_mm': 0.0, 'reached': True}