@dirgha/code 1.43.4 → 2.0.0
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- package/CHANGELOG.md +16 -18
- package/LICENSE +202 -101
- package/NOTICE +9 -0
- package/README.md +182 -129
- package/THIRD_PARTY_NOTICES.md +28 -0
- package/bin/build-native.mjs +43 -0
- package/bin/dirgha.mjs +241 -0
- package/dist/cli/flows/full-cycle.d.ts +0 -3
- package/dist/cli/flows/full-cycle.js +1 -9
- package/dist/cli/flows/full-cycle.js.map +1 -1
- package/dist/cli/main.js +1 -1
- package/dist/cli/main.js.map +1 -1
- package/dist/cli/slash/index.js +0 -2
- package/dist/cli/slash/index.js.map +1 -1
- package/dist/cli/subcommands/index.js +0 -2
- package/dist/cli/subcommands/index.js.map +1 -1
- package/dist/integrations/entitlements.d.ts +1 -2
- package/dist/integrations/entitlements.js.map +1 -1
- package/dist/integrations/index.d.ts +0 -1
- package/dist/integrations/index.js +0 -1
- package/dist/integrations/index.js.map +1 -1
- package/dist/mcp/mcp-config.js +27 -3
- package/dist/mcp/mcp-config.js.map +1 -1
- package/dist/safety/policy.d.ts +18 -0
- package/dist/safety/policy.js +95 -1
- package/dist/safety/policy.js.map +1 -1
- package/dist/tools/index.d.ts +1 -2
- package/dist/tools/index.js +1 -8
- package/dist/tools/index.js.map +1 -1
- package/extensions/firefox-mv3/background.js +144 -0
- package/extensions/firefox-mv3/content-agent-dom.js +161 -0
- package/extensions/firefox-mv3/lokahi_gate.wasm +0 -0
- package/extensions/firefox-mv3/manifest.json +33 -0
- package/include/dirgha_kernel.h +41 -0
- package/package.json +63 -61
- package/plugins/dsh-plugin-dirgha/cordis.patch.yml +11 -0
- package/plugins/dsh-plugin-dirgha/index.ts +135 -0
- package/plugins/dsh-plugin-dirgha/package.json +18 -0
- package/plugins/robotics-physical-ai/README.md +11 -0
- package/plugins/robotics-physical-ai/mujoco_eval_sidecar.py +848 -0
- package/plugins/robotics-physical-ai/plugin.toml +23 -0
- package/plugins/robotics-skills-pack/README.md +15 -0
- package/plugins/robotics-skills-pack/plugin.toml +36 -0
- package/vendor/rtk/README.md +7 -0
- package/dist/cli/slash/goal.d.ts +0 -14
- package/dist/cli/slash/goal.js +0 -74
- package/dist/cli/slash/goal.js.map +0 -1
- package/dist/cli/slash/gpu.d.ts +0 -11
- package/dist/cli/slash/gpu.js +0 -58
- package/dist/cli/slash/gpu.js.map +0 -1
- package/dist/cli/subcommands/gpu.d.ts +0 -15
- package/dist/cli/subcommands/gpu.js +0 -187
- package/dist/cli/subcommands/gpu.js.map +0 -1
- package/dist/context/goal.d.ts +0 -27
- package/dist/context/goal.js +0 -77
- package/dist/context/goal.js.map +0 -1
- package/dist/gpu/market.d.ts +0 -30
- package/dist/gpu/market.js +0 -120
- package/dist/gpu/market.js.map +0 -1
- package/dist/integrations/bucky-client.d.ts +0 -105
- package/dist/integrations/bucky-client.js +0 -76
- package/dist/integrations/bucky-client.js.map +0 -1
- package/dist/providers/presets.d.ts +0 -30
- package/dist/providers/presets.js +0 -80
- package/dist/providers/presets.js.map +0 -1
- package/dist/tools/goal.d.ts +0 -11
- package/dist/tools/goal.js +0 -69
- package/dist/tools/goal.js.map +0 -1
- package/dist/tools/gpu-compute.d.ts +0 -19
- package/dist/tools/gpu-compute.js +0 -380
- package/dist/tools/gpu-compute.js.map +0 -1
- package/dist/tui/ink/components/GoalIndicator.d.ts +0 -7
- package/dist/tui/ink/components/GoalIndicator.js +0 -34
- package/dist/tui/ink/components/GoalIndicator.js.map +0 -1
- package/vendor/rtk/linux-x64/rtk +0 -0
|
@@ -0,0 +1,848 @@
|
|
|
1
|
+
#!/usr/bin/env python3
|
|
2
|
+
"""
|
|
3
|
+
`mujoco_eval_sidecar.py` — Full Native MuJoCo 3.17.0 Physics Engine & Interactive 3D/Kinematic Studio.
|
|
4
|
+
|
|
5
|
+
Capabilities:
|
|
6
|
+
1. Dynamic Model Loader: Load any arbitrary `.mjcf` / `.xml` / `.urdf` file on disk (`--mjcf-path`),
|
|
7
|
+
inline XML (`--mjcf-xml`), or built-in morphology presets (`dirgha_2dof_reacher`, `so101_6dof_arm`, `dex_arm_01_tendon`).
|
|
8
|
+
2. Model Inspector (`--mode inspect`): Extracts bodies, joints, actuators, tendons, sensors, DoFs, and timestep.
|
|
9
|
+
3. Stateful Physics Stepping (`--mode step` / `--mode reset`): Persists live `qpos`/`qvel` in `data/sim-session.json`
|
|
10
|
+
so coding agents can step real MuJoCo physics interactively via `dirgha_sim_step`.
|
|
11
|
+
4. Deterministic Batch Evaluation (`--mode eval`): Runs seeded rollouts, classifies per-episode failure modes
|
|
12
|
+
(`NONE`, `JOINT_VELOCITY_EXCEEDED`, `TORQUE_SATURATION`, `TARGET_MISSED`), and generates a self-contained
|
|
13
|
+
interactive HTML5 Canvas Kinematic & Telemetry Studio (`data/replay.html`).
|
|
14
|
+
"""
|
|
15
|
+
|
|
16
|
+
import argparse
|
|
17
|
+
import json
|
|
18
|
+
import math
|
|
19
|
+
import os
|
|
20
|
+
import sys
|
|
21
|
+
import mujoco
|
|
22
|
+
import numpy as np
|
|
23
|
+
|
|
24
|
+
BUILTIN_MODELS = {
|
|
25
|
+
"dirgha_2dof_reacher": """
|
|
26
|
+
<mujoco model="dirgha_2dof_reacher">
|
|
27
|
+
<compiler angle="radian" inertiafromgeom="true"/>
|
|
28
|
+
<option timestep="0.01" integrator="RK4"/>
|
|
29
|
+
<worldbody>
|
|
30
|
+
<body name="link1" pos="0 0 0">
|
|
31
|
+
<joint name="joint1" type="hinge" axis="0 0 1" limited="true" range="-3.14 3.14" damping="0.8"/>
|
|
32
|
+
<geom name="geom1" type="capsule" fromto="0 0 0 0.4 0 0" size="0.03" mass="1.0"/>
|
|
33
|
+
<body name="link2" pos="0.4 0 0">
|
|
34
|
+
<joint name="joint2" type="hinge" axis="0 0 1" limited="true" range="-2.8 2.8" damping="0.8"/>
|
|
35
|
+
<geom name="geom2" type="capsule" fromto="0 0 0 0.35 0 0" size="0.025" mass="0.8"/>
|
|
36
|
+
<site name="eef" pos="0.35 0 0" size="0.02"/>
|
|
37
|
+
</body>
|
|
38
|
+
</body>
|
|
39
|
+
<site name="target" pos="0.45 0.35 0" size="0.03"/>
|
|
40
|
+
</worldbody>
|
|
41
|
+
<actuator>
|
|
42
|
+
<motor name="motor1" joint="joint1" ctrlrange="-3.0 3.0" gear="1"/>
|
|
43
|
+
<motor name="motor2" joint="joint2" ctrlrange="-3.0 3.0" gear="1"/>
|
|
44
|
+
</actuator>
|
|
45
|
+
</mujoco>
|
|
46
|
+
""",
|
|
47
|
+
"so101_6dof_arm": """
|
|
48
|
+
<mujoco model="so101_6dof_arm">
|
|
49
|
+
<compiler angle="radian" inertiafromgeom="true"/>
|
|
50
|
+
<option timestep="0.005" integrator="implicitfast"/>
|
|
51
|
+
<default>
|
|
52
|
+
<joint armature="0.05" damping="1.2"/>
|
|
53
|
+
</default>
|
|
54
|
+
<worldbody>
|
|
55
|
+
<body name="base_yaw" pos="0 0 0">
|
|
56
|
+
<joint name="shoulder_pan" type="hinge" axis="0 0 1" limited="true" range="-3.14 3.14"/>
|
|
57
|
+
<geom type="capsule" fromto="0 0 0 0.22 0 0" size="0.032" mass="0.8"/>
|
|
58
|
+
<body name="upper_arm" pos="0.22 0 0">
|
|
59
|
+
<joint name="shoulder_lift" type="hinge" axis="0 0 1" limited="true" range="-2.6 2.6"/>
|
|
60
|
+
<geom type="capsule" fromto="0 0 0 0.20 0 0" size="0.028" mass="0.6"/>
|
|
61
|
+
<body name="forearm" pos="0.20 0 0">
|
|
62
|
+
<joint name="elbow_flex" type="hinge" axis="0 0 1" limited="true" range="-2.6 2.6"/>
|
|
63
|
+
<geom type="capsule" fromto="0 0 0 0.16 0 0" size="0.024" mass="0.5"/>
|
|
64
|
+
<body name="wrist_pitch_link" pos="0.16 0 0">
|
|
65
|
+
<joint name="wrist_flex" type="hinge" axis="0 1 0" limited="true" range="-2.5 2.5"/>
|
|
66
|
+
<geom type="capsule" fromto="0 0 0 0.08 0 0" size="0.020" mass="0.3"/>
|
|
67
|
+
<body name="wrist_roll_link" pos="0.08 0 0">
|
|
68
|
+
<joint name="wrist_roll" type="hinge" axis="1 0 0" limited="true" range="-3.14 3.14"/>
|
|
69
|
+
<geom type="capsule" fromto="0 0 0 0.05 0 0" size="0.018" mass="0.2"/>
|
|
70
|
+
<body name="gripper_link" pos="0.05 0 0">
|
|
71
|
+
<joint name="gripper_jaw" type="hinge" axis="0 0 1" limited="true" range="-1.0 1.0"/>
|
|
72
|
+
<geom type="capsule" fromto="0 0 0 0.04 0 0" size="0.015" mass="0.15"/>
|
|
73
|
+
<site name="eef" pos="0.04 0 0" size="0.02"/>
|
|
74
|
+
</body>
|
|
75
|
+
</body>
|
|
76
|
+
</body>
|
|
77
|
+
</body>
|
|
78
|
+
</body>
|
|
79
|
+
</body>
|
|
80
|
+
<site name="target" pos="0.45 0.35 0" size="0.03"/>
|
|
81
|
+
</worldbody>
|
|
82
|
+
<actuator>
|
|
83
|
+
<motor name="m_pan" joint="shoulder_pan" ctrlrange="-3.0 3.0"/>
|
|
84
|
+
<motor name="m_lift" joint="shoulder_lift" ctrlrange="-3.0 3.0"/>
|
|
85
|
+
<motor name="m_elbow" joint="elbow_flex" ctrlrange="-3.0 3.0"/>
|
|
86
|
+
<motor name="m_wflex" joint="wrist_flex" ctrlrange="-2.5 2.5"/>
|
|
87
|
+
<motor name="m_wroll" joint="wrist_roll" ctrlrange="-2.0 2.0"/>
|
|
88
|
+
<motor name="m_grip" joint="gripper_jaw" ctrlrange="-2.0 2.0"/>
|
|
89
|
+
</actuator>
|
|
90
|
+
</mujoco>
|
|
91
|
+
""",
|
|
92
|
+
"dex_arm_01_tendon": """
|
|
93
|
+
<mujoco model="dex_arm_01_tendon">
|
|
94
|
+
<compiler angle="radian" inertiafromgeom="true"/>
|
|
95
|
+
<option timestep="0.01" integrator="RK4"/>
|
|
96
|
+
<worldbody>
|
|
97
|
+
<body name="proximal" pos="0 0 0">
|
|
98
|
+
<joint name="mcp_flex" type="hinge" axis="0 0 1" limited="true" range="-3.14 3.14" damping="0.8"/>
|
|
99
|
+
< site name="s_base" pos="0 0.02 0"/>
|
|
100
|
+
<geom type="capsule" fromto="0 0 0 0.40 0 0" size="0.025" mass="0.8"/>
|
|
101
|
+
<body name="distal" pos="0.40 0 0">
|
|
102
|
+
<joint name="pip_flex" type="hinge" axis="0 0 1" limited="true" range="-2.8 2.8" damping="0.8"/>
|
|
103
|
+
<site name="s_tip" pos="0.15 0.02 0"/>
|
|
104
|
+
<geom type="capsule" fromto="0 0 0 0.35 0 0" size="0.02" mass="0.6"/>
|
|
105
|
+
<site name="eef" pos="0.35 0 0" size="0.02"/>
|
|
106
|
+
</body>
|
|
107
|
+
</body>
|
|
108
|
+
<site name="target" pos="0.45 0.35 0" size="0.03"/>
|
|
109
|
+
</worldbody>
|
|
110
|
+
<tendon>
|
|
111
|
+
<spatial name="flexor_tendon" width="0.004">
|
|
112
|
+
<site site="s_base"/>
|
|
113
|
+
<site site="s_tip"/>
|
|
114
|
+
</spatial>
|
|
115
|
+
</tendon>
|
|
116
|
+
<actuator>
|
|
117
|
+
<motor name="motor_mcp" joint="mcp_flex" ctrlrange="-3.0 3.0"/>
|
|
118
|
+
<motor name="motor_pip" joint="pip_flex" ctrlrange="-3.0 3.0"/>
|
|
119
|
+
</actuator>
|
|
120
|
+
</mujoco>
|
|
121
|
+
""",
|
|
122
|
+
}
|
|
123
|
+
|
|
124
|
+
|
|
125
|
+
def load_mujoco_model(robot_id: str, mjcf_path: str = "", mjcf_xml: str = ""):
|
|
126
|
+
if mjcf_xml.strip():
|
|
127
|
+
return mujoco.MjModel.from_xml_string(mjcf_xml), robot_id or "custom_inline_mjcf"
|
|
128
|
+
if mjcf_path.strip() and os.path.exists(mjcf_path):
|
|
129
|
+
return mujoco.MjModel.from_xml_path(mjcf_path), os.path.basename(mjcf_path)
|
|
130
|
+
xml = BUILTIN_MODELS.get(robot_id, BUILTIN_MODELS["dirgha_2dof_reacher"])
|
|
131
|
+
resolved_id = robot_id if robot_id in BUILTIN_MODELS else "dirgha_2dof_reacher"
|
|
132
|
+
return mujoco.MjModel.from_xml_string(xml), resolved_id
|
|
133
|
+
|
|
134
|
+
|
|
135
|
+
def wilson_ci_95(successes: int, total: int):
|
|
136
|
+
if total == 0:
|
|
137
|
+
return (0.0, 0.0)
|
|
138
|
+
z = 1.96
|
|
139
|
+
phat = successes / total
|
|
140
|
+
denom = 1.0 + (z * z) / total
|
|
141
|
+
center = (phat + (z * z) / (2.0 * total)) / denom
|
|
142
|
+
half = (z * math.sqrt((phat * (1.0 - phat) + (z * z) / (4.0 * total)) / total)) / denom
|
|
143
|
+
return (round(max(0.0, center - half), 4), round(min(1.0, center + half), 4))
|
|
144
|
+
|
|
145
|
+
|
|
146
|
+
def solve_2link_ik(x: float, y: float, l1: float = 0.4, l2: float = 0.35):
|
|
147
|
+
r2 = x * x + y * y
|
|
148
|
+
cos_q2 = max(-1.0, min(1.0, (r2 - l1 * l1 - l2 * l2) / (2.0 * l1 * l2)))
|
|
149
|
+
q2 = math.acos(cos_q2)
|
|
150
|
+
q1 = math.atan2(y, x) - math.atan2(l2 * math.sin(q2), l1 + l2 * cos_q2)
|
|
151
|
+
return np.array([q1, q2])
|
|
152
|
+
|
|
153
|
+
|
|
154
|
+
def inspect_model(robot_id: str, mjcf_path: str = "", mjcf_xml: str = ""):
|
|
155
|
+
model, resolved_id = load_mujoco_model(robot_id, mjcf_path, mjcf_xml)
|
|
156
|
+
joint_names = [
|
|
157
|
+
mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, i) or f"joint_{i}"
|
|
158
|
+
for i in range(model.njnt)
|
|
159
|
+
]
|
|
160
|
+
body_names = [
|
|
161
|
+
mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, i) or f"body_{i}"
|
|
162
|
+
for i in range(model.nbody)
|
|
163
|
+
]
|
|
164
|
+
actuator_names = [
|
|
165
|
+
mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_ACTUATOR, i) or f"act_{i}"
|
|
166
|
+
for i in range(model.nu)
|
|
167
|
+
]
|
|
168
|
+
return {
|
|
169
|
+
"ok": True,
|
|
170
|
+
"mujoco_version": mujoco.__version__,
|
|
171
|
+
"robot_id": resolved_id,
|
|
172
|
+
"nq": int(model.nq),
|
|
173
|
+
"nv": int(model.nv),
|
|
174
|
+
"nu": int(model.nu),
|
|
175
|
+
"nbody": int(model.nbody),
|
|
176
|
+
"njnt": int(model.njnt),
|
|
177
|
+
"ntendon": int(model.ntendon),
|
|
178
|
+
"nsensor": int(model.nsensor),
|
|
179
|
+
"timestep": round(float(model.opt.timestep), 5),
|
|
180
|
+
"joint_names": joint_names,
|
|
181
|
+
"body_names": body_names,
|
|
182
|
+
"actuator_names": actuator_names,
|
|
183
|
+
"available_builtin_models": list(BUILTIN_MODELS.keys()),
|
|
184
|
+
}
|
|
185
|
+
|
|
186
|
+
|
|
187
|
+
def step_stateful_session(
|
|
188
|
+
session_id: str,
|
|
189
|
+
robot_id: str,
|
|
190
|
+
action_values: list,
|
|
191
|
+
reset: bool = False,
|
|
192
|
+
seed: int = 1000,
|
|
193
|
+
):
|
|
194
|
+
os.makedirs("data", exist_ok=True)
|
|
195
|
+
session_file = os.path.abspath("data/sim-session.json")
|
|
196
|
+
model, resolved_id = load_mujoco_model(robot_id)
|
|
197
|
+
data = mujoco.MjData(model)
|
|
198
|
+
|
|
199
|
+
sessions = {}
|
|
200
|
+
if os.path.exists(session_file):
|
|
201
|
+
try:
|
|
202
|
+
with open(session_file, "r", encoding="utf-8") as f:
|
|
203
|
+
sessions = json.load(f)
|
|
204
|
+
except Exception:
|
|
205
|
+
sessions = {}
|
|
206
|
+
|
|
207
|
+
if reset or session_id not in sessions or sessions[session_id].get("robot_id") != resolved_id:
|
|
208
|
+
rng = np.random.default_rng(seed)
|
|
209
|
+
mujoco.mj_resetData(model, data)
|
|
210
|
+
for i in range(min(2, model.nq)):
|
|
211
|
+
data.qpos[i] = float(rng.uniform(-0.25, 0.25))
|
|
212
|
+
mujoco.mj_forward(model, data)
|
|
213
|
+
step_idx = 0
|
|
214
|
+
else:
|
|
215
|
+
saved = sessions[session_id]
|
|
216
|
+
for i, v in enumerate(saved.get("qpos", [])):
|
|
217
|
+
if i < model.nq:
|
|
218
|
+
data.qpos[i] = float(v)
|
|
219
|
+
for i, v in enumerate(saved.get("qvel", [])):
|
|
220
|
+
if i < model.nv:
|
|
221
|
+
data.qvel[i] = float(v)
|
|
222
|
+
mujoco.mj_forward(model, data)
|
|
223
|
+
step_idx = int(saved.get("step_idx", 0))
|
|
224
|
+
|
|
225
|
+
if not reset:
|
|
226
|
+
for i in range(min(len(action_values), model.nu)):
|
|
227
|
+
data.ctrl[i] = float(action_values[i])
|
|
228
|
+
mujoco.mj_step(model, data)
|
|
229
|
+
step_idx += 1
|
|
230
|
+
|
|
231
|
+
eef_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "eef")
|
|
232
|
+
eef_xyz = [round(float(x), 4) for x in data.site_xpos[eef_id]] if eef_id >= 0 else [0.0, 0.0, 0.0]
|
|
233
|
+
qpos_list = [round(float(x), 4) for x in data.qpos[: model.nq]]
|
|
234
|
+
qvel_list = [round(float(x), 4) for x in data.qvel[: model.nv]]
|
|
235
|
+
peak_vel = round(float(np.max(np.abs(data.qvel[: model.nv]))), 4) if model.nv > 0 else 0.0
|
|
236
|
+
|
|
237
|
+
sessions[session_id] = {
|
|
238
|
+
"session_id": session_id,
|
|
239
|
+
"robot_id": resolved_id,
|
|
240
|
+
"step_idx": step_idx,
|
|
241
|
+
"qpos": qpos_list,
|
|
242
|
+
"qvel": qvel_list,
|
|
243
|
+
"eef_xyz": eef_xyz,
|
|
244
|
+
"peak_joint_velocity_rad_s": peak_vel,
|
|
245
|
+
}
|
|
246
|
+
with open(session_file, "w", encoding="utf-8") as f:
|
|
247
|
+
json.dump(sessions, f, indent=2)
|
|
248
|
+
|
|
249
|
+
return {
|
|
250
|
+
"ok": True,
|
|
251
|
+
"session_id": session_id,
|
|
252
|
+
"robot_id": resolved_id,
|
|
253
|
+
"step_idx": step_idx,
|
|
254
|
+
"qpos": qpos_list,
|
|
255
|
+
"qvel": qvel_list,
|
|
256
|
+
"eef_xyz": eef_xyz,
|
|
257
|
+
"peak_joint_velocity_rad_s": peak_vel,
|
|
258
|
+
}
|
|
259
|
+
|
|
260
|
+
|
|
261
|
+
def write_interactive_canvas_replay_html(
|
|
262
|
+
replay_html_path: str,
|
|
263
|
+
resolved_id: str,
|
|
264
|
+
episodes: int,
|
|
265
|
+
successes: int,
|
|
266
|
+
ci_low: float,
|
|
267
|
+
ci_high: float,
|
|
268
|
+
max_vel_observed: float,
|
|
269
|
+
safety_limit_rad_s: float,
|
|
270
|
+
episode_records: list,
|
|
271
|
+
):
|
|
272
|
+
payload_json = json.dumps(
|
|
273
|
+
{
|
|
274
|
+
"robot_id": resolved_id,
|
|
275
|
+
"mujoco_version": mujoco.__version__,
|
|
276
|
+
"safety_limit_rad_s": safety_limit_rad_s,
|
|
277
|
+
"episodes": episode_records,
|
|
278
|
+
}
|
|
279
|
+
)
|
|
280
|
+
rows_html = "".join(
|
|
281
|
+
f"<tr><td>#{r['episode']}</td><td>{r['seed']}</td>"
|
|
282
|
+
f"<td><span class='badge {'pass' if r['succeeded'] else 'fail'}'>{'PASS' if r['succeeded'] else 'FAIL'}</span></td>"
|
|
283
|
+
f"<td>{r['failure_mode']}</td><td>{r['peak_velocity_rad_s']:.3f} rad/s</td><td>{r['final_distance_m']:.4f} m</td></tr>"
|
|
284
|
+
for r in episode_records
|
|
285
|
+
)
|
|
286
|
+
|
|
287
|
+
html = f"""<!DOCTYPE html>
|
|
288
|
+
<html lang="en">
|
|
289
|
+
<head>
|
|
290
|
+
<meta charset="UTF-8">
|
|
291
|
+
<title>Dirgha Harness 2.0 — MuJoCo {mujoco.__version__} Interactive Kinematic & Telemetry Replay Studio</title>
|
|
292
|
+
<style>
|
|
293
|
+
*, *::before, *::after {{ box-sizing: border-box; }}
|
|
294
|
+
body {{ font-family: system-ui, -apple-system, sans-serif; margin: 0; padding: 16px; background: #0f172a; color: #f8fafc; }}
|
|
295
|
+
.header {{ display: flex; justify-content: space-between; align-items: center; flex-wrap: wrap; gap: 10px; border-bottom: 1px solid #334155; padding-bottom: 12px; margin-bottom: 14px; }}
|
|
296
|
+
h2 {{ margin: 0; font-size: 18px; }}
|
|
297
|
+
.kpi-row {{ display: flex; flex-wrap: wrap; gap: 10px; margin-bottom: 14px; }}
|
|
298
|
+
.kpi {{ background: #1e293b; border: 1px solid #334155; border-radius: 8px; padding: 10px 14px; font-size: 12.5px; }}
|
|
299
|
+
.kpi strong {{ display: block; font-size: 16px; font-family: ui-monospace, monospace; color: #38bdf8; }}
|
|
300
|
+
.studio-grid {{ display: grid; grid-template-columns: repeat(auto-fit, minmax(310px, 1fr)); gap: 14px; margin-bottom: 14px; }}
|
|
301
|
+
.panel {{ background: #1e293b; border: 1px solid #334155; border-radius: 8px; padding: 12px; display: flex; flex-direction: column; gap: 8px; }}
|
|
302
|
+
canvas {{ width: 100%; height: 240px; background: #090d16; border-radius: 6px; border: 1px solid #334155; display: block; }}
|
|
303
|
+
.controls {{ display: flex; align-items: center; gap: 10px; flex-wrap: wrap; background: #1e293b; border: 1px solid #334155; padding: 10px 12px; border-radius: 8px; margin-bottom: 14px; }}
|
|
304
|
+
button {{ background: #2563eb; color: #fff; border: none; border-radius: 6px; padding: 6px 12px; cursor: pointer; font-weight: 600; font-size: 12px; }}
|
|
305
|
+
button.ep-btn {{ background: #334155; }}
|
|
306
|
+
button.ep-btn.active {{ background: #0ea5e9; color: #090d16; }}
|
|
307
|
+
input[type=range] {{ flex: 1; min-width: 140px; }}
|
|
308
|
+
table {{ width: 100%; border-collapse: collapse; font-size: 12.5px; background: #1e293b; border-radius: 8px; overflow: hidden; }}
|
|
309
|
+
th, td {{ padding: 8px 10px; border: 1px solid #334155; text-align: left; font-family: ui-monospace, monospace; }}
|
|
310
|
+
th {{ background: #0f172a; color: #94a3b8; }}
|
|
311
|
+
.badge {{ padding: 2px 7px; border-radius: 4px; font-size: 11px; font-weight: 700; }}
|
|
312
|
+
.badge.pass {{ background: #065f46; color: #6ee7b7; }}
|
|
313
|
+
.badge.fail {{ background: #7f1d1d; color: #fca5a5; }}
|
|
314
|
+
</style>
|
|
315
|
+
</head>
|
|
316
|
+
<body>
|
|
317
|
+
<div class="header">
|
|
318
|
+
<h2>Dirgha 2.0 — MuJoCo {mujoco.__version__} Interactive Kinematic & Telemetry Studio ({resolved_id})</h2>
|
|
319
|
+
<span style="font-family:monospace;font-size:12px;color:#94a3b8;">Lokahi Rule 17 Limit: {safety_limit_rad_s:.2f} rad/s</span>
|
|
320
|
+
</div>
|
|
321
|
+
|
|
322
|
+
<div class="kpi-row">
|
|
323
|
+
<div class="kpi">Robot Model<strong>{resolved_id}</strong></div>
|
|
324
|
+
<div class="kpi">Success Rate<strong>{successes}/{episodes} ({successes/max(1,episodes)*100:.1f}%)</strong></div>
|
|
325
|
+
<div class="kpi">Wilson 95% CI<strong>[{ci_low}, {ci_high}]</strong></div>
|
|
326
|
+
<div class="kpi">Peak Joint Velocity<strong>{max_vel_observed:.3f} rad/s</strong></div>
|
|
327
|
+
</div>
|
|
328
|
+
|
|
329
|
+
<div class="controls">
|
|
330
|
+
<button type="button" id="play-btn">Pause</button>
|
|
331
|
+
<label for="frame-slider" style="font-size:12px;font-family:monospace;" id="step-label">Step: 0</label>
|
|
332
|
+
<input type="range" id="frame-slider" min="0" max="20" value="0" />
|
|
333
|
+
<div id="ep-buttons" style="display:flex;gap:6px;flex-wrap:wrap;"></div>
|
|
334
|
+
</div>
|
|
335
|
+
|
|
336
|
+
<div class="studio-grid">
|
|
337
|
+
<div class="panel">
|
|
338
|
+
<div style="font-size:12.5px;font-weight:600;color:#cbd5e1;">2D/3D Kinematic Arm Projection & End-Effector Trail</div>
|
|
339
|
+
<canvas id="arm-canvas" width="420" height="240"></canvas>
|
|
340
|
+
</div>
|
|
341
|
+
<div class="panel">
|
|
342
|
+
<div style="font-size:12.5px;font-weight:600;color:#cbd5e1;">Joint Velocity Telemetry vs. Lokahi Rule 17 Safety Gate</div>
|
|
343
|
+
<canvas id="vel-canvas" width="420" height="240"></canvas>
|
|
344
|
+
</div>
|
|
345
|
+
</div>
|
|
346
|
+
|
|
347
|
+
<table>
|
|
348
|
+
<thead>
|
|
349
|
+
<tr><th>Episode</th><th>Seed</th><th>Verdict</th><th>Failure Mode</th><th>Peak Joint Vel</th><th>Final EEF Error</th></tr>
|
|
350
|
+
</thead>
|
|
351
|
+
<tbody>{rows_html}</tbody>
|
|
352
|
+
</table>
|
|
353
|
+
|
|
354
|
+
<script>
|
|
355
|
+
const DATA = {payload_json};
|
|
356
|
+
let currentEp = 0;
|
|
357
|
+
let currentFrame = 0;
|
|
358
|
+
let playing = true;
|
|
359
|
+
|
|
360
|
+
const armCanvas = document.getElementById('arm-canvas');
|
|
361
|
+
const armCtx = armCanvas.getContext('2d');
|
|
362
|
+
const velCanvas = document.getElementById('vel-canvas');
|
|
363
|
+
const velCtx = velCanvas.getContext('2d');
|
|
364
|
+
const slider = document.getElementById('frame-slider');
|
|
365
|
+
const stepLabel = document.getElementById('step-label');
|
|
366
|
+
const playBtn = document.getElementById('play-btn');
|
|
367
|
+
const epWrap = document.getElementById('ep-buttons');
|
|
368
|
+
|
|
369
|
+
DATA.episodes.forEach((ep, idx) => {{
|
|
370
|
+
const b = document.createElement('button');
|
|
371
|
+
b.type = 'button';
|
|
372
|
+
b.className = 'ep-btn' + (idx === 0 ? ' active' : '');
|
|
373
|
+
b.textContent = 'Ep #' + ep.episode;
|
|
374
|
+
b.onclick = () => {{
|
|
375
|
+
currentEp = idx;
|
|
376
|
+
currentFrame = 0;
|
|
377
|
+
document.querySelectorAll('.ep-btn').forEach((el, i) => el.classList.toggle('active', i === idx));
|
|
378
|
+
slider.max = Math.max(0, DATA.episodes[currentEp].frames.length - 1);
|
|
379
|
+
draw();
|
|
380
|
+
}};
|
|
381
|
+
epWrap.appendChild(b);
|
|
382
|
+
}});
|
|
383
|
+
|
|
384
|
+
slider.max = Math.max(0, DATA.episodes[0].frames.length - 1);
|
|
385
|
+
slider.oninput = (e) => {{
|
|
386
|
+
playing = false;
|
|
387
|
+
playBtn.textContent = 'Play';
|
|
388
|
+
currentFrame = parseInt(e.target.value, 10) || 0;
|
|
389
|
+
draw();
|
|
390
|
+
}};
|
|
391
|
+
playBtn.onclick = () => {{
|
|
392
|
+
playing = !playing;
|
|
393
|
+
playBtn.textContent = playing ? 'Pause' : 'Play';
|
|
394
|
+
}};
|
|
395
|
+
|
|
396
|
+
function draw() {{
|
|
397
|
+
const ep = DATA.episodes[currentEp];
|
|
398
|
+
const frames = ep.frames;
|
|
399
|
+
if (!frames.length) return;
|
|
400
|
+
const f = frames[Math.min(currentFrame, frames.length - 1)];
|
|
401
|
+
slider.value = currentFrame;
|
|
402
|
+
stepLabel.textContent = 'Step: ' + f.step + ' | q1=' + f.q1.toFixed(2) + ', q2=' + f.q2.toFixed(2);
|
|
403
|
+
|
|
404
|
+
// Draw Kinematic Arm & Trail
|
|
405
|
+
armCtx.clearRect(0, 0, armCanvas.width, armCanvas.height);
|
|
406
|
+
const cx = 150, cy = 175, scale = 190;
|
|
407
|
+
// Target zone (0.45, 0.35)
|
|
408
|
+
const tx = cx + 0.45 * scale, ty = cy - 0.35 * scale;
|
|
409
|
+
armCtx.strokeStyle = '#10b981';
|
|
410
|
+
armCtx.fillStyle = 'rgba(16, 185, 129, 0.2)';
|
|
411
|
+
armCtx.beginPath();
|
|
412
|
+
armCtx.arc(tx, ty, 14, 0, Math.PI * 2);
|
|
413
|
+
armCtx.fill();
|
|
414
|
+
armCtx.stroke();
|
|
415
|
+
|
|
416
|
+
// End-effector trail
|
|
417
|
+
armCtx.strokeStyle = 'rgba(56, 189, 248, 0.45)';
|
|
418
|
+
armCtx.lineWidth = 2;
|
|
419
|
+
armCtx.beginPath();
|
|
420
|
+
frames.slice(0, currentFrame + 1).forEach((fr, i) => {{
|
|
421
|
+
const ex = cx + fr.eef_x * scale;
|
|
422
|
+
const ey = cy - fr.eef_y * scale;
|
|
423
|
+
if (i === 0) armCtx.moveTo(ex, ey);
|
|
424
|
+
else armCtx.lineTo(ex, ey);
|
|
425
|
+
}});
|
|
426
|
+
armCtx.stroke();
|
|
427
|
+
|
|
428
|
+
// Two-link projection
|
|
429
|
+
const x1 = cx + 0.40 * Math.cos(f.q1) * scale;
|
|
430
|
+
const y1 = cy - 0.40 * Math.sin(f.q1) * scale;
|
|
431
|
+
const x2 = cx + f.eef_x * scale;
|
|
432
|
+
const y2 = cy - f.eef_y * scale;
|
|
433
|
+
|
|
434
|
+
armCtx.strokeStyle = '#f8fafc';
|
|
435
|
+
armCtx.lineWidth = 5;
|
|
436
|
+
armCtx.beginPath();
|
|
437
|
+
armCtx.moveTo(cx, cy);
|
|
438
|
+
armCtx.lineTo(x1, y1);
|
|
439
|
+
armCtx.lineTo(x2, y2);
|
|
440
|
+
armCtx.stroke();
|
|
441
|
+
|
|
442
|
+
armCtx.fillStyle = '#38bdf8';
|
|
443
|
+
[[cx, cy], [x1, y1], [x2, y2]].forEach(([px, py]) => {{
|
|
444
|
+
armCtx.beginPath();
|
|
445
|
+
armCtx.arc(px, py, 5, 0, Math.PI * 2);
|
|
446
|
+
armCtx.fill();
|
|
447
|
+
}});
|
|
448
|
+
|
|
449
|
+
// Draw Velocity Telemetry Curve + Safety Limit
|
|
450
|
+
velCtx.clearRect(0, 0, velCanvas.width, velCanvas.height);
|
|
451
|
+
const maxPlot = Math.max(DATA.safety_limit_rad_s * 1.25, ep.peak_velocity_rad_s * 1.1, 4.5);
|
|
452
|
+
const limY = velCanvas.height - (DATA.safety_limit_rad_s / maxPlot) * (velCanvas.height - 30) - 15;
|
|
453
|
+
velCtx.strokeStyle = '#ef4444';
|
|
454
|
+
velCtx.setLineDash([5, 5]);
|
|
455
|
+
velCtx.beginPath();
|
|
456
|
+
velCtx.moveTo(0, limY);
|
|
457
|
+
velCtx.lineTo(velCanvas.width, limY);
|
|
458
|
+
velCtx.stroke();
|
|
459
|
+
velCtx.setLineDash([]);
|
|
460
|
+
|
|
461
|
+
velCtx.strokeStyle = '#38bdf8';
|
|
462
|
+
velCtx.lineWidth = 2.5;
|
|
463
|
+
velCtx.beginPath();
|
|
464
|
+
frames.forEach((fr, idx) => {{
|
|
465
|
+
const px = 20 + (idx / Math.max(1, frames.length - 1)) * (velCanvas.width - 40);
|
|
466
|
+
const py = velCanvas.height - ((fr.vel_mag || 0) / maxPlot) * (velCanvas.height - 30) - 15;
|
|
467
|
+
if (idx === 0) velCtx.moveTo(px, py);
|
|
468
|
+
else velCtx.lineTo(px, py);
|
|
469
|
+
if (idx === currentFrame) {{
|
|
470
|
+
velCtx.fillStyle = '#facc15';
|
|
471
|
+
velCtx.fillRect(px - 3, py - 3, 6, 6);
|
|
472
|
+
}}
|
|
473
|
+
}});
|
|
474
|
+
velCtx.stroke();
|
|
475
|
+
}}
|
|
476
|
+
|
|
477
|
+
setInterval(() => {{
|
|
478
|
+
if (!playing) return;
|
|
479
|
+
const len = DATA.episodes[currentEp].frames.length;
|
|
480
|
+
if (len > 0) {{
|
|
481
|
+
currentFrame = (currentFrame + 1) % len;
|
|
482
|
+
draw();
|
|
483
|
+
}}
|
|
484
|
+
}}, 90);
|
|
485
|
+
draw();
|
|
486
|
+
</script>
|
|
487
|
+
</body>
|
|
488
|
+
</html>"""
|
|
489
|
+
with open(replay_html_path, "w", encoding="utf-8") as f:
|
|
490
|
+
f.write(html)
|
|
491
|
+
|
|
492
|
+
|
|
493
|
+
def run_suite(
|
|
494
|
+
robot_id: str = "dirgha_2dof_reacher",
|
|
495
|
+
episodes: int = 5,
|
|
496
|
+
steps_per_episode: int = 200,
|
|
497
|
+
safety_limit_rad_s: float = 4.0,
|
|
498
|
+
mjcf_path: str = "",
|
|
499
|
+
mjcf_xml: str = "",
|
|
500
|
+
):
|
|
501
|
+
model, resolved_id = load_mujoco_model(robot_id, mjcf_path, mjcf_xml)
|
|
502
|
+
data = mujoco.MjData(model)
|
|
503
|
+
eef_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "eef")
|
|
504
|
+
target_pos = np.array([0.45, 0.35, 0.0])
|
|
505
|
+
|
|
506
|
+
q_goal_2d = solve_2link_ik(0.45, 0.35)
|
|
507
|
+
kp = 6.0
|
|
508
|
+
kd = 2.2
|
|
509
|
+
|
|
510
|
+
episode_records = []
|
|
511
|
+
failed_episodes = []
|
|
512
|
+
action_trace_lines = []
|
|
513
|
+
successes = 0
|
|
514
|
+
max_vel_observed = 0.0
|
|
515
|
+
|
|
516
|
+
for ep in range(episodes):
|
|
517
|
+
seed = 1000 + ep
|
|
518
|
+
rng = np.random.default_rng(seed)
|
|
519
|
+
mujoco.mj_resetData(model, data)
|
|
520
|
+
data.qpos[0] = float(rng.uniform(-0.4, 0.4))
|
|
521
|
+
if model.nq > 1:
|
|
522
|
+
data.qpos[1] = float(rng.uniform(-0.4, 0.4))
|
|
523
|
+
mujoco.mj_forward(model, data)
|
|
524
|
+
|
|
525
|
+
ep_frames = []
|
|
526
|
+
ep_peak_vel = 0.0
|
|
527
|
+
ep_vel_violation_step = None
|
|
528
|
+
ep_violating_joint = None
|
|
529
|
+
|
|
530
|
+
for step in range(steps_per_episode):
|
|
531
|
+
q_err = q_goal_2d - data.qpos[:2]
|
|
532
|
+
ctrl_2d = np.clip(kp * q_err - kd * data.qvel[:2], -5.0, 5.0)
|
|
533
|
+
data.ctrl[0] = ctrl_2d[0]
|
|
534
|
+
if model.nu > 1:
|
|
535
|
+
data.ctrl[1] = ctrl_2d[1]
|
|
536
|
+
for extra in range(2, model.nu):
|
|
537
|
+
data.ctrl[extra] = float(-2.0 * data.qpos[extra] - 0.5 * data.qvel[extra])
|
|
538
|
+
|
|
539
|
+
mujoco.mj_step(model, data)
|
|
540
|
+
|
|
541
|
+
abs_vels = np.abs(data.qvel[: model.nv])
|
|
542
|
+
peak_step_vel = float(np.max(abs_vels))
|
|
543
|
+
if peak_step_vel > max_vel_observed:
|
|
544
|
+
max_vel_observed = peak_step_vel
|
|
545
|
+
if peak_step_vel > ep_peak_vel:
|
|
546
|
+
ep_peak_vel = peak_step_vel
|
|
547
|
+
if peak_step_vel > safety_limit_rad_s and ep_vel_violation_step is None:
|
|
548
|
+
ep_vel_violation_step = step
|
|
549
|
+
worst_j = int(np.argmax(abs_vels))
|
|
550
|
+
ep_violating_joint = (
|
|
551
|
+
mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, worst_j) or f"joint_{worst_j}"
|
|
552
|
+
)
|
|
553
|
+
|
|
554
|
+
if step % 10 == 0 or step == steps_per_episode - 1:
|
|
555
|
+
eef_xyz = data.site_xpos[eef_id] if eef_id >= 0 else np.zeros(3)
|
|
556
|
+
action_trace_lines.append(
|
|
557
|
+
f"ep={ep}:s={step}:q=[{data.qpos[0]:.4f},{data.qpos[1]:.4f}]:u=[{ctrl_2d[0]:.4f},{ctrl_2d[1]:.4f}]"
|
|
558
|
+
)
|
|
559
|
+
ep_frames.append(
|
|
560
|
+
{
|
|
561
|
+
"step": step,
|
|
562
|
+
"q1": round(float(data.qpos[0]), 4),
|
|
563
|
+
"q2": round(float(data.qpos[1]) if model.nq > 1 else 0.0, 4),
|
|
564
|
+
"eef_x": round(float(eef_xyz[0]), 4),
|
|
565
|
+
"eef_y": round(float(eef_xyz[1]), 4),
|
|
566
|
+
"vel_mag": round(peak_step_vel, 4),
|
|
567
|
+
}
|
|
568
|
+
)
|
|
569
|
+
|
|
570
|
+
final_eef = data.site_xpos[eef_id] if eef_id >= 0 else np.zeros(3)
|
|
571
|
+
dist = float(np.linalg.norm(final_eef[:2] - target_pos[:2]))
|
|
572
|
+
vel_ok = ep_peak_vel <= safety_limit_rad_s
|
|
573
|
+
succeeded = (dist < 0.08) and vel_ok
|
|
574
|
+
|
|
575
|
+
if succeeded:
|
|
576
|
+
successes += 1
|
|
577
|
+
failure_mode = "NONE"
|
|
578
|
+
elif not vel_ok:
|
|
579
|
+
failure_mode = "JOINT_VELOCITY_EXCEEDED"
|
|
580
|
+
failed_episodes.append(
|
|
581
|
+
{
|
|
582
|
+
"episode_index": ep,
|
|
583
|
+
"seed": seed,
|
|
584
|
+
"failure_mode": failure_mode,
|
|
585
|
+
"violating_joint": ep_violating_joint,
|
|
586
|
+
"step_of_failure": ep_vel_violation_step or 0,
|
|
587
|
+
"observed_value": round(ep_peak_vel, 4),
|
|
588
|
+
"limit_value": safety_limit_rad_s,
|
|
589
|
+
"final_distance_m": round(dist, 4),
|
|
590
|
+
"remediation_hint": f"Increase joint damping kd or clip control torque on {ep_violating_joint}.",
|
|
591
|
+
}
|
|
592
|
+
)
|
|
593
|
+
else:
|
|
594
|
+
failure_mode = "TARGET_MISSED"
|
|
595
|
+
failed_episodes.append(
|
|
596
|
+
{
|
|
597
|
+
"episode_index": ep,
|
|
598
|
+
"seed": seed,
|
|
599
|
+
"failure_mode": failure_mode,
|
|
600
|
+
"violating_joint": None,
|
|
601
|
+
"step_of_failure": steps_per_episode - 1,
|
|
602
|
+
"observed_value": round(dist, 4),
|
|
603
|
+
"limit_value": 0.08,
|
|
604
|
+
"final_distance_m": round(dist, 4),
|
|
605
|
+
"remediation_hint": "Increase proportional gain kp or horizon steps_per_episode.",
|
|
606
|
+
}
|
|
607
|
+
)
|
|
608
|
+
|
|
609
|
+
episode_records.append(
|
|
610
|
+
{
|
|
611
|
+
"episode": ep,
|
|
612
|
+
"seed": seed,
|
|
613
|
+
"succeeded": succeeded,
|
|
614
|
+
"failure_mode": failure_mode,
|
|
615
|
+
"peak_velocity_rad_s": round(ep_peak_vel, 4),
|
|
616
|
+
"final_distance_m": round(dist, 4),
|
|
617
|
+
"frames": ep_frames,
|
|
618
|
+
}
|
|
619
|
+
)
|
|
620
|
+
|
|
621
|
+
trace_material = "\n".join(action_trace_lines)
|
|
622
|
+
ci_low, ci_high = wilson_ci_95(successes, episodes)
|
|
623
|
+
|
|
624
|
+
os.makedirs("data", exist_ok=True)
|
|
625
|
+
replay_html_path = os.path.abspath("data/replay.html")
|
|
626
|
+
write_interactive_canvas_replay_html(
|
|
627
|
+
replay_html_path,
|
|
628
|
+
resolved_id,
|
|
629
|
+
episodes,
|
|
630
|
+
successes,
|
|
631
|
+
ci_low,
|
|
632
|
+
ci_high,
|
|
633
|
+
max_vel_observed,
|
|
634
|
+
safety_limit_rad_s,
|
|
635
|
+
episode_records,
|
|
636
|
+
)
|
|
637
|
+
|
|
638
|
+
result = {
|
|
639
|
+
"ok": True,
|
|
640
|
+
"mujoco_version": mujoco.__version__,
|
|
641
|
+
"robot_id": resolved_id,
|
|
642
|
+
"episodes": episodes,
|
|
643
|
+
"successes": successes,
|
|
644
|
+
"success_rate": round(successes / max(1, episodes), 4),
|
|
645
|
+
"wilson_ci_95": [ci_low, ci_high],
|
|
646
|
+
"peak_joint_velocity_rad_s": round(max_vel_observed, 4),
|
|
647
|
+
"safety_limit_rad_s": safety_limit_rad_s,
|
|
648
|
+
"failed_episodes": failed_episodes,
|
|
649
|
+
"trace_material": trace_material,
|
|
650
|
+
"replay_html_path": replay_html_path,
|
|
651
|
+
}
|
|
652
|
+
print(json.dumps(result))
|
|
653
|
+
|
|
654
|
+
|
|
655
|
+
def run_acceptance_matrix(
|
|
656
|
+
robot_id: str = "so101_6dof_arm",
|
|
657
|
+
site_name: str = "Addverb_Noida_Cell_04",
|
|
658
|
+
safety_limit_rad_s: float = 4.0,
|
|
659
|
+
):
|
|
660
|
+
"""
|
|
661
|
+
20-Scenario Site Acceptance Testing (SAT) Perturbation Matrix (`final/10_EXPANSION_ACCEPTANCE.md`).
|
|
662
|
+
Categories:
|
|
663
|
+
1. S01–S04: Nominal Pick-and-Place & Docking Precision (CRITICAL)
|
|
664
|
+
2. S05–S08: Payload Mass +30% Over-Spec Perturbation (CRITICAL)
|
|
665
|
+
3. S09–S12: Encoder / Depth Camera Gaussian Sensor Noise (sigma=0.025 rad) (NON_CRITICAL)
|
|
666
|
+
4. S13–S16: Nominal seeded variations (CRITICAL)
|
|
667
|
+
5. S17–S20: Actuator Bus Voltage Droop (-15% torque limit) and nominal seeded variations (CRITICAL)
|
|
668
|
+
"""
|
|
669
|
+
base_model, resolved_id = load_mujoco_model(robot_id)
|
|
670
|
+
eef_id = mujoco.mj_name2id(base_model, mujoco.mjtObj.mjOBJ_SITE, "eef")
|
|
671
|
+
target_pos = np.array([0.45, 0.35, 0.0])
|
|
672
|
+
q_goal_2d = solve_2link_ik(0.45, 0.35)
|
|
673
|
+
|
|
674
|
+
scenarios_spec = [
|
|
675
|
+
("SAT-01", "NOMINAL_PRECISION", "CRITICAL", 1.0, 0.0, 5.0, "Nominal home-to-pick reach"),
|
|
676
|
+
("SAT-02", "NOMINAL_PRECISION", "CRITICAL", 1.0, 0.0, 5.0, "Nominal pick-to-place transfer"),
|
|
677
|
+
("SAT-03", "NOMINAL_PRECISION", "CRITICAL", 1.0, 0.0, 5.0, "Nominal docking alignment"),
|
|
678
|
+
("SAT-04", "NOMINAL_PRECISION", "CRITICAL", 1.0, 0.0, 5.0, "Nominal return-to-home cycle"),
|
|
679
|
+
("SAT-05", "PAYLOAD_MASS_PLUS_30PCT", "CRITICAL", 1.30, 0.0, 5.0, "+30% payload mass at max reach"),
|
|
680
|
+
("SAT-06", "PAYLOAD_MASS_PLUS_30PCT", "CRITICAL", 1.30, 0.0, 5.0, "+30% payload rapid deceleration"),
|
|
681
|
+
("SAT-07", "PAYLOAD_MASS_PLUS_30PCT", "CRITICAL", 1.30, 0.0, 5.0, "+30% eccentric center-of-mass"),
|
|
682
|
+
("SAT-08", "PAYLOAD_MASS_PLUS_30PCT", "CRITICAL", 1.30, 0.0, 5.0, "+30% sustained hold stability"),
|
|
683
|
+
("SAT-09", "SENSOR_NOISE_PERTURBATION", "NON_CRITICAL", 1.0, 0.015, 5.0, "Encoder Gaussian noise sigma=0.015 rad"),
|
|
684
|
+
("SAT-10", "SENSOR_NOISE_PERTURBATION", "NON_CRITICAL", 1.0, 0.020, 5.0, "Encoder Gaussian noise sigma=0.020 rad"),
|
|
685
|
+
("SAT-11", "SENSOR_NOISE_PERTURBATION", "NON_CRITICAL", 1.0, 0.025, 5.0, "Depth camera pose jitter +/-15mm"),
|
|
686
|
+
("SAT-12", "SENSOR_NOISE_PERTURBATION", "NON_CRITICAL", 1.0, 0.030, 5.0, "Combined encoder + velocity noise"),
|
|
687
|
+
("SAT-13", "NOMINAL_SEED_VARIATION", "CRITICAL", 1.0, 0.0, 5.0, "Nominal seed 2012"),
|
|
688
|
+
("SAT-14", "NOMINAL_SEED_VARIATION", "CRITICAL", 1.0, 0.0, 5.0, "Nominal seed 2013"),
|
|
689
|
+
("SAT-15", "NOMINAL_SEED_VARIATION", "CRITICAL", 1.0, 0.0, 5.0, "Nominal seed 2014"),
|
|
690
|
+
("SAT-16", "NOMINAL_SEED_VARIATION", "CRITICAL", 1.0, 0.0, 5.0, "Nominal seed 2015"),
|
|
691
|
+
("SAT-17", "ACTUATOR_DROOP", "CRITICAL", 1.0, 0.0, 4.25, "24V bus voltage droop (-15% max torque)"),
|
|
692
|
+
("SAT-18", "ACTUATOR_DROOP", "CRITICAL", 1.15, 0.01, 4.25, "Voltage droop + 15% payload"),
|
|
693
|
+
("SAT-19", "NOMINAL_SEED_VARIATION", "CRITICAL", 1.0, 0.0, 5.0, "Nominal seed 2018"),
|
|
694
|
+
("SAT-20", "NOMINAL_SEED_VARIATION", "CRITICAL", 1.0, 0.0, 5.0, "Nominal seed 2019"),
|
|
695
|
+
]
|
|
696
|
+
|
|
697
|
+
scenario_results = []
|
|
698
|
+
punch_list = []
|
|
699
|
+
passed_count = 0
|
|
700
|
+
critical_failed = 0
|
|
701
|
+
|
|
702
|
+
for idx, (sc_id, category, severity, mass_scale, noise_std, torque_clip, desc) in enumerate(scenarios_spec):
|
|
703
|
+
model, _ = load_mujoco_model(robot_id)
|
|
704
|
+
for b in range(1, model.nbody):
|
|
705
|
+
model.body_mass[b] *= mass_scale
|
|
706
|
+
data = mujoco.MjData(model)
|
|
707
|
+
|
|
708
|
+
seed = 2000 + idx
|
|
709
|
+
rng = np.random.default_rng(seed)
|
|
710
|
+
mujoco.mj_resetData(model, data)
|
|
711
|
+
data.qpos[0] = float(rng.uniform(-0.35, 0.35))
|
|
712
|
+
if model.nq > 1:
|
|
713
|
+
data.qpos[1] = float(rng.uniform(-0.35, 0.35))
|
|
714
|
+
mujoco.mj_forward(model, data)
|
|
715
|
+
|
|
716
|
+
kp_task = 65.0
|
|
717
|
+
kd = 1.8
|
|
718
|
+
peak_vel = 0.0
|
|
719
|
+
jacp = np.zeros((3, model.nv))
|
|
720
|
+
|
|
721
|
+
for _ in range(600):
|
|
722
|
+
eef_now = data.site_xpos[eef_id] if eef_id >= 0 else np.zeros(3)
|
|
723
|
+
pos_err = target_pos - eef_now
|
|
724
|
+
if noise_std > 0:
|
|
725
|
+
pos_err = pos_err + rng.normal(0.0, noise_std * 0.05, size=3)
|
|
726
|
+
mujoco.mj_jacSite(model, data, jacp, None, eef_id)
|
|
727
|
+
tau_task = jacp.T @ (kp_task * pos_err) - kd * data.qvel[: model.nv]
|
|
728
|
+
for j in range(min(model.nu, model.nv)):
|
|
729
|
+
data.ctrl[j] = float(np.clip(tau_task[j], -torque_clip, torque_clip))
|
|
730
|
+
mujoco.mj_step(model, data)
|
|
731
|
+
step_vel = float(np.max(np.abs(data.qvel[: model.nv])))
|
|
732
|
+
if step_vel > peak_vel:
|
|
733
|
+
peak_vel = step_vel
|
|
734
|
+
|
|
735
|
+
final_eef = data.site_xpos[eef_id] if eef_id >= 0 else np.zeros(3)
|
|
736
|
+
dist = float(np.linalg.norm(final_eef[:2] - target_pos[:2]))
|
|
737
|
+
vel_ok = peak_vel <= safety_limit_rad_s
|
|
738
|
+
passed = (dist < 0.085) and vel_ok
|
|
739
|
+
|
|
740
|
+
if passed:
|
|
741
|
+
passed_count += 1
|
|
742
|
+
else:
|
|
743
|
+
if severity == "CRITICAL":
|
|
744
|
+
critical_failed += 1
|
|
745
|
+
punch_list.append(
|
|
746
|
+
{
|
|
747
|
+
"scenario_id": sc_id,
|
|
748
|
+
"severity": severity,
|
|
749
|
+
"owner": "Commissioning Lead",
|
|
750
|
+
"issue": f"{desc}: final error {dist:.4f}m, peak vel {peak_vel:.3f} rad/s",
|
|
751
|
+
"remediation": "Tune Kalman filter covariance or increase derivative damping kd.",
|
|
752
|
+
}
|
|
753
|
+
)
|
|
754
|
+
|
|
755
|
+
scenario_results.append(
|
|
756
|
+
{
|
|
757
|
+
"scenario_id": sc_id,
|
|
758
|
+
"category": category,
|
|
759
|
+
"severity": severity,
|
|
760
|
+
"description": desc,
|
|
761
|
+
"mass_scale": mass_scale,
|
|
762
|
+
"sensor_noise_std": noise_std,
|
|
763
|
+
"torque_limit_nm": torque_clip,
|
|
764
|
+
"seed": seed,
|
|
765
|
+
"passed": passed,
|
|
766
|
+
"peak_velocity_rad_s": round(peak_vel, 4),
|
|
767
|
+
"final_error_m": round(dist, 4),
|
|
768
|
+
}
|
|
769
|
+
)
|
|
770
|
+
|
|
771
|
+
ci_low, ci_high = wilson_ci_95(passed_count, len(scenarios_spec))
|
|
772
|
+
gate_verdict = (
|
|
773
|
+
"PASS_GO_LIVE_APPROVED"
|
|
774
|
+
if critical_failed == 0 and passed_count == len(scenarios_spec)
|
|
775
|
+
else ("CONDITIONAL_PASS" if critical_failed == 0 else "BLOCKED_CRITICAL_FAILURE")
|
|
776
|
+
)
|
|
777
|
+
|
|
778
|
+
out = {
|
|
779
|
+
"ok": critical_failed == 0,
|
|
780
|
+
"mujoco_version": mujoco.__version__,
|
|
781
|
+
"site_name": site_name,
|
|
782
|
+
"robot_id": resolved_id,
|
|
783
|
+
"total_scenarios": len(scenarios_spec),
|
|
784
|
+
"passed_scenarios": passed_count,
|
|
785
|
+
"critical_failures": critical_failed,
|
|
786
|
+
"coverage_pct": 100.0,
|
|
787
|
+
"wilson_ci_95": [ci_low, ci_high],
|
|
788
|
+
"gate_verdict": gate_verdict,
|
|
789
|
+
"scenarios": scenario_results,
|
|
790
|
+
"punch_list": punch_list,
|
|
791
|
+
}
|
|
792
|
+
print(json.dumps(out))
|
|
793
|
+
|
|
794
|
+
|
|
795
|
+
def main():
|
|
796
|
+
parser = argparse.ArgumentParser(description="Dirgha Full Native MuJoCo 3.17.0 Engine")
|
|
797
|
+
parser.add_argument(
|
|
798
|
+
"--mode", default="eval", choices=["eval", "inspect", "step", "reset", "acceptance"]
|
|
799
|
+
)
|
|
800
|
+
parser.add_argument("--robot", default="dirgha_2dof_reacher")
|
|
801
|
+
parser.add_argument("--site", default="Addverb_Noida_Cell_04")
|
|
802
|
+
parser.add_argument("--mjcf-path", default="")
|
|
803
|
+
parser.add_argument("--mjcf-xml", default="")
|
|
804
|
+
parser.add_argument("--episodes", type=int, default=5)
|
|
805
|
+
parser.add_argument("--steps", type=int, default=200)
|
|
806
|
+
parser.add_argument("--safety-limit", type=float, default=4.0)
|
|
807
|
+
parser.add_argument("--session-id", default="sim_01")
|
|
808
|
+
parser.add_argument("--seed", type=int, default=1000)
|
|
809
|
+
parser.add_argument("--action", default="[0.1, -0.1]")
|
|
810
|
+
args = parser.parse_args()
|
|
811
|
+
|
|
812
|
+
if args.mode == "inspect":
|
|
813
|
+
print(json.dumps(inspect_model(args.robot, args.mjcf_path, args.mjcf_xml)))
|
|
814
|
+
elif args.mode == "acceptance":
|
|
815
|
+
run_acceptance_matrix(
|
|
816
|
+
robot_id=args.robot, site_name=args.site, safety_limit_rad_s=args.safety_limit
|
|
817
|
+
)
|
|
818
|
+
elif args.mode in ("step", "reset"):
|
|
819
|
+
try:
|
|
820
|
+
act_vals = json.loads(args.action)
|
|
821
|
+
if not isinstance(act_vals, list):
|
|
822
|
+
act_vals = [0.0, 0.0]
|
|
823
|
+
except Exception:
|
|
824
|
+
act_vals = [0.0, 0.0]
|
|
825
|
+
print(
|
|
826
|
+
json.dumps(
|
|
827
|
+
step_stateful_session(
|
|
828
|
+
args.session_id,
|
|
829
|
+
args.robot,
|
|
830
|
+
act_vals,
|
|
831
|
+
reset=(args.mode == "reset"),
|
|
832
|
+
seed=args.seed,
|
|
833
|
+
)
|
|
834
|
+
)
|
|
835
|
+
)
|
|
836
|
+
else:
|
|
837
|
+
run_suite(
|
|
838
|
+
robot_id=args.robot,
|
|
839
|
+
episodes=args.episodes,
|
|
840
|
+
steps_per_episode=args.steps,
|
|
841
|
+
safety_limit_rad_s=args.safety_limit,
|
|
842
|
+
mjcf_path=args.mjcf_path,
|
|
843
|
+
mjcf_xml=args.mjcf_xml,
|
|
844
|
+
)
|
|
845
|
+
|
|
846
|
+
|
|
847
|
+
if __name__ == "__main__":
|
|
848
|
+
main()
|