mujoco-react 0.1.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/LICENSE +177 -0
- package/README.md +510 -0
- package/dist/index.d.ts +1080 -0
- package/dist/index.js +3518 -0
- package/dist/index.js.map +1 -0
- package/package.json +64 -0
- package/src/components/ContactListener.tsx +26 -0
- package/src/components/ContactMarkers.tsx +81 -0
- package/src/components/Debug.tsx +227 -0
- package/src/components/DragInteraction.tsx +227 -0
- package/src/components/FlexRenderer.tsx +102 -0
- package/src/components/IkGizmo.tsx +146 -0
- package/src/components/SceneLights.tsx +131 -0
- package/src/components/SceneRenderer.tsx +104 -0
- package/src/components/SelectionHighlight.tsx +69 -0
- package/src/components/TendonRenderer.tsx +84 -0
- package/src/components/TrajectoryPlayer.tsx +44 -0
- package/src/core/GenericIK.ts +339 -0
- package/src/core/MujocoCanvas.tsx +72 -0
- package/src/core/MujocoProvider.tsx +78 -0
- package/src/core/MujocoSimProvider.tsx +1201 -0
- package/src/core/SceneLoader.ts +275 -0
- package/src/hooks/useActuators.ts +36 -0
- package/src/hooks/useBodyState.ts +56 -0
- package/src/hooks/useContacts.ts +125 -0
- package/src/hooks/useCtrl.ts +40 -0
- package/src/hooks/useCtrlNoise.ts +59 -0
- package/src/hooks/useGamepad.ts +77 -0
- package/src/hooks/useGravityCompensation.ts +22 -0
- package/src/hooks/useJointState.ts +64 -0
- package/src/hooks/useKeyboardTeleop.ts +97 -0
- package/src/hooks/usePolicy.ts +56 -0
- package/src/hooks/useSensor.ts +83 -0
- package/src/hooks/useSitePosition.ts +62 -0
- package/src/hooks/useTrajectoryPlayer.ts +105 -0
- package/src/hooks/useTrajectoryRecorder.ts +97 -0
- package/src/hooks/useVideoRecorder.ts +82 -0
- package/src/index.ts +108 -0
- package/src/rendering/CapsuleGeometry.ts +35 -0
- package/src/rendering/GeomBuilder.ts +140 -0
- package/src/rendering/Reflector.ts +225 -0
- package/src/types.ts +619 -0
package/dist/index.js
ADDED
|
@@ -0,0 +1,3518 @@
|
|
|
1
|
+
import loadMujoco from 'mujoco-js';
|
|
2
|
+
import { createContext, forwardRef, useEffect, useContext, useState, useRef, useCallback, useMemo } from 'react';
|
|
3
|
+
import { jsx, jsxs, Fragment } from 'react/jsx-runtime';
|
|
4
|
+
import { Canvas, useThree, useFrame } from '@react-three/fiber';
|
|
5
|
+
import * as THREE from 'three';
|
|
6
|
+
import { PivotControls } from '@react-three/drei';
|
|
7
|
+
|
|
8
|
+
// src/core/MujocoProvider.tsx
|
|
9
|
+
var MujocoContext = createContext({
|
|
10
|
+
mujoco: null,
|
|
11
|
+
status: "loading",
|
|
12
|
+
error: null
|
|
13
|
+
});
|
|
14
|
+
function useMujoco() {
|
|
15
|
+
return useContext(MujocoContext);
|
|
16
|
+
}
|
|
17
|
+
function MujocoProvider({ wasmUrl, children, onError }) {
|
|
18
|
+
const [status, setStatus] = useState("loading");
|
|
19
|
+
const [error, setError] = useState(null);
|
|
20
|
+
const moduleRef = useRef(null);
|
|
21
|
+
const isMounted = useRef(true);
|
|
22
|
+
useEffect(() => {
|
|
23
|
+
isMounted.current = true;
|
|
24
|
+
loadMujoco({
|
|
25
|
+
...wasmUrl ? { locateFile: (path) => path.endsWith(".wasm") ? wasmUrl : path } : {},
|
|
26
|
+
printErr: (text) => {
|
|
27
|
+
if (text.includes("Aborted") && isMounted.current) {
|
|
28
|
+
setError("Simulation crashed. Reload page.");
|
|
29
|
+
setStatus("error");
|
|
30
|
+
}
|
|
31
|
+
}
|
|
32
|
+
}).then((inst) => {
|
|
33
|
+
if (isMounted.current) {
|
|
34
|
+
moduleRef.current = inst;
|
|
35
|
+
setStatus("ready");
|
|
36
|
+
}
|
|
37
|
+
}).catch((err) => {
|
|
38
|
+
if (isMounted.current) {
|
|
39
|
+
const msg = err.message || "Failed to init spatial simulation";
|
|
40
|
+
setError(msg);
|
|
41
|
+
setStatus("error");
|
|
42
|
+
onError?.(new Error(msg));
|
|
43
|
+
}
|
|
44
|
+
});
|
|
45
|
+
return () => {
|
|
46
|
+
isMounted.current = false;
|
|
47
|
+
};
|
|
48
|
+
}, [wasmUrl]);
|
|
49
|
+
return /* @__PURE__ */ jsx(
|
|
50
|
+
MujocoContext.Provider,
|
|
51
|
+
{
|
|
52
|
+
value: { mujoco: moduleRef.current, status, error },
|
|
53
|
+
children
|
|
54
|
+
}
|
|
55
|
+
);
|
|
56
|
+
}
|
|
57
|
+
|
|
58
|
+
// src/core/GenericIK.ts
|
|
59
|
+
var DEFAULTS = {
|
|
60
|
+
maxIterations: 50,
|
|
61
|
+
damping: 0.01,
|
|
62
|
+
tolerance: 1e-3,
|
|
63
|
+
epsilon: 1e-6,
|
|
64
|
+
posWeight: 1,
|
|
65
|
+
rotWeight: 0.3
|
|
66
|
+
};
|
|
67
|
+
var GenericIK = class {
|
|
68
|
+
mujoco;
|
|
69
|
+
constructor(mujoco) {
|
|
70
|
+
this.mujoco = mujoco;
|
|
71
|
+
}
|
|
72
|
+
/**
|
|
73
|
+
* Solve IK for a target 6-DOF pose.
|
|
74
|
+
* @param model MuJoCo model
|
|
75
|
+
* @param data MuJoCo data (qpos will be temporarily modified, then restored)
|
|
76
|
+
* @param siteId Index of the end-effector site to control
|
|
77
|
+
* @param numJoints Number of arm joints (assumes qpos[0..numJoints-1])
|
|
78
|
+
* @param targetPos Target position in world frame
|
|
79
|
+
* @param targetQuat Target orientation in world frame
|
|
80
|
+
* @param currentQ Current joint angles (length = numJoints)
|
|
81
|
+
* @param opts Optional solver parameters
|
|
82
|
+
* @returns Joint angles array, or null if solver diverged
|
|
83
|
+
*/
|
|
84
|
+
solve(model, data, siteId, numJoints, targetPos, targetQuat, currentQ, opts) {
|
|
85
|
+
const o = { ...DEFAULTS, ...opts };
|
|
86
|
+
const n = numJoints;
|
|
87
|
+
const savedQpos = new Float64Array(data.qpos.length);
|
|
88
|
+
savedQpos.set(data.qpos);
|
|
89
|
+
const R_target = quatToMat3(targetQuat);
|
|
90
|
+
const q = new Float64Array(n);
|
|
91
|
+
for (let i = 0; i < n; i++) q[i] = currentQ[i];
|
|
92
|
+
const J = new Float64Array(6 * n);
|
|
93
|
+
const JJt = new Float64Array(36);
|
|
94
|
+
const rhs = new Float64Array(6);
|
|
95
|
+
const x = new Float64Array(6);
|
|
96
|
+
const dq = new Float64Array(n);
|
|
97
|
+
const baseSitePos = new Float64Array(3);
|
|
98
|
+
const baseSiteMat = new Float64Array(9);
|
|
99
|
+
const pertSitePos = new Float64Array(3);
|
|
100
|
+
const pertSiteMat = new Float64Array(9);
|
|
101
|
+
let bestQ = null;
|
|
102
|
+
let bestErr = Infinity;
|
|
103
|
+
for (let iter = 0; iter < o.maxIterations; iter++) {
|
|
104
|
+
for (let i = 0; i < n; i++) data.qpos[i] = q[i];
|
|
105
|
+
this.mujoco.mj_forward(model, data);
|
|
106
|
+
const sp = data.site_xpos;
|
|
107
|
+
const sm = data.site_xmat;
|
|
108
|
+
const off3 = siteId * 3;
|
|
109
|
+
const off9 = siteId * 9;
|
|
110
|
+
for (let i = 0; i < 3; i++) baseSitePos[i] = sp[off3 + i];
|
|
111
|
+
for (let i = 0; i < 9; i++) baseSiteMat[i] = sm[off9 + i];
|
|
112
|
+
const posErr0 = targetPos.x - baseSitePos[0];
|
|
113
|
+
const posErr1 = targetPos.y - baseSitePos[1];
|
|
114
|
+
const posErr2 = targetPos.z - baseSitePos[2];
|
|
115
|
+
const rotErr = orientationError(baseSiteMat, R_target);
|
|
116
|
+
const error = [
|
|
117
|
+
posErr0 * o.posWeight,
|
|
118
|
+
posErr1 * o.posWeight,
|
|
119
|
+
posErr2 * o.posWeight,
|
|
120
|
+
rotErr[0] * o.rotWeight,
|
|
121
|
+
rotErr[1] * o.rotWeight,
|
|
122
|
+
rotErr[2] * o.rotWeight
|
|
123
|
+
];
|
|
124
|
+
const errNorm = Math.sqrt(
|
|
125
|
+
error[0] * error[0] + error[1] * error[1] + error[2] * error[2] + error[3] * error[3] + error[4] * error[4] + error[5] * error[5]
|
|
126
|
+
);
|
|
127
|
+
if (errNorm < bestErr) {
|
|
128
|
+
bestErr = errNorm;
|
|
129
|
+
bestQ = Array.from(q);
|
|
130
|
+
}
|
|
131
|
+
if (errNorm < o.tolerance) break;
|
|
132
|
+
for (let j = 0; j < n; j++) {
|
|
133
|
+
const saved = data.qpos[j];
|
|
134
|
+
data.qpos[j] = q[j] + o.epsilon;
|
|
135
|
+
this.mujoco.mj_forward(model, data);
|
|
136
|
+
for (let i = 0; i < 3; i++) pertSitePos[i] = sp[off3 + i];
|
|
137
|
+
for (let i = 0; i < 9; i++) pertSiteMat[i] = sm[off9 + i];
|
|
138
|
+
J[0 * n + j] = (pertSitePos[0] - baseSitePos[0]) / o.epsilon * o.posWeight;
|
|
139
|
+
J[1 * n + j] = (pertSitePos[1] - baseSitePos[1]) / o.epsilon * o.posWeight;
|
|
140
|
+
J[2 * n + j] = (pertSitePos[2] - baseSitePos[2]) / o.epsilon * o.posWeight;
|
|
141
|
+
const dRot = angularDelta(baseSiteMat, pertSiteMat);
|
|
142
|
+
J[3 * n + j] = dRot[0] / o.epsilon * o.rotWeight;
|
|
143
|
+
J[4 * n + j] = dRot[1] / o.epsilon * o.rotWeight;
|
|
144
|
+
J[5 * n + j] = dRot[2] / o.epsilon * o.rotWeight;
|
|
145
|
+
data.qpos[j] = saved;
|
|
146
|
+
}
|
|
147
|
+
for (let i = 0; i < n; i++) data.qpos[i] = q[i];
|
|
148
|
+
for (let r = 0; r < 6; r++) {
|
|
149
|
+
for (let c = 0; c < 6; c++) {
|
|
150
|
+
let sum = 0;
|
|
151
|
+
for (let k = 0; k < n; k++) {
|
|
152
|
+
sum += J[r * n + k] * J[c * n + k];
|
|
153
|
+
}
|
|
154
|
+
JJt[r * 6 + c] = sum + (r === c ? o.damping : 0);
|
|
155
|
+
}
|
|
156
|
+
}
|
|
157
|
+
for (let i = 0; i < 6; i++) rhs[i] = error[i];
|
|
158
|
+
solve6x6(JJt, rhs, x);
|
|
159
|
+
for (let j = 0; j < n; j++) {
|
|
160
|
+
let sum = 0;
|
|
161
|
+
for (let r = 0; r < 6; r++) {
|
|
162
|
+
sum += J[r * n + j] * x[r];
|
|
163
|
+
}
|
|
164
|
+
dq[j] = sum;
|
|
165
|
+
}
|
|
166
|
+
for (let i = 0; i < n; i++) q[i] += dq[i];
|
|
167
|
+
}
|
|
168
|
+
data.qpos.set(savedQpos);
|
|
169
|
+
this.mujoco.mj_forward(model, data);
|
|
170
|
+
return bestQ;
|
|
171
|
+
}
|
|
172
|
+
};
|
|
173
|
+
function quatToMat3(q) {
|
|
174
|
+
const m = new Float64Array(9);
|
|
175
|
+
const x = q.x, y = q.y, z = q.z, w = q.w;
|
|
176
|
+
const xx = x * x, yy = y * y, zz = z * z;
|
|
177
|
+
const xy = x * y, xz = x * z, yz = y * z;
|
|
178
|
+
const wx = w * x, wy = w * y, wz = w * z;
|
|
179
|
+
m[0] = 1 - 2 * (yy + zz);
|
|
180
|
+
m[1] = 2 * (xy - wz);
|
|
181
|
+
m[2] = 2 * (xz + wy);
|
|
182
|
+
m[3] = 2 * (xy + wz);
|
|
183
|
+
m[4] = 1 - 2 * (xx + zz);
|
|
184
|
+
m[5] = 2 * (yz - wx);
|
|
185
|
+
m[6] = 2 * (xz - wy);
|
|
186
|
+
m[7] = 2 * (yz + wx);
|
|
187
|
+
m[8] = 1 - 2 * (xx + yy);
|
|
188
|
+
return m;
|
|
189
|
+
}
|
|
190
|
+
function orientationError(R_cur, R_tgt) {
|
|
191
|
+
const Re = new Float64Array(9);
|
|
192
|
+
for (let i = 0; i < 3; i++) {
|
|
193
|
+
for (let j = 0; j < 3; j++) {
|
|
194
|
+
let s2 = 0;
|
|
195
|
+
for (let k = 0; k < 3; k++) {
|
|
196
|
+
s2 += R_tgt[i * 3 + k] * R_cur[j * 3 + k];
|
|
197
|
+
}
|
|
198
|
+
Re[i * 3 + j] = s2;
|
|
199
|
+
}
|
|
200
|
+
}
|
|
201
|
+
const trace = Re[0] + Re[4] + Re[8];
|
|
202
|
+
const cosAngle = Math.max(-1, Math.min(1, (trace - 1) * 0.5));
|
|
203
|
+
const angle = Math.acos(cosAngle);
|
|
204
|
+
if (angle < 1e-6) {
|
|
205
|
+
return [0, 0, 0];
|
|
206
|
+
}
|
|
207
|
+
if (angle > Math.PI - 1e-6) {
|
|
208
|
+
return [
|
|
209
|
+
0.5 * (Re[7] - Re[5]),
|
|
210
|
+
0.5 * (Re[2] - Re[6]),
|
|
211
|
+
0.5 * (Re[3] - Re[1])
|
|
212
|
+
];
|
|
213
|
+
}
|
|
214
|
+
const s = angle / (2 * Math.sin(angle));
|
|
215
|
+
return [
|
|
216
|
+
s * (Re[7] - Re[5]),
|
|
217
|
+
s * (Re[2] - Re[6]),
|
|
218
|
+
s * (Re[3] - Re[1])
|
|
219
|
+
];
|
|
220
|
+
}
|
|
221
|
+
function angularDelta(R_base, R_pert) {
|
|
222
|
+
const dR = new Float64Array(9);
|
|
223
|
+
for (let i = 0; i < 3; i++) {
|
|
224
|
+
for (let j = 0; j < 3; j++) {
|
|
225
|
+
let s = 0;
|
|
226
|
+
for (let k = 0; k < 3; k++) {
|
|
227
|
+
s += R_pert[i * 3 + k] * R_base[j * 3 + k];
|
|
228
|
+
}
|
|
229
|
+
dR[i * 3 + j] = s;
|
|
230
|
+
}
|
|
231
|
+
}
|
|
232
|
+
return [
|
|
233
|
+
0.5 * (dR[7] - dR[5]),
|
|
234
|
+
0.5 * (dR[2] - dR[6]),
|
|
235
|
+
0.5 * (dR[3] - dR[1])
|
|
236
|
+
];
|
|
237
|
+
}
|
|
238
|
+
function solve6x6(A, b, x) {
|
|
239
|
+
const N = 6;
|
|
240
|
+
const a = new Float64Array(A);
|
|
241
|
+
const r = new Float64Array(b);
|
|
242
|
+
for (let col = 0; col < N; col++) {
|
|
243
|
+
let maxVal = Math.abs(a[col * N + col]);
|
|
244
|
+
let maxRow = col;
|
|
245
|
+
for (let row = col + 1; row < N; row++) {
|
|
246
|
+
const val = Math.abs(a[row * N + col]);
|
|
247
|
+
if (val > maxVal) {
|
|
248
|
+
maxVal = val;
|
|
249
|
+
maxRow = row;
|
|
250
|
+
}
|
|
251
|
+
}
|
|
252
|
+
if (maxRow !== col) {
|
|
253
|
+
for (let k = 0; k < N; k++) {
|
|
254
|
+
const tmp2 = a[col * N + k];
|
|
255
|
+
a[col * N + k] = a[maxRow * N + k];
|
|
256
|
+
a[maxRow * N + k] = tmp2;
|
|
257
|
+
}
|
|
258
|
+
const tmp = r[col];
|
|
259
|
+
r[col] = r[maxRow];
|
|
260
|
+
r[maxRow] = tmp;
|
|
261
|
+
}
|
|
262
|
+
const pivot = a[col * N + col];
|
|
263
|
+
if (Math.abs(pivot) < 1e-12) {
|
|
264
|
+
x.fill(0);
|
|
265
|
+
return;
|
|
266
|
+
}
|
|
267
|
+
for (let row = col + 1; row < N; row++) {
|
|
268
|
+
const factor = a[row * N + col] / pivot;
|
|
269
|
+
for (let k = col; k < N; k++) {
|
|
270
|
+
a[row * N + k] -= factor * a[col * N + k];
|
|
271
|
+
}
|
|
272
|
+
r[row] -= factor * r[col];
|
|
273
|
+
}
|
|
274
|
+
}
|
|
275
|
+
for (let row = N - 1; row >= 0; row--) {
|
|
276
|
+
let sum = r[row];
|
|
277
|
+
for (let k = row + 1; k < N; k++) {
|
|
278
|
+
sum -= a[row * N + k] * x[k];
|
|
279
|
+
}
|
|
280
|
+
x[row] = sum / a[row * N + row];
|
|
281
|
+
}
|
|
282
|
+
}
|
|
283
|
+
|
|
284
|
+
// src/core/SceneLoader.ts
|
|
285
|
+
function getName(mjModel, address) {
|
|
286
|
+
let name = "";
|
|
287
|
+
let idx = address;
|
|
288
|
+
let safety = 0;
|
|
289
|
+
while (mjModel.names[idx] !== 0 && safety < 100) {
|
|
290
|
+
name += String.fromCharCode(mjModel.names[idx++]);
|
|
291
|
+
safety++;
|
|
292
|
+
}
|
|
293
|
+
return name;
|
|
294
|
+
}
|
|
295
|
+
function findSiteByName(mjModel, name) {
|
|
296
|
+
for (let i = 0; i < mjModel.nsite; i++) {
|
|
297
|
+
if (getName(mjModel, mjModel.name_siteadr[i]).includes(name)) return i;
|
|
298
|
+
}
|
|
299
|
+
return -1;
|
|
300
|
+
}
|
|
301
|
+
function findActuatorByName(mjModel, name) {
|
|
302
|
+
for (let i = 0; i < mjModel.nu; i++) {
|
|
303
|
+
if (getName(mjModel, mjModel.name_actuatoradr[i]).includes(name)) return i;
|
|
304
|
+
}
|
|
305
|
+
return -1;
|
|
306
|
+
}
|
|
307
|
+
function findKeyframeByName(mjModel, name) {
|
|
308
|
+
for (let i = 0; i < mjModel.nkey; i++) {
|
|
309
|
+
if (getName(mjModel, mjModel.name_keyadr[i]) === name) return i;
|
|
310
|
+
}
|
|
311
|
+
return -1;
|
|
312
|
+
}
|
|
313
|
+
function findBodyByName(mjModel, name) {
|
|
314
|
+
for (let i = 0; i < mjModel.nbody; i++) {
|
|
315
|
+
if (getName(mjModel, mjModel.name_bodyadr[i]) === name) return i;
|
|
316
|
+
}
|
|
317
|
+
return -1;
|
|
318
|
+
}
|
|
319
|
+
function findJointByName(mjModel, name) {
|
|
320
|
+
for (let i = 0; i < mjModel.njnt; i++) {
|
|
321
|
+
if (getName(mjModel, mjModel.name_jntadr[i]) === name) return i;
|
|
322
|
+
}
|
|
323
|
+
return -1;
|
|
324
|
+
}
|
|
325
|
+
function findGeomByName(mjModel, name) {
|
|
326
|
+
for (let i = 0; i < mjModel.ngeom; i++) {
|
|
327
|
+
if (getName(mjModel, mjModel.name_geomadr[i]) === name) return i;
|
|
328
|
+
}
|
|
329
|
+
return -1;
|
|
330
|
+
}
|
|
331
|
+
function findSensorByName(mjModel, name) {
|
|
332
|
+
for (let i = 0; i < mjModel.nsensor; i++) {
|
|
333
|
+
if (getName(mjModel, mjModel.name_sensoradr[i]) === name) return i;
|
|
334
|
+
}
|
|
335
|
+
return -1;
|
|
336
|
+
}
|
|
337
|
+
function findTendonByName(mjModel, name) {
|
|
338
|
+
for (let i = 0; i < (mjModel.ntendon ?? 0); i++) {
|
|
339
|
+
if (getName(mjModel, mjModel.name_tendonadr[i]) === name) return i;
|
|
340
|
+
}
|
|
341
|
+
return -1;
|
|
342
|
+
}
|
|
343
|
+
function sceneObjectToXml(obj) {
|
|
344
|
+
const joint = obj.freejoint ? "<freejoint/>" : "";
|
|
345
|
+
const pos = obj.position.map((v) => v.toFixed(3)).join(" ");
|
|
346
|
+
const size = obj.size.map((v) => v.toFixed(3)).join(" ");
|
|
347
|
+
const rgba = obj.rgba.join(" ");
|
|
348
|
+
const mass = obj.mass ? ` mass="${obj.mass}"` : "";
|
|
349
|
+
const friction = obj.friction ? ` friction="${obj.friction}"` : "";
|
|
350
|
+
const solref = obj.solref ? ` solref="${obj.solref}"` : "";
|
|
351
|
+
const solimp = obj.solimp ? ` solimp="${obj.solimp}"` : "";
|
|
352
|
+
const condim = obj.condim ? ` condim="${obj.condim}"` : "";
|
|
353
|
+
return `<body name="${obj.name}" pos="${pos}">${joint}<geom type="${obj.type}" size="${size}" rgba="${rgba}" contype="1" conaffinity="1"${mass}${friction}${solref}${solimp}${condim}/></body>`;
|
|
354
|
+
}
|
|
355
|
+
async function loadScene(mujoco, config, onProgress) {
|
|
356
|
+
try {
|
|
357
|
+
mujoco.FS.unmount("/working");
|
|
358
|
+
} catch {
|
|
359
|
+
}
|
|
360
|
+
try {
|
|
361
|
+
mujoco.FS.mkdir("/working");
|
|
362
|
+
} catch {
|
|
363
|
+
}
|
|
364
|
+
const baseUrl = config.baseUrl || `https://raw.githubusercontent.com/google-deepmind/mujoco_menagerie/main/${config.robotId}/`;
|
|
365
|
+
const downloaded = /* @__PURE__ */ new Set();
|
|
366
|
+
const queue = [config.sceneFile];
|
|
367
|
+
const parser = new DOMParser();
|
|
368
|
+
while (queue.length > 0) {
|
|
369
|
+
const fname = queue.shift();
|
|
370
|
+
if (downloaded.has(fname)) continue;
|
|
371
|
+
downloaded.add(fname);
|
|
372
|
+
onProgress?.(`Downloading ${fname}...`);
|
|
373
|
+
const res = await fetch(baseUrl + fname);
|
|
374
|
+
if (!res.ok) {
|
|
375
|
+
console.warn(`Failed to fetch ${fname}: ${res.status} ${res.statusText}`);
|
|
376
|
+
continue;
|
|
377
|
+
}
|
|
378
|
+
const dirParts = fname.split("/");
|
|
379
|
+
dirParts.pop();
|
|
380
|
+
let currentPath = "/working";
|
|
381
|
+
for (const part of dirParts) {
|
|
382
|
+
currentPath += "/" + part;
|
|
383
|
+
try {
|
|
384
|
+
mujoco.FS.mkdir(currentPath);
|
|
385
|
+
} catch {
|
|
386
|
+
}
|
|
387
|
+
}
|
|
388
|
+
if (fname.endsWith(".xml")) {
|
|
389
|
+
let text = await res.text();
|
|
390
|
+
for (const patch of config.xmlPatches ?? []) {
|
|
391
|
+
if (fname.endsWith(patch.target) || fname === patch.target) {
|
|
392
|
+
if (patch.replace) {
|
|
393
|
+
text = text.replace(patch.replace[0], patch.replace[1]);
|
|
394
|
+
}
|
|
395
|
+
if (patch.inject && patch.injectAfter) {
|
|
396
|
+
const idx = text.indexOf(patch.injectAfter);
|
|
397
|
+
if (idx !== -1) {
|
|
398
|
+
const tagEnd = text.indexOf(">", idx + patch.injectAfter.length);
|
|
399
|
+
if (tagEnd !== -1) {
|
|
400
|
+
text = text.slice(0, tagEnd + 1) + patch.inject + text.slice(tagEnd + 1);
|
|
401
|
+
}
|
|
402
|
+
}
|
|
403
|
+
}
|
|
404
|
+
}
|
|
405
|
+
}
|
|
406
|
+
if (fname === config.sceneFile && config.sceneObjects?.length) {
|
|
407
|
+
const xml = config.sceneObjects.map((obj) => sceneObjectToXml(obj)).join("");
|
|
408
|
+
text = text.replace("</worldbody>", xml + "</worldbody>");
|
|
409
|
+
}
|
|
410
|
+
mujoco.FS.writeFile(`/working/${fname}`, text);
|
|
411
|
+
scanDependencies(text, fname, parser, downloaded, queue);
|
|
412
|
+
} else {
|
|
413
|
+
const buffer = new Uint8Array(await res.arrayBuffer());
|
|
414
|
+
mujoco.FS.writeFile(`/working/${fname}`, buffer);
|
|
415
|
+
}
|
|
416
|
+
}
|
|
417
|
+
onProgress?.("Loading model...");
|
|
418
|
+
const mjModel = mujoco.MjModel.loadFromXML(`/working/${config.sceneFile}`);
|
|
419
|
+
const mjData = new mujoco.MjData(mjModel);
|
|
420
|
+
const siteId = findSiteByName(mjModel, config.tcpSiteName ?? "tcp");
|
|
421
|
+
const gripperId = findActuatorByName(mjModel, config.gripperActuatorName ?? "gripper");
|
|
422
|
+
if (config.homeJoints) {
|
|
423
|
+
for (let i = 0; i < config.homeJoints.length; i++) {
|
|
424
|
+
mjData.ctrl[i] = config.homeJoints[i];
|
|
425
|
+
if (mjModel.actuator_trnid[2 * i + 1] === 1) {
|
|
426
|
+
const jointId = mjModel.actuator_trnid[2 * i];
|
|
427
|
+
if (jointId >= 0 && jointId < mjModel.njnt) {
|
|
428
|
+
const qposAdr = mjModel.jnt_qposadr[jointId];
|
|
429
|
+
mjData.qpos[qposAdr] = config.homeJoints[i];
|
|
430
|
+
}
|
|
431
|
+
}
|
|
432
|
+
}
|
|
433
|
+
}
|
|
434
|
+
mujoco.mj_forward(mjModel, mjData);
|
|
435
|
+
return { mjModel, mjData, siteId, gripperId };
|
|
436
|
+
}
|
|
437
|
+
function scanDependencies(xmlString, currentFile, parser, downloaded, queue) {
|
|
438
|
+
const xmlDoc = parser.parseFromString(xmlString, "text/xml");
|
|
439
|
+
const compiler = xmlDoc.querySelector("compiler");
|
|
440
|
+
const meshDir = compiler?.getAttribute("meshdir") || "";
|
|
441
|
+
const textureDir = compiler?.getAttribute("texturedir") || "";
|
|
442
|
+
const currentDir = currentFile.includes("/") ? currentFile.substring(0, currentFile.lastIndexOf("/") + 1) : "";
|
|
443
|
+
xmlDoc.querySelectorAll("[file]").forEach((el) => {
|
|
444
|
+
const fileAttr = el.getAttribute("file");
|
|
445
|
+
if (!fileAttr) return;
|
|
446
|
+
let prefix = "";
|
|
447
|
+
if (el.tagName.toLowerCase() === "mesh") {
|
|
448
|
+
prefix = meshDir ? meshDir + "/" : "";
|
|
449
|
+
} else if (["texture", "hfield"].includes(el.tagName.toLowerCase())) {
|
|
450
|
+
prefix = textureDir ? textureDir + "/" : "";
|
|
451
|
+
}
|
|
452
|
+
let fullPath = (currentDir + prefix + fileAttr).replace(/\/\//g, "/");
|
|
453
|
+
const parts = fullPath.split("/");
|
|
454
|
+
const norm = [];
|
|
455
|
+
for (const p of parts) {
|
|
456
|
+
if (p === "..") norm.pop();
|
|
457
|
+
else if (p !== ".") norm.push(p);
|
|
458
|
+
}
|
|
459
|
+
fullPath = norm.join("/");
|
|
460
|
+
if (!downloaded.has(fullPath)) queue.push(fullPath);
|
|
461
|
+
});
|
|
462
|
+
}
|
|
463
|
+
var JOINT_TYPE_NAMES = ["free", "ball", "slide", "hinge"];
|
|
464
|
+
var GEOM_TYPE_NAMES = ["plane", "hfield", "sphere", "capsule", "ellipsoid", "cylinder", "box", "mesh"];
|
|
465
|
+
var SENSOR_TYPE_NAMES = {
|
|
466
|
+
0: "touch",
|
|
467
|
+
1: "accelerometer",
|
|
468
|
+
2: "velocimeter",
|
|
469
|
+
3: "gyro",
|
|
470
|
+
4: "force",
|
|
471
|
+
5: "torque",
|
|
472
|
+
6: "magnetometer",
|
|
473
|
+
7: "rangefinder",
|
|
474
|
+
8: "camprojection",
|
|
475
|
+
9: "jointpos",
|
|
476
|
+
10: "jointvel",
|
|
477
|
+
11: "tendonpos",
|
|
478
|
+
12: "tendonvel",
|
|
479
|
+
13: "actuatorpos",
|
|
480
|
+
14: "actuatorvel",
|
|
481
|
+
15: "actuatorfrc",
|
|
482
|
+
16: "jointactfrc",
|
|
483
|
+
17: "tendonactfrc",
|
|
484
|
+
18: "ballquat",
|
|
485
|
+
19: "ballangvel",
|
|
486
|
+
20: "jointlimitpos",
|
|
487
|
+
21: "jointlimitvel",
|
|
488
|
+
22: "jointlimitfrc",
|
|
489
|
+
23: "tendonlimitpos",
|
|
490
|
+
24: "tendonlimitvel",
|
|
491
|
+
25: "tendonlimitfrc",
|
|
492
|
+
26: "framepos",
|
|
493
|
+
27: "framequat",
|
|
494
|
+
28: "framexaxis",
|
|
495
|
+
29: "frameyaxis",
|
|
496
|
+
30: "framezaxis",
|
|
497
|
+
31: "framelinvel",
|
|
498
|
+
32: "frameangvel",
|
|
499
|
+
33: "framelinacc",
|
|
500
|
+
34: "frameangacc",
|
|
501
|
+
35: "subtreecom",
|
|
502
|
+
36: "subtreelinvel",
|
|
503
|
+
37: "subtreeangmom",
|
|
504
|
+
38: "insidesite",
|
|
505
|
+
39: "geomdist",
|
|
506
|
+
40: "geomnormal",
|
|
507
|
+
41: "geomfromto",
|
|
508
|
+
42: "contact",
|
|
509
|
+
43: "e_potential",
|
|
510
|
+
44: "e_kinetic",
|
|
511
|
+
45: "clock",
|
|
512
|
+
46: "tactile",
|
|
513
|
+
47: "plugin",
|
|
514
|
+
48: "user"
|
|
515
|
+
};
|
|
516
|
+
var _applyForce = new Float64Array(3);
|
|
517
|
+
var _applyTorque = new Float64Array(3);
|
|
518
|
+
var _applyPoint = new Float64Array(3);
|
|
519
|
+
var _rayPnt = new Float64Array(3);
|
|
520
|
+
var _rayVec = new Float64Array(3);
|
|
521
|
+
var _rayGeomId = new Int32Array(1);
|
|
522
|
+
var MujocoSimContext = createContext(null);
|
|
523
|
+
function useMujocoSim() {
|
|
524
|
+
const ctx = useContext(MujocoSimContext);
|
|
525
|
+
if (!ctx)
|
|
526
|
+
throw new Error("useMujocoSim must be used inside <MujocoSimProvider>");
|
|
527
|
+
return ctx;
|
|
528
|
+
}
|
|
529
|
+
function useBeforePhysicsStep(callback) {
|
|
530
|
+
const { beforeStepCallbacks } = useMujocoSim();
|
|
531
|
+
const callbackRef = useRef(callback);
|
|
532
|
+
callbackRef.current = callback;
|
|
533
|
+
useEffect(() => {
|
|
534
|
+
const wrapped = (model, data) => callbackRef.current(model, data);
|
|
535
|
+
beforeStepCallbacks.current.add(wrapped);
|
|
536
|
+
return () => {
|
|
537
|
+
beforeStepCallbacks.current.delete(wrapped);
|
|
538
|
+
};
|
|
539
|
+
}, [beforeStepCallbacks]);
|
|
540
|
+
}
|
|
541
|
+
function useAfterPhysicsStep(callback) {
|
|
542
|
+
const { afterStepCallbacks } = useMujocoSim();
|
|
543
|
+
const callbackRef = useRef(callback);
|
|
544
|
+
callbackRef.current = callback;
|
|
545
|
+
useEffect(() => {
|
|
546
|
+
const wrapped = (model, data) => callbackRef.current(model, data);
|
|
547
|
+
afterStepCallbacks.current.add(wrapped);
|
|
548
|
+
return () => {
|
|
549
|
+
afterStepCallbacks.current.delete(wrapped);
|
|
550
|
+
};
|
|
551
|
+
}, [afterStepCallbacks]);
|
|
552
|
+
}
|
|
553
|
+
function MujocoSimProvider({
|
|
554
|
+
mujoco,
|
|
555
|
+
config,
|
|
556
|
+
onReady,
|
|
557
|
+
onError,
|
|
558
|
+
onStep,
|
|
559
|
+
onSelection,
|
|
560
|
+
gravity,
|
|
561
|
+
timestep,
|
|
562
|
+
substeps,
|
|
563
|
+
paused,
|
|
564
|
+
speed,
|
|
565
|
+
interpolate,
|
|
566
|
+
children
|
|
567
|
+
}) {
|
|
568
|
+
const { gl, camera } = useThree();
|
|
569
|
+
const [status, setStatus] = useState("loading");
|
|
570
|
+
const mjModelRef = useRef(null);
|
|
571
|
+
const mjDataRef = useRef(null);
|
|
572
|
+
const mujocoRef = useRef(mujoco);
|
|
573
|
+
const configRef = useRef(config);
|
|
574
|
+
const siteIdRef = useRef(-1);
|
|
575
|
+
const gripperIdRef = useRef(-1);
|
|
576
|
+
const ikEnabledRef = useRef(false);
|
|
577
|
+
const ikCalculatingRef = useRef(false);
|
|
578
|
+
const pausedRef = useRef(paused ?? false);
|
|
579
|
+
const speedRef = useRef(speed ?? 1);
|
|
580
|
+
const substepsRef = useRef(substeps ?? 1);
|
|
581
|
+
const interpolateRef = useRef(interpolate ?? false);
|
|
582
|
+
const firstIkEnableRef = useRef(true);
|
|
583
|
+
const stepsToRunRef = useRef(0);
|
|
584
|
+
useRef(null);
|
|
585
|
+
useRef(null);
|
|
586
|
+
useRef(0);
|
|
587
|
+
const onSelectionRef = useRef(onSelection);
|
|
588
|
+
onSelectionRef.current = onSelection;
|
|
589
|
+
const onStepRef = useRef(onStep);
|
|
590
|
+
onStepRef.current = onStep;
|
|
591
|
+
const beforeStepCallbacks = useRef(/* @__PURE__ */ new Set());
|
|
592
|
+
const afterStepCallbacks = useRef(/* @__PURE__ */ new Set());
|
|
593
|
+
configRef.current = config;
|
|
594
|
+
useEffect(() => {
|
|
595
|
+
pausedRef.current = paused ?? false;
|
|
596
|
+
}, [paused]);
|
|
597
|
+
useEffect(() => {
|
|
598
|
+
speedRef.current = speed ?? 1;
|
|
599
|
+
}, [speed]);
|
|
600
|
+
useEffect(() => {
|
|
601
|
+
substepsRef.current = substeps ?? 1;
|
|
602
|
+
}, [substeps]);
|
|
603
|
+
useEffect(() => {
|
|
604
|
+
interpolateRef.current = interpolate ?? false;
|
|
605
|
+
}, [interpolate]);
|
|
606
|
+
useEffect(() => {
|
|
607
|
+
if (!gravity) return;
|
|
608
|
+
const model = mjModelRef.current;
|
|
609
|
+
if (!model?.opt?.gravity) return;
|
|
610
|
+
model.opt.gravity[0] = gravity[0];
|
|
611
|
+
model.opt.gravity[1] = gravity[1];
|
|
612
|
+
model.opt.gravity[2] = gravity[2];
|
|
613
|
+
}, [gravity]);
|
|
614
|
+
useEffect(() => {
|
|
615
|
+
if (timestep === void 0) return;
|
|
616
|
+
const model = mjModelRef.current;
|
|
617
|
+
if (!model?.opt) return;
|
|
618
|
+
model.opt.timestep = timestep;
|
|
619
|
+
}, [timestep]);
|
|
620
|
+
const ikTargetRef = useRef(new THREE.Group());
|
|
621
|
+
const genericIkRef = useRef(new GenericIK(mujoco));
|
|
622
|
+
const gizmoAnimRef = useRef({
|
|
623
|
+
active: false,
|
|
624
|
+
startPos: new THREE.Vector3(),
|
|
625
|
+
endPos: new THREE.Vector3(),
|
|
626
|
+
startRot: new THREE.Quaternion(),
|
|
627
|
+
endRot: new THREE.Quaternion(),
|
|
628
|
+
startTime: 0,
|
|
629
|
+
duration: 1e3
|
|
630
|
+
});
|
|
631
|
+
const cameraAnimRef = useRef({
|
|
632
|
+
active: false,
|
|
633
|
+
startPos: new THREE.Vector3(),
|
|
634
|
+
endPos: new THREE.Vector3(),
|
|
635
|
+
startRot: new THREE.Quaternion(),
|
|
636
|
+
endRot: new THREE.Quaternion(),
|
|
637
|
+
startTarget: new THREE.Vector3(),
|
|
638
|
+
endTarget: new THREE.Vector3(),
|
|
639
|
+
startTime: 0,
|
|
640
|
+
duration: 0,
|
|
641
|
+
resolve: null
|
|
642
|
+
});
|
|
643
|
+
const orbitTargetRef = useRef(new THREE.Vector3(0, 0, 0));
|
|
644
|
+
const syncGizmoToSite = useCallback((data, siteId, target) => {
|
|
645
|
+
if (siteId === -1) return;
|
|
646
|
+
const sitePos = data.site_xpos.subarray(siteId * 3, siteId * 3 + 3);
|
|
647
|
+
const siteMat = data.site_xmat.subarray(siteId * 9, siteId * 9 + 9);
|
|
648
|
+
target.position.set(sitePos[0], sitePos[1], sitePos[2]);
|
|
649
|
+
const m = new THREE.Matrix4().set(
|
|
650
|
+
siteMat[0],
|
|
651
|
+
siteMat[1],
|
|
652
|
+
siteMat[2],
|
|
653
|
+
0,
|
|
654
|
+
siteMat[3],
|
|
655
|
+
siteMat[4],
|
|
656
|
+
siteMat[5],
|
|
657
|
+
0,
|
|
658
|
+
siteMat[6],
|
|
659
|
+
siteMat[7],
|
|
660
|
+
siteMat[8],
|
|
661
|
+
0,
|
|
662
|
+
0,
|
|
663
|
+
0,
|
|
664
|
+
0,
|
|
665
|
+
1
|
|
666
|
+
);
|
|
667
|
+
target.quaternion.setFromRotationMatrix(m);
|
|
668
|
+
}, []);
|
|
669
|
+
const ikSolveFn = useCallback(
|
|
670
|
+
(pos, quat, currentQ) => {
|
|
671
|
+
const model = mjModelRef.current;
|
|
672
|
+
const data = mjDataRef.current;
|
|
673
|
+
if (!model || !data || siteIdRef.current === -1) return null;
|
|
674
|
+
return genericIkRef.current.solve(
|
|
675
|
+
model,
|
|
676
|
+
data,
|
|
677
|
+
siteIdRef.current,
|
|
678
|
+
configRef.current.numArmJoints ?? 7,
|
|
679
|
+
pos,
|
|
680
|
+
quat,
|
|
681
|
+
currentQ
|
|
682
|
+
);
|
|
683
|
+
},
|
|
684
|
+
[]
|
|
685
|
+
);
|
|
686
|
+
const ikSolveFnRef = useRef(ikSolveFn);
|
|
687
|
+
ikSolveFnRef.current = ikSolveFn;
|
|
688
|
+
useEffect(() => {
|
|
689
|
+
let disposed = false;
|
|
690
|
+
(async () => {
|
|
691
|
+
try {
|
|
692
|
+
const result = await loadScene(mujoco, config);
|
|
693
|
+
if (disposed) {
|
|
694
|
+
result.mjModel.delete();
|
|
695
|
+
result.mjData.delete();
|
|
696
|
+
return;
|
|
697
|
+
}
|
|
698
|
+
mjModelRef.current = result.mjModel;
|
|
699
|
+
mjDataRef.current = result.mjData;
|
|
700
|
+
siteIdRef.current = result.siteId;
|
|
701
|
+
gripperIdRef.current = result.gripperId;
|
|
702
|
+
if (gravity && result.mjModel.opt?.gravity) {
|
|
703
|
+
result.mjModel.opt.gravity[0] = gravity[0];
|
|
704
|
+
result.mjModel.opt.gravity[1] = gravity[1];
|
|
705
|
+
result.mjModel.opt.gravity[2] = gravity[2];
|
|
706
|
+
}
|
|
707
|
+
if (timestep !== void 0 && result.mjModel.opt) {
|
|
708
|
+
result.mjModel.opt.timestep = timestep;
|
|
709
|
+
}
|
|
710
|
+
if (ikTargetRef.current) {
|
|
711
|
+
syncGizmoToSite(result.mjData, result.siteId, ikTargetRef.current);
|
|
712
|
+
}
|
|
713
|
+
setStatus("ready");
|
|
714
|
+
} catch (e) {
|
|
715
|
+
if (!disposed) {
|
|
716
|
+
setStatus("error");
|
|
717
|
+
onError?.(e instanceof Error ? e : new Error(String(e)));
|
|
718
|
+
}
|
|
719
|
+
}
|
|
720
|
+
})();
|
|
721
|
+
return () => {
|
|
722
|
+
disposed = true;
|
|
723
|
+
mjModelRef.current?.delete();
|
|
724
|
+
mjDataRef.current?.delete();
|
|
725
|
+
mjModelRef.current = null;
|
|
726
|
+
mjDataRef.current = null;
|
|
727
|
+
try {
|
|
728
|
+
mujoco.FS.unmount("/working");
|
|
729
|
+
} catch {
|
|
730
|
+
}
|
|
731
|
+
};
|
|
732
|
+
}, [mujoco, config]);
|
|
733
|
+
useEffect(() => {
|
|
734
|
+
if (status === "ready" && onReady) {
|
|
735
|
+
onReady(apiRef.current);
|
|
736
|
+
}
|
|
737
|
+
}, [status]);
|
|
738
|
+
useFrame((state) => {
|
|
739
|
+
const model = mjModelRef.current;
|
|
740
|
+
const data = mjDataRef.current;
|
|
741
|
+
if (!model || !data) return;
|
|
742
|
+
const ga = gizmoAnimRef.current;
|
|
743
|
+
const target = ikTargetRef.current;
|
|
744
|
+
if (ga.active && target) {
|
|
745
|
+
const now = performance.now();
|
|
746
|
+
const elapsed = now - ga.startTime;
|
|
747
|
+
const t = Math.min(elapsed / ga.duration, 1);
|
|
748
|
+
const ease = 1 - Math.pow(1 - t, 3);
|
|
749
|
+
target.position.lerpVectors(ga.startPos, ga.endPos, ease);
|
|
750
|
+
target.quaternion.slerpQuaternions(ga.startRot, ga.endRot, ease);
|
|
751
|
+
if (t >= 1) ga.active = false;
|
|
752
|
+
}
|
|
753
|
+
const ca = cameraAnimRef.current;
|
|
754
|
+
if (ca.active) {
|
|
755
|
+
const now = performance.now();
|
|
756
|
+
const progress = Math.min((now - ca.startTime) / ca.duration, 1);
|
|
757
|
+
const ease = progress < 0.5 ? 4 * progress * progress * progress : 1 - Math.pow(-2 * progress + 2, 3) / 2;
|
|
758
|
+
camera.position.lerpVectors(ca.startPos, ca.endPos, ease);
|
|
759
|
+
camera.quaternion.slerpQuaternions(ca.startRot, ca.endRot, ease);
|
|
760
|
+
orbitTargetRef.current.lerpVectors(ca.startTarget, ca.endTarget, ease);
|
|
761
|
+
const orbitControls = state.controls;
|
|
762
|
+
if (orbitControls?.target) {
|
|
763
|
+
orbitControls.target.copy(orbitTargetRef.current);
|
|
764
|
+
}
|
|
765
|
+
if (progress >= 1) {
|
|
766
|
+
ca.active = false;
|
|
767
|
+
camera.position.copy(ca.endPos);
|
|
768
|
+
camera.quaternion.copy(ca.endRot);
|
|
769
|
+
orbitTargetRef.current.copy(ca.endTarget);
|
|
770
|
+
ca.resolve?.();
|
|
771
|
+
ca.resolve = null;
|
|
772
|
+
}
|
|
773
|
+
}
|
|
774
|
+
const shouldStep = !pausedRef.current || stepsToRunRef.current > 0;
|
|
775
|
+
if (!shouldStep) return;
|
|
776
|
+
for (let i = 0; i < model.nv; i++) {
|
|
777
|
+
data.qfrc_applied[i] = 0;
|
|
778
|
+
}
|
|
779
|
+
for (const cb of beforeStepCallbacks.current) {
|
|
780
|
+
cb(model, data);
|
|
781
|
+
}
|
|
782
|
+
if (ikEnabledRef.current && target) {
|
|
783
|
+
ikCalculatingRef.current = true;
|
|
784
|
+
const numArm = configRef.current.numArmJoints ?? 7;
|
|
785
|
+
const currentQ = [];
|
|
786
|
+
for (let i = 0; i < numArm; i++) currentQ.push(data.qpos[i]);
|
|
787
|
+
const solution = ikSolveFnRef.current(target.position, target.quaternion, currentQ);
|
|
788
|
+
if (solution) {
|
|
789
|
+
for (let i = 0; i < numArm; i++) data.ctrl[i] = solution[i];
|
|
790
|
+
}
|
|
791
|
+
} else {
|
|
792
|
+
ikCalculatingRef.current = false;
|
|
793
|
+
}
|
|
794
|
+
const numSubsteps = substepsRef.current;
|
|
795
|
+
if (stepsToRunRef.current > 0) {
|
|
796
|
+
for (let s = 0; s < stepsToRunRef.current; s++) {
|
|
797
|
+
mujoco.mj_step(model, data);
|
|
798
|
+
}
|
|
799
|
+
stepsToRunRef.current = 0;
|
|
800
|
+
} else {
|
|
801
|
+
const startSimTime = data.time;
|
|
802
|
+
const frameTime = 1 / 60 * speedRef.current;
|
|
803
|
+
while (data.time - startSimTime < frameTime) {
|
|
804
|
+
for (let s = 0; s < numSubsteps; s++) {
|
|
805
|
+
mujoco.mj_step(model, data);
|
|
806
|
+
}
|
|
807
|
+
}
|
|
808
|
+
}
|
|
809
|
+
for (const cb of afterStepCallbacks.current) {
|
|
810
|
+
cb(model, data);
|
|
811
|
+
}
|
|
812
|
+
onStepRef.current?.(data.time);
|
|
813
|
+
}, -1);
|
|
814
|
+
const reset = useCallback(() => {
|
|
815
|
+
const model = mjModelRef.current;
|
|
816
|
+
const data = mjDataRef.current;
|
|
817
|
+
if (!model || !data) return;
|
|
818
|
+
gizmoAnimRef.current.active = false;
|
|
819
|
+
mujoco.mj_resetData(model, data);
|
|
820
|
+
const homeJoints = configRef.current.homeJoints;
|
|
821
|
+
if (homeJoints) {
|
|
822
|
+
for (let i = 0; i < homeJoints.length; i++) {
|
|
823
|
+
data.ctrl[i] = homeJoints[i];
|
|
824
|
+
if (model.actuator_trnid[2 * i + 1] === 1) {
|
|
825
|
+
const jointId = model.actuator_trnid[2 * i];
|
|
826
|
+
if (jointId >= 0 && jointId < model.njnt) {
|
|
827
|
+
const qposAdr = model.jnt_qposadr[jointId];
|
|
828
|
+
data.qpos[qposAdr] = homeJoints[i];
|
|
829
|
+
}
|
|
830
|
+
}
|
|
831
|
+
}
|
|
832
|
+
}
|
|
833
|
+
configRef.current.onReset?.(model, data);
|
|
834
|
+
mujoco.mj_forward(model, data);
|
|
835
|
+
if (ikTargetRef.current) {
|
|
836
|
+
syncGizmoToSite(data, siteIdRef.current, ikTargetRef.current);
|
|
837
|
+
}
|
|
838
|
+
firstIkEnableRef.current = true;
|
|
839
|
+
ikEnabledRef.current = false;
|
|
840
|
+
}, [mujoco, syncGizmoToSite]);
|
|
841
|
+
const setIkEnabled = useCallback((enabled) => {
|
|
842
|
+
ikEnabledRef.current = enabled;
|
|
843
|
+
const data = mjDataRef.current;
|
|
844
|
+
if (enabled && data && !gizmoAnimRef.current.active && ikTargetRef.current) {
|
|
845
|
+
syncGizmoToSite(data, siteIdRef.current, ikTargetRef.current);
|
|
846
|
+
firstIkEnableRef.current = false;
|
|
847
|
+
}
|
|
848
|
+
}, [syncGizmoToSite]);
|
|
849
|
+
const syncTargetToSite = useCallback(() => {
|
|
850
|
+
const data = mjDataRef.current;
|
|
851
|
+
const target = ikTargetRef.current;
|
|
852
|
+
if (data && target) syncGizmoToSite(data, siteIdRef.current, target);
|
|
853
|
+
}, [syncGizmoToSite]);
|
|
854
|
+
const solveIK = useCallback(
|
|
855
|
+
(pos, quat, currentQ) => {
|
|
856
|
+
return ikSolveFnRef.current(pos, quat, currentQ);
|
|
857
|
+
},
|
|
858
|
+
[]
|
|
859
|
+
);
|
|
860
|
+
const moveTarget = useCallback(
|
|
861
|
+
(pos, duration = 0) => {
|
|
862
|
+
if (!ikEnabledRef.current) setIkEnabled(true);
|
|
863
|
+
const target = ikTargetRef.current;
|
|
864
|
+
if (!target) return;
|
|
865
|
+
const targetPos = pos.clone();
|
|
866
|
+
const targetRot = new THREE.Quaternion().setFromEuler(new THREE.Euler(Math.PI, 0, 0));
|
|
867
|
+
if (duration > 0) {
|
|
868
|
+
const ga = gizmoAnimRef.current;
|
|
869
|
+
ga.active = true;
|
|
870
|
+
ga.startPos.copy(target.position);
|
|
871
|
+
ga.endPos.copy(targetPos);
|
|
872
|
+
ga.startRot.copy(target.quaternion);
|
|
873
|
+
ga.endRot.copy(targetRot);
|
|
874
|
+
ga.startTime = performance.now();
|
|
875
|
+
ga.duration = duration;
|
|
876
|
+
} else {
|
|
877
|
+
gizmoAnimRef.current.active = false;
|
|
878
|
+
target.position.copy(targetPos);
|
|
879
|
+
target.quaternion.copy(targetRot);
|
|
880
|
+
}
|
|
881
|
+
},
|
|
882
|
+
[setIkEnabled]
|
|
883
|
+
);
|
|
884
|
+
const setSpeed = useCallback((multiplier) => {
|
|
885
|
+
speedRef.current = multiplier;
|
|
886
|
+
}, []);
|
|
887
|
+
const togglePause = useCallback(() => {
|
|
888
|
+
pausedRef.current = !pausedRef.current;
|
|
889
|
+
return pausedRef.current;
|
|
890
|
+
}, []);
|
|
891
|
+
const setPaused = useCallback((p) => {
|
|
892
|
+
pausedRef.current = p;
|
|
893
|
+
}, []);
|
|
894
|
+
const step = useCallback((n = 1) => {
|
|
895
|
+
stepsToRunRef.current = n;
|
|
896
|
+
}, []);
|
|
897
|
+
const getTime = useCallback(() => {
|
|
898
|
+
return mjDataRef.current?.time ?? 0;
|
|
899
|
+
}, []);
|
|
900
|
+
const getTimestep = useCallback(() => {
|
|
901
|
+
return mjModelRef.current?.opt?.timestep ?? 2e-3;
|
|
902
|
+
}, []);
|
|
903
|
+
const saveState = useCallback(() => {
|
|
904
|
+
const data = mjDataRef.current;
|
|
905
|
+
if (!data) return { time: 0, qpos: new Float64Array(0), qvel: new Float64Array(0), ctrl: new Float64Array(0), act: new Float64Array(0), qfrc_applied: new Float64Array(0) };
|
|
906
|
+
return {
|
|
907
|
+
time: data.time,
|
|
908
|
+
qpos: new Float64Array(data.qpos),
|
|
909
|
+
qvel: new Float64Array(data.qvel),
|
|
910
|
+
ctrl: new Float64Array(data.ctrl),
|
|
911
|
+
act: new Float64Array(data.act),
|
|
912
|
+
qfrc_applied: new Float64Array(data.qfrc_applied)
|
|
913
|
+
};
|
|
914
|
+
}, []);
|
|
915
|
+
const restoreState = useCallback((snapshot) => {
|
|
916
|
+
const model = mjModelRef.current;
|
|
917
|
+
const data = mjDataRef.current;
|
|
918
|
+
if (!model || !data) return;
|
|
919
|
+
data.time = snapshot.time;
|
|
920
|
+
data.qpos.set(snapshot.qpos);
|
|
921
|
+
data.qvel.set(snapshot.qvel);
|
|
922
|
+
data.ctrl.set(snapshot.ctrl);
|
|
923
|
+
if (snapshot.act.length > 0) data.act.set(snapshot.act);
|
|
924
|
+
data.qfrc_applied.set(snapshot.qfrc_applied);
|
|
925
|
+
mujoco.mj_forward(model, data);
|
|
926
|
+
}, [mujoco]);
|
|
927
|
+
const setQpos = useCallback((values) => {
|
|
928
|
+
const model = mjModelRef.current;
|
|
929
|
+
const data = mjDataRef.current;
|
|
930
|
+
if (!model || !data) return;
|
|
931
|
+
const arr = values instanceof Float64Array ? values : new Float64Array(values);
|
|
932
|
+
data.qpos.set(arr.subarray(0, Math.min(arr.length, model.nq)));
|
|
933
|
+
mujoco.mj_forward(model, data);
|
|
934
|
+
}, [mujoco]);
|
|
935
|
+
const setQvel = useCallback((values) => {
|
|
936
|
+
const data = mjDataRef.current;
|
|
937
|
+
if (!data) return;
|
|
938
|
+
const arr = values instanceof Float64Array ? values : new Float64Array(values);
|
|
939
|
+
data.qvel.set(arr.subarray(0, Math.min(arr.length, mjModelRef.current?.nv ?? 0)));
|
|
940
|
+
}, []);
|
|
941
|
+
const getQpos = useCallback(() => {
|
|
942
|
+
return mjDataRef.current ? new Float64Array(mjDataRef.current.qpos) : new Float64Array(0);
|
|
943
|
+
}, []);
|
|
944
|
+
const getQvel = useCallback(() => {
|
|
945
|
+
return mjDataRef.current ? new Float64Array(mjDataRef.current.qvel) : new Float64Array(0);
|
|
946
|
+
}, []);
|
|
947
|
+
const setCtrl = useCallback((nameOrValues, value) => {
|
|
948
|
+
const model = mjModelRef.current;
|
|
949
|
+
const data = mjDataRef.current;
|
|
950
|
+
if (!model || !data) return;
|
|
951
|
+
if (typeof nameOrValues === "string") {
|
|
952
|
+
const id = findActuatorByName(model, nameOrValues);
|
|
953
|
+
if (id >= 0 && value !== void 0) data.ctrl[id] = value;
|
|
954
|
+
} else {
|
|
955
|
+
for (const [name, val] of Object.entries(nameOrValues)) {
|
|
956
|
+
const id = findActuatorByName(model, name);
|
|
957
|
+
if (id >= 0) data.ctrl[id] = val;
|
|
958
|
+
}
|
|
959
|
+
}
|
|
960
|
+
}, []);
|
|
961
|
+
const getCtrl = useCallback(() => {
|
|
962
|
+
return mjDataRef.current ? new Float64Array(mjDataRef.current.ctrl) : new Float64Array(0);
|
|
963
|
+
}, []);
|
|
964
|
+
const applyForce = useCallback((bodyName, force, point) => {
|
|
965
|
+
const model = mjModelRef.current;
|
|
966
|
+
const data = mjDataRef.current;
|
|
967
|
+
if (!model || !data) return;
|
|
968
|
+
const bodyId = findBodyByName(model, bodyName);
|
|
969
|
+
if (bodyId < 0) return;
|
|
970
|
+
_applyForce[0] = force.x;
|
|
971
|
+
_applyForce[1] = force.y;
|
|
972
|
+
_applyForce[2] = force.z;
|
|
973
|
+
_applyTorque[0] = 0;
|
|
974
|
+
_applyTorque[1] = 0;
|
|
975
|
+
_applyTorque[2] = 0;
|
|
976
|
+
if (point) {
|
|
977
|
+
_applyPoint[0] = point.x;
|
|
978
|
+
_applyPoint[1] = point.y;
|
|
979
|
+
_applyPoint[2] = point.z;
|
|
980
|
+
} else {
|
|
981
|
+
const i3 = bodyId * 3;
|
|
982
|
+
_applyPoint[0] = data.xpos[i3];
|
|
983
|
+
_applyPoint[1] = data.xpos[i3 + 1];
|
|
984
|
+
_applyPoint[2] = data.xpos[i3 + 2];
|
|
985
|
+
}
|
|
986
|
+
mujoco.mj_applyFT(model, data, _applyForce, _applyTorque, _applyPoint, bodyId, data.qfrc_applied);
|
|
987
|
+
}, [mujoco]);
|
|
988
|
+
const applyTorqueApi = useCallback((bodyName, torque) => {
|
|
989
|
+
const model = mjModelRef.current;
|
|
990
|
+
const data = mjDataRef.current;
|
|
991
|
+
if (!model || !data) return;
|
|
992
|
+
const bodyId = findBodyByName(model, bodyName);
|
|
993
|
+
if (bodyId < 0) return;
|
|
994
|
+
_applyForce[0] = 0;
|
|
995
|
+
_applyForce[1] = 0;
|
|
996
|
+
_applyForce[2] = 0;
|
|
997
|
+
_applyTorque[0] = torque.x;
|
|
998
|
+
_applyTorque[1] = torque.y;
|
|
999
|
+
_applyTorque[2] = torque.z;
|
|
1000
|
+
const i3 = bodyId * 3;
|
|
1001
|
+
_applyPoint[0] = data.xpos[i3];
|
|
1002
|
+
_applyPoint[1] = data.xpos[i3 + 1];
|
|
1003
|
+
_applyPoint[2] = data.xpos[i3 + 2];
|
|
1004
|
+
mujoco.mj_applyFT(model, data, _applyForce, _applyTorque, _applyPoint, bodyId, data.qfrc_applied);
|
|
1005
|
+
}, [mujoco]);
|
|
1006
|
+
const setExternalForce = useCallback((bodyName, force, torque) => {
|
|
1007
|
+
const model = mjModelRef.current;
|
|
1008
|
+
const data = mjDataRef.current;
|
|
1009
|
+
if (!model || !data) return;
|
|
1010
|
+
const bodyId = findBodyByName(model, bodyName);
|
|
1011
|
+
if (bodyId < 0) return;
|
|
1012
|
+
const i6 = bodyId * 6;
|
|
1013
|
+
data.xfrc_applied[i6] = torque.x;
|
|
1014
|
+
data.xfrc_applied[i6 + 1] = torque.y;
|
|
1015
|
+
data.xfrc_applied[i6 + 2] = torque.z;
|
|
1016
|
+
data.xfrc_applied[i6 + 3] = force.x;
|
|
1017
|
+
data.xfrc_applied[i6 + 4] = force.y;
|
|
1018
|
+
data.xfrc_applied[i6 + 5] = force.z;
|
|
1019
|
+
}, []);
|
|
1020
|
+
const applyGeneralizedForce = useCallback((values) => {
|
|
1021
|
+
const data = mjDataRef.current;
|
|
1022
|
+
if (!data) return;
|
|
1023
|
+
const nv = mjModelRef.current?.nv ?? 0;
|
|
1024
|
+
for (let i = 0; i < Math.min(values.length, nv); i++) {
|
|
1025
|
+
data.qfrc_applied[i] += values[i];
|
|
1026
|
+
}
|
|
1027
|
+
}, []);
|
|
1028
|
+
const getSensorData = useCallback((name) => {
|
|
1029
|
+
const model = mjModelRef.current;
|
|
1030
|
+
const data = mjDataRef.current;
|
|
1031
|
+
if (!model || !data) return null;
|
|
1032
|
+
const id = findSensorByName(model, name);
|
|
1033
|
+
if (id < 0) return null;
|
|
1034
|
+
const adr = model.sensor_adr[id];
|
|
1035
|
+
const dim = model.sensor_dim[id];
|
|
1036
|
+
return new Float64Array(data.sensordata.subarray(adr, adr + dim));
|
|
1037
|
+
}, []);
|
|
1038
|
+
const getContacts = useCallback(() => {
|
|
1039
|
+
const model = mjModelRef.current;
|
|
1040
|
+
const data = mjDataRef.current;
|
|
1041
|
+
if (!model || !data) return [];
|
|
1042
|
+
const contacts = [];
|
|
1043
|
+
const ncon = data.ncon;
|
|
1044
|
+
for (let i = 0; i < ncon; i++) {
|
|
1045
|
+
try {
|
|
1046
|
+
const c = data.contact.get(i);
|
|
1047
|
+
contacts.push({
|
|
1048
|
+
geom1: c.geom1,
|
|
1049
|
+
geom1Name: getName(model, model.name_geomadr[c.geom1]),
|
|
1050
|
+
geom2: c.geom2,
|
|
1051
|
+
geom2Name: getName(model, model.name_geomadr[c.geom2]),
|
|
1052
|
+
pos: [c.pos[0], c.pos[1], c.pos[2]],
|
|
1053
|
+
depth: c.dist
|
|
1054
|
+
});
|
|
1055
|
+
} catch {
|
|
1056
|
+
break;
|
|
1057
|
+
}
|
|
1058
|
+
}
|
|
1059
|
+
return contacts;
|
|
1060
|
+
}, []);
|
|
1061
|
+
const getBodies = useCallback(() => {
|
|
1062
|
+
const model = mjModelRef.current;
|
|
1063
|
+
if (!model) return [];
|
|
1064
|
+
const result = [];
|
|
1065
|
+
for (let i = 0; i < model.nbody; i++) {
|
|
1066
|
+
result.push({
|
|
1067
|
+
id: i,
|
|
1068
|
+
name: getName(model, model.name_bodyadr[i]),
|
|
1069
|
+
mass: model.body_mass[i],
|
|
1070
|
+
parentId: model.body_parentid[i]
|
|
1071
|
+
});
|
|
1072
|
+
}
|
|
1073
|
+
return result;
|
|
1074
|
+
}, []);
|
|
1075
|
+
const getJoints = useCallback(() => {
|
|
1076
|
+
const model = mjModelRef.current;
|
|
1077
|
+
if (!model) return [];
|
|
1078
|
+
const result = [];
|
|
1079
|
+
for (let i = 0; i < model.njnt; i++) {
|
|
1080
|
+
const type = model.jnt_type[i];
|
|
1081
|
+
const limited = model.jnt_limited ? model.jnt_limited[i] !== 0 : false;
|
|
1082
|
+
result.push({
|
|
1083
|
+
id: i,
|
|
1084
|
+
name: getName(model, model.name_jntadr[i]),
|
|
1085
|
+
type,
|
|
1086
|
+
typeName: JOINT_TYPE_NAMES[type] ?? `unknown(${type})`,
|
|
1087
|
+
range: [model.jnt_range[2 * i], model.jnt_range[2 * i + 1]],
|
|
1088
|
+
limited,
|
|
1089
|
+
bodyId: model.jnt_bodyid[i],
|
|
1090
|
+
qposAdr: model.jnt_qposadr[i],
|
|
1091
|
+
dofAdr: model.jnt_dofadr[i]
|
|
1092
|
+
});
|
|
1093
|
+
}
|
|
1094
|
+
return result;
|
|
1095
|
+
}, []);
|
|
1096
|
+
const getGeoms = useCallback(() => {
|
|
1097
|
+
const model = mjModelRef.current;
|
|
1098
|
+
if (!model) return [];
|
|
1099
|
+
const result = [];
|
|
1100
|
+
for (let i = 0; i < model.ngeom; i++) {
|
|
1101
|
+
const type = model.geom_type[i];
|
|
1102
|
+
result.push({
|
|
1103
|
+
id: i,
|
|
1104
|
+
name: getName(model, model.name_geomadr[i]),
|
|
1105
|
+
type,
|
|
1106
|
+
typeName: GEOM_TYPE_NAMES[type] ?? `unknown(${type})`,
|
|
1107
|
+
size: [model.geom_size[3 * i], model.geom_size[3 * i + 1], model.geom_size[3 * i + 2]],
|
|
1108
|
+
bodyId: model.geom_bodyid[i]
|
|
1109
|
+
});
|
|
1110
|
+
}
|
|
1111
|
+
return result;
|
|
1112
|
+
}, []);
|
|
1113
|
+
const getSites = useCallback(() => {
|
|
1114
|
+
const model = mjModelRef.current;
|
|
1115
|
+
if (!model) return [];
|
|
1116
|
+
const result = [];
|
|
1117
|
+
for (let i = 0; i < model.nsite; i++) {
|
|
1118
|
+
result.push({
|
|
1119
|
+
id: i,
|
|
1120
|
+
name: getName(model, model.name_siteadr[i]),
|
|
1121
|
+
bodyId: model.site_bodyid ? model.site_bodyid[i] : -1
|
|
1122
|
+
});
|
|
1123
|
+
}
|
|
1124
|
+
return result;
|
|
1125
|
+
}, []);
|
|
1126
|
+
const getActuatorsApi = useCallback(() => {
|
|
1127
|
+
const model = mjModelRef.current;
|
|
1128
|
+
if (!model) return [];
|
|
1129
|
+
const result = [];
|
|
1130
|
+
for (let i = 0; i < model.nu; i++) {
|
|
1131
|
+
const hasRange = model.actuator_ctrlrange[2 * i] < model.actuator_ctrlrange[2 * i + 1];
|
|
1132
|
+
result.push({
|
|
1133
|
+
id: i,
|
|
1134
|
+
name: getName(model, model.name_actuatoradr[i]),
|
|
1135
|
+
range: hasRange ? [model.actuator_ctrlrange[2 * i], model.actuator_ctrlrange[2 * i + 1]] : [-Infinity, Infinity]
|
|
1136
|
+
});
|
|
1137
|
+
}
|
|
1138
|
+
return result;
|
|
1139
|
+
}, []);
|
|
1140
|
+
const getSensors = useCallback(() => {
|
|
1141
|
+
const model = mjModelRef.current;
|
|
1142
|
+
if (!model) return [];
|
|
1143
|
+
const result = [];
|
|
1144
|
+
for (let i = 0; i < model.nsensor; i++) {
|
|
1145
|
+
const type = model.sensor_type[i];
|
|
1146
|
+
result.push({
|
|
1147
|
+
id: i,
|
|
1148
|
+
name: getName(model, model.name_sensoradr[i]),
|
|
1149
|
+
type,
|
|
1150
|
+
typeName: SENSOR_TYPE_NAMES[type] ?? `unknown(${type})`,
|
|
1151
|
+
dim: model.sensor_dim[i],
|
|
1152
|
+
adr: model.sensor_adr[i]
|
|
1153
|
+
});
|
|
1154
|
+
}
|
|
1155
|
+
return result;
|
|
1156
|
+
}, []);
|
|
1157
|
+
const getModelOption = useCallback(() => {
|
|
1158
|
+
const model = mjModelRef.current;
|
|
1159
|
+
if (!model?.opt) return { timestep: 2e-3, gravity: [0, 0, -9.81], integrator: 0 };
|
|
1160
|
+
return {
|
|
1161
|
+
timestep: model.opt.timestep,
|
|
1162
|
+
gravity: [model.opt.gravity[0], model.opt.gravity[1], model.opt.gravity[2]],
|
|
1163
|
+
integrator: model.opt.integrator
|
|
1164
|
+
};
|
|
1165
|
+
}, []);
|
|
1166
|
+
const setGravity = useCallback((g) => {
|
|
1167
|
+
const model = mjModelRef.current;
|
|
1168
|
+
if (!model?.opt?.gravity) return;
|
|
1169
|
+
model.opt.gravity[0] = g[0];
|
|
1170
|
+
model.opt.gravity[1] = g[1];
|
|
1171
|
+
model.opt.gravity[2] = g[2];
|
|
1172
|
+
}, []);
|
|
1173
|
+
const setTimestepApi = useCallback((dt) => {
|
|
1174
|
+
const model = mjModelRef.current;
|
|
1175
|
+
if (!model?.opt) return;
|
|
1176
|
+
model.opt.timestep = dt;
|
|
1177
|
+
}, []);
|
|
1178
|
+
const raycast = useCallback((origin, direction, maxDist = 100) => {
|
|
1179
|
+
const model = mjModelRef.current;
|
|
1180
|
+
const data = mjDataRef.current;
|
|
1181
|
+
if (!model || !data) return null;
|
|
1182
|
+
_rayPnt[0] = origin.x;
|
|
1183
|
+
_rayPnt[1] = origin.y;
|
|
1184
|
+
_rayPnt[2] = origin.z;
|
|
1185
|
+
const dir = direction.clone().normalize();
|
|
1186
|
+
_rayVec[0] = dir.x;
|
|
1187
|
+
_rayVec[1] = dir.y;
|
|
1188
|
+
_rayVec[2] = dir.z;
|
|
1189
|
+
_rayGeomId[0] = -1;
|
|
1190
|
+
try {
|
|
1191
|
+
const dist = mujoco.mj_ray(model, data, _rayPnt, _rayVec, null, 1, -1, _rayGeomId);
|
|
1192
|
+
if (dist < 0 || dist > maxDist) return null;
|
|
1193
|
+
const geomId = _rayGeomId[0];
|
|
1194
|
+
const bodyId = geomId >= 0 ? model.geom_bodyid[geomId] : -1;
|
|
1195
|
+
return {
|
|
1196
|
+
point: new THREE.Vector3(
|
|
1197
|
+
origin.x + dir.x * dist,
|
|
1198
|
+
origin.y + dir.y * dist,
|
|
1199
|
+
origin.z + dir.z * dist
|
|
1200
|
+
),
|
|
1201
|
+
bodyId,
|
|
1202
|
+
geomId,
|
|
1203
|
+
distance: dist
|
|
1204
|
+
};
|
|
1205
|
+
} catch {
|
|
1206
|
+
return null;
|
|
1207
|
+
}
|
|
1208
|
+
}, [mujoco]);
|
|
1209
|
+
const applyKeyframe = useCallback((nameOrIndex) => {
|
|
1210
|
+
const model = mjModelRef.current;
|
|
1211
|
+
const data = mjDataRef.current;
|
|
1212
|
+
if (!model || !data) return;
|
|
1213
|
+
let keyId;
|
|
1214
|
+
if (typeof nameOrIndex === "number") {
|
|
1215
|
+
keyId = nameOrIndex;
|
|
1216
|
+
} else {
|
|
1217
|
+
keyId = findKeyframeByName(model, nameOrIndex);
|
|
1218
|
+
}
|
|
1219
|
+
if (keyId < 0 || keyId >= model.nkey) {
|
|
1220
|
+
console.warn(`applyKeyframe: keyframe "${nameOrIndex}" not found`);
|
|
1221
|
+
return;
|
|
1222
|
+
}
|
|
1223
|
+
const nq = model.nq;
|
|
1224
|
+
const nu = model.nu;
|
|
1225
|
+
const qposOffset = keyId * nq;
|
|
1226
|
+
for (let i = 0; i < nq; i++) data.qpos[i] = model.key_qpos[qposOffset + i];
|
|
1227
|
+
const ctrlOffset = keyId * nu;
|
|
1228
|
+
for (let i = 0; i < nu; i++) data.ctrl[i] = model.key_ctrl[ctrlOffset + i];
|
|
1229
|
+
if (model.key_qvel) {
|
|
1230
|
+
const qvelOffset = keyId * model.nv;
|
|
1231
|
+
for (let i = 0; i < model.nv; i++) data.qvel[i] = model.key_qvel[qvelOffset + i];
|
|
1232
|
+
}
|
|
1233
|
+
mujoco.mj_forward(model, data);
|
|
1234
|
+
if (ikTargetRef.current) {
|
|
1235
|
+
syncGizmoToSite(data, siteIdRef.current, ikTargetRef.current);
|
|
1236
|
+
}
|
|
1237
|
+
}, [mujoco, syncGizmoToSite]);
|
|
1238
|
+
const getKeyframeNames = useCallback(() => {
|
|
1239
|
+
const model = mjModelRef.current;
|
|
1240
|
+
if (!model) return [];
|
|
1241
|
+
const names = [];
|
|
1242
|
+
for (let i = 0; i < model.nkey; i++) {
|
|
1243
|
+
names.push(getName(model, model.name_keyadr[i]));
|
|
1244
|
+
}
|
|
1245
|
+
return names;
|
|
1246
|
+
}, []);
|
|
1247
|
+
const getKeyframeCount = useCallback(() => {
|
|
1248
|
+
return mjModelRef.current?.nkey ?? 0;
|
|
1249
|
+
}, []);
|
|
1250
|
+
const loadSceneApi = useCallback(async (newConfig) => {
|
|
1251
|
+
try {
|
|
1252
|
+
mjModelRef.current?.delete();
|
|
1253
|
+
mjDataRef.current?.delete();
|
|
1254
|
+
mjModelRef.current = null;
|
|
1255
|
+
mjDataRef.current = null;
|
|
1256
|
+
setStatus("loading");
|
|
1257
|
+
const result = await loadScene(mujoco, newConfig);
|
|
1258
|
+
mjModelRef.current = result.mjModel;
|
|
1259
|
+
mjDataRef.current = result.mjData;
|
|
1260
|
+
siteIdRef.current = result.siteId;
|
|
1261
|
+
gripperIdRef.current = result.gripperId;
|
|
1262
|
+
configRef.current = newConfig;
|
|
1263
|
+
if (ikTargetRef.current) {
|
|
1264
|
+
syncGizmoToSite(result.mjData, result.siteId, ikTargetRef.current);
|
|
1265
|
+
}
|
|
1266
|
+
setStatus("ready");
|
|
1267
|
+
} catch (e) {
|
|
1268
|
+
setStatus("error");
|
|
1269
|
+
throw e;
|
|
1270
|
+
}
|
|
1271
|
+
}, [mujoco, syncGizmoToSite]);
|
|
1272
|
+
const getGizmoStats = useCallback(() => {
|
|
1273
|
+
const target = ikTargetRef.current;
|
|
1274
|
+
if (!ikCalculatingRef.current || !target) return null;
|
|
1275
|
+
return {
|
|
1276
|
+
pos: target.position.clone(),
|
|
1277
|
+
rot: new THREE.Euler().setFromQuaternion(target.quaternion)
|
|
1278
|
+
};
|
|
1279
|
+
}, []);
|
|
1280
|
+
const getCanvasSnapshot = useCallback(
|
|
1281
|
+
(width, height, mimeType = "image/jpeg") => {
|
|
1282
|
+
if (width && height) {
|
|
1283
|
+
const tempCanvas = document.createElement("canvas");
|
|
1284
|
+
tempCanvas.width = width;
|
|
1285
|
+
tempCanvas.height = height;
|
|
1286
|
+
const ctx = tempCanvas.getContext("2d");
|
|
1287
|
+
if (ctx) {
|
|
1288
|
+
ctx.drawImage(gl.domElement, 0, 0, width, height);
|
|
1289
|
+
return tempCanvas.toDataURL(mimeType, mimeType === "image/jpeg" ? 0.8 : void 0);
|
|
1290
|
+
}
|
|
1291
|
+
}
|
|
1292
|
+
return gl.domElement.toDataURL(mimeType, mimeType === "image/jpeg" ? 0.8 : void 0);
|
|
1293
|
+
},
|
|
1294
|
+
[gl]
|
|
1295
|
+
);
|
|
1296
|
+
const project2DTo3D = useCallback(
|
|
1297
|
+
(x, y, cameraPos, lookAt) => {
|
|
1298
|
+
const virtCam = camera.clone();
|
|
1299
|
+
virtCam.position.copy(cameraPos);
|
|
1300
|
+
virtCam.lookAt(lookAt);
|
|
1301
|
+
virtCam.updateMatrixWorld();
|
|
1302
|
+
virtCam.updateProjectionMatrix();
|
|
1303
|
+
const ndc = new THREE.Vector2(x * 2 - 1, -(y * 2 - 1));
|
|
1304
|
+
const raycaster = new THREE.Raycaster();
|
|
1305
|
+
raycaster.setFromCamera(ndc, virtCam);
|
|
1306
|
+
const objects = [];
|
|
1307
|
+
const scene = camera.parent;
|
|
1308
|
+
if (scene) {
|
|
1309
|
+
scene.traverse((c) => {
|
|
1310
|
+
if (c.isMesh) objects.push(c);
|
|
1311
|
+
});
|
|
1312
|
+
}
|
|
1313
|
+
const hits = raycaster.intersectObjects(objects);
|
|
1314
|
+
if (hits.length > 0) {
|
|
1315
|
+
const hitObj = hits[0].object;
|
|
1316
|
+
const geomId = hitObj.userData.geomID !== void 0 ? hitObj.userData.geomID : -1;
|
|
1317
|
+
let obj = hitObj;
|
|
1318
|
+
while (obj && obj.userData.bodyID === void 0 && obj.parent) {
|
|
1319
|
+
obj = obj.parent;
|
|
1320
|
+
}
|
|
1321
|
+
const bodyId = obj && obj.userData.bodyID !== void 0 ? obj.userData.bodyID : -1;
|
|
1322
|
+
return { point: hits[0].point, bodyId, geomId };
|
|
1323
|
+
}
|
|
1324
|
+
return null;
|
|
1325
|
+
},
|
|
1326
|
+
[camera, gl]
|
|
1327
|
+
);
|
|
1328
|
+
const setBodyMass = useCallback((name, mass) => {
|
|
1329
|
+
const model = mjModelRef.current;
|
|
1330
|
+
if (!model) return;
|
|
1331
|
+
const id = findBodyByName(model, name);
|
|
1332
|
+
if (id < 0) return;
|
|
1333
|
+
model.body_mass[id] = mass;
|
|
1334
|
+
}, []);
|
|
1335
|
+
const setGeomFriction = useCallback((name, friction) => {
|
|
1336
|
+
const model = mjModelRef.current;
|
|
1337
|
+
if (!model) return;
|
|
1338
|
+
const id = findGeomByName(model, name);
|
|
1339
|
+
if (id < 0) return;
|
|
1340
|
+
model.geom_friction[id * 3] = friction[0];
|
|
1341
|
+
model.geom_friction[id * 3 + 1] = friction[1];
|
|
1342
|
+
model.geom_friction[id * 3 + 2] = friction[2];
|
|
1343
|
+
}, []);
|
|
1344
|
+
const setGeomSize = useCallback((name, size) => {
|
|
1345
|
+
const model = mjModelRef.current;
|
|
1346
|
+
if (!model) return;
|
|
1347
|
+
const id = findGeomByName(model, name);
|
|
1348
|
+
if (id < 0) return;
|
|
1349
|
+
model.geom_size[id * 3] = size[0];
|
|
1350
|
+
model.geom_size[id * 3 + 1] = size[1];
|
|
1351
|
+
model.geom_size[id * 3 + 2] = size[2];
|
|
1352
|
+
}, []);
|
|
1353
|
+
const getCameraState = useCallback(() => {
|
|
1354
|
+
return { position: camera.position.clone(), target: orbitTargetRef.current.clone() };
|
|
1355
|
+
}, [camera]);
|
|
1356
|
+
const moveCameraTo = useCallback(
|
|
1357
|
+
(position, target, durationMs) => {
|
|
1358
|
+
return new Promise((resolve) => {
|
|
1359
|
+
const ca = cameraAnimRef.current;
|
|
1360
|
+
ca.active = true;
|
|
1361
|
+
ca.startTime = performance.now();
|
|
1362
|
+
ca.duration = durationMs;
|
|
1363
|
+
ca.startPos.copy(camera.position);
|
|
1364
|
+
ca.startRot.copy(camera.quaternion);
|
|
1365
|
+
ca.startTarget.copy(orbitTargetRef.current);
|
|
1366
|
+
ca.endPos.copy(position);
|
|
1367
|
+
ca.endTarget.copy(target);
|
|
1368
|
+
const dummyCam = camera.clone();
|
|
1369
|
+
dummyCam.position.copy(position);
|
|
1370
|
+
dummyCam.lookAt(target);
|
|
1371
|
+
ca.endRot.copy(dummyCam.quaternion);
|
|
1372
|
+
ca.resolve = resolve;
|
|
1373
|
+
setTimeout(resolve, durationMs + 100);
|
|
1374
|
+
});
|
|
1375
|
+
},
|
|
1376
|
+
[camera]
|
|
1377
|
+
);
|
|
1378
|
+
const api = useMemo(
|
|
1379
|
+
() => ({
|
|
1380
|
+
get status() {
|
|
1381
|
+
return status;
|
|
1382
|
+
},
|
|
1383
|
+
config,
|
|
1384
|
+
reset,
|
|
1385
|
+
setSpeed,
|
|
1386
|
+
togglePause,
|
|
1387
|
+
setPaused,
|
|
1388
|
+
step,
|
|
1389
|
+
getTime,
|
|
1390
|
+
getTimestep,
|
|
1391
|
+
applyKeyframe,
|
|
1392
|
+
saveState,
|
|
1393
|
+
restoreState,
|
|
1394
|
+
setQpos,
|
|
1395
|
+
setQvel,
|
|
1396
|
+
getQpos,
|
|
1397
|
+
getQvel,
|
|
1398
|
+
setCtrl,
|
|
1399
|
+
getCtrl,
|
|
1400
|
+
applyForce,
|
|
1401
|
+
applyTorque: applyTorqueApi,
|
|
1402
|
+
setExternalForce,
|
|
1403
|
+
applyGeneralizedForce,
|
|
1404
|
+
getSensorData,
|
|
1405
|
+
getContacts,
|
|
1406
|
+
getBodies,
|
|
1407
|
+
getJoints,
|
|
1408
|
+
getGeoms,
|
|
1409
|
+
getSites,
|
|
1410
|
+
getActuators: getActuatorsApi,
|
|
1411
|
+
getSensors,
|
|
1412
|
+
getModelOption,
|
|
1413
|
+
setGravity,
|
|
1414
|
+
setTimestep: setTimestepApi,
|
|
1415
|
+
raycast,
|
|
1416
|
+
getKeyframeNames,
|
|
1417
|
+
getKeyframeCount,
|
|
1418
|
+
loadScene: loadSceneApi,
|
|
1419
|
+
setIkEnabled,
|
|
1420
|
+
moveTarget,
|
|
1421
|
+
syncTargetToSite,
|
|
1422
|
+
solveIK,
|
|
1423
|
+
getGizmoStats,
|
|
1424
|
+
getCanvasSnapshot,
|
|
1425
|
+
project2DTo3D,
|
|
1426
|
+
getCameraState,
|
|
1427
|
+
moveCameraTo,
|
|
1428
|
+
setBodyMass,
|
|
1429
|
+
setGeomFriction,
|
|
1430
|
+
setGeomSize,
|
|
1431
|
+
mjModelRef,
|
|
1432
|
+
mjDataRef
|
|
1433
|
+
}),
|
|
1434
|
+
[
|
|
1435
|
+
status,
|
|
1436
|
+
config,
|
|
1437
|
+
reset,
|
|
1438
|
+
setSpeed,
|
|
1439
|
+
togglePause,
|
|
1440
|
+
setPaused,
|
|
1441
|
+
step,
|
|
1442
|
+
getTime,
|
|
1443
|
+
getTimestep,
|
|
1444
|
+
applyKeyframe,
|
|
1445
|
+
saveState,
|
|
1446
|
+
restoreState,
|
|
1447
|
+
setQpos,
|
|
1448
|
+
setQvel,
|
|
1449
|
+
getQpos,
|
|
1450
|
+
getQvel,
|
|
1451
|
+
setCtrl,
|
|
1452
|
+
getCtrl,
|
|
1453
|
+
applyForce,
|
|
1454
|
+
applyTorqueApi,
|
|
1455
|
+
setExternalForce,
|
|
1456
|
+
applyGeneralizedForce,
|
|
1457
|
+
getSensorData,
|
|
1458
|
+
getContacts,
|
|
1459
|
+
getBodies,
|
|
1460
|
+
getJoints,
|
|
1461
|
+
getGeoms,
|
|
1462
|
+
getSites,
|
|
1463
|
+
getActuatorsApi,
|
|
1464
|
+
getSensors,
|
|
1465
|
+
getModelOption,
|
|
1466
|
+
setGravity,
|
|
1467
|
+
setTimestepApi,
|
|
1468
|
+
raycast,
|
|
1469
|
+
getKeyframeNames,
|
|
1470
|
+
getKeyframeCount,
|
|
1471
|
+
loadSceneApi,
|
|
1472
|
+
setIkEnabled,
|
|
1473
|
+
moveTarget,
|
|
1474
|
+
syncTargetToSite,
|
|
1475
|
+
solveIK,
|
|
1476
|
+
getGizmoStats,
|
|
1477
|
+
getCanvasSnapshot,
|
|
1478
|
+
project2DTo3D,
|
|
1479
|
+
getCameraState,
|
|
1480
|
+
moveCameraTo,
|
|
1481
|
+
setBodyMass,
|
|
1482
|
+
setGeomFriction,
|
|
1483
|
+
setGeomSize
|
|
1484
|
+
]
|
|
1485
|
+
);
|
|
1486
|
+
const apiRef = useRef(api);
|
|
1487
|
+
apiRef.current = api;
|
|
1488
|
+
const contextValue = useMemo(
|
|
1489
|
+
() => ({
|
|
1490
|
+
api,
|
|
1491
|
+
mjModelRef,
|
|
1492
|
+
mjDataRef,
|
|
1493
|
+
mujocoRef,
|
|
1494
|
+
configRef,
|
|
1495
|
+
siteIdRef,
|
|
1496
|
+
gripperIdRef,
|
|
1497
|
+
ikEnabledRef,
|
|
1498
|
+
ikCalculatingRef,
|
|
1499
|
+
pausedRef,
|
|
1500
|
+
speedRef,
|
|
1501
|
+
substepsRef,
|
|
1502
|
+
ikTargetRef,
|
|
1503
|
+
genericIkRef,
|
|
1504
|
+
ikSolveFnRef,
|
|
1505
|
+
firstIkEnableRef,
|
|
1506
|
+
gizmoAnimRef,
|
|
1507
|
+
cameraAnimRef,
|
|
1508
|
+
onSelectionRef,
|
|
1509
|
+
beforeStepCallbacks,
|
|
1510
|
+
afterStepCallbacks,
|
|
1511
|
+
status
|
|
1512
|
+
}),
|
|
1513
|
+
[api, status]
|
|
1514
|
+
);
|
|
1515
|
+
return /* @__PURE__ */ jsx(MujocoSimContext.Provider, { value: contextValue, children });
|
|
1516
|
+
}
|
|
1517
|
+
var MujocoCanvas = forwardRef(
|
|
1518
|
+
function MujocoCanvas2({
|
|
1519
|
+
config,
|
|
1520
|
+
onReady,
|
|
1521
|
+
onError,
|
|
1522
|
+
onStep,
|
|
1523
|
+
onSelection,
|
|
1524
|
+
// Declarative physics config (spec 1.1)
|
|
1525
|
+
gravity,
|
|
1526
|
+
timestep,
|
|
1527
|
+
substeps,
|
|
1528
|
+
paused,
|
|
1529
|
+
speed,
|
|
1530
|
+
interpolate,
|
|
1531
|
+
gravityCompensation,
|
|
1532
|
+
mjcfLights,
|
|
1533
|
+
children,
|
|
1534
|
+
...canvasProps
|
|
1535
|
+
}, ref) {
|
|
1536
|
+
const { mujoco, status: wasmStatus, error: wasmError } = useMujoco();
|
|
1537
|
+
useEffect(() => {
|
|
1538
|
+
if (wasmStatus === "error" && onError) {
|
|
1539
|
+
onError(new Error(wasmError ?? "WASM load failed"));
|
|
1540
|
+
}
|
|
1541
|
+
}, [wasmStatus, wasmError, onError]);
|
|
1542
|
+
if (wasmStatus === "error" || wasmStatus === "loading" || !mujoco) {
|
|
1543
|
+
return null;
|
|
1544
|
+
}
|
|
1545
|
+
return /* @__PURE__ */ jsx(Canvas, { ref, ...canvasProps, children: /* @__PURE__ */ jsx(
|
|
1546
|
+
MujocoSimProvider,
|
|
1547
|
+
{
|
|
1548
|
+
mujoco,
|
|
1549
|
+
config,
|
|
1550
|
+
onReady,
|
|
1551
|
+
onError,
|
|
1552
|
+
onStep,
|
|
1553
|
+
onSelection,
|
|
1554
|
+
gravity,
|
|
1555
|
+
timestep,
|
|
1556
|
+
substeps,
|
|
1557
|
+
paused,
|
|
1558
|
+
speed,
|
|
1559
|
+
interpolate,
|
|
1560
|
+
children
|
|
1561
|
+
}
|
|
1562
|
+
) });
|
|
1563
|
+
}
|
|
1564
|
+
);
|
|
1565
|
+
var CapsuleGeometry = class extends THREE.BufferGeometry {
|
|
1566
|
+
parameters;
|
|
1567
|
+
constructor(radius = 1, length = 1, capSegments = 4, radialSegments = 8) {
|
|
1568
|
+
super();
|
|
1569
|
+
this.type = "CapsuleGeometry";
|
|
1570
|
+
this.parameters = { radius, length, capSegments, radialSegments };
|
|
1571
|
+
const path = new THREE.Path();
|
|
1572
|
+
path.absarc(0, -length / 2, radius, Math.PI * 1.5, 0, false);
|
|
1573
|
+
path.absarc(0, length / 2, radius, 0, Math.PI * 0.5, false);
|
|
1574
|
+
const latheGeometry = new THREE.LatheGeometry(path.getPoints(capSegments), radialSegments);
|
|
1575
|
+
const self = this;
|
|
1576
|
+
self.setIndex(latheGeometry.getIndex());
|
|
1577
|
+
self.setAttribute("position", latheGeometry.getAttribute("position"));
|
|
1578
|
+
self.setAttribute("normal", latheGeometry.getAttribute("normal"));
|
|
1579
|
+
self.setAttribute("uv", latheGeometry.getAttribute("uv"));
|
|
1580
|
+
}
|
|
1581
|
+
};
|
|
1582
|
+
var Reflector = class extends THREE.Mesh {
|
|
1583
|
+
isReflector = true;
|
|
1584
|
+
camera;
|
|
1585
|
+
reflectorPlane = new THREE.Plane();
|
|
1586
|
+
normal = new THREE.Vector3();
|
|
1587
|
+
reflectorWorldPosition = new THREE.Vector3();
|
|
1588
|
+
cameraWorldPosition = new THREE.Vector3();
|
|
1589
|
+
rotationMatrix = new THREE.Matrix4();
|
|
1590
|
+
lookAtPosition = new THREE.Vector3(0, 0, -1);
|
|
1591
|
+
clipPlane = new THREE.Vector4();
|
|
1592
|
+
view = new THREE.Vector3();
|
|
1593
|
+
target = new THREE.Vector3();
|
|
1594
|
+
q = new THREE.Vector4();
|
|
1595
|
+
textureMatrix = new THREE.Matrix4();
|
|
1596
|
+
virtualCamera;
|
|
1597
|
+
renderTarget;
|
|
1598
|
+
constructor(geometry, options = {}) {
|
|
1599
|
+
super(geometry);
|
|
1600
|
+
this.type = "Reflector";
|
|
1601
|
+
this.camera = new THREE.PerspectiveCamera();
|
|
1602
|
+
const color = options.color !== void 0 ? new THREE.Color(options.color) : new THREE.Color(8355711);
|
|
1603
|
+
const textureWidth = options.textureWidth || 512;
|
|
1604
|
+
const textureHeight = options.textureHeight || 512;
|
|
1605
|
+
const clipBias = options.clipBias || 0;
|
|
1606
|
+
const multisample = options.multisample !== void 0 ? options.multisample : 4;
|
|
1607
|
+
const blendTexture = options.texture || void 0;
|
|
1608
|
+
const mixStrength = options.mixStrength !== void 0 ? options.mixStrength : 0.25;
|
|
1609
|
+
this.virtualCamera = this.camera;
|
|
1610
|
+
this.renderTarget = new THREE.WebGLRenderTarget(textureWidth, textureHeight, {
|
|
1611
|
+
samples: multisample,
|
|
1612
|
+
type: THREE.HalfFloatType
|
|
1613
|
+
});
|
|
1614
|
+
this.material = new THREE.MeshPhysicalMaterial({
|
|
1615
|
+
map: blendTexture,
|
|
1616
|
+
color,
|
|
1617
|
+
roughness: 0.5,
|
|
1618
|
+
metalness: 0.1
|
|
1619
|
+
});
|
|
1620
|
+
this.material.onBeforeCompile = (shader) => {
|
|
1621
|
+
shader.uniforms.tDiffuse = { value: this.renderTarget.texture };
|
|
1622
|
+
shader.uniforms.textureMatrix = { value: this.textureMatrix };
|
|
1623
|
+
shader.uniforms.mixStrength = { value: mixStrength };
|
|
1624
|
+
const bodyStart = shader.vertexShader.indexOf("void main() {");
|
|
1625
|
+
shader.vertexShader = "uniform mat4 textureMatrix;\nvarying vec4 vUvReflection;\n" + shader.vertexShader.slice(0, bodyStart) + shader.vertexShader.slice(bodyStart, -1) + " vUvReflection = textureMatrix * vec4( position, 1.0 );\n}";
|
|
1626
|
+
const fragmentBodyStart = shader.fragmentShader.indexOf("void main() {");
|
|
1627
|
+
shader.fragmentShader = "uniform sampler2D tDiffuse;\nuniform float mixStrength;\nvarying vec4 vUvReflection;\n" + shader.fragmentShader.slice(0, fragmentBodyStart) + shader.fragmentShader.slice(fragmentBodyStart, -1) + " vec4 reflectionColor = texture2DProj( tDiffuse, vUvReflection );\n gl_FragColor = vec4( mix( gl_FragColor.rgb, reflectionColor.rgb, mixStrength ), gl_FragColor.a );\n}";
|
|
1628
|
+
};
|
|
1629
|
+
this.receiveShadow = true;
|
|
1630
|
+
this.onBeforeRender = (renderer, scene, camera) => {
|
|
1631
|
+
this.reflectorWorldPosition.setFromMatrixPosition(this.matrixWorld);
|
|
1632
|
+
this.cameraWorldPosition.setFromMatrixPosition(camera.matrixWorld);
|
|
1633
|
+
this.rotationMatrix.extractRotation(this.matrixWorld);
|
|
1634
|
+
this.normal.set(0, 0, 1);
|
|
1635
|
+
this.normal.applyMatrix4(this.rotationMatrix);
|
|
1636
|
+
this.view.subVectors(this.reflectorWorldPosition, this.cameraWorldPosition);
|
|
1637
|
+
if (this.view.dot(this.normal) > 0) return;
|
|
1638
|
+
this.view.reflect(this.normal).negate();
|
|
1639
|
+
this.view.add(this.reflectorWorldPosition);
|
|
1640
|
+
this.rotationMatrix.extractRotation(camera.matrixWorld);
|
|
1641
|
+
this.lookAtPosition.set(0, 0, -1);
|
|
1642
|
+
this.lookAtPosition.applyMatrix4(this.rotationMatrix);
|
|
1643
|
+
this.lookAtPosition.add(this.cameraWorldPosition);
|
|
1644
|
+
this.target.subVectors(this.reflectorWorldPosition, this.lookAtPosition);
|
|
1645
|
+
this.target.reflect(this.normal).negate();
|
|
1646
|
+
this.target.add(this.reflectorWorldPosition);
|
|
1647
|
+
this.virtualCamera.position.copy(this.view);
|
|
1648
|
+
this.virtualCamera.up.set(0, 1, 0);
|
|
1649
|
+
this.virtualCamera.up.applyMatrix4(this.rotationMatrix);
|
|
1650
|
+
this.virtualCamera.up.reflect(this.normal);
|
|
1651
|
+
this.virtualCamera.lookAt(this.target);
|
|
1652
|
+
this.virtualCamera.far = camera.far;
|
|
1653
|
+
this.virtualCamera.updateMatrixWorld();
|
|
1654
|
+
this.virtualCamera.projectionMatrix.copy(camera.projectionMatrix);
|
|
1655
|
+
this.textureMatrix.set(
|
|
1656
|
+
0.5,
|
|
1657
|
+
0,
|
|
1658
|
+
0,
|
|
1659
|
+
0.5,
|
|
1660
|
+
0,
|
|
1661
|
+
0.5,
|
|
1662
|
+
0,
|
|
1663
|
+
0.5,
|
|
1664
|
+
0,
|
|
1665
|
+
0,
|
|
1666
|
+
0.5,
|
|
1667
|
+
0.5,
|
|
1668
|
+
0,
|
|
1669
|
+
0,
|
|
1670
|
+
0,
|
|
1671
|
+
1
|
|
1672
|
+
);
|
|
1673
|
+
this.textureMatrix.multiply(this.virtualCamera.projectionMatrix);
|
|
1674
|
+
this.textureMatrix.multiply(this.virtualCamera.matrixWorldInverse);
|
|
1675
|
+
this.textureMatrix.multiply(this.matrixWorld);
|
|
1676
|
+
this.reflectorPlane.setFromNormalAndCoplanarPoint(this.normal, this.reflectorWorldPosition);
|
|
1677
|
+
this.reflectorPlane.applyMatrix4(this.virtualCamera.matrixWorldInverse);
|
|
1678
|
+
this.clipPlane.set(this.reflectorPlane.normal.x, this.reflectorPlane.normal.y, this.reflectorPlane.normal.z, this.reflectorPlane.constant);
|
|
1679
|
+
const projectionMatrix = this.virtualCamera.projectionMatrix;
|
|
1680
|
+
this.q.x = (Math.sign(this.clipPlane.x) + projectionMatrix.elements[8]) / projectionMatrix.elements[0];
|
|
1681
|
+
this.q.y = (Math.sign(this.clipPlane.y) + projectionMatrix.elements[9]) / projectionMatrix.elements[5];
|
|
1682
|
+
this.q.z = -1;
|
|
1683
|
+
this.q.w = (1 + projectionMatrix.elements[10]) / projectionMatrix.elements[14];
|
|
1684
|
+
this.clipPlane.multiplyScalar(2 / this.clipPlane.dot(this.q));
|
|
1685
|
+
projectionMatrix.elements[2] = this.clipPlane.x;
|
|
1686
|
+
projectionMatrix.elements[6] = this.clipPlane.y;
|
|
1687
|
+
projectionMatrix.elements[10] = this.clipPlane.z + 1 - clipBias;
|
|
1688
|
+
projectionMatrix.elements[14] = this.clipPlane.w;
|
|
1689
|
+
this.visible = false;
|
|
1690
|
+
const currentRenderTarget = renderer.getRenderTarget();
|
|
1691
|
+
const currentXrEnabled = renderer.xr.enabled;
|
|
1692
|
+
const currentShadowAutoUpdate = renderer.shadowMap.autoUpdate;
|
|
1693
|
+
renderer.xr.enabled = false;
|
|
1694
|
+
renderer.shadowMap.autoUpdate = false;
|
|
1695
|
+
renderer.setRenderTarget(this.renderTarget);
|
|
1696
|
+
renderer.state.buffers.depth.setMask(true);
|
|
1697
|
+
if (renderer.autoClear === false) renderer.clear();
|
|
1698
|
+
renderer.render(scene, this.virtualCamera);
|
|
1699
|
+
renderer.xr.enabled = currentXrEnabled;
|
|
1700
|
+
renderer.shadowMap.autoUpdate = currentShadowAutoUpdate;
|
|
1701
|
+
renderer.setRenderTarget(currentRenderTarget);
|
|
1702
|
+
const viewport = camera.viewport;
|
|
1703
|
+
if (viewport !== void 0) {
|
|
1704
|
+
renderer.state.viewport(viewport);
|
|
1705
|
+
}
|
|
1706
|
+
this.visible = true;
|
|
1707
|
+
};
|
|
1708
|
+
}
|
|
1709
|
+
getRenderTarget() {
|
|
1710
|
+
return this.renderTarget;
|
|
1711
|
+
}
|
|
1712
|
+
dispose() {
|
|
1713
|
+
this.renderTarget.dispose();
|
|
1714
|
+
const mesh = this;
|
|
1715
|
+
if (Array.isArray(mesh.material)) {
|
|
1716
|
+
mesh.material.forEach((m) => m.dispose());
|
|
1717
|
+
} else {
|
|
1718
|
+
mesh.material.dispose();
|
|
1719
|
+
}
|
|
1720
|
+
}
|
|
1721
|
+
};
|
|
1722
|
+
|
|
1723
|
+
// src/rendering/GeomBuilder.ts
|
|
1724
|
+
var GeomBuilder = class {
|
|
1725
|
+
mujoco;
|
|
1726
|
+
constructor(mujoco) {
|
|
1727
|
+
this.mujoco = mujoco;
|
|
1728
|
+
}
|
|
1729
|
+
/**
|
|
1730
|
+
* Creates a Three.js Object3D (usually a Mesh) for a specific geometry in the MuJoCo model.
|
|
1731
|
+
* Returns null if the geometry shouldn't be rendered (e.g., invisible collision triggers).
|
|
1732
|
+
*/
|
|
1733
|
+
create(mjModel, g) {
|
|
1734
|
+
if (mjModel.geom_group[g] === 3) return null;
|
|
1735
|
+
const type = mjModel.geom_type[g];
|
|
1736
|
+
const size = mjModel.geom_size.subarray(g * 3, g * 3 + 3);
|
|
1737
|
+
const pos = mjModel.geom_pos.subarray(g * 3, g * 3 + 3);
|
|
1738
|
+
const quat = mjModel.geom_quat.subarray(g * 4, g * 4 + 4);
|
|
1739
|
+
const matId = mjModel.geom_matid[g];
|
|
1740
|
+
const color = new THREE.Color(16777215);
|
|
1741
|
+
let opacity = 1;
|
|
1742
|
+
if (matId >= 0) {
|
|
1743
|
+
const rgba = mjModel.mat_rgba.subarray(matId * 4, matId * 4 + 4);
|
|
1744
|
+
color.setRGB(rgba[0], rgba[1], rgba[2]);
|
|
1745
|
+
opacity = rgba[3];
|
|
1746
|
+
} else {
|
|
1747
|
+
const rgba = mjModel.geom_rgba.subarray(g * 4, g * 4 + 4);
|
|
1748
|
+
color.setRGB(rgba[0], rgba[1], rgba[2]);
|
|
1749
|
+
opacity = rgba[3];
|
|
1750
|
+
}
|
|
1751
|
+
const MG = this.mujoco.mjtGeom;
|
|
1752
|
+
let geo = null;
|
|
1753
|
+
const getVal = (v) => v?.value ?? v;
|
|
1754
|
+
if (type === getVal(MG.mjGEOM_PLANE)) {
|
|
1755
|
+
geo = new THREE.PlaneGeometry(size[0] * 2 || 5, size[1] * 2 || 5);
|
|
1756
|
+
} else if (type === getVal(MG.mjGEOM_SPHERE)) {
|
|
1757
|
+
geo = new THREE.SphereGeometry(size[0], 24, 24);
|
|
1758
|
+
} else if (type === getVal(MG.mjGEOM_CAPSULE)) {
|
|
1759
|
+
geo = new CapsuleGeometry(size[0], size[1] * 2, 24, 12);
|
|
1760
|
+
geo.rotateX(Math.PI / 2);
|
|
1761
|
+
} else if (type === getVal(MG.mjGEOM_BOX)) {
|
|
1762
|
+
geo = new THREE.BoxGeometry(size[0] * 2, size[1] * 2, size[2] * 2);
|
|
1763
|
+
} else if (type === getVal(MG.mjGEOM_CYLINDER)) {
|
|
1764
|
+
geo = new THREE.CylinderGeometry(size[0], size[0], size[1] * 2, 24);
|
|
1765
|
+
geo.rotateX(Math.PI / 2);
|
|
1766
|
+
} else if (type === getVal(MG.mjGEOM_MESH)) {
|
|
1767
|
+
const mId = mjModel.geom_dataid[g];
|
|
1768
|
+
const vAdr = mjModel.mesh_vertadr[mId];
|
|
1769
|
+
const vNum = mjModel.mesh_vertnum[mId];
|
|
1770
|
+
const fAdr = mjModel.mesh_faceadr[mId];
|
|
1771
|
+
const fNum = mjModel.mesh_facenum[mId];
|
|
1772
|
+
geo = new THREE.BufferGeometry();
|
|
1773
|
+
geo.setAttribute("position", new THREE.Float32BufferAttribute(mjModel.mesh_vert.subarray(vAdr * 3, (vAdr + vNum) * 3), 3));
|
|
1774
|
+
geo.setIndex(Array.from(mjModel.mesh_face.subarray(fAdr * 3, (fAdr + fNum) * 3)));
|
|
1775
|
+
geo.computeVertexNormals();
|
|
1776
|
+
}
|
|
1777
|
+
if (geo) {
|
|
1778
|
+
let mesh;
|
|
1779
|
+
if (type === getVal(MG.mjGEOM_PLANE)) {
|
|
1780
|
+
mesh = new Reflector(geo, {
|
|
1781
|
+
clipBias: 3e-3,
|
|
1782
|
+
textureWidth: 1024,
|
|
1783
|
+
textureHeight: 1024,
|
|
1784
|
+
color,
|
|
1785
|
+
mixStrength: 0.25
|
|
1786
|
+
});
|
|
1787
|
+
} else {
|
|
1788
|
+
mesh = new THREE.Mesh(geo, new THREE.MeshStandardMaterial({
|
|
1789
|
+
color,
|
|
1790
|
+
transparent: opacity < 1,
|
|
1791
|
+
opacity,
|
|
1792
|
+
roughness: 0.6,
|
|
1793
|
+
metalness: 0.2
|
|
1794
|
+
}));
|
|
1795
|
+
mesh.castShadow = true;
|
|
1796
|
+
mesh.receiveShadow = true;
|
|
1797
|
+
}
|
|
1798
|
+
mesh.position.set(pos[0], pos[1], pos[2]);
|
|
1799
|
+
mesh.quaternion.set(quat[1], quat[2], quat[3], quat[0]);
|
|
1800
|
+
mesh.userData.bodyID = mjModel.geom_bodyid[g];
|
|
1801
|
+
mesh.userData.geomID = g;
|
|
1802
|
+
return mesh;
|
|
1803
|
+
}
|
|
1804
|
+
return null;
|
|
1805
|
+
}
|
|
1806
|
+
};
|
|
1807
|
+
function SceneRenderer() {
|
|
1808
|
+
const { mjModelRef, mjDataRef, mujocoRef, onSelectionRef, status } = useMujocoSim();
|
|
1809
|
+
const groupRef = useRef(null);
|
|
1810
|
+
const bodyRefs = useRef([]);
|
|
1811
|
+
const prevModelRef = useRef(null);
|
|
1812
|
+
const geomBuilder = useMemo(() => {
|
|
1813
|
+
return new GeomBuilder(mujocoRef.current);
|
|
1814
|
+
}, [mujocoRef.current]);
|
|
1815
|
+
useEffect(() => {
|
|
1816
|
+
if (status !== "ready") return;
|
|
1817
|
+
const model = mjModelRef.current;
|
|
1818
|
+
const group = groupRef.current;
|
|
1819
|
+
if (!model || !group) return;
|
|
1820
|
+
if (prevModelRef.current === model) return;
|
|
1821
|
+
prevModelRef.current = model;
|
|
1822
|
+
while (group.children.length > 0) {
|
|
1823
|
+
group.remove(group.children[0]);
|
|
1824
|
+
}
|
|
1825
|
+
const refs = [];
|
|
1826
|
+
for (let i = 0; i < model.nbody; i++) {
|
|
1827
|
+
const bodyGroup = new THREE.Group();
|
|
1828
|
+
bodyGroup.userData.bodyID = i;
|
|
1829
|
+
for (let g = 0; g < model.ngeom; g++) {
|
|
1830
|
+
if (model.geom_bodyid[g] === i) {
|
|
1831
|
+
const mesh = geomBuilder.create(model, g);
|
|
1832
|
+
if (mesh) bodyGroup.add(mesh);
|
|
1833
|
+
}
|
|
1834
|
+
}
|
|
1835
|
+
group.add(bodyGroup);
|
|
1836
|
+
refs.push(bodyGroup);
|
|
1837
|
+
}
|
|
1838
|
+
bodyRefs.current = refs;
|
|
1839
|
+
}, [status, geomBuilder, mjModelRef]);
|
|
1840
|
+
useFrame(() => {
|
|
1841
|
+
const data = mjDataRef.current;
|
|
1842
|
+
if (!data) return;
|
|
1843
|
+
const bodies = bodyRefs.current;
|
|
1844
|
+
for (let i = 0; i < bodies.length; i++) {
|
|
1845
|
+
const ref = bodies[i];
|
|
1846
|
+
if (!ref) continue;
|
|
1847
|
+
ref.position.set(
|
|
1848
|
+
data.xpos[i * 3],
|
|
1849
|
+
data.xpos[i * 3 + 1],
|
|
1850
|
+
data.xpos[i * 3 + 2]
|
|
1851
|
+
);
|
|
1852
|
+
ref.quaternion.set(
|
|
1853
|
+
data.xquat[i * 4 + 1],
|
|
1854
|
+
data.xquat[i * 4 + 2],
|
|
1855
|
+
data.xquat[i * 4 + 3],
|
|
1856
|
+
data.xquat[i * 4]
|
|
1857
|
+
);
|
|
1858
|
+
}
|
|
1859
|
+
});
|
|
1860
|
+
return /* @__PURE__ */ jsx(
|
|
1861
|
+
"group",
|
|
1862
|
+
{
|
|
1863
|
+
ref: groupRef,
|
|
1864
|
+
onDoubleClick: (e) => {
|
|
1865
|
+
e.stopPropagation();
|
|
1866
|
+
let obj = e.object;
|
|
1867
|
+
while (obj && obj.userData.bodyID === void 0 && obj.parent) {
|
|
1868
|
+
obj = obj.parent;
|
|
1869
|
+
}
|
|
1870
|
+
if (obj && obj.userData.bodyID !== void 0 && obj.userData.bodyID > 0) {
|
|
1871
|
+
const model = mjModelRef.current;
|
|
1872
|
+
if (model && onSelectionRef.current) {
|
|
1873
|
+
const name = getName(model, model.name_bodyadr[obj.userData.bodyID]);
|
|
1874
|
+
onSelectionRef.current(obj.userData.bodyID, name);
|
|
1875
|
+
}
|
|
1876
|
+
}
|
|
1877
|
+
}
|
|
1878
|
+
}
|
|
1879
|
+
);
|
|
1880
|
+
}
|
|
1881
|
+
var _mat4 = new THREE.Matrix4();
|
|
1882
|
+
var _pos = new THREE.Vector3();
|
|
1883
|
+
var _quat = new THREE.Quaternion();
|
|
1884
|
+
var _scale = new THREE.Vector3(1, 1, 1);
|
|
1885
|
+
function IkGizmo({ siteName, scale = 0.18, onDrag }) {
|
|
1886
|
+
const {
|
|
1887
|
+
ikTargetRef,
|
|
1888
|
+
mjModelRef,
|
|
1889
|
+
mjDataRef,
|
|
1890
|
+
siteIdRef,
|
|
1891
|
+
api,
|
|
1892
|
+
ikEnabledRef,
|
|
1893
|
+
status
|
|
1894
|
+
} = useMujocoSim();
|
|
1895
|
+
const wrapperRef = useRef(null);
|
|
1896
|
+
const pivotRef = useRef(null);
|
|
1897
|
+
const draggingRef = useRef(false);
|
|
1898
|
+
const localSiteIdRef = useRef(-1);
|
|
1899
|
+
const { controls } = useThree();
|
|
1900
|
+
useEffect(() => {
|
|
1901
|
+
const model = mjModelRef.current;
|
|
1902
|
+
if (!model || status !== "ready") {
|
|
1903
|
+
localSiteIdRef.current = -1;
|
|
1904
|
+
return;
|
|
1905
|
+
}
|
|
1906
|
+
if (siteName) {
|
|
1907
|
+
localSiteIdRef.current = findSiteByName(model, siteName);
|
|
1908
|
+
} else {
|
|
1909
|
+
localSiteIdRef.current = siteIdRef.current;
|
|
1910
|
+
}
|
|
1911
|
+
}, [siteName, status, mjModelRef, siteIdRef]);
|
|
1912
|
+
useFrame(() => {
|
|
1913
|
+
const data = mjDataRef.current;
|
|
1914
|
+
const sid = localSiteIdRef.current;
|
|
1915
|
+
if (!data || sid < 0 || !wrapperRef.current) return;
|
|
1916
|
+
if (!draggingRef.current) {
|
|
1917
|
+
const p = data.site_xpos;
|
|
1918
|
+
const m = data.site_xmat;
|
|
1919
|
+
const i3 = sid * 3;
|
|
1920
|
+
const i9 = sid * 9;
|
|
1921
|
+
wrapperRef.current.position.set(p[i3], p[i3 + 1], p[i3 + 2]);
|
|
1922
|
+
_mat4.set(
|
|
1923
|
+
m[i9],
|
|
1924
|
+
m[i9 + 1],
|
|
1925
|
+
m[i9 + 2],
|
|
1926
|
+
0,
|
|
1927
|
+
m[i9 + 3],
|
|
1928
|
+
m[i9 + 4],
|
|
1929
|
+
m[i9 + 5],
|
|
1930
|
+
0,
|
|
1931
|
+
m[i9 + 6],
|
|
1932
|
+
m[i9 + 7],
|
|
1933
|
+
m[i9 + 8],
|
|
1934
|
+
0,
|
|
1935
|
+
0,
|
|
1936
|
+
0,
|
|
1937
|
+
0,
|
|
1938
|
+
1
|
|
1939
|
+
);
|
|
1940
|
+
wrapperRef.current.quaternion.setFromRotationMatrix(_mat4);
|
|
1941
|
+
if (pivotRef.current) {
|
|
1942
|
+
pivotRef.current.matrix.identity();
|
|
1943
|
+
}
|
|
1944
|
+
}
|
|
1945
|
+
});
|
|
1946
|
+
if (status !== "ready") return null;
|
|
1947
|
+
return /* @__PURE__ */ jsx("group", { ref: wrapperRef, children: /* @__PURE__ */ jsx(
|
|
1948
|
+
PivotControls,
|
|
1949
|
+
{
|
|
1950
|
+
ref: pivotRef,
|
|
1951
|
+
autoTransform: true,
|
|
1952
|
+
scale,
|
|
1953
|
+
fixed: false,
|
|
1954
|
+
depthTest: false,
|
|
1955
|
+
disableScaling: true,
|
|
1956
|
+
onDragStart: () => {
|
|
1957
|
+
draggingRef.current = true;
|
|
1958
|
+
if (!onDrag) {
|
|
1959
|
+
if (!ikEnabledRef.current) api.setIkEnabled(true);
|
|
1960
|
+
}
|
|
1961
|
+
if (controls) controls.enabled = false;
|
|
1962
|
+
},
|
|
1963
|
+
onDragEnd: () => {
|
|
1964
|
+
draggingRef.current = false;
|
|
1965
|
+
if (pivotRef.current) {
|
|
1966
|
+
pivotRef.current.matrix.identity();
|
|
1967
|
+
pivotRef.current.matrixWorldNeedsUpdate = true;
|
|
1968
|
+
}
|
|
1969
|
+
if (controls) controls.enabled = true;
|
|
1970
|
+
},
|
|
1971
|
+
onDrag: (_l, _dl, world) => {
|
|
1972
|
+
world.decompose(_pos, _quat, _scale);
|
|
1973
|
+
if (onDrag) {
|
|
1974
|
+
onDrag(_pos.clone(), _quat.clone());
|
|
1975
|
+
} else {
|
|
1976
|
+
const target = ikTargetRef.current;
|
|
1977
|
+
if (target) {
|
|
1978
|
+
target.position.copy(_pos);
|
|
1979
|
+
target.quaternion.copy(_quat);
|
|
1980
|
+
}
|
|
1981
|
+
}
|
|
1982
|
+
},
|
|
1983
|
+
children: /* @__PURE__ */ jsx("mesh", { visible: false, children: /* @__PURE__ */ jsx("sphereGeometry", { args: [1e-3] }) })
|
|
1984
|
+
}
|
|
1985
|
+
) });
|
|
1986
|
+
}
|
|
1987
|
+
var _dummy = new THREE.Object3D();
|
|
1988
|
+
function ContactMarkers({
|
|
1989
|
+
maxContacts = 100,
|
|
1990
|
+
radius = 5e-3,
|
|
1991
|
+
color = "#4f46e5",
|
|
1992
|
+
visible = true
|
|
1993
|
+
} = {}) {
|
|
1994
|
+
const { mjDataRef, status } = useMujocoSim();
|
|
1995
|
+
const meshRef = useRef(null);
|
|
1996
|
+
useFrame(() => {
|
|
1997
|
+
const mesh = meshRef.current;
|
|
1998
|
+
const data = mjDataRef.current;
|
|
1999
|
+
if (!mesh || !data || !visible) {
|
|
2000
|
+
if (mesh) mesh.count = 0;
|
|
2001
|
+
return;
|
|
2002
|
+
}
|
|
2003
|
+
const ncon = data.ncon;
|
|
2004
|
+
const count = Math.min(ncon, maxContacts);
|
|
2005
|
+
for (let i = 0; i < count; i++) {
|
|
2006
|
+
try {
|
|
2007
|
+
const c = data.contact.get(i);
|
|
2008
|
+
if (!c) break;
|
|
2009
|
+
_dummy.position.set(c.pos[0], c.pos[1], c.pos[2]);
|
|
2010
|
+
_dummy.updateMatrix();
|
|
2011
|
+
mesh.setMatrixAt(i, _dummy.matrix);
|
|
2012
|
+
} catch {
|
|
2013
|
+
mesh.count = i;
|
|
2014
|
+
mesh.instanceMatrix.needsUpdate = true;
|
|
2015
|
+
return;
|
|
2016
|
+
}
|
|
2017
|
+
}
|
|
2018
|
+
mesh.count = count;
|
|
2019
|
+
mesh.instanceMatrix.needsUpdate = true;
|
|
2020
|
+
});
|
|
2021
|
+
if (status !== "ready") return null;
|
|
2022
|
+
return /* @__PURE__ */ jsxs("instancedMesh", { ref: meshRef, args: [void 0, void 0, maxContacts], children: [
|
|
2023
|
+
/* @__PURE__ */ jsx("sphereGeometry", { args: [radius, 8, 8] }),
|
|
2024
|
+
/* @__PURE__ */ jsx(
|
|
2025
|
+
"meshStandardMaterial",
|
|
2026
|
+
{
|
|
2027
|
+
color,
|
|
2028
|
+
emissive: color,
|
|
2029
|
+
emissiveIntensity: 0.3,
|
|
2030
|
+
roughness: 0.5
|
|
2031
|
+
}
|
|
2032
|
+
)
|
|
2033
|
+
] });
|
|
2034
|
+
}
|
|
2035
|
+
var _force = new Float64Array(3);
|
|
2036
|
+
var _torque = new Float64Array(3);
|
|
2037
|
+
var _point = new Float64Array(3);
|
|
2038
|
+
var _bodyPos = new THREE.Vector3();
|
|
2039
|
+
var _bodyQuat = new THREE.Quaternion();
|
|
2040
|
+
var _worldHit = new THREE.Vector3();
|
|
2041
|
+
var _raycaster = new THREE.Raycaster();
|
|
2042
|
+
var _mouse = new THREE.Vector2();
|
|
2043
|
+
function DragInteraction({
|
|
2044
|
+
stiffness = 250,
|
|
2045
|
+
showArrow = true
|
|
2046
|
+
}) {
|
|
2047
|
+
const { mjDataRef, mujocoRef, mjModelRef, status } = useMujocoSim();
|
|
2048
|
+
const { gl, camera, scene, controls } = useThree();
|
|
2049
|
+
const draggingRef = useRef(false);
|
|
2050
|
+
const bodyIdRef = useRef(-1);
|
|
2051
|
+
const grabDistanceRef = useRef(0);
|
|
2052
|
+
const localHitRef = useRef(new THREE.Vector3());
|
|
2053
|
+
const grabWorldRef = useRef(new THREE.Vector3());
|
|
2054
|
+
const mouseWorldRef = useRef(new THREE.Vector3());
|
|
2055
|
+
const arrowRef = useRef(null);
|
|
2056
|
+
const groupRef = useRef(null);
|
|
2057
|
+
useEffect(() => {
|
|
2058
|
+
if (!showArrow || !groupRef.current) return;
|
|
2059
|
+
const arrow = new THREE.ArrowHelper(
|
|
2060
|
+
new THREE.Vector3(0, 1, 0),
|
|
2061
|
+
new THREE.Vector3(),
|
|
2062
|
+
0.1,
|
|
2063
|
+
16729156
|
|
2064
|
+
);
|
|
2065
|
+
arrow.visible = false;
|
|
2066
|
+
arrow.line.material.transparent = true;
|
|
2067
|
+
arrow.line.material.opacity = 0.6;
|
|
2068
|
+
arrow.cone.material.transparent = true;
|
|
2069
|
+
arrow.cone.material.opacity = 0.6;
|
|
2070
|
+
groupRef.current.add(arrow);
|
|
2071
|
+
arrowRef.current = arrow;
|
|
2072
|
+
return () => {
|
|
2073
|
+
if (groupRef.current) groupRef.current.remove(arrow);
|
|
2074
|
+
arrow.dispose();
|
|
2075
|
+
arrowRef.current = null;
|
|
2076
|
+
};
|
|
2077
|
+
}, [showArrow]);
|
|
2078
|
+
useEffect(() => {
|
|
2079
|
+
const canvas = gl.domElement;
|
|
2080
|
+
const onPointerDown = (evt) => {
|
|
2081
|
+
if (evt.button !== 0) return;
|
|
2082
|
+
if (!evt.ctrlKey && !evt.metaKey) return;
|
|
2083
|
+
const rect = canvas.getBoundingClientRect();
|
|
2084
|
+
_mouse.set(
|
|
2085
|
+
(evt.clientX - rect.left) / rect.width * 2 - 1,
|
|
2086
|
+
-((evt.clientY - rect.top) / rect.height) * 2 + 1
|
|
2087
|
+
);
|
|
2088
|
+
_raycaster.setFromCamera(_mouse, camera);
|
|
2089
|
+
const hits = _raycaster.intersectObjects(scene.children, true);
|
|
2090
|
+
for (const hit of hits) {
|
|
2091
|
+
let obj = hit.object;
|
|
2092
|
+
while (obj && obj.userData.bodyID === void 0 && obj.parent) {
|
|
2093
|
+
obj = obj.parent;
|
|
2094
|
+
}
|
|
2095
|
+
const bid = obj?.userData.bodyID;
|
|
2096
|
+
if (bid !== void 0 && bid > 0) {
|
|
2097
|
+
bodyIdRef.current = bid;
|
|
2098
|
+
draggingRef.current = true;
|
|
2099
|
+
grabDistanceRef.current = hit.distance;
|
|
2100
|
+
const data = mjDataRef.current;
|
|
2101
|
+
if (data) {
|
|
2102
|
+
const i3 = bid * 3;
|
|
2103
|
+
const i4 = bid * 4;
|
|
2104
|
+
_bodyPos.set(data.xpos[i3], data.xpos[i3 + 1], data.xpos[i3 + 2]);
|
|
2105
|
+
_bodyQuat.set(
|
|
2106
|
+
data.xquat[i4 + 1],
|
|
2107
|
+
data.xquat[i4 + 2],
|
|
2108
|
+
data.xquat[i4 + 3],
|
|
2109
|
+
data.xquat[i4]
|
|
2110
|
+
);
|
|
2111
|
+
localHitRef.current.copy(hit.point).sub(_bodyPos);
|
|
2112
|
+
localHitRef.current.applyQuaternion(_bodyQuat.clone().invert());
|
|
2113
|
+
}
|
|
2114
|
+
mouseWorldRef.current.copy(hit.point);
|
|
2115
|
+
grabWorldRef.current.copy(hit.point);
|
|
2116
|
+
if (controls) controls.enabled = false;
|
|
2117
|
+
break;
|
|
2118
|
+
}
|
|
2119
|
+
}
|
|
2120
|
+
};
|
|
2121
|
+
const onPointerMove = (evt) => {
|
|
2122
|
+
if (!draggingRef.current) return;
|
|
2123
|
+
if (evt.buttons === 0) {
|
|
2124
|
+
draggingRef.current = false;
|
|
2125
|
+
bodyIdRef.current = -1;
|
|
2126
|
+
if (controls) controls.enabled = true;
|
|
2127
|
+
return;
|
|
2128
|
+
}
|
|
2129
|
+
const rect = canvas.getBoundingClientRect();
|
|
2130
|
+
_mouse.set(
|
|
2131
|
+
(evt.clientX - rect.left) / rect.width * 2 - 1,
|
|
2132
|
+
-((evt.clientY - rect.top) / rect.height) * 2 + 1
|
|
2133
|
+
);
|
|
2134
|
+
_raycaster.setFromCamera(_mouse, camera);
|
|
2135
|
+
mouseWorldRef.current.copy(_raycaster.ray.origin).addScaledVector(_raycaster.ray.direction, grabDistanceRef.current);
|
|
2136
|
+
};
|
|
2137
|
+
const onPointerUp = () => {
|
|
2138
|
+
if (!draggingRef.current) return;
|
|
2139
|
+
draggingRef.current = false;
|
|
2140
|
+
bodyIdRef.current = -1;
|
|
2141
|
+
if (controls) controls.enabled = true;
|
|
2142
|
+
};
|
|
2143
|
+
canvas.addEventListener("pointerdown", onPointerDown);
|
|
2144
|
+
canvas.addEventListener("pointermove", onPointerMove);
|
|
2145
|
+
window.addEventListener("pointerup", onPointerUp);
|
|
2146
|
+
window.addEventListener("pointercancel", onPointerUp);
|
|
2147
|
+
return () => {
|
|
2148
|
+
canvas.removeEventListener("pointerdown", onPointerDown);
|
|
2149
|
+
canvas.removeEventListener("pointermove", onPointerMove);
|
|
2150
|
+
window.removeEventListener("pointerup", onPointerUp);
|
|
2151
|
+
window.removeEventListener("pointercancel", onPointerUp);
|
|
2152
|
+
};
|
|
2153
|
+
}, [gl, camera, scene, controls, mjDataRef]);
|
|
2154
|
+
useBeforePhysicsStep((model, data) => {
|
|
2155
|
+
if (!draggingRef.current || bodyIdRef.current <= 0) return;
|
|
2156
|
+
const bid = bodyIdRef.current;
|
|
2157
|
+
const mujoco = mujocoRef.current;
|
|
2158
|
+
const i3 = bid * 3;
|
|
2159
|
+
const i4 = bid * 4;
|
|
2160
|
+
_bodyPos.set(data.xpos[i3], data.xpos[i3 + 1], data.xpos[i3 + 2]);
|
|
2161
|
+
_bodyQuat.set(
|
|
2162
|
+
data.xquat[i4 + 1],
|
|
2163
|
+
data.xquat[i4 + 2],
|
|
2164
|
+
data.xquat[i4 + 3],
|
|
2165
|
+
data.xquat[i4]
|
|
2166
|
+
);
|
|
2167
|
+
_worldHit.copy(localHitRef.current);
|
|
2168
|
+
_worldHit.applyQuaternion(_bodyQuat);
|
|
2169
|
+
_worldHit.add(_bodyPos);
|
|
2170
|
+
grabWorldRef.current.copy(_worldHit);
|
|
2171
|
+
const mass = model.body_mass[bid];
|
|
2172
|
+
const s = stiffness * mass;
|
|
2173
|
+
_force[0] = (mouseWorldRef.current.x - _worldHit.x) * s;
|
|
2174
|
+
_force[1] = (mouseWorldRef.current.y - _worldHit.y) * s;
|
|
2175
|
+
_force[2] = (mouseWorldRef.current.z - _worldHit.z) * s;
|
|
2176
|
+
_point[0] = _worldHit.x;
|
|
2177
|
+
_point[1] = _worldHit.y;
|
|
2178
|
+
_point[2] = _worldHit.z;
|
|
2179
|
+
_torque[0] = 0;
|
|
2180
|
+
_torque[1] = 0;
|
|
2181
|
+
_torque[2] = 0;
|
|
2182
|
+
mujoco.mj_applyFT(model, data, _force, _torque, _point, bid, data.qfrc_applied);
|
|
2183
|
+
});
|
|
2184
|
+
useFrame(() => {
|
|
2185
|
+
const arrow = arrowRef.current;
|
|
2186
|
+
if (!arrow) return;
|
|
2187
|
+
if (draggingRef.current && bodyIdRef.current > 0) {
|
|
2188
|
+
arrow.visible = true;
|
|
2189
|
+
const dir = mouseWorldRef.current.clone().sub(grabWorldRef.current);
|
|
2190
|
+
const len = dir.length();
|
|
2191
|
+
if (len > 1e-3) {
|
|
2192
|
+
dir.normalize();
|
|
2193
|
+
arrow.position.copy(grabWorldRef.current);
|
|
2194
|
+
arrow.setDirection(dir);
|
|
2195
|
+
arrow.setLength(len, Math.min(len * 0.2, 0.05), Math.min(len * 0.1, 0.03));
|
|
2196
|
+
}
|
|
2197
|
+
} else {
|
|
2198
|
+
arrow.visible = false;
|
|
2199
|
+
}
|
|
2200
|
+
});
|
|
2201
|
+
if (status !== "ready") return null;
|
|
2202
|
+
return /* @__PURE__ */ jsx("group", { ref: groupRef });
|
|
2203
|
+
}
|
|
2204
|
+
function SceneLights({ intensity = 1 }) {
|
|
2205
|
+
const { mjModelRef, status } = useMujocoSim();
|
|
2206
|
+
const { scene } = useThree();
|
|
2207
|
+
const lightsRef = useRef([]);
|
|
2208
|
+
const targetsRef = useRef([]);
|
|
2209
|
+
useEffect(() => {
|
|
2210
|
+
const model = mjModelRef.current;
|
|
2211
|
+
if (!model || status !== "ready") return;
|
|
2212
|
+
for (const light of lightsRef.current) {
|
|
2213
|
+
scene.remove(light);
|
|
2214
|
+
light.dispose();
|
|
2215
|
+
}
|
|
2216
|
+
for (const t of targetsRef.current) scene.remove(t);
|
|
2217
|
+
lightsRef.current = [];
|
|
2218
|
+
targetsRef.current = [];
|
|
2219
|
+
const nlight = model.nlight ?? 0;
|
|
2220
|
+
if (nlight === 0) return;
|
|
2221
|
+
for (let i = 0; i < nlight; i++) {
|
|
2222
|
+
const active = model.light_active ? model.light_active[i] : 1;
|
|
2223
|
+
if (!active) continue;
|
|
2224
|
+
const lightType = model.light_type ? model.light_type[i] : 0;
|
|
2225
|
+
const isDirectional = lightType === 0;
|
|
2226
|
+
const castShadow = model.light_castshadow ? model.light_castshadow[i] !== 0 : false;
|
|
2227
|
+
const mjIntensity = model.light_intensity ? model.light_intensity[i] : 1;
|
|
2228
|
+
const finalIntensity = intensity * mjIntensity;
|
|
2229
|
+
const dr = model.light_diffuse ? model.light_diffuse[3 * i] : 1;
|
|
2230
|
+
const dg = model.light_diffuse ? model.light_diffuse[3 * i + 1] : 1;
|
|
2231
|
+
const db = model.light_diffuse ? model.light_diffuse[3 * i + 2] : 1;
|
|
2232
|
+
const color = new THREE.Color(dr, dg, db);
|
|
2233
|
+
const px = model.light_pos[3 * i];
|
|
2234
|
+
const py = model.light_pos[3 * i + 1];
|
|
2235
|
+
const pz = model.light_pos[3 * i + 2];
|
|
2236
|
+
const dx = model.light_dir[3 * i];
|
|
2237
|
+
const dy = model.light_dir[3 * i + 1];
|
|
2238
|
+
const dz = model.light_dir[3 * i + 2];
|
|
2239
|
+
if (isDirectional) {
|
|
2240
|
+
const light = new THREE.DirectionalLight(color, finalIntensity);
|
|
2241
|
+
light.position.set(px, py, pz);
|
|
2242
|
+
light.target.position.set(px + dx, py + dy, pz + dz);
|
|
2243
|
+
light.castShadow = castShadow;
|
|
2244
|
+
if (castShadow) {
|
|
2245
|
+
light.shadow.mapSize.width = 1024;
|
|
2246
|
+
light.shadow.mapSize.height = 1024;
|
|
2247
|
+
light.shadow.camera.near = 0.1;
|
|
2248
|
+
light.shadow.camera.far = 50;
|
|
2249
|
+
const d = 5;
|
|
2250
|
+
light.shadow.camera.left = -d;
|
|
2251
|
+
light.shadow.camera.right = d;
|
|
2252
|
+
light.shadow.camera.top = d;
|
|
2253
|
+
light.shadow.camera.bottom = -d;
|
|
2254
|
+
}
|
|
2255
|
+
scene.add(light);
|
|
2256
|
+
scene.add(light.target);
|
|
2257
|
+
lightsRef.current.push(light);
|
|
2258
|
+
targetsRef.current.push(light.target);
|
|
2259
|
+
} else {
|
|
2260
|
+
const cutoff = model.light_cutoff ? model.light_cutoff[i] : 45;
|
|
2261
|
+
const exponent = model.light_exponent ? model.light_exponent[i] : 10;
|
|
2262
|
+
const angle = cutoff * Math.PI / 180;
|
|
2263
|
+
const light = new THREE.SpotLight(color, finalIntensity, 0, angle, exponent / 128);
|
|
2264
|
+
light.position.set(px, py, pz);
|
|
2265
|
+
light.target.position.set(px + dx, py + dy, pz + dz);
|
|
2266
|
+
light.castShadow = castShadow;
|
|
2267
|
+
if (model.light_attenuation) {
|
|
2268
|
+
const att1 = model.light_attenuation[3 * i + 1];
|
|
2269
|
+
const att2 = model.light_attenuation[3 * i + 2];
|
|
2270
|
+
light.decay = att2 > 0 ? 2 : att1 > 0 ? 1 : 0;
|
|
2271
|
+
light.distance = att1 > 0 ? 1 / att1 : 0;
|
|
2272
|
+
}
|
|
2273
|
+
if (castShadow) {
|
|
2274
|
+
light.shadow.mapSize.width = 512;
|
|
2275
|
+
light.shadow.mapSize.height = 512;
|
|
2276
|
+
}
|
|
2277
|
+
scene.add(light);
|
|
2278
|
+
scene.add(light.target);
|
|
2279
|
+
lightsRef.current.push(light);
|
|
2280
|
+
targetsRef.current.push(light.target);
|
|
2281
|
+
}
|
|
2282
|
+
}
|
|
2283
|
+
return () => {
|
|
2284
|
+
for (const light of lightsRef.current) {
|
|
2285
|
+
scene.remove(light);
|
|
2286
|
+
light.dispose();
|
|
2287
|
+
}
|
|
2288
|
+
for (const t of targetsRef.current) scene.remove(t);
|
|
2289
|
+
lightsRef.current = [];
|
|
2290
|
+
targetsRef.current = [];
|
|
2291
|
+
};
|
|
2292
|
+
}, [status, mjModelRef, scene, intensity]);
|
|
2293
|
+
return null;
|
|
2294
|
+
}
|
|
2295
|
+
var JOINT_COLORS = {
|
|
2296
|
+
0: 16711680,
|
|
2297
|
+
// free - red
|
|
2298
|
+
1: 65280,
|
|
2299
|
+
// ball - green
|
|
2300
|
+
2: 255,
|
|
2301
|
+
// slide - blue
|
|
2302
|
+
3: 16776960
|
|
2303
|
+
// hinge - yellow
|
|
2304
|
+
};
|
|
2305
|
+
function Debug({
|
|
2306
|
+
showGeoms = false,
|
|
2307
|
+
showSites = false,
|
|
2308
|
+
showJoints = false,
|
|
2309
|
+
showContacts = false,
|
|
2310
|
+
showCOM = false,
|
|
2311
|
+
showInertia = false,
|
|
2312
|
+
showTendons = false
|
|
2313
|
+
}) {
|
|
2314
|
+
const { mjModelRef, mjDataRef, status } = useMujocoSim();
|
|
2315
|
+
const { scene } = useThree();
|
|
2316
|
+
const groupRef = useRef(null);
|
|
2317
|
+
const debugGeometry = useMemo(() => {
|
|
2318
|
+
const model = mjModelRef.current;
|
|
2319
|
+
if (!model || status !== "ready") return null;
|
|
2320
|
+
const geoms = [];
|
|
2321
|
+
const sites = [];
|
|
2322
|
+
const joints = [];
|
|
2323
|
+
const comMarkers = [];
|
|
2324
|
+
if (showGeoms) {
|
|
2325
|
+
for (let i = 0; i < model.ngeom; i++) {
|
|
2326
|
+
const type = model.geom_type[i];
|
|
2327
|
+
const s = model.geom_size;
|
|
2328
|
+
let geometry = null;
|
|
2329
|
+
switch (type) {
|
|
2330
|
+
case 2:
|
|
2331
|
+
geometry = new THREE.SphereGeometry(s[3 * i], 12, 8);
|
|
2332
|
+
break;
|
|
2333
|
+
case 3:
|
|
2334
|
+
geometry = new THREE.CapsuleGeometry(s[3 * i], s[3 * i + 1] * 2, 6, 8);
|
|
2335
|
+
break;
|
|
2336
|
+
case 5:
|
|
2337
|
+
geometry = new THREE.CylinderGeometry(s[3 * i], s[3 * i], s[3 * i + 1] * 2, 12);
|
|
2338
|
+
break;
|
|
2339
|
+
case 6:
|
|
2340
|
+
geometry = new THREE.BoxGeometry(s[3 * i] * 2, s[3 * i + 1] * 2, s[3 * i + 2] * 2);
|
|
2341
|
+
break;
|
|
2342
|
+
}
|
|
2343
|
+
if (geometry) {
|
|
2344
|
+
const mat = new THREE.MeshBasicMaterial({ color: 65280, wireframe: true, transparent: true, opacity: 0.3 });
|
|
2345
|
+
const mesh = new THREE.Mesh(geometry, mat);
|
|
2346
|
+
mesh.userData.geomId = i;
|
|
2347
|
+
mesh.userData.bodyId = model.geom_bodyid[i];
|
|
2348
|
+
geoms.push(mesh);
|
|
2349
|
+
}
|
|
2350
|
+
}
|
|
2351
|
+
}
|
|
2352
|
+
if (showSites) {
|
|
2353
|
+
for (let i = 0; i < model.nsite; i++) {
|
|
2354
|
+
const geometry = new THREE.OctahedronGeometry(0.01);
|
|
2355
|
+
const mat = new THREE.MeshBasicMaterial({ color: 16711935, transparent: true, opacity: 0.7 });
|
|
2356
|
+
const mesh = new THREE.Mesh(geometry, mat);
|
|
2357
|
+
mesh.userData.siteId = i;
|
|
2358
|
+
sites.push(mesh);
|
|
2359
|
+
}
|
|
2360
|
+
}
|
|
2361
|
+
if (showJoints) {
|
|
2362
|
+
for (let i = 0; i < model.njnt; i++) {
|
|
2363
|
+
const type = model.jnt_type[i];
|
|
2364
|
+
const color = JOINT_COLORS[type] ?? 16777215;
|
|
2365
|
+
const arrow = new THREE.ArrowHelper(
|
|
2366
|
+
new THREE.Vector3(0, 0, 1),
|
|
2367
|
+
new THREE.Vector3(),
|
|
2368
|
+
0.05,
|
|
2369
|
+
color,
|
|
2370
|
+
0.01,
|
|
2371
|
+
5e-3
|
|
2372
|
+
);
|
|
2373
|
+
arrow.userData.jointId = i;
|
|
2374
|
+
joints.push(arrow);
|
|
2375
|
+
}
|
|
2376
|
+
}
|
|
2377
|
+
if (showCOM) {
|
|
2378
|
+
for (let i = 1; i < model.nbody; i++) {
|
|
2379
|
+
const geometry = new THREE.SphereGeometry(5e-3, 6, 6);
|
|
2380
|
+
const mat = new THREE.MeshBasicMaterial({ color: 16711680 });
|
|
2381
|
+
const mesh = new THREE.Mesh(geometry, mat);
|
|
2382
|
+
mesh.userData.bodyId = i;
|
|
2383
|
+
comMarkers.push(mesh);
|
|
2384
|
+
}
|
|
2385
|
+
}
|
|
2386
|
+
return { geoms, sites, joints, comMarkers };
|
|
2387
|
+
}, [status, mjModelRef, showGeoms, showSites, showJoints, showCOM]);
|
|
2388
|
+
useEffect(() => {
|
|
2389
|
+
const group = groupRef.current;
|
|
2390
|
+
if (!group || !debugGeometry) return;
|
|
2391
|
+
const allObjects = [
|
|
2392
|
+
...debugGeometry.geoms,
|
|
2393
|
+
...debugGeometry.sites,
|
|
2394
|
+
...debugGeometry.joints,
|
|
2395
|
+
...debugGeometry.comMarkers
|
|
2396
|
+
];
|
|
2397
|
+
for (const obj of allObjects) group.add(obj);
|
|
2398
|
+
return () => {
|
|
2399
|
+
for (const obj of allObjects) {
|
|
2400
|
+
group.remove(obj);
|
|
2401
|
+
if (obj.geometry) obj.geometry.dispose();
|
|
2402
|
+
}
|
|
2403
|
+
};
|
|
2404
|
+
}, [debugGeometry]);
|
|
2405
|
+
useFrame(() => {
|
|
2406
|
+
const model = mjModelRef.current;
|
|
2407
|
+
const data = mjDataRef.current;
|
|
2408
|
+
if (!model || !data || !debugGeometry) return;
|
|
2409
|
+
for (const mesh of debugGeometry.geoms) {
|
|
2410
|
+
const bid = mesh.userData.bodyId;
|
|
2411
|
+
const i3 = bid * 3;
|
|
2412
|
+
const i4 = bid * 4;
|
|
2413
|
+
mesh.position.set(data.xpos[i3], data.xpos[i3 + 1], data.xpos[i3 + 2]);
|
|
2414
|
+
mesh.quaternion.set(
|
|
2415
|
+
data.xquat[i4 + 1],
|
|
2416
|
+
data.xquat[i4 + 2],
|
|
2417
|
+
data.xquat[i4 + 3],
|
|
2418
|
+
data.xquat[i4]
|
|
2419
|
+
);
|
|
2420
|
+
const gid = mesh.userData.geomId;
|
|
2421
|
+
const gp = model.geom_pos;
|
|
2422
|
+
mesh.position.add(new THREE.Vector3(gp[3 * gid], gp[3 * gid + 1], gp[3 * gid + 2]).applyQuaternion(mesh.quaternion));
|
|
2423
|
+
}
|
|
2424
|
+
for (const mesh of debugGeometry.sites) {
|
|
2425
|
+
const sid = mesh.userData.siteId;
|
|
2426
|
+
mesh.position.set(
|
|
2427
|
+
data.site_xpos[3 * sid],
|
|
2428
|
+
data.site_xpos[3 * sid + 1],
|
|
2429
|
+
data.site_xpos[3 * sid + 2]
|
|
2430
|
+
);
|
|
2431
|
+
}
|
|
2432
|
+
for (const mesh of debugGeometry.comMarkers) {
|
|
2433
|
+
const bid = mesh.userData.bodyId;
|
|
2434
|
+
const i3 = bid * 3;
|
|
2435
|
+
mesh.position.set(data.xpos[i3], data.xpos[i3 + 1], data.xpos[i3 + 2]);
|
|
2436
|
+
}
|
|
2437
|
+
});
|
|
2438
|
+
const contactGroupRef = useRef(null);
|
|
2439
|
+
const contactArrowsRef = useRef([]);
|
|
2440
|
+
useFrame(() => {
|
|
2441
|
+
if (!showContacts) return;
|
|
2442
|
+
const model = mjModelRef.current;
|
|
2443
|
+
const data = mjDataRef.current;
|
|
2444
|
+
const group = contactGroupRef.current;
|
|
2445
|
+
if (!model || !data || !group) return;
|
|
2446
|
+
for (const arrow of contactArrowsRef.current) {
|
|
2447
|
+
group.remove(arrow);
|
|
2448
|
+
arrow.dispose();
|
|
2449
|
+
}
|
|
2450
|
+
contactArrowsRef.current = [];
|
|
2451
|
+
const ncon = data.ncon;
|
|
2452
|
+
for (let i = 0; i < Math.min(ncon, 50); i++) {
|
|
2453
|
+
try {
|
|
2454
|
+
const c = data.contact.get(i);
|
|
2455
|
+
const pos = new THREE.Vector3(c.pos[0], c.pos[1], c.pos[2]);
|
|
2456
|
+
const normal = new THREE.Vector3(c.frame[0], c.frame[1], c.frame[2]);
|
|
2457
|
+
const force = Math.abs(c.dist) * 100;
|
|
2458
|
+
const length = Math.min(force * 0.01, 0.1);
|
|
2459
|
+
if (length > 1e-3) {
|
|
2460
|
+
const arrow = new THREE.ArrowHelper(normal, pos, length, 16729156, length * 0.3, length * 0.15);
|
|
2461
|
+
group.add(arrow);
|
|
2462
|
+
contactArrowsRef.current.push(arrow);
|
|
2463
|
+
}
|
|
2464
|
+
} catch {
|
|
2465
|
+
break;
|
|
2466
|
+
}
|
|
2467
|
+
}
|
|
2468
|
+
});
|
|
2469
|
+
if (status !== "ready") return null;
|
|
2470
|
+
return /* @__PURE__ */ jsxs(Fragment, { children: [
|
|
2471
|
+
/* @__PURE__ */ jsx("group", { ref: groupRef }),
|
|
2472
|
+
showContacts && /* @__PURE__ */ jsx("group", { ref: contactGroupRef })
|
|
2473
|
+
] });
|
|
2474
|
+
}
|
|
2475
|
+
var DEFAULT_TENDON_COLOR = new THREE.Color(0.3, 0.3, 0.8);
|
|
2476
|
+
var DEFAULT_TENDON_WIDTH = 2e-3;
|
|
2477
|
+
function TendonRenderer() {
|
|
2478
|
+
const { mjModelRef, mjDataRef, status } = useMujocoSim();
|
|
2479
|
+
const groupRef = useRef(null);
|
|
2480
|
+
const meshesRef = useRef([]);
|
|
2481
|
+
useFrame(() => {
|
|
2482
|
+
const model = mjModelRef.current;
|
|
2483
|
+
const data = mjDataRef.current;
|
|
2484
|
+
const group = groupRef.current;
|
|
2485
|
+
if (!model || !data || !group) return;
|
|
2486
|
+
const ntendon = model.ntendon ?? 0;
|
|
2487
|
+
if (ntendon === 0) return;
|
|
2488
|
+
for (const mesh of meshesRef.current) {
|
|
2489
|
+
group.remove(mesh);
|
|
2490
|
+
mesh.geometry.dispose();
|
|
2491
|
+
mesh.material.dispose();
|
|
2492
|
+
}
|
|
2493
|
+
meshesRef.current = [];
|
|
2494
|
+
for (let t = 0; t < ntendon; t++) {
|
|
2495
|
+
const wrapAdr = model.ten_wrapadr[t];
|
|
2496
|
+
const wrapNum = model.ten_wrapnum[t];
|
|
2497
|
+
if (wrapNum < 2) continue;
|
|
2498
|
+
const points = [];
|
|
2499
|
+
for (let w = 0; w < wrapNum; w++) {
|
|
2500
|
+
const idx = (wrapAdr + w) * 3;
|
|
2501
|
+
if (data.wrap_xpos && idx + 2 < data.wrap_xpos.length) {
|
|
2502
|
+
const x = data.wrap_xpos[idx];
|
|
2503
|
+
const y = data.wrap_xpos[idx + 1];
|
|
2504
|
+
const z = data.wrap_xpos[idx + 2];
|
|
2505
|
+
if (x !== 0 || y !== 0 || z !== 0) {
|
|
2506
|
+
points.push(new THREE.Vector3(x, y, z));
|
|
2507
|
+
}
|
|
2508
|
+
}
|
|
2509
|
+
}
|
|
2510
|
+
if (points.length < 2) continue;
|
|
2511
|
+
const curve = new THREE.CatmullRomCurve3(points, false);
|
|
2512
|
+
const geometry = new THREE.TubeGeometry(
|
|
2513
|
+
curve,
|
|
2514
|
+
Math.max(points.length * 2, 4),
|
|
2515
|
+
DEFAULT_TENDON_WIDTH,
|
|
2516
|
+
6,
|
|
2517
|
+
false
|
|
2518
|
+
);
|
|
2519
|
+
const material = new THREE.MeshStandardMaterial({
|
|
2520
|
+
color: DEFAULT_TENDON_COLOR,
|
|
2521
|
+
roughness: 0.6,
|
|
2522
|
+
metalness: 0.1
|
|
2523
|
+
});
|
|
2524
|
+
const mesh = new THREE.Mesh(geometry, material);
|
|
2525
|
+
group.add(mesh);
|
|
2526
|
+
meshesRef.current.push(mesh);
|
|
2527
|
+
}
|
|
2528
|
+
});
|
|
2529
|
+
if (status !== "ready") return null;
|
|
2530
|
+
return /* @__PURE__ */ jsx("group", { ref: groupRef });
|
|
2531
|
+
}
|
|
2532
|
+
function FlexRenderer() {
|
|
2533
|
+
const { mjModelRef, mjDataRef, status } = useMujocoSim();
|
|
2534
|
+
const groupRef = useRef(null);
|
|
2535
|
+
const meshesRef = useRef([]);
|
|
2536
|
+
useEffect(() => {
|
|
2537
|
+
const model = mjModelRef.current;
|
|
2538
|
+
const group = groupRef.current;
|
|
2539
|
+
if (!model || !group || status !== "ready") return;
|
|
2540
|
+
const nflex = model.nflex ?? 0;
|
|
2541
|
+
if (nflex === 0) return;
|
|
2542
|
+
for (let f = 0; f < nflex; f++) {
|
|
2543
|
+
const vertAdr = model.flex_vertadr[f];
|
|
2544
|
+
const vertNum = model.flex_vertnum[f];
|
|
2545
|
+
if (vertNum === 0) continue;
|
|
2546
|
+
const geometry = new THREE.BufferGeometry();
|
|
2547
|
+
const positions = new Float32Array(vertNum * 3);
|
|
2548
|
+
geometry.setAttribute("position", new THREE.BufferAttribute(positions, 3));
|
|
2549
|
+
geometry.computeVertexNormals();
|
|
2550
|
+
let color = new THREE.Color(0.5, 0.5, 0.5);
|
|
2551
|
+
if (model.flex_rgba) {
|
|
2552
|
+
color = new THREE.Color(
|
|
2553
|
+
model.flex_rgba[4 * f],
|
|
2554
|
+
model.flex_rgba[4 * f + 1],
|
|
2555
|
+
model.flex_rgba[4 * f + 2]
|
|
2556
|
+
);
|
|
2557
|
+
}
|
|
2558
|
+
const material = new THREE.MeshStandardMaterial({
|
|
2559
|
+
color,
|
|
2560
|
+
roughness: 0.7,
|
|
2561
|
+
side: THREE.DoubleSide
|
|
2562
|
+
});
|
|
2563
|
+
const mesh = new THREE.Mesh(geometry, material);
|
|
2564
|
+
mesh.userData.flexId = f;
|
|
2565
|
+
mesh.userData.vertAdr = vertAdr;
|
|
2566
|
+
mesh.userData.vertNum = vertNum;
|
|
2567
|
+
group.add(mesh);
|
|
2568
|
+
meshesRef.current.push(mesh);
|
|
2569
|
+
}
|
|
2570
|
+
return () => {
|
|
2571
|
+
for (const mesh of meshesRef.current) {
|
|
2572
|
+
group.remove(mesh);
|
|
2573
|
+
mesh.geometry.dispose();
|
|
2574
|
+
mesh.material.dispose();
|
|
2575
|
+
}
|
|
2576
|
+
meshesRef.current = [];
|
|
2577
|
+
};
|
|
2578
|
+
}, [status, mjModelRef]);
|
|
2579
|
+
useFrame(() => {
|
|
2580
|
+
const data = mjDataRef.current;
|
|
2581
|
+
if (!data || !data.flexvert_xpos) return;
|
|
2582
|
+
for (const mesh of meshesRef.current) {
|
|
2583
|
+
const vertAdr = mesh.userData.vertAdr;
|
|
2584
|
+
const vertNum = mesh.userData.vertNum;
|
|
2585
|
+
const posAttr = mesh.geometry.getAttribute("position");
|
|
2586
|
+
if (!posAttr) continue;
|
|
2587
|
+
for (let v = 0; v < vertNum; v++) {
|
|
2588
|
+
const srcIdx = (vertAdr + v) * 3;
|
|
2589
|
+
posAttr.setXYZ(v, data.flexvert_xpos[srcIdx], data.flexvert_xpos[srcIdx + 1], data.flexvert_xpos[srcIdx + 2]);
|
|
2590
|
+
}
|
|
2591
|
+
posAttr.needsUpdate = true;
|
|
2592
|
+
mesh.geometry.computeVertexNormals();
|
|
2593
|
+
}
|
|
2594
|
+
});
|
|
2595
|
+
if (status !== "ready") return null;
|
|
2596
|
+
return /* @__PURE__ */ jsx("group", { ref: groupRef });
|
|
2597
|
+
}
|
|
2598
|
+
function useContacts(bodyName, callback) {
|
|
2599
|
+
const { mjModelRef } = useMujocoSim();
|
|
2600
|
+
const contactsRef = useRef([]);
|
|
2601
|
+
const bodyIdRef = useRef(-1);
|
|
2602
|
+
const callbackRef = useRef(callback);
|
|
2603
|
+
callbackRef.current = callback;
|
|
2604
|
+
useEffect(() => {
|
|
2605
|
+
if (!bodyName) {
|
|
2606
|
+
bodyIdRef.current = -1;
|
|
2607
|
+
return;
|
|
2608
|
+
}
|
|
2609
|
+
const model = mjModelRef.current;
|
|
2610
|
+
if (!model) return;
|
|
2611
|
+
bodyIdRef.current = findBodyByName(model, bodyName);
|
|
2612
|
+
}, [bodyName, mjModelRef]);
|
|
2613
|
+
useAfterPhysicsStep((model, data) => {
|
|
2614
|
+
const ncon = data.ncon;
|
|
2615
|
+
if (ncon === 0) {
|
|
2616
|
+
if (contactsRef.current.length > 0) contactsRef.current = [];
|
|
2617
|
+
callbackRef.current?.([]);
|
|
2618
|
+
return;
|
|
2619
|
+
}
|
|
2620
|
+
const contacts = [];
|
|
2621
|
+
const filterBody = bodyIdRef.current;
|
|
2622
|
+
for (let i = 0; i < ncon; i++) {
|
|
2623
|
+
try {
|
|
2624
|
+
const c = data.contact.get(i);
|
|
2625
|
+
if (filterBody >= 0) {
|
|
2626
|
+
const b1 = model.geom_bodyid[c.geom1];
|
|
2627
|
+
const b2 = model.geom_bodyid[c.geom2];
|
|
2628
|
+
if (b1 !== filterBody && b2 !== filterBody) continue;
|
|
2629
|
+
}
|
|
2630
|
+
contacts.push({
|
|
2631
|
+
geom1: c.geom1,
|
|
2632
|
+
geom1Name: getName(model, model.name_geomadr[c.geom1]),
|
|
2633
|
+
geom2: c.geom2,
|
|
2634
|
+
geom2Name: getName(model, model.name_geomadr[c.geom2]),
|
|
2635
|
+
pos: [c.pos[0], c.pos[1], c.pos[2]],
|
|
2636
|
+
depth: c.dist
|
|
2637
|
+
});
|
|
2638
|
+
} catch {
|
|
2639
|
+
break;
|
|
2640
|
+
}
|
|
2641
|
+
}
|
|
2642
|
+
contactsRef.current = contacts;
|
|
2643
|
+
callbackRef.current?.(contacts);
|
|
2644
|
+
});
|
|
2645
|
+
return contactsRef;
|
|
2646
|
+
}
|
|
2647
|
+
function useContactEvents(bodyName, handlers) {
|
|
2648
|
+
const prevPairsRef = useRef(/* @__PURE__ */ new Set());
|
|
2649
|
+
const onEnterRef = useRef(handlers.onEnter);
|
|
2650
|
+
const onExitRef = useRef(handlers.onExit);
|
|
2651
|
+
onEnterRef.current = handlers.onEnter;
|
|
2652
|
+
onExitRef.current = handlers.onExit;
|
|
2653
|
+
const prevContactMapRef = useRef(/* @__PURE__ */ new Map());
|
|
2654
|
+
const onContacts = useCallback((contacts) => {
|
|
2655
|
+
const currentPairs = /* @__PURE__ */ new Set();
|
|
2656
|
+
const currentMap = /* @__PURE__ */ new Map();
|
|
2657
|
+
for (const c of contacts) {
|
|
2658
|
+
const key = `${Math.min(c.geom1, c.geom2)}_${Math.max(c.geom1, c.geom2)}`;
|
|
2659
|
+
currentPairs.add(key);
|
|
2660
|
+
currentMap.set(key, c);
|
|
2661
|
+
}
|
|
2662
|
+
for (const key of currentPairs) {
|
|
2663
|
+
if (!prevPairsRef.current.has(key)) {
|
|
2664
|
+
onEnterRef.current?.(currentMap.get(key));
|
|
2665
|
+
}
|
|
2666
|
+
}
|
|
2667
|
+
for (const key of prevPairsRef.current) {
|
|
2668
|
+
if (!currentPairs.has(key)) {
|
|
2669
|
+
const prev = prevContactMapRef.current.get(key);
|
|
2670
|
+
if (prev) onExitRef.current?.(prev);
|
|
2671
|
+
}
|
|
2672
|
+
}
|
|
2673
|
+
prevPairsRef.current = currentPairs;
|
|
2674
|
+
prevContactMapRef.current = currentMap;
|
|
2675
|
+
}, []);
|
|
2676
|
+
useContacts(bodyName, onContacts);
|
|
2677
|
+
}
|
|
2678
|
+
|
|
2679
|
+
// src/components/ContactListener.tsx
|
|
2680
|
+
function ContactListener({
|
|
2681
|
+
body,
|
|
2682
|
+
onContactEnter,
|
|
2683
|
+
onContactExit
|
|
2684
|
+
}) {
|
|
2685
|
+
useContactEvents(body, {
|
|
2686
|
+
onEnter: onContactEnter,
|
|
2687
|
+
onExit: onContactExit
|
|
2688
|
+
});
|
|
2689
|
+
return null;
|
|
2690
|
+
}
|
|
2691
|
+
function useTrajectoryPlayer(trajectory, options = {}) {
|
|
2692
|
+
const { mjModelRef, mjDataRef, mujocoRef, pausedRef } = useMujocoSim();
|
|
2693
|
+
const fps = options.fps ?? 30;
|
|
2694
|
+
const loop = options.loop ?? false;
|
|
2695
|
+
const playingRef = useRef(false);
|
|
2696
|
+
const frameRef = useRef(0);
|
|
2697
|
+
const lastFrameTimeRef = useRef(0);
|
|
2698
|
+
const play = useCallback(() => {
|
|
2699
|
+
playingRef.current = true;
|
|
2700
|
+
pausedRef.current = true;
|
|
2701
|
+
lastFrameTimeRef.current = performance.now();
|
|
2702
|
+
}, [pausedRef]);
|
|
2703
|
+
const pause = useCallback(() => {
|
|
2704
|
+
playingRef.current = false;
|
|
2705
|
+
}, []);
|
|
2706
|
+
const seek = useCallback((frameIdx) => {
|
|
2707
|
+
frameRef.current = Math.max(0, Math.min(frameIdx, trajectory.length - 1));
|
|
2708
|
+
const model = mjModelRef.current;
|
|
2709
|
+
const data = mjDataRef.current;
|
|
2710
|
+
if (!model || !data || !trajectory[frameRef.current]) return;
|
|
2711
|
+
const qpos = trajectory[frameRef.current];
|
|
2712
|
+
for (let i = 0; i < Math.min(qpos.length, model.nq); i++) {
|
|
2713
|
+
data.qpos[i] = qpos[i];
|
|
2714
|
+
}
|
|
2715
|
+
mujocoRef.current.mj_forward(model, data);
|
|
2716
|
+
}, [trajectory, mjModelRef, mjDataRef, mujocoRef]);
|
|
2717
|
+
const reset = useCallback(() => {
|
|
2718
|
+
frameRef.current = 0;
|
|
2719
|
+
playingRef.current = false;
|
|
2720
|
+
pausedRef.current = false;
|
|
2721
|
+
}, [pausedRef]);
|
|
2722
|
+
useFrame(() => {
|
|
2723
|
+
if (!playingRef.current || trajectory.length === 0) return;
|
|
2724
|
+
const now = performance.now();
|
|
2725
|
+
const elapsed = now - lastFrameTimeRef.current;
|
|
2726
|
+
const frameInterval = 1e3 / fps;
|
|
2727
|
+
if (elapsed < frameInterval) return;
|
|
2728
|
+
lastFrameTimeRef.current = now;
|
|
2729
|
+
const model = mjModelRef.current;
|
|
2730
|
+
const data = mjDataRef.current;
|
|
2731
|
+
if (!model || !data) return;
|
|
2732
|
+
const qpos = trajectory[frameRef.current];
|
|
2733
|
+
if (!qpos) return;
|
|
2734
|
+
for (let i = 0; i < Math.min(qpos.length, model.nq); i++) {
|
|
2735
|
+
data.qpos[i] = qpos[i];
|
|
2736
|
+
}
|
|
2737
|
+
mujocoRef.current.mj_forward(model, data);
|
|
2738
|
+
frameRef.current++;
|
|
2739
|
+
if (frameRef.current >= trajectory.length) {
|
|
2740
|
+
if (loop) {
|
|
2741
|
+
frameRef.current = 0;
|
|
2742
|
+
} else {
|
|
2743
|
+
playingRef.current = false;
|
|
2744
|
+
pausedRef.current = false;
|
|
2745
|
+
}
|
|
2746
|
+
}
|
|
2747
|
+
});
|
|
2748
|
+
return {
|
|
2749
|
+
play,
|
|
2750
|
+
pause,
|
|
2751
|
+
seek,
|
|
2752
|
+
reset,
|
|
2753
|
+
get frame() {
|
|
2754
|
+
return frameRef.current;
|
|
2755
|
+
},
|
|
2756
|
+
get playing() {
|
|
2757
|
+
return playingRef.current;
|
|
2758
|
+
},
|
|
2759
|
+
get totalFrames() {
|
|
2760
|
+
return trajectory.length;
|
|
2761
|
+
}
|
|
2762
|
+
};
|
|
2763
|
+
}
|
|
2764
|
+
|
|
2765
|
+
// src/components/TrajectoryPlayer.tsx
|
|
2766
|
+
function TrajectoryPlayer({
|
|
2767
|
+
trajectory,
|
|
2768
|
+
fps = 30,
|
|
2769
|
+
loop = false,
|
|
2770
|
+
playing = false,
|
|
2771
|
+
onFrame
|
|
2772
|
+
}) {
|
|
2773
|
+
const player = useTrajectoryPlayer(trajectory, { fps, loop });
|
|
2774
|
+
useEffect(() => {
|
|
2775
|
+
if (playing) {
|
|
2776
|
+
player.play();
|
|
2777
|
+
} else {
|
|
2778
|
+
player.pause();
|
|
2779
|
+
}
|
|
2780
|
+
}, [playing]);
|
|
2781
|
+
useEffect(() => {
|
|
2782
|
+
if (onFrame) {
|
|
2783
|
+
const interval = setInterval(() => {
|
|
2784
|
+
if (player.playing) onFrame(player.frame);
|
|
2785
|
+
}, 1e3 / fps);
|
|
2786
|
+
return () => clearInterval(interval);
|
|
2787
|
+
}
|
|
2788
|
+
}, [onFrame, fps]);
|
|
2789
|
+
return null;
|
|
2790
|
+
}
|
|
2791
|
+
function SelectionHighlight({
|
|
2792
|
+
bodyId,
|
|
2793
|
+
color = "#ff4444",
|
|
2794
|
+
emissiveIntensity = 0.3
|
|
2795
|
+
}) {
|
|
2796
|
+
const { scene } = useThree();
|
|
2797
|
+
const prevMeshesRef = useRef([]);
|
|
2798
|
+
useEffect(() => {
|
|
2799
|
+
for (const entry of prevMeshesRef.current) {
|
|
2800
|
+
const mat = entry.mesh.material;
|
|
2801
|
+
if (mat.emissive) {
|
|
2802
|
+
mat.emissive.copy(entry.originalEmissive);
|
|
2803
|
+
mat.emissiveIntensity = entry.originalIntensity;
|
|
2804
|
+
}
|
|
2805
|
+
}
|
|
2806
|
+
prevMeshesRef.current = [];
|
|
2807
|
+
if (bodyId === null || bodyId < 0) return;
|
|
2808
|
+
const highlightColor = new THREE.Color(color);
|
|
2809
|
+
scene.traverse((obj) => {
|
|
2810
|
+
if (obj.userData.bodyID === bodyId && obj.isMesh) {
|
|
2811
|
+
const mesh = obj;
|
|
2812
|
+
const mat = mesh.material;
|
|
2813
|
+
if (mat.emissive) {
|
|
2814
|
+
prevMeshesRef.current.push({
|
|
2815
|
+
mesh,
|
|
2816
|
+
originalEmissive: mat.emissive.clone(),
|
|
2817
|
+
originalIntensity: mat.emissiveIntensity ?? 0
|
|
2818
|
+
});
|
|
2819
|
+
mat.emissive.copy(highlightColor);
|
|
2820
|
+
mat.emissiveIntensity = emissiveIntensity;
|
|
2821
|
+
}
|
|
2822
|
+
}
|
|
2823
|
+
});
|
|
2824
|
+
return () => {
|
|
2825
|
+
for (const entry of prevMeshesRef.current) {
|
|
2826
|
+
const mat = entry.mesh.material;
|
|
2827
|
+
if (mat.emissive) {
|
|
2828
|
+
mat.emissive.copy(entry.originalEmissive);
|
|
2829
|
+
mat.emissiveIntensity = entry.originalIntensity;
|
|
2830
|
+
}
|
|
2831
|
+
}
|
|
2832
|
+
prevMeshesRef.current = [];
|
|
2833
|
+
};
|
|
2834
|
+
}, [bodyId, color, emissiveIntensity, scene]);
|
|
2835
|
+
return null;
|
|
2836
|
+
}
|
|
2837
|
+
function useActuators() {
|
|
2838
|
+
const { mjModelRef, status } = useMujocoSim();
|
|
2839
|
+
return useMemo(() => {
|
|
2840
|
+
if (status !== "ready") return [];
|
|
2841
|
+
const model = mjModelRef.current;
|
|
2842
|
+
if (!model) return [];
|
|
2843
|
+
const actuators = [];
|
|
2844
|
+
for (let i = 0; i < model.nu; i++) {
|
|
2845
|
+
const name = getName(model, model.name_actuatoradr[i]);
|
|
2846
|
+
const lo = model.actuator_ctrlrange[i * 2];
|
|
2847
|
+
const hi = model.actuator_ctrlrange[i * 2 + 1];
|
|
2848
|
+
const hasRange = lo < hi;
|
|
2849
|
+
const range = hasRange ? [lo, hi] : [-Infinity, Infinity];
|
|
2850
|
+
actuators.push({ id: i, name, range });
|
|
2851
|
+
}
|
|
2852
|
+
return actuators;
|
|
2853
|
+
}, [status, mjModelRef]);
|
|
2854
|
+
}
|
|
2855
|
+
var _mat42 = new THREE.Matrix4();
|
|
2856
|
+
function useSitePosition(siteName) {
|
|
2857
|
+
const { mjModelRef, mjDataRef, status } = useMujocoSim();
|
|
2858
|
+
const siteIdRef = useRef(-1);
|
|
2859
|
+
const positionRef = useRef(new THREE.Vector3());
|
|
2860
|
+
const quaternionRef = useRef(new THREE.Quaternion());
|
|
2861
|
+
useEffect(() => {
|
|
2862
|
+
const model = mjModelRef.current;
|
|
2863
|
+
if (!model || status !== "ready") {
|
|
2864
|
+
siteIdRef.current = -1;
|
|
2865
|
+
return;
|
|
2866
|
+
}
|
|
2867
|
+
siteIdRef.current = findSiteByName(model, siteName);
|
|
2868
|
+
}, [siteName, status, mjModelRef]);
|
|
2869
|
+
useFrame(() => {
|
|
2870
|
+
const data = mjDataRef.current;
|
|
2871
|
+
const sid = siteIdRef.current;
|
|
2872
|
+
if (!data || sid < 0) return;
|
|
2873
|
+
const i3 = sid * 3;
|
|
2874
|
+
const i9 = sid * 9;
|
|
2875
|
+
positionRef.current.set(
|
|
2876
|
+
data.site_xpos[i3],
|
|
2877
|
+
data.site_xpos[i3 + 1],
|
|
2878
|
+
data.site_xpos[i3 + 2]
|
|
2879
|
+
);
|
|
2880
|
+
const m = data.site_xmat;
|
|
2881
|
+
_mat42.set(
|
|
2882
|
+
m[i9],
|
|
2883
|
+
m[i9 + 1],
|
|
2884
|
+
m[i9 + 2],
|
|
2885
|
+
0,
|
|
2886
|
+
m[i9 + 3],
|
|
2887
|
+
m[i9 + 4],
|
|
2888
|
+
m[i9 + 5],
|
|
2889
|
+
0,
|
|
2890
|
+
m[i9 + 6],
|
|
2891
|
+
m[i9 + 7],
|
|
2892
|
+
m[i9 + 8],
|
|
2893
|
+
0,
|
|
2894
|
+
0,
|
|
2895
|
+
0,
|
|
2896
|
+
0,
|
|
2897
|
+
1
|
|
2898
|
+
);
|
|
2899
|
+
quaternionRef.current.setFromRotationMatrix(_mat42);
|
|
2900
|
+
});
|
|
2901
|
+
return { position: positionRef, quaternion: quaternionRef };
|
|
2902
|
+
}
|
|
2903
|
+
|
|
2904
|
+
// src/hooks/useGravityCompensation.ts
|
|
2905
|
+
function useGravityCompensation(enabled = true) {
|
|
2906
|
+
useBeforePhysicsStep((model, data) => {
|
|
2907
|
+
if (!enabled) return;
|
|
2908
|
+
for (let i = 0; i < model.nv; i++) {
|
|
2909
|
+
data.qfrc_applied[i] += data.qfrc_bias[i];
|
|
2910
|
+
}
|
|
2911
|
+
});
|
|
2912
|
+
}
|
|
2913
|
+
function useSensor(name) {
|
|
2914
|
+
const { mjModelRef, mjDataRef, status } = useMujocoSim();
|
|
2915
|
+
const sensorIdRef = useRef(-1);
|
|
2916
|
+
const sensorAdrRef = useRef(0);
|
|
2917
|
+
const sensorDimRef = useRef(0);
|
|
2918
|
+
const valueRef = useRef(new Float64Array(0));
|
|
2919
|
+
useEffect(() => {
|
|
2920
|
+
const model = mjModelRef.current;
|
|
2921
|
+
if (!model || status !== "ready") return;
|
|
2922
|
+
for (let i = 0; i < model.nsensor; i++) {
|
|
2923
|
+
if (getName(model, model.name_sensoradr[i]) === name) {
|
|
2924
|
+
sensorIdRef.current = i;
|
|
2925
|
+
sensorAdrRef.current = model.sensor_adr[i];
|
|
2926
|
+
sensorDimRef.current = model.sensor_dim[i];
|
|
2927
|
+
valueRef.current = new Float64Array(model.sensor_dim[i]);
|
|
2928
|
+
return;
|
|
2929
|
+
}
|
|
2930
|
+
}
|
|
2931
|
+
sensorIdRef.current = -1;
|
|
2932
|
+
}, [name, status, mjModelRef]);
|
|
2933
|
+
useAfterPhysicsStep((_model, data) => {
|
|
2934
|
+
if (sensorIdRef.current < 0) return;
|
|
2935
|
+
const adr = sensorAdrRef.current;
|
|
2936
|
+
const dim = sensorDimRef.current;
|
|
2937
|
+
for (let i = 0; i < dim; i++) {
|
|
2938
|
+
valueRef.current[i] = data.sensordata[adr + i];
|
|
2939
|
+
}
|
|
2940
|
+
});
|
|
2941
|
+
return { value: valueRef, size: sensorDimRef.current };
|
|
2942
|
+
}
|
|
2943
|
+
function useSensors() {
|
|
2944
|
+
const { mjModelRef, status } = useMujocoSim();
|
|
2945
|
+
return useMemo(() => {
|
|
2946
|
+
const model = mjModelRef.current;
|
|
2947
|
+
if (!model || status !== "ready") return [];
|
|
2948
|
+
const SENSOR_TYPE_NAMES2 = {
|
|
2949
|
+
0: "touch",
|
|
2950
|
+
1: "accelerometer",
|
|
2951
|
+
2: "velocimeter",
|
|
2952
|
+
3: "gyro",
|
|
2953
|
+
4: "force",
|
|
2954
|
+
5: "torque",
|
|
2955
|
+
6: "magnetometer",
|
|
2956
|
+
7: "rangefinder",
|
|
2957
|
+
8: "jointpos",
|
|
2958
|
+
9: "jointvel",
|
|
2959
|
+
10: "tendonpos",
|
|
2960
|
+
11: "tendonvel",
|
|
2961
|
+
12: "actuatorpos",
|
|
2962
|
+
13: "actuatorvel",
|
|
2963
|
+
14: "actuatorfrc"
|
|
2964
|
+
};
|
|
2965
|
+
const result = [];
|
|
2966
|
+
for (let i = 0; i < model.nsensor; i++) {
|
|
2967
|
+
const type = model.sensor_type[i];
|
|
2968
|
+
result.push({
|
|
2969
|
+
id: i,
|
|
2970
|
+
name: getName(model, model.name_sensoradr[i]),
|
|
2971
|
+
type,
|
|
2972
|
+
typeName: SENSOR_TYPE_NAMES2[type] ?? `unknown(${type})`,
|
|
2973
|
+
dim: model.sensor_dim[i],
|
|
2974
|
+
adr: model.sensor_adr[i]
|
|
2975
|
+
});
|
|
2976
|
+
}
|
|
2977
|
+
return result;
|
|
2978
|
+
}, [mjModelRef, status]);
|
|
2979
|
+
}
|
|
2980
|
+
function useJointState(name) {
|
|
2981
|
+
const { mjModelRef, mjDataRef, status } = useMujocoSim();
|
|
2982
|
+
const jointIdRef = useRef(-1);
|
|
2983
|
+
const qposAdrRef = useRef(0);
|
|
2984
|
+
const dofAdrRef = useRef(0);
|
|
2985
|
+
const qposDimRef = useRef(1);
|
|
2986
|
+
const dofDimRef = useRef(1);
|
|
2987
|
+
const positionRef = useRef(0);
|
|
2988
|
+
const velocityRef = useRef(0);
|
|
2989
|
+
useEffect(() => {
|
|
2990
|
+
const model = mjModelRef.current;
|
|
2991
|
+
if (!model || status !== "ready") return;
|
|
2992
|
+
for (let i = 0; i < model.njnt; i++) {
|
|
2993
|
+
if (getName(model, model.name_jntadr[i]) === name) {
|
|
2994
|
+
jointIdRef.current = i;
|
|
2995
|
+
qposAdrRef.current = model.jnt_qposadr[i];
|
|
2996
|
+
dofAdrRef.current = model.jnt_dofadr[i];
|
|
2997
|
+
const type = model.jnt_type[i];
|
|
2998
|
+
if (type === 0) {
|
|
2999
|
+
qposDimRef.current = 7;
|
|
3000
|
+
dofDimRef.current = 6;
|
|
3001
|
+
} else if (type === 1) {
|
|
3002
|
+
qposDimRef.current = 4;
|
|
3003
|
+
dofDimRef.current = 3;
|
|
3004
|
+
} else {
|
|
3005
|
+
qposDimRef.current = 1;
|
|
3006
|
+
dofDimRef.current = 1;
|
|
3007
|
+
}
|
|
3008
|
+
return;
|
|
3009
|
+
}
|
|
3010
|
+
}
|
|
3011
|
+
jointIdRef.current = -1;
|
|
3012
|
+
}, [name, status, mjModelRef]);
|
|
3013
|
+
useAfterPhysicsStep((_model, data) => {
|
|
3014
|
+
if (jointIdRef.current < 0) return;
|
|
3015
|
+
const qa = qposAdrRef.current;
|
|
3016
|
+
const da = dofAdrRef.current;
|
|
3017
|
+
if (qposDimRef.current === 1) {
|
|
3018
|
+
positionRef.current = data.qpos[qa];
|
|
3019
|
+
velocityRef.current = data.qvel[da];
|
|
3020
|
+
} else {
|
|
3021
|
+
positionRef.current = new Float64Array(data.qpos.subarray(qa, qa + qposDimRef.current));
|
|
3022
|
+
velocityRef.current = new Float64Array(data.qvel.subarray(da, da + dofDimRef.current));
|
|
3023
|
+
}
|
|
3024
|
+
});
|
|
3025
|
+
return { position: positionRef, velocity: velocityRef };
|
|
3026
|
+
}
|
|
3027
|
+
function useBodyState(name) {
|
|
3028
|
+
const { mjModelRef, status } = useMujocoSim();
|
|
3029
|
+
const bodyIdRef = useRef(-1);
|
|
3030
|
+
const position = useRef(new THREE.Vector3());
|
|
3031
|
+
const quaternion = useRef(new THREE.Quaternion());
|
|
3032
|
+
const linearVelocity = useRef(new THREE.Vector3());
|
|
3033
|
+
const angularVelocity = useRef(new THREE.Vector3());
|
|
3034
|
+
useEffect(() => {
|
|
3035
|
+
const model = mjModelRef.current;
|
|
3036
|
+
if (!model || status !== "ready") return;
|
|
3037
|
+
bodyIdRef.current = findBodyByName(model, name);
|
|
3038
|
+
}, [name, status, mjModelRef]);
|
|
3039
|
+
useAfterPhysicsStep((_model, data) => {
|
|
3040
|
+
const bid = bodyIdRef.current;
|
|
3041
|
+
if (bid < 0) return;
|
|
3042
|
+
const i3 = bid * 3;
|
|
3043
|
+
position.current.set(data.xpos[i3], data.xpos[i3 + 1], data.xpos[i3 + 2]);
|
|
3044
|
+
const i4 = bid * 4;
|
|
3045
|
+
quaternion.current.set(
|
|
3046
|
+
data.xquat[i4 + 1],
|
|
3047
|
+
data.xquat[i4 + 2],
|
|
3048
|
+
data.xquat[i4 + 3],
|
|
3049
|
+
data.xquat[i4]
|
|
3050
|
+
);
|
|
3051
|
+
if (data.cvel) {
|
|
3052
|
+
const i6 = bid * 6;
|
|
3053
|
+
angularVelocity.current.set(data.cvel[i6], data.cvel[i6 + 1], data.cvel[i6 + 2]);
|
|
3054
|
+
linearVelocity.current.set(data.cvel[i6 + 3], data.cvel[i6 + 4], data.cvel[i6 + 5]);
|
|
3055
|
+
}
|
|
3056
|
+
});
|
|
3057
|
+
return { position, quaternion, linearVelocity, angularVelocity };
|
|
3058
|
+
}
|
|
3059
|
+
function useCtrl(name) {
|
|
3060
|
+
const { mjModelRef, mjDataRef, status } = useMujocoSim();
|
|
3061
|
+
const actuatorIdRef = useRef(-1);
|
|
3062
|
+
const valueRef = useRef(0);
|
|
3063
|
+
useEffect(() => {
|
|
3064
|
+
const model = mjModelRef.current;
|
|
3065
|
+
if (!model || status !== "ready") return;
|
|
3066
|
+
actuatorIdRef.current = findActuatorByName(model, name);
|
|
3067
|
+
}, [name, status, mjModelRef]);
|
|
3068
|
+
const setValue = useCallback((value) => {
|
|
3069
|
+
const data = mjDataRef.current;
|
|
3070
|
+
if (!data || actuatorIdRef.current < 0) return;
|
|
3071
|
+
data.ctrl[actuatorIdRef.current] = value;
|
|
3072
|
+
valueRef.current = value;
|
|
3073
|
+
}, [mjDataRef]);
|
|
3074
|
+
return [valueRef, setValue];
|
|
3075
|
+
}
|
|
3076
|
+
function useKeyboardTeleop(config) {
|
|
3077
|
+
const { mjModelRef, mjDataRef, status } = useMujocoSim();
|
|
3078
|
+
const pressedRef = useRef(/* @__PURE__ */ new Set());
|
|
3079
|
+
const toggleStateRef = useRef(/* @__PURE__ */ new Map());
|
|
3080
|
+
const enabledRef = useRef(config.enabled ?? true);
|
|
3081
|
+
enabledRef.current = config.enabled ?? true;
|
|
3082
|
+
const bindingsRef = useRef(config.bindings);
|
|
3083
|
+
bindingsRef.current = config.bindings;
|
|
3084
|
+
const actuatorCacheRef = useRef(/* @__PURE__ */ new Map());
|
|
3085
|
+
useEffect(() => {
|
|
3086
|
+
const model = mjModelRef.current;
|
|
3087
|
+
if (!model || status !== "ready") return;
|
|
3088
|
+
const cache = /* @__PURE__ */ new Map();
|
|
3089
|
+
for (const binding of Object.values(config.bindings)) {
|
|
3090
|
+
if (!cache.has(binding.actuator)) {
|
|
3091
|
+
cache.set(binding.actuator, findActuatorByName(model, binding.actuator));
|
|
3092
|
+
}
|
|
3093
|
+
}
|
|
3094
|
+
actuatorCacheRef.current = cache;
|
|
3095
|
+
}, [config.bindings, status, mjModelRef]);
|
|
3096
|
+
useEffect(() => {
|
|
3097
|
+
const onKeyDown = (e) => {
|
|
3098
|
+
if (!enabledRef.current) return;
|
|
3099
|
+
const key = e.key.toLowerCase();
|
|
3100
|
+
if (bindingsRef.current[key]) {
|
|
3101
|
+
pressedRef.current.add(key);
|
|
3102
|
+
const binding = bindingsRef.current[key];
|
|
3103
|
+
if (binding.toggle) {
|
|
3104
|
+
const current = toggleStateRef.current.get(key) ?? false;
|
|
3105
|
+
toggleStateRef.current.set(key, !current);
|
|
3106
|
+
}
|
|
3107
|
+
}
|
|
3108
|
+
};
|
|
3109
|
+
const onKeyUp = (e) => {
|
|
3110
|
+
pressedRef.current.delete(e.key.toLowerCase());
|
|
3111
|
+
};
|
|
3112
|
+
window.addEventListener("keydown", onKeyDown);
|
|
3113
|
+
window.addEventListener("keyup", onKeyUp);
|
|
3114
|
+
return () => {
|
|
3115
|
+
window.removeEventListener("keydown", onKeyDown);
|
|
3116
|
+
window.removeEventListener("keyup", onKeyUp);
|
|
3117
|
+
};
|
|
3118
|
+
}, []);
|
|
3119
|
+
useBeforePhysicsStep((_model, data) => {
|
|
3120
|
+
if (!enabledRef.current) return;
|
|
3121
|
+
const bindings = bindingsRef.current;
|
|
3122
|
+
const cache = actuatorCacheRef.current;
|
|
3123
|
+
for (const [key, binding] of Object.entries(bindings)) {
|
|
3124
|
+
const actId = cache.get(binding.actuator);
|
|
3125
|
+
if (actId === void 0 || actId < 0) continue;
|
|
3126
|
+
if (binding.toggle) {
|
|
3127
|
+
const state = toggleStateRef.current.get(key) ?? false;
|
|
3128
|
+
data.ctrl[actId] = state ? binding.toggle[1] : binding.toggle[0];
|
|
3129
|
+
} else if (pressedRef.current.has(key)) {
|
|
3130
|
+
if (binding.delta !== void 0) {
|
|
3131
|
+
data.ctrl[actId] += binding.delta;
|
|
3132
|
+
} else if (binding.set !== void 0) {
|
|
3133
|
+
data.ctrl[actId] = binding.set;
|
|
3134
|
+
}
|
|
3135
|
+
}
|
|
3136
|
+
}
|
|
3137
|
+
});
|
|
3138
|
+
}
|
|
3139
|
+
function usePolicy(config) {
|
|
3140
|
+
const { mjModelRef } = useMujocoSim();
|
|
3141
|
+
const lastActionTimeRef = useRef(0);
|
|
3142
|
+
const lastActionRef = useRef(null);
|
|
3143
|
+
const isRunningRef = useRef(true);
|
|
3144
|
+
const configRef = useRef(config);
|
|
3145
|
+
configRef.current = config;
|
|
3146
|
+
useBeforePhysicsStep((model, data) => {
|
|
3147
|
+
if (!isRunningRef.current) return;
|
|
3148
|
+
const cfg = configRef.current;
|
|
3149
|
+
model.opt?.timestep ?? 2e-3;
|
|
3150
|
+
const interval = 1 / cfg.frequency;
|
|
3151
|
+
if (data.time - lastActionTimeRef.current >= interval) {
|
|
3152
|
+
const obs = cfg.onObservation(model, data);
|
|
3153
|
+
cfg.onAction(obs, model, data);
|
|
3154
|
+
lastActionTimeRef.current = data.time;
|
|
3155
|
+
lastActionRef.current = obs;
|
|
3156
|
+
}
|
|
3157
|
+
});
|
|
3158
|
+
return {
|
|
3159
|
+
get isRunning() {
|
|
3160
|
+
return isRunningRef.current;
|
|
3161
|
+
},
|
|
3162
|
+
start: () => {
|
|
3163
|
+
isRunningRef.current = true;
|
|
3164
|
+
},
|
|
3165
|
+
stop: () => {
|
|
3166
|
+
isRunningRef.current = false;
|
|
3167
|
+
},
|
|
3168
|
+
get lastObservation() {
|
|
3169
|
+
return lastActionRef.current;
|
|
3170
|
+
}
|
|
3171
|
+
};
|
|
3172
|
+
}
|
|
3173
|
+
function useTrajectoryRecorder(options = {}) {
|
|
3174
|
+
const { mjModelRef } = useMujocoSim();
|
|
3175
|
+
const recordingRef = useRef(false);
|
|
3176
|
+
const framesRef = useRef([]);
|
|
3177
|
+
const fields = options.fields ?? ["qpos"];
|
|
3178
|
+
useAfterPhysicsStep((_model, data) => {
|
|
3179
|
+
if (!recordingRef.current) return;
|
|
3180
|
+
const frame = {
|
|
3181
|
+
time: data.time,
|
|
3182
|
+
qpos: new Float64Array(data.qpos)
|
|
3183
|
+
};
|
|
3184
|
+
if (fields.includes("qvel")) frame.qvel = new Float64Array(data.qvel);
|
|
3185
|
+
if (fields.includes("ctrl")) frame.ctrl = new Float64Array(data.ctrl);
|
|
3186
|
+
if (fields.includes("sensordata") && data.sensordata) {
|
|
3187
|
+
frame.sensordata = new Float64Array(data.sensordata);
|
|
3188
|
+
}
|
|
3189
|
+
framesRef.current.push(frame);
|
|
3190
|
+
});
|
|
3191
|
+
const start = useCallback(() => {
|
|
3192
|
+
framesRef.current = [];
|
|
3193
|
+
recordingRef.current = true;
|
|
3194
|
+
}, []);
|
|
3195
|
+
const stop = useCallback(() => {
|
|
3196
|
+
recordingRef.current = false;
|
|
3197
|
+
return framesRef.current;
|
|
3198
|
+
}, []);
|
|
3199
|
+
const downloadJSON = useCallback(() => {
|
|
3200
|
+
const frames = framesRef.current;
|
|
3201
|
+
const data = frames.map((f) => ({
|
|
3202
|
+
time: f.time,
|
|
3203
|
+
qpos: Array.from(f.qpos),
|
|
3204
|
+
...f.qvel ? { qvel: Array.from(f.qvel) } : {},
|
|
3205
|
+
...f.ctrl ? { ctrl: Array.from(f.ctrl) } : {},
|
|
3206
|
+
...f.sensordata ? { sensordata: Array.from(f.sensordata) } : {}
|
|
3207
|
+
}));
|
|
3208
|
+
const blob = new Blob([JSON.stringify(data, null, 2)], { type: "application/json" });
|
|
3209
|
+
const url = URL.createObjectURL(blob);
|
|
3210
|
+
const a = document.createElement("a");
|
|
3211
|
+
a.href = url;
|
|
3212
|
+
a.download = "trajectory.json";
|
|
3213
|
+
a.click();
|
|
3214
|
+
URL.revokeObjectURL(url);
|
|
3215
|
+
}, []);
|
|
3216
|
+
const downloadCSV = useCallback(() => {
|
|
3217
|
+
const frames = framesRef.current;
|
|
3218
|
+
if (frames.length === 0) return;
|
|
3219
|
+
const nq = frames[0].qpos.length;
|
|
3220
|
+
const headers = ["time", ...Array.from({ length: nq }, (_, i) => `qpos_${i}`)];
|
|
3221
|
+
const rows = frames.map(
|
|
3222
|
+
(f) => [f.time, ...Array.from(f.qpos)].join(",")
|
|
3223
|
+
);
|
|
3224
|
+
const csv = [headers.join(","), ...rows].join("\n");
|
|
3225
|
+
const blob = new Blob([csv], { type: "text/csv" });
|
|
3226
|
+
const url = URL.createObjectURL(blob);
|
|
3227
|
+
const a = document.createElement("a");
|
|
3228
|
+
a.href = url;
|
|
3229
|
+
a.download = "trajectory.csv";
|
|
3230
|
+
a.click();
|
|
3231
|
+
URL.revokeObjectURL(url);
|
|
3232
|
+
}, []);
|
|
3233
|
+
return {
|
|
3234
|
+
start,
|
|
3235
|
+
stop,
|
|
3236
|
+
downloadJSON,
|
|
3237
|
+
downloadCSV,
|
|
3238
|
+
get recording() {
|
|
3239
|
+
return recordingRef.current;
|
|
3240
|
+
},
|
|
3241
|
+
get frameCount() {
|
|
3242
|
+
return framesRef.current.length;
|
|
3243
|
+
},
|
|
3244
|
+
get frames() {
|
|
3245
|
+
return framesRef.current;
|
|
3246
|
+
}
|
|
3247
|
+
};
|
|
3248
|
+
}
|
|
3249
|
+
function useGamepad(config) {
|
|
3250
|
+
const { mjModelRef, status } = useMujocoSim();
|
|
3251
|
+
const configRef = useRef(config);
|
|
3252
|
+
configRef.current = config;
|
|
3253
|
+
const axisCacheRef = useRef(/* @__PURE__ */ new Map());
|
|
3254
|
+
const buttonCacheRef = useRef(/* @__PURE__ */ new Map());
|
|
3255
|
+
useEffect(() => {
|
|
3256
|
+
const model = mjModelRef.current;
|
|
3257
|
+
if (!model || status !== "ready") return;
|
|
3258
|
+
axisCacheRef.current.clear();
|
|
3259
|
+
buttonCacheRef.current.clear();
|
|
3260
|
+
for (const [idx, name] of Object.entries(config.axes ?? {})) {
|
|
3261
|
+
axisCacheRef.current.set(Number(idx), findActuatorByName(model, name));
|
|
3262
|
+
}
|
|
3263
|
+
for (const [idx, name] of Object.entries(config.buttons ?? {})) {
|
|
3264
|
+
buttonCacheRef.current.set(Number(idx), findActuatorByName(model, name));
|
|
3265
|
+
}
|
|
3266
|
+
}, [config.axes, config.buttons, status, mjModelRef]);
|
|
3267
|
+
useBeforePhysicsStep((_model, data) => {
|
|
3268
|
+
const cfg = configRef.current;
|
|
3269
|
+
if (cfg.enabled === false) return;
|
|
3270
|
+
const gamepads = navigator.getGamepads?.();
|
|
3271
|
+
if (!gamepads) return;
|
|
3272
|
+
const gp = gamepads[cfg.gamepadIndex ?? 0];
|
|
3273
|
+
if (!gp) return;
|
|
3274
|
+
const deadzone = cfg.deadzone ?? 0.1;
|
|
3275
|
+
const scale = cfg.scale ?? 1;
|
|
3276
|
+
for (const [axisIdx, actId] of axisCacheRef.current) {
|
|
3277
|
+
if (actId < 0 || axisIdx >= gp.axes.length) continue;
|
|
3278
|
+
let val = gp.axes[axisIdx];
|
|
3279
|
+
if (Math.abs(val) < deadzone) val = 0;
|
|
3280
|
+
data.ctrl[actId] = val * scale;
|
|
3281
|
+
}
|
|
3282
|
+
for (const [btnIdx, actId] of buttonCacheRef.current) {
|
|
3283
|
+
if (actId < 0 || btnIdx >= gp.buttons.length) continue;
|
|
3284
|
+
data.ctrl[actId] = gp.buttons[btnIdx].value;
|
|
3285
|
+
}
|
|
3286
|
+
});
|
|
3287
|
+
}
|
|
3288
|
+
function useVideoRecorder(options = {}) {
|
|
3289
|
+
const { gl } = useThree();
|
|
3290
|
+
const recorderRef = useRef(null);
|
|
3291
|
+
const chunksRef = useRef([]);
|
|
3292
|
+
const recordingRef = useRef(false);
|
|
3293
|
+
const start = useCallback(() => {
|
|
3294
|
+
const canvas = gl.domElement;
|
|
3295
|
+
const fps = options.fps ?? 30;
|
|
3296
|
+
const mimeType = options.mimeType ?? "video/webm";
|
|
3297
|
+
const stream = canvas.captureStream(fps);
|
|
3298
|
+
const recorder = new MediaRecorder(stream, {
|
|
3299
|
+
mimeType: MediaRecorder.isTypeSupported(mimeType) ? mimeType : "video/webm"
|
|
3300
|
+
});
|
|
3301
|
+
chunksRef.current = [];
|
|
3302
|
+
recorder.ondataavailable = (e) => {
|
|
3303
|
+
if (e.data.size > 0) chunksRef.current.push(e.data);
|
|
3304
|
+
};
|
|
3305
|
+
recorder.start();
|
|
3306
|
+
recorderRef.current = recorder;
|
|
3307
|
+
recordingRef.current = true;
|
|
3308
|
+
}, [gl, options.fps, options.mimeType]);
|
|
3309
|
+
const stop = useCallback(() => {
|
|
3310
|
+
return new Promise((resolve) => {
|
|
3311
|
+
const recorder = recorderRef.current;
|
|
3312
|
+
if (!recorder || recorder.state === "inactive") {
|
|
3313
|
+
resolve(new Blob([]));
|
|
3314
|
+
return;
|
|
3315
|
+
}
|
|
3316
|
+
recorder.onstop = () => {
|
|
3317
|
+
const blob = new Blob(chunksRef.current, { type: recorder.mimeType });
|
|
3318
|
+
chunksRef.current = [];
|
|
3319
|
+
recordingRef.current = false;
|
|
3320
|
+
recorderRef.current = null;
|
|
3321
|
+
resolve(blob);
|
|
3322
|
+
};
|
|
3323
|
+
recorder.stop();
|
|
3324
|
+
});
|
|
3325
|
+
}, []);
|
|
3326
|
+
const download = useCallback(async (filename = "recording.webm") => {
|
|
3327
|
+
const blob = await stop();
|
|
3328
|
+
if (blob.size === 0) return;
|
|
3329
|
+
const url = URL.createObjectURL(blob);
|
|
3330
|
+
const a = document.createElement("a");
|
|
3331
|
+
a.href = url;
|
|
3332
|
+
a.download = filename;
|
|
3333
|
+
a.click();
|
|
3334
|
+
URL.revokeObjectURL(url);
|
|
3335
|
+
}, [stop]);
|
|
3336
|
+
return {
|
|
3337
|
+
start,
|
|
3338
|
+
stop,
|
|
3339
|
+
download,
|
|
3340
|
+
get recording() {
|
|
3341
|
+
return recordingRef.current;
|
|
3342
|
+
}
|
|
3343
|
+
};
|
|
3344
|
+
}
|
|
3345
|
+
function useCtrlNoise(config = {}) {
|
|
3346
|
+
const { mjModelRef } = useMujocoSim();
|
|
3347
|
+
const configRef = useRef(config);
|
|
3348
|
+
configRef.current = config;
|
|
3349
|
+
const noiseRef = useRef(null);
|
|
3350
|
+
useBeforePhysicsStep((_model, data) => {
|
|
3351
|
+
const cfg = configRef.current;
|
|
3352
|
+
if (cfg.enabled === false) return;
|
|
3353
|
+
const rate = cfg.rate ?? 0.01;
|
|
3354
|
+
const std = cfg.std ?? 0.05;
|
|
3355
|
+
const nu = mjModelRef.current?.nu ?? 0;
|
|
3356
|
+
if (nu === 0) return;
|
|
3357
|
+
if (!noiseRef.current || noiseRef.current.length !== nu) {
|
|
3358
|
+
noiseRef.current = new Float64Array(nu);
|
|
3359
|
+
}
|
|
3360
|
+
const noise = noiseRef.current;
|
|
3361
|
+
for (let i = 0; i < nu; i++) {
|
|
3362
|
+
const u1 = Math.random();
|
|
3363
|
+
const u2 = Math.random();
|
|
3364
|
+
const gaussian = Math.sqrt(-2 * Math.log(u1)) * Math.cos(2 * Math.PI * u2);
|
|
3365
|
+
noise[i] = (1 - rate) * noise[i] + rate * gaussian * std;
|
|
3366
|
+
data.ctrl[i] += noise[i];
|
|
3367
|
+
}
|
|
3368
|
+
});
|
|
3369
|
+
}
|
|
3370
|
+
/**
|
|
3371
|
+
* @license
|
|
3372
|
+
* SPDX-License-Identifier: Apache-2.0
|
|
3373
|
+
*/
|
|
3374
|
+
/**
|
|
3375
|
+
* @license
|
|
3376
|
+
* SPDX-License-Identifier: Apache-2.0
|
|
3377
|
+
*/
|
|
3378
|
+
/**
|
|
3379
|
+
* @license
|
|
3380
|
+
* SPDX-License-Identifier: Apache-2.0
|
|
3381
|
+
*
|
|
3382
|
+
* ContactMarkers — instanced sphere visualization of MuJoCo contacts (spec 6.2)
|
|
3383
|
+
*
|
|
3384
|
+
* Fixed from original: reads data.ncon first, accesses contact via .get(i),
|
|
3385
|
+
* limits to maxContacts to avoid WASM heap OOM.
|
|
3386
|
+
*/
|
|
3387
|
+
/**
|
|
3388
|
+
* @license
|
|
3389
|
+
* SPDX-License-Identifier: Apache-2.0
|
|
3390
|
+
*
|
|
3391
|
+
* SceneLights — auto-create Three.js lights from MJCF <light> elements (spec 6.3)
|
|
3392
|
+
*
|
|
3393
|
+
* WASM fields used: model.nlight, light_pos, light_dir, light_diffuse,
|
|
3394
|
+
* light_specular, light_active, light_type, light_castshadow,
|
|
3395
|
+
* light_attenuation, light_cutoff, light_exponent, light_intensity
|
|
3396
|
+
*
|
|
3397
|
+
* light_type: 0 = directional, 1 = spot (maps to mjLIGHT_DIRECTIONAL/mjLIGHT_SPOT)
|
|
3398
|
+
* Note: light_directional does NOT exist in WASM — use light_type instead.
|
|
3399
|
+
*/
|
|
3400
|
+
/**
|
|
3401
|
+
* @license
|
|
3402
|
+
* SPDX-License-Identifier: Apache-2.0
|
|
3403
|
+
*
|
|
3404
|
+
* Debug — visualization overlay for MuJoCo scene elements (spec 6.1)
|
|
3405
|
+
*/
|
|
3406
|
+
/**
|
|
3407
|
+
* @license
|
|
3408
|
+
* SPDX-License-Identifier: Apache-2.0
|
|
3409
|
+
*
|
|
3410
|
+
* TendonRenderer — render tendons as tube geometries (spec 6.4)
|
|
3411
|
+
*
|
|
3412
|
+
* WASM fields used: model.ntendon, model.ten_wrapadr, model.ten_wrapnum
|
|
3413
|
+
* data.wrap_xpos, data.ten_wrapadr (runtime)
|
|
3414
|
+
*
|
|
3415
|
+
* Note: ten_rgba and ten_width are NOT available in mujoco-js 0.0.7.
|
|
3416
|
+
* Tendons use a default color and width.
|
|
3417
|
+
*/
|
|
3418
|
+
/**
|
|
3419
|
+
* @license
|
|
3420
|
+
* SPDX-License-Identifier: Apache-2.0
|
|
3421
|
+
*
|
|
3422
|
+
* FlexRenderer — render deformable flex bodies (spec 6.4)
|
|
3423
|
+
*/
|
|
3424
|
+
/**
|
|
3425
|
+
* @license
|
|
3426
|
+
* SPDX-License-Identifier: Apache-2.0
|
|
3427
|
+
*
|
|
3428
|
+
* useContacts — structured contact query hook (spec 2.4)
|
|
3429
|
+
* useContactEvents — contact enter/exit events (spec 2.5)
|
|
3430
|
+
*/
|
|
3431
|
+
/**
|
|
3432
|
+
* @license
|
|
3433
|
+
* SPDX-License-Identifier: Apache-2.0
|
|
3434
|
+
*
|
|
3435
|
+
* ContactListener — component form of contact events (spec 2.5)
|
|
3436
|
+
*/
|
|
3437
|
+
/**
|
|
3438
|
+
* @license
|
|
3439
|
+
* SPDX-License-Identifier: Apache-2.0
|
|
3440
|
+
*
|
|
3441
|
+
* useTrajectoryPlayer — trajectory playback/scrubbing (spec 13.2)
|
|
3442
|
+
*/
|
|
3443
|
+
/**
|
|
3444
|
+
* @license
|
|
3445
|
+
* SPDX-License-Identifier: Apache-2.0
|
|
3446
|
+
*
|
|
3447
|
+
* TrajectoryPlayer — component form of trajectory playback (spec 13.2)
|
|
3448
|
+
*/
|
|
3449
|
+
/**
|
|
3450
|
+
* @license
|
|
3451
|
+
* SPDX-License-Identifier: Apache-2.0
|
|
3452
|
+
*
|
|
3453
|
+
* SelectionHighlight — highlight a selected body with emissive color (spec 6.5)
|
|
3454
|
+
*/
|
|
3455
|
+
/**
|
|
3456
|
+
* @license
|
|
3457
|
+
* SPDX-License-Identifier: Apache-2.0
|
|
3458
|
+
*
|
|
3459
|
+
* useSensor / useSensors — MuJoCo sensor access hooks (spec 2.1)
|
|
3460
|
+
*/
|
|
3461
|
+
/**
|
|
3462
|
+
* @license
|
|
3463
|
+
* SPDX-License-Identifier: Apache-2.0
|
|
3464
|
+
*
|
|
3465
|
+
* useJointState — per-joint position/velocity access (spec 2.3)
|
|
3466
|
+
*/
|
|
3467
|
+
/**
|
|
3468
|
+
* @license
|
|
3469
|
+
* SPDX-License-Identifier: Apache-2.0
|
|
3470
|
+
*
|
|
3471
|
+
* useBodyState — per-body position/velocity tracking (spec 2.2)
|
|
3472
|
+
*/
|
|
3473
|
+
/**
|
|
3474
|
+
* @license
|
|
3475
|
+
* SPDX-License-Identifier: Apache-2.0
|
|
3476
|
+
*
|
|
3477
|
+
* useCtrl — clean read/write access to a named actuator's ctrl value (spec 3.1)
|
|
3478
|
+
*/
|
|
3479
|
+
/**
|
|
3480
|
+
* @license
|
|
3481
|
+
* SPDX-License-Identifier: Apache-2.0
|
|
3482
|
+
*
|
|
3483
|
+
* useKeyboardTeleop — keyboard teleoperation hook (spec 12.1)
|
|
3484
|
+
*/
|
|
3485
|
+
/**
|
|
3486
|
+
* @license
|
|
3487
|
+
* SPDX-License-Identifier: Apache-2.0
|
|
3488
|
+
*
|
|
3489
|
+
* usePolicy — policy decimation loop hook (spec 10.1)
|
|
3490
|
+
*/
|
|
3491
|
+
/**
|
|
3492
|
+
* @license
|
|
3493
|
+
* SPDX-License-Identifier: Apache-2.0
|
|
3494
|
+
*
|
|
3495
|
+
* useTrajectoryRecorder — trajectory recording hook (spec 13.1)
|
|
3496
|
+
*/
|
|
3497
|
+
/**
|
|
3498
|
+
* @license
|
|
3499
|
+
* SPDX-License-Identifier: Apache-2.0
|
|
3500
|
+
*
|
|
3501
|
+
* useGamepad — gamepad teleoperation hook (spec 12.2)
|
|
3502
|
+
*/
|
|
3503
|
+
/**
|
|
3504
|
+
* @license
|
|
3505
|
+
* SPDX-License-Identifier: Apache-2.0
|
|
3506
|
+
*
|
|
3507
|
+
* useVideoRecorder — canvas video recording hook (spec 13.3)
|
|
3508
|
+
*/
|
|
3509
|
+
/**
|
|
3510
|
+
* @license
|
|
3511
|
+
* SPDX-License-Identifier: Apache-2.0
|
|
3512
|
+
*
|
|
3513
|
+
* useCtrlNoise — control noise / perturbation hook (spec 3.2)
|
|
3514
|
+
*/
|
|
3515
|
+
|
|
3516
|
+
export { ContactListener, ContactMarkers, Debug, DragInteraction, FlexRenderer, IkGizmo, MujocoCanvas, MujocoProvider, MujocoSimProvider, SceneLights, SceneRenderer, SelectionHighlight, TendonRenderer, TrajectoryPlayer, findActuatorByName, findBodyByName, findGeomByName, findJointByName, findKeyframeByName, findSensorByName, findSiteByName, findTendonByName, getName, loadScene, useActuators, useAfterPhysicsStep, useBeforePhysicsStep, useBodyState, useContactEvents, useContacts, useCtrl, useCtrlNoise, useGamepad, useGravityCompensation, useJointState, useKeyboardTeleop, useMujoco, useMujocoSim, usePolicy, useSensor, useSensors, useSitePosition, useTrajectoryPlayer, useTrajectoryRecorder, useVideoRecorder };
|
|
3517
|
+
//# sourceMappingURL=index.js.map
|
|
3518
|
+
//# sourceMappingURL=index.js.map
|