mujoco-react 0.1.0 → 0.3.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/README.md +209 -45
- package/dist/index.d.ts +180 -97
- package/dist/index.js +1148 -772
- package/dist/index.js.map +1 -1
- package/package.json +1 -1
- package/src/components/ContactMarkers.tsx +12 -19
- package/src/components/Debug.tsx +168 -33
- package/src/components/DragInteraction.tsx +1 -1
- package/src/components/IkController.tsx +262 -0
- package/src/components/IkGizmo.tsx +17 -25
- package/src/components/SceneLights.tsx +2 -112
- package/src/components/SceneRenderer.tsx +8 -6
- package/src/components/SelectionHighlight.tsx +2 -49
- package/src/components/TendonRenderer.tsx +90 -26
- package/src/components/TrajectoryPlayer.tsx +14 -10
- package/src/core/IkContext.tsx +40 -0
- package/src/core/MujocoCanvas.tsx +6 -3
- package/src/core/MujocoProvider.tsx +12 -4
- package/src/core/MujocoSimProvider.tsx +69 -334
- package/src/core/SceneLoader.ts +44 -11
- package/src/core/createController.tsx +91 -0
- package/src/hooks/useCameraAnimation.ts +102 -0
- package/src/hooks/useContacts.ts +52 -22
- package/src/hooks/useJointState.ts +18 -2
- package/src/hooks/useSceneLights.ts +117 -0
- package/src/hooks/useSelectionHighlight.ts +65 -0
- package/src/index.ts +16 -1
- package/src/types.ts +59 -22
package/dist/index.js
CHANGED
|
@@ -2,7 +2,7 @@ import loadMujoco from 'mujoco-js';
|
|
|
2
2
|
import { createContext, forwardRef, useEffect, useContext, useState, useRef, useCallback, useMemo } from 'react';
|
|
3
3
|
import { jsx, jsxs, Fragment } from 'react/jsx-runtime';
|
|
4
4
|
import { Canvas, useThree, useFrame } from '@react-three/fiber';
|
|
5
|
-
import * as
|
|
5
|
+
import * as THREE11 from 'three';
|
|
6
6
|
import { PivotControls } from '@react-three/drei';
|
|
7
7
|
|
|
8
8
|
// src/core/MujocoProvider.tsx
|
|
@@ -14,14 +14,14 @@ var MujocoContext = createContext({
|
|
|
14
14
|
function useMujoco() {
|
|
15
15
|
return useContext(MujocoContext);
|
|
16
16
|
}
|
|
17
|
-
function MujocoProvider({ wasmUrl, children, onError }) {
|
|
17
|
+
function MujocoProvider({ wasmUrl, timeout = 3e4, children, onError }) {
|
|
18
18
|
const [status, setStatus] = useState("loading");
|
|
19
19
|
const [error, setError] = useState(null);
|
|
20
20
|
const moduleRef = useRef(null);
|
|
21
21
|
const isMounted = useRef(true);
|
|
22
22
|
useEffect(() => {
|
|
23
23
|
isMounted.current = true;
|
|
24
|
-
loadMujoco({
|
|
24
|
+
const wasmPromise = loadMujoco({
|
|
25
25
|
...wasmUrl ? { locateFile: (path) => path.endsWith(".wasm") ? wasmUrl : path } : {},
|
|
26
26
|
printErr: (text) => {
|
|
27
27
|
if (text.includes("Aborted") && isMounted.current) {
|
|
@@ -29,7 +29,11 @@ function MujocoProvider({ wasmUrl, children, onError }) {
|
|
|
29
29
|
setStatus("error");
|
|
30
30
|
}
|
|
31
31
|
}
|
|
32
|
-
})
|
|
32
|
+
});
|
|
33
|
+
const timeoutPromise = new Promise(
|
|
34
|
+
(_, reject) => setTimeout(() => reject(new Error(`WASM module load timed out after ${timeout}ms`)), timeout)
|
|
35
|
+
);
|
|
36
|
+
Promise.race([wasmPromise, timeoutPromise]).then((inst) => {
|
|
33
37
|
if (isMounted.current) {
|
|
34
38
|
moduleRef.current = inst;
|
|
35
39
|
setStatus("ready");
|
|
@@ -45,7 +49,7 @@ function MujocoProvider({ wasmUrl, children, onError }) {
|
|
|
45
49
|
return () => {
|
|
46
50
|
isMounted.current = false;
|
|
47
51
|
};
|
|
48
|
-
}, [wasmUrl]);
|
|
52
|
+
}, [wasmUrl, timeout]);
|
|
49
53
|
return /* @__PURE__ */ jsx(
|
|
50
54
|
MujocoContext.Provider,
|
|
51
55
|
{
|
|
@@ -55,229 +59,12 @@ function MujocoProvider({ wasmUrl, children, onError }) {
|
|
|
55
59
|
);
|
|
56
60
|
}
|
|
57
61
|
|
|
58
|
-
// src/
|
|
59
|
-
|
|
60
|
-
|
|
61
|
-
|
|
62
|
-
|
|
63
|
-
|
|
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];
|
|
62
|
+
// src/types.ts
|
|
63
|
+
function getContact(data, i) {
|
|
64
|
+
try {
|
|
65
|
+
return data.contact.get(i);
|
|
66
|
+
} catch {
|
|
67
|
+
return void 0;
|
|
281
68
|
}
|
|
282
69
|
}
|
|
283
70
|
|
|
@@ -340,6 +127,16 @@ function findTendonByName(mjModel, name) {
|
|
|
340
127
|
}
|
|
341
128
|
return -1;
|
|
342
129
|
}
|
|
130
|
+
function getActuatedScalarQposAdr(mjModel, actuatorId) {
|
|
131
|
+
if (actuatorId < 0 || actuatorId >= mjModel.nu) return -1;
|
|
132
|
+
const trnType = mjModel.actuator_trntype?.[actuatorId];
|
|
133
|
+
if (trnType !== void 0 && trnType !== 0 && trnType !== 1) return -1;
|
|
134
|
+
const jointId = mjModel.actuator_trnid[2 * actuatorId];
|
|
135
|
+
if (jointId < 0 || jointId >= mjModel.njnt) return -1;
|
|
136
|
+
const jntType = mjModel.jnt_type[jointId];
|
|
137
|
+
if (jntType !== 2 && jntType !== 3) return -1;
|
|
138
|
+
return mjModel.jnt_qposadr[jointId];
|
|
139
|
+
}
|
|
343
140
|
function sceneObjectToXml(obj) {
|
|
344
141
|
const joint = obj.freejoint ? "<freejoint/>" : "";
|
|
345
142
|
const pos = obj.position.map((v) => v.toFixed(3)).join(" ");
|
|
@@ -390,7 +187,13 @@ async function loadScene(mujoco, config, onProgress) {
|
|
|
390
187
|
for (const patch of config.xmlPatches ?? []) {
|
|
391
188
|
if (fname.endsWith(patch.target) || fname === patch.target) {
|
|
392
189
|
if (patch.replace) {
|
|
393
|
-
|
|
190
|
+
const [from, to] = patch.replace;
|
|
191
|
+
if (text.includes(from)) {
|
|
192
|
+
text = text.replace(from, to);
|
|
193
|
+
} else {
|
|
194
|
+
const preview = from.length > 80 ? `${from.slice(0, 80)}...` : from;
|
|
195
|
+
console.warn(`XML patch replace pattern not found in ${fname}: "${preview}"`);
|
|
196
|
+
}
|
|
394
197
|
}
|
|
395
198
|
if (patch.inject && patch.injectAfter) {
|
|
396
199
|
const idx = text.indexOf(patch.injectAfter);
|
|
@@ -398,7 +201,12 @@ async function loadScene(mujoco, config, onProgress) {
|
|
|
398
201
|
const tagEnd = text.indexOf(">", idx + patch.injectAfter.length);
|
|
399
202
|
if (tagEnd !== -1) {
|
|
400
203
|
text = text.slice(0, tagEnd + 1) + patch.inject + text.slice(tagEnd + 1);
|
|
204
|
+
} else {
|
|
205
|
+
console.warn(`XML patch inject failed in ${fname}: could not find tag end after "${patch.injectAfter}"`);
|
|
401
206
|
}
|
|
207
|
+
} else {
|
|
208
|
+
const preview = patch.injectAfter.length > 80 ? `${patch.injectAfter.slice(0, 80)}...` : patch.injectAfter;
|
|
209
|
+
console.warn(`XML patch inject anchor not found in ${fname}: "${preview}"`);
|
|
402
210
|
}
|
|
403
211
|
}
|
|
404
212
|
}
|
|
@@ -420,14 +228,12 @@ async function loadScene(mujoco, config, onProgress) {
|
|
|
420
228
|
const siteId = findSiteByName(mjModel, config.tcpSiteName ?? "tcp");
|
|
421
229
|
const gripperId = findActuatorByName(mjModel, config.gripperActuatorName ?? "gripper");
|
|
422
230
|
if (config.homeJoints) {
|
|
423
|
-
|
|
231
|
+
const homeCount = Math.min(config.homeJoints.length, mjModel.nu);
|
|
232
|
+
for (let i = 0; i < homeCount; i++) {
|
|
424
233
|
mjData.ctrl[i] = config.homeJoints[i];
|
|
425
|
-
|
|
426
|
-
|
|
427
|
-
|
|
428
|
-
const qposAdr = mjModel.jnt_qposadr[jointId];
|
|
429
|
-
mjData.qpos[qposAdr] = config.homeJoints[i];
|
|
430
|
-
}
|
|
234
|
+
const qposAdr = getActuatedScalarQposAdr(mjModel, i);
|
|
235
|
+
if (qposAdr !== -1) {
|
|
236
|
+
mjData.qpos[qposAdr] = config.homeJoints[i];
|
|
431
237
|
}
|
|
432
238
|
}
|
|
433
239
|
}
|
|
@@ -437,8 +243,9 @@ async function loadScene(mujoco, config, onProgress) {
|
|
|
437
243
|
function scanDependencies(xmlString, currentFile, parser, downloaded, queue) {
|
|
438
244
|
const xmlDoc = parser.parseFromString(xmlString, "text/xml");
|
|
439
245
|
const compiler = xmlDoc.querySelector("compiler");
|
|
440
|
-
const
|
|
441
|
-
const
|
|
246
|
+
const assetDir = compiler?.getAttribute("assetdir") || "";
|
|
247
|
+
const meshDir = compiler?.getAttribute("meshdir") || assetDir;
|
|
248
|
+
const textureDir = compiler?.getAttribute("texturedir") || assetDir;
|
|
442
249
|
const currentDir = currentFile.includes("/") ? currentFile.substring(0, currentFile.lastIndexOf("/") + 1) : "";
|
|
443
250
|
xmlDoc.querySelectorAll("[file]").forEach((el) => {
|
|
444
251
|
const fileAttr = el.getAttribute("file");
|
|
@@ -519,6 +326,8 @@ var _applyPoint = new Float64Array(3);
|
|
|
519
326
|
var _rayPnt = new Float64Array(3);
|
|
520
327
|
var _rayVec = new Float64Array(3);
|
|
521
328
|
var _rayGeomId = new Int32Array(1);
|
|
329
|
+
var _projRaycaster = new THREE11.Raycaster();
|
|
330
|
+
var _projNdc = new THREE11.Vector2();
|
|
522
331
|
var MujocoSimContext = createContext(null);
|
|
523
332
|
function useMujocoSim() {
|
|
524
333
|
const ctx = useContext(MujocoSimContext);
|
|
@@ -553,6 +362,7 @@ function useAfterPhysicsStep(callback) {
|
|
|
553
362
|
function MujocoSimProvider({
|
|
554
363
|
mujoco,
|
|
555
364
|
config,
|
|
365
|
+
apiRef: externalApiRef,
|
|
556
366
|
onReady,
|
|
557
367
|
onError,
|
|
558
368
|
onStep,
|
|
@@ -571,16 +381,12 @@ function MujocoSimProvider({
|
|
|
571
381
|
const mjDataRef = useRef(null);
|
|
572
382
|
const mujocoRef = useRef(mujoco);
|
|
573
383
|
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
384
|
const pausedRef = useRef(paused ?? false);
|
|
579
385
|
const speedRef = useRef(speed ?? 1);
|
|
580
386
|
const substepsRef = useRef(substeps ?? 1);
|
|
581
387
|
const interpolateRef = useRef(interpolate ?? false);
|
|
582
|
-
const firstIkEnableRef = useRef(true);
|
|
583
388
|
const stepsToRunRef = useRef(0);
|
|
389
|
+
const loadGenRef = useRef(0);
|
|
584
390
|
useRef(null);
|
|
585
391
|
useRef(null);
|
|
586
392
|
useRef(0);
|
|
@@ -590,6 +396,7 @@ function MujocoSimProvider({
|
|
|
590
396
|
onStepRef.current = onStep;
|
|
591
397
|
const beforeStepCallbacks = useRef(/* @__PURE__ */ new Set());
|
|
592
398
|
const afterStepCallbacks = useRef(/* @__PURE__ */ new Set());
|
|
399
|
+
const resetCallbacks = useRef(/* @__PURE__ */ new Set());
|
|
593
400
|
configRef.current = config;
|
|
594
401
|
useEffect(() => {
|
|
595
402
|
pausedRef.current = paused ?? false;
|
|
@@ -617,74 +424,6 @@ function MujocoSimProvider({
|
|
|
617
424
|
if (!model?.opt) return;
|
|
618
425
|
model.opt.timestep = timestep;
|
|
619
426
|
}, [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
427
|
useEffect(() => {
|
|
689
428
|
let disposed = false;
|
|
690
429
|
(async () => {
|
|
@@ -697,8 +436,6 @@ function MujocoSimProvider({
|
|
|
697
436
|
}
|
|
698
437
|
mjModelRef.current = result.mjModel;
|
|
699
438
|
mjDataRef.current = result.mjData;
|
|
700
|
-
siteIdRef.current = result.siteId;
|
|
701
|
-
gripperIdRef.current = result.gripperId;
|
|
702
439
|
if (gravity && result.mjModel.opt?.gravity) {
|
|
703
440
|
result.mjModel.opt.gravity[0] = gravity[0];
|
|
704
441
|
result.mjModel.opt.gravity[1] = gravity[1];
|
|
@@ -707,9 +444,6 @@ function MujocoSimProvider({
|
|
|
707
444
|
if (timestep !== void 0 && result.mjModel.opt) {
|
|
708
445
|
result.mjModel.opt.timestep = timestep;
|
|
709
446
|
}
|
|
710
|
-
if (ikTargetRef.current) {
|
|
711
|
-
syncGizmoToSite(result.mjData, result.siteId, ikTargetRef.current);
|
|
712
|
-
}
|
|
713
447
|
setStatus("ready");
|
|
714
448
|
} catch (e) {
|
|
715
449
|
if (!disposed) {
|
|
@@ -731,46 +465,22 @@ function MujocoSimProvider({
|
|
|
731
465
|
};
|
|
732
466
|
}, [mujoco, config]);
|
|
733
467
|
useEffect(() => {
|
|
734
|
-
if (status === "ready"
|
|
735
|
-
|
|
468
|
+
if (status === "ready") {
|
|
469
|
+
const api2 = apiRef.current;
|
|
470
|
+
if (onReady) onReady(api2);
|
|
471
|
+
if (externalApiRef) {
|
|
472
|
+
if (typeof externalApiRef === "function") {
|
|
473
|
+
externalApiRef(api2);
|
|
474
|
+
} else {
|
|
475
|
+
externalApiRef.current = api2;
|
|
476
|
+
}
|
|
477
|
+
}
|
|
736
478
|
}
|
|
737
479
|
}, [status]);
|
|
738
|
-
useFrame((
|
|
480
|
+
useFrame(() => {
|
|
739
481
|
const model = mjModelRef.current;
|
|
740
482
|
const data = mjDataRef.current;
|
|
741
483
|
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
484
|
const shouldStep = !pausedRef.current || stepsToRunRef.current > 0;
|
|
775
485
|
if (!shouldStep) return;
|
|
776
486
|
for (let i = 0; i < model.nv; i++) {
|
|
@@ -779,18 +489,6 @@ function MujocoSimProvider({
|
|
|
779
489
|
for (const cb of beforeStepCallbacks.current) {
|
|
780
490
|
cb(model, data);
|
|
781
491
|
}
|
|
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
492
|
const numSubsteps = substepsRef.current;
|
|
795
493
|
if (stepsToRunRef.current > 0) {
|
|
796
494
|
for (let s = 0; s < stepsToRunRef.current; s++) {
|
|
@@ -815,72 +513,24 @@ function MujocoSimProvider({
|
|
|
815
513
|
const model = mjModelRef.current;
|
|
816
514
|
const data = mjDataRef.current;
|
|
817
515
|
if (!model || !data) return;
|
|
818
|
-
gizmoAnimRef.current.active = false;
|
|
819
516
|
mujoco.mj_resetData(model, data);
|
|
820
517
|
const homeJoints = configRef.current.homeJoints;
|
|
821
518
|
if (homeJoints) {
|
|
822
|
-
|
|
519
|
+
const homeCount = Math.min(homeJoints.length, model.nu);
|
|
520
|
+
for (let i = 0; i < homeCount; i++) {
|
|
823
521
|
data.ctrl[i] = homeJoints[i];
|
|
824
|
-
|
|
825
|
-
|
|
826
|
-
|
|
827
|
-
const qposAdr = model.jnt_qposadr[jointId];
|
|
828
|
-
data.qpos[qposAdr] = homeJoints[i];
|
|
829
|
-
}
|
|
522
|
+
const qposAdr = getActuatedScalarQposAdr(model, i);
|
|
523
|
+
if (qposAdr !== -1) {
|
|
524
|
+
data.qpos[qposAdr] = homeJoints[i];
|
|
830
525
|
}
|
|
831
526
|
}
|
|
832
527
|
}
|
|
833
528
|
configRef.current.onReset?.(model, data);
|
|
834
529
|
mujoco.mj_forward(model, data);
|
|
835
|
-
|
|
836
|
-
|
|
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;
|
|
530
|
+
for (const cb of resetCallbacks.current) {
|
|
531
|
+
cb();
|
|
847
532
|
}
|
|
848
|
-
}, [
|
|
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
|
-
);
|
|
533
|
+
}, [mujoco]);
|
|
884
534
|
const setSpeed = useCallback((multiplier) => {
|
|
885
535
|
speedRef.current = multiplier;
|
|
886
536
|
}, []);
|
|
@@ -1042,19 +692,16 @@ function MujocoSimProvider({
|
|
|
1042
692
|
const contacts = [];
|
|
1043
693
|
const ncon = data.ncon;
|
|
1044
694
|
for (let i = 0; i < ncon; i++) {
|
|
1045
|
-
|
|
1046
|
-
|
|
1047
|
-
|
|
1048
|
-
|
|
1049
|
-
|
|
1050
|
-
|
|
1051
|
-
|
|
1052
|
-
|
|
1053
|
-
|
|
1054
|
-
|
|
1055
|
-
} catch {
|
|
1056
|
-
break;
|
|
1057
|
-
}
|
|
695
|
+
const c = getContact(data, i);
|
|
696
|
+
if (!c) break;
|
|
697
|
+
contacts.push({
|
|
698
|
+
geom1: c.geom1,
|
|
699
|
+
geom1Name: getName(model, model.name_geomadr[c.geom1]),
|
|
700
|
+
geom2: c.geom2,
|
|
701
|
+
geom2Name: getName(model, model.name_geomadr[c.geom2]),
|
|
702
|
+
pos: [c.pos[0], c.pos[1], c.pos[2]],
|
|
703
|
+
depth: c.dist
|
|
704
|
+
});
|
|
1058
705
|
}
|
|
1059
706
|
return contacts;
|
|
1060
707
|
}, []);
|
|
@@ -1193,7 +840,7 @@ function MujocoSimProvider({
|
|
|
1193
840
|
const geomId = _rayGeomId[0];
|
|
1194
841
|
const bodyId = geomId >= 0 ? model.geom_bodyid[geomId] : -1;
|
|
1195
842
|
return {
|
|
1196
|
-
point: new
|
|
843
|
+
point: new THREE11.Vector3(
|
|
1197
844
|
origin.x + dir.x * dist,
|
|
1198
845
|
origin.y + dir.y * dist,
|
|
1199
846
|
origin.z + dir.z * dist
|
|
@@ -1231,10 +878,10 @@ function MujocoSimProvider({
|
|
|
1231
878
|
for (let i = 0; i < model.nv; i++) data.qvel[i] = model.key_qvel[qvelOffset + i];
|
|
1232
879
|
}
|
|
1233
880
|
mujoco.mj_forward(model, data);
|
|
1234
|
-
|
|
1235
|
-
|
|
881
|
+
for (const cb of resetCallbacks.current) {
|
|
882
|
+
cb();
|
|
1236
883
|
}
|
|
1237
|
-
}, [mujoco
|
|
884
|
+
}, [mujoco]);
|
|
1238
885
|
const getKeyframeNames = useCallback(() => {
|
|
1239
886
|
const model = mjModelRef.current;
|
|
1240
887
|
if (!model) return [];
|
|
@@ -1248,6 +895,7 @@ function MujocoSimProvider({
|
|
|
1248
895
|
return mjModelRef.current?.nkey ?? 0;
|
|
1249
896
|
}, []);
|
|
1250
897
|
const loadSceneApi = useCallback(async (newConfig) => {
|
|
898
|
+
const gen = ++loadGenRef.current;
|
|
1251
899
|
try {
|
|
1252
900
|
mjModelRef.current?.delete();
|
|
1253
901
|
mjDataRef.current?.delete();
|
|
@@ -1255,28 +903,21 @@ function MujocoSimProvider({
|
|
|
1255
903
|
mjDataRef.current = null;
|
|
1256
904
|
setStatus("loading");
|
|
1257
905
|
const result = await loadScene(mujoco, newConfig);
|
|
906
|
+
if (gen !== loadGenRef.current) {
|
|
907
|
+
result.mjModel.delete();
|
|
908
|
+
result.mjData.delete();
|
|
909
|
+
return;
|
|
910
|
+
}
|
|
1258
911
|
mjModelRef.current = result.mjModel;
|
|
1259
912
|
mjDataRef.current = result.mjData;
|
|
1260
|
-
siteIdRef.current = result.siteId;
|
|
1261
|
-
gripperIdRef.current = result.gripperId;
|
|
1262
913
|
configRef.current = newConfig;
|
|
1263
|
-
if (ikTargetRef.current) {
|
|
1264
|
-
syncGizmoToSite(result.mjData, result.siteId, ikTargetRef.current);
|
|
1265
|
-
}
|
|
1266
914
|
setStatus("ready");
|
|
1267
915
|
} catch (e) {
|
|
916
|
+
if (gen !== loadGenRef.current) return;
|
|
1268
917
|
setStatus("error");
|
|
1269
918
|
throw e;
|
|
1270
919
|
}
|
|
1271
|
-
}, [mujoco
|
|
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
|
-
}, []);
|
|
920
|
+
}, [mujoco]);
|
|
1280
921
|
const getCanvasSnapshot = useCallback(
|
|
1281
922
|
(width, height, mimeType = "image/jpeg") => {
|
|
1282
923
|
if (width && height) {
|
|
@@ -1300,9 +941,8 @@ function MujocoSimProvider({
|
|
|
1300
941
|
virtCam.lookAt(lookAt);
|
|
1301
942
|
virtCam.updateMatrixWorld();
|
|
1302
943
|
virtCam.updateProjectionMatrix();
|
|
1303
|
-
|
|
1304
|
-
|
|
1305
|
-
raycaster.setFromCamera(ndc, virtCam);
|
|
944
|
+
_projNdc.set(x * 2 - 1, -(y * 2 - 1));
|
|
945
|
+
_projRaycaster.setFromCamera(_projNdc, virtCam);
|
|
1306
946
|
const objects = [];
|
|
1307
947
|
const scene = camera.parent;
|
|
1308
948
|
if (scene) {
|
|
@@ -1310,7 +950,7 @@ function MujocoSimProvider({
|
|
|
1310
950
|
if (c.isMesh) objects.push(c);
|
|
1311
951
|
});
|
|
1312
952
|
}
|
|
1313
|
-
const hits =
|
|
953
|
+
const hits = _projRaycaster.intersectObjects(objects);
|
|
1314
954
|
if (hits.length > 0) {
|
|
1315
955
|
const hitObj = hits[0].object;
|
|
1316
956
|
const geomId = hitObj.userData.geomID !== void 0 ? hitObj.userData.geomID : -1;
|
|
@@ -1350,31 +990,6 @@ function MujocoSimProvider({
|
|
|
1350
990
|
model.geom_size[id * 3 + 1] = size[1];
|
|
1351
991
|
model.geom_size[id * 3 + 2] = size[2];
|
|
1352
992
|
}, []);
|
|
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
993
|
const api = useMemo(
|
|
1379
994
|
() => ({
|
|
1380
995
|
get status() {
|
|
@@ -1416,15 +1031,8 @@ function MujocoSimProvider({
|
|
|
1416
1031
|
getKeyframeNames,
|
|
1417
1032
|
getKeyframeCount,
|
|
1418
1033
|
loadScene: loadSceneApi,
|
|
1419
|
-
setIkEnabled,
|
|
1420
|
-
moveTarget,
|
|
1421
|
-
syncTargetToSite,
|
|
1422
|
-
solveIK,
|
|
1423
|
-
getGizmoStats,
|
|
1424
1034
|
getCanvasSnapshot,
|
|
1425
1035
|
project2DTo3D,
|
|
1426
|
-
getCameraState,
|
|
1427
|
-
moveCameraTo,
|
|
1428
1036
|
setBodyMass,
|
|
1429
1037
|
setGeomFriction,
|
|
1430
1038
|
setGeomSize,
|
|
@@ -1469,109 +1077,578 @@ function MujocoSimProvider({
|
|
|
1469
1077
|
getKeyframeNames,
|
|
1470
1078
|
getKeyframeCount,
|
|
1471
1079
|
loadSceneApi,
|
|
1472
|
-
setIkEnabled,
|
|
1473
|
-
moveTarget,
|
|
1474
|
-
syncTargetToSite,
|
|
1475
|
-
solveIK,
|
|
1476
|
-
getGizmoStats,
|
|
1477
1080
|
getCanvasSnapshot,
|
|
1478
1081
|
project2DTo3D,
|
|
1479
|
-
getCameraState,
|
|
1480
|
-
moveCameraTo,
|
|
1481
1082
|
setBodyMass,
|
|
1482
1083
|
setGeomFriction,
|
|
1483
1084
|
setGeomSize
|
|
1484
1085
|
]
|
|
1485
1086
|
);
|
|
1486
|
-
const apiRef = useRef(api);
|
|
1487
|
-
apiRef.current = api;
|
|
1087
|
+
const apiRef = useRef(api);
|
|
1088
|
+
apiRef.current = api;
|
|
1089
|
+
const contextValue = useMemo(
|
|
1090
|
+
() => ({
|
|
1091
|
+
api,
|
|
1092
|
+
mjModelRef,
|
|
1093
|
+
mjDataRef,
|
|
1094
|
+
mujocoRef,
|
|
1095
|
+
configRef,
|
|
1096
|
+
pausedRef,
|
|
1097
|
+
speedRef,
|
|
1098
|
+
substepsRef,
|
|
1099
|
+
onSelectionRef,
|
|
1100
|
+
beforeStepCallbacks,
|
|
1101
|
+
afterStepCallbacks,
|
|
1102
|
+
resetCallbacks,
|
|
1103
|
+
status
|
|
1104
|
+
}),
|
|
1105
|
+
[api, status]
|
|
1106
|
+
);
|
|
1107
|
+
return /* @__PURE__ */ jsx(MujocoSimContext.Provider, { value: contextValue, children });
|
|
1108
|
+
}
|
|
1109
|
+
var MujocoCanvas = forwardRef(
|
|
1110
|
+
function MujocoCanvas2({
|
|
1111
|
+
config,
|
|
1112
|
+
onReady,
|
|
1113
|
+
onError,
|
|
1114
|
+
onStep,
|
|
1115
|
+
onSelection,
|
|
1116
|
+
// Declarative physics config (spec 1.1)
|
|
1117
|
+
gravity,
|
|
1118
|
+
timestep,
|
|
1119
|
+
substeps,
|
|
1120
|
+
paused,
|
|
1121
|
+
speed,
|
|
1122
|
+
interpolate,
|
|
1123
|
+
gravityCompensation,
|
|
1124
|
+
mjcfLights,
|
|
1125
|
+
children,
|
|
1126
|
+
...canvasProps
|
|
1127
|
+
}, ref) {
|
|
1128
|
+
const { mujoco, status: wasmStatus, error: wasmError } = useMujoco();
|
|
1129
|
+
useEffect(() => {
|
|
1130
|
+
if (wasmStatus === "error" && onError) {
|
|
1131
|
+
onError(new Error(wasmError ?? "WASM load failed"));
|
|
1132
|
+
}
|
|
1133
|
+
}, [wasmStatus, wasmError, onError]);
|
|
1134
|
+
if (wasmStatus === "error" || wasmStatus === "loading" || !mujoco) {
|
|
1135
|
+
return null;
|
|
1136
|
+
}
|
|
1137
|
+
return /* @__PURE__ */ jsx(Canvas, { ...canvasProps, children: /* @__PURE__ */ jsx(
|
|
1138
|
+
MujocoSimProvider,
|
|
1139
|
+
{
|
|
1140
|
+
mujoco,
|
|
1141
|
+
config,
|
|
1142
|
+
apiRef: ref,
|
|
1143
|
+
onReady,
|
|
1144
|
+
onError,
|
|
1145
|
+
onStep,
|
|
1146
|
+
onSelection,
|
|
1147
|
+
gravity,
|
|
1148
|
+
timestep,
|
|
1149
|
+
substeps,
|
|
1150
|
+
paused,
|
|
1151
|
+
speed,
|
|
1152
|
+
interpolate,
|
|
1153
|
+
children
|
|
1154
|
+
}
|
|
1155
|
+
) });
|
|
1156
|
+
}
|
|
1157
|
+
);
|
|
1158
|
+
function shallowEqual(a, b) {
|
|
1159
|
+
const keysA = Object.keys(a);
|
|
1160
|
+
const keysB = Object.keys(b);
|
|
1161
|
+
if (keysA.length !== keysB.length) return false;
|
|
1162
|
+
for (const key of keysA) {
|
|
1163
|
+
if (a[key] !== b[key]) return false;
|
|
1164
|
+
}
|
|
1165
|
+
return true;
|
|
1166
|
+
}
|
|
1167
|
+
function createController(options, Impl) {
|
|
1168
|
+
function Controller({
|
|
1169
|
+
config,
|
|
1170
|
+
children
|
|
1171
|
+
}) {
|
|
1172
|
+
const configObj = config ?? {};
|
|
1173
|
+
const stableRef = useRef(configObj);
|
|
1174
|
+
if (!shallowEqual(stableRef.current, configObj)) {
|
|
1175
|
+
stableRef.current = configObj;
|
|
1176
|
+
}
|
|
1177
|
+
const stableConfig = stableRef.current;
|
|
1178
|
+
const mergedConfig = useMemo(
|
|
1179
|
+
() => ({ ...options.defaultConfig, ...stableConfig }),
|
|
1180
|
+
[stableConfig]
|
|
1181
|
+
);
|
|
1182
|
+
return /* @__PURE__ */ jsx(Impl, { config: mergedConfig, children });
|
|
1183
|
+
}
|
|
1184
|
+
Controller.displayName = options.name;
|
|
1185
|
+
Controller.controllerName = options.name;
|
|
1186
|
+
Controller.defaultConfig = options.defaultConfig ?? {};
|
|
1187
|
+
return Controller;
|
|
1188
|
+
}
|
|
1189
|
+
var IkContext = createContext(null);
|
|
1190
|
+
function useIk(options) {
|
|
1191
|
+
const ctx = useContext(IkContext);
|
|
1192
|
+
if (!ctx && !options?.optional) {
|
|
1193
|
+
throw new Error("useIk() must be used inside an <IkController>");
|
|
1194
|
+
}
|
|
1195
|
+
return ctx;
|
|
1196
|
+
}
|
|
1197
|
+
|
|
1198
|
+
// src/core/GenericIK.ts
|
|
1199
|
+
var DEFAULTS = {
|
|
1200
|
+
maxIterations: 50,
|
|
1201
|
+
damping: 0.01,
|
|
1202
|
+
tolerance: 1e-3,
|
|
1203
|
+
epsilon: 1e-6,
|
|
1204
|
+
posWeight: 1,
|
|
1205
|
+
rotWeight: 0.3
|
|
1206
|
+
};
|
|
1207
|
+
var GenericIK = class {
|
|
1208
|
+
mujoco;
|
|
1209
|
+
constructor(mujoco) {
|
|
1210
|
+
this.mujoco = mujoco;
|
|
1211
|
+
}
|
|
1212
|
+
/**
|
|
1213
|
+
* Solve IK for a target 6-DOF pose.
|
|
1214
|
+
* @param model MuJoCo model
|
|
1215
|
+
* @param data MuJoCo data (qpos will be temporarily modified, then restored)
|
|
1216
|
+
* @param siteId Index of the end-effector site to control
|
|
1217
|
+
* @param numJoints Number of arm joints (assumes qpos[0..numJoints-1])
|
|
1218
|
+
* @param targetPos Target position in world frame
|
|
1219
|
+
* @param targetQuat Target orientation in world frame
|
|
1220
|
+
* @param currentQ Current joint angles (length = numJoints)
|
|
1221
|
+
* @param opts Optional solver parameters
|
|
1222
|
+
* @returns Joint angles array, or null if solver diverged
|
|
1223
|
+
*/
|
|
1224
|
+
solve(model, data, siteId, numJoints, targetPos, targetQuat, currentQ, opts) {
|
|
1225
|
+
const o = { ...DEFAULTS, ...opts };
|
|
1226
|
+
const n = numJoints;
|
|
1227
|
+
const savedQpos = new Float64Array(data.qpos.length);
|
|
1228
|
+
savedQpos.set(data.qpos);
|
|
1229
|
+
const R_target = quatToMat3(targetQuat);
|
|
1230
|
+
const q = new Float64Array(n);
|
|
1231
|
+
for (let i = 0; i < n; i++) q[i] = currentQ[i];
|
|
1232
|
+
const J = new Float64Array(6 * n);
|
|
1233
|
+
const JJt = new Float64Array(36);
|
|
1234
|
+
const rhs = new Float64Array(6);
|
|
1235
|
+
const x = new Float64Array(6);
|
|
1236
|
+
const dq = new Float64Array(n);
|
|
1237
|
+
const baseSitePos = new Float64Array(3);
|
|
1238
|
+
const baseSiteMat = new Float64Array(9);
|
|
1239
|
+
const pertSitePos = new Float64Array(3);
|
|
1240
|
+
const pertSiteMat = new Float64Array(9);
|
|
1241
|
+
let bestQ = null;
|
|
1242
|
+
let bestErr = Infinity;
|
|
1243
|
+
for (let iter = 0; iter < o.maxIterations; iter++) {
|
|
1244
|
+
for (let i = 0; i < n; i++) data.qpos[i] = q[i];
|
|
1245
|
+
this.mujoco.mj_forward(model, data);
|
|
1246
|
+
const sp = data.site_xpos;
|
|
1247
|
+
const sm = data.site_xmat;
|
|
1248
|
+
const off3 = siteId * 3;
|
|
1249
|
+
const off9 = siteId * 9;
|
|
1250
|
+
for (let i = 0; i < 3; i++) baseSitePos[i] = sp[off3 + i];
|
|
1251
|
+
for (let i = 0; i < 9; i++) baseSiteMat[i] = sm[off9 + i];
|
|
1252
|
+
const posErr0 = targetPos.x - baseSitePos[0];
|
|
1253
|
+
const posErr1 = targetPos.y - baseSitePos[1];
|
|
1254
|
+
const posErr2 = targetPos.z - baseSitePos[2];
|
|
1255
|
+
const rotErr = orientationError(baseSiteMat, R_target);
|
|
1256
|
+
const error = [
|
|
1257
|
+
posErr0 * o.posWeight,
|
|
1258
|
+
posErr1 * o.posWeight,
|
|
1259
|
+
posErr2 * o.posWeight,
|
|
1260
|
+
rotErr[0] * o.rotWeight,
|
|
1261
|
+
rotErr[1] * o.rotWeight,
|
|
1262
|
+
rotErr[2] * o.rotWeight
|
|
1263
|
+
];
|
|
1264
|
+
const errNorm = Math.sqrt(
|
|
1265
|
+
error[0] * error[0] + error[1] * error[1] + error[2] * error[2] + error[3] * error[3] + error[4] * error[4] + error[5] * error[5]
|
|
1266
|
+
);
|
|
1267
|
+
if (errNorm < bestErr) {
|
|
1268
|
+
bestErr = errNorm;
|
|
1269
|
+
bestQ = Array.from(q);
|
|
1270
|
+
}
|
|
1271
|
+
if (errNorm < o.tolerance) break;
|
|
1272
|
+
for (let j = 0; j < n; j++) {
|
|
1273
|
+
const saved = data.qpos[j];
|
|
1274
|
+
data.qpos[j] = q[j] + o.epsilon;
|
|
1275
|
+
this.mujoco.mj_forward(model, data);
|
|
1276
|
+
for (let i = 0; i < 3; i++) pertSitePos[i] = sp[off3 + i];
|
|
1277
|
+
for (let i = 0; i < 9; i++) pertSiteMat[i] = sm[off9 + i];
|
|
1278
|
+
J[0 * n + j] = (pertSitePos[0] - baseSitePos[0]) / o.epsilon * o.posWeight;
|
|
1279
|
+
J[1 * n + j] = (pertSitePos[1] - baseSitePos[1]) / o.epsilon * o.posWeight;
|
|
1280
|
+
J[2 * n + j] = (pertSitePos[2] - baseSitePos[2]) / o.epsilon * o.posWeight;
|
|
1281
|
+
const dRot = angularDelta(baseSiteMat, pertSiteMat);
|
|
1282
|
+
J[3 * n + j] = dRot[0] / o.epsilon * o.rotWeight;
|
|
1283
|
+
J[4 * n + j] = dRot[1] / o.epsilon * o.rotWeight;
|
|
1284
|
+
J[5 * n + j] = dRot[2] / o.epsilon * o.rotWeight;
|
|
1285
|
+
data.qpos[j] = saved;
|
|
1286
|
+
}
|
|
1287
|
+
for (let i = 0; i < n; i++) data.qpos[i] = q[i];
|
|
1288
|
+
for (let r = 0; r < 6; r++) {
|
|
1289
|
+
for (let c = 0; c < 6; c++) {
|
|
1290
|
+
let sum = 0;
|
|
1291
|
+
for (let k = 0; k < n; k++) {
|
|
1292
|
+
sum += J[r * n + k] * J[c * n + k];
|
|
1293
|
+
}
|
|
1294
|
+
JJt[r * 6 + c] = sum + (r === c ? o.damping : 0);
|
|
1295
|
+
}
|
|
1296
|
+
}
|
|
1297
|
+
for (let i = 0; i < 6; i++) rhs[i] = error[i];
|
|
1298
|
+
solve6x6(JJt, rhs, x);
|
|
1299
|
+
for (let j = 0; j < n; j++) {
|
|
1300
|
+
let sum = 0;
|
|
1301
|
+
for (let r = 0; r < 6; r++) {
|
|
1302
|
+
sum += J[r * n + j] * x[r];
|
|
1303
|
+
}
|
|
1304
|
+
dq[j] = sum;
|
|
1305
|
+
}
|
|
1306
|
+
for (let i = 0; i < n; i++) q[i] += dq[i];
|
|
1307
|
+
}
|
|
1308
|
+
data.qpos.set(savedQpos);
|
|
1309
|
+
this.mujoco.mj_forward(model, data);
|
|
1310
|
+
return bestQ;
|
|
1311
|
+
}
|
|
1312
|
+
};
|
|
1313
|
+
function quatToMat3(q) {
|
|
1314
|
+
const m = new Float64Array(9);
|
|
1315
|
+
const x = q.x, y = q.y, z = q.z, w = q.w;
|
|
1316
|
+
const xx = x * x, yy = y * y, zz = z * z;
|
|
1317
|
+
const xy = x * y, xz = x * z, yz = y * z;
|
|
1318
|
+
const wx = w * x, wy = w * y, wz = w * z;
|
|
1319
|
+
m[0] = 1 - 2 * (yy + zz);
|
|
1320
|
+
m[1] = 2 * (xy - wz);
|
|
1321
|
+
m[2] = 2 * (xz + wy);
|
|
1322
|
+
m[3] = 2 * (xy + wz);
|
|
1323
|
+
m[4] = 1 - 2 * (xx + zz);
|
|
1324
|
+
m[5] = 2 * (yz - wx);
|
|
1325
|
+
m[6] = 2 * (xz - wy);
|
|
1326
|
+
m[7] = 2 * (yz + wx);
|
|
1327
|
+
m[8] = 1 - 2 * (xx + yy);
|
|
1328
|
+
return m;
|
|
1329
|
+
}
|
|
1330
|
+
function orientationError(R_cur, R_tgt) {
|
|
1331
|
+
const Re = new Float64Array(9);
|
|
1332
|
+
for (let i = 0; i < 3; i++) {
|
|
1333
|
+
for (let j = 0; j < 3; j++) {
|
|
1334
|
+
let s2 = 0;
|
|
1335
|
+
for (let k = 0; k < 3; k++) {
|
|
1336
|
+
s2 += R_tgt[i * 3 + k] * R_cur[j * 3 + k];
|
|
1337
|
+
}
|
|
1338
|
+
Re[i * 3 + j] = s2;
|
|
1339
|
+
}
|
|
1340
|
+
}
|
|
1341
|
+
const trace = Re[0] + Re[4] + Re[8];
|
|
1342
|
+
const cosAngle = Math.max(-1, Math.min(1, (trace - 1) * 0.5));
|
|
1343
|
+
const angle = Math.acos(cosAngle);
|
|
1344
|
+
if (angle < 1e-6) {
|
|
1345
|
+
return [0, 0, 0];
|
|
1346
|
+
}
|
|
1347
|
+
if (angle > Math.PI - 1e-6) {
|
|
1348
|
+
return [
|
|
1349
|
+
0.5 * (Re[7] - Re[5]),
|
|
1350
|
+
0.5 * (Re[2] - Re[6]),
|
|
1351
|
+
0.5 * (Re[3] - Re[1])
|
|
1352
|
+
];
|
|
1353
|
+
}
|
|
1354
|
+
const s = angle / (2 * Math.sin(angle));
|
|
1355
|
+
return [
|
|
1356
|
+
s * (Re[7] - Re[5]),
|
|
1357
|
+
s * (Re[2] - Re[6]),
|
|
1358
|
+
s * (Re[3] - Re[1])
|
|
1359
|
+
];
|
|
1360
|
+
}
|
|
1361
|
+
function angularDelta(R_base, R_pert) {
|
|
1362
|
+
const dR = new Float64Array(9);
|
|
1363
|
+
for (let i = 0; i < 3; i++) {
|
|
1364
|
+
for (let j = 0; j < 3; j++) {
|
|
1365
|
+
let s = 0;
|
|
1366
|
+
for (let k = 0; k < 3; k++) {
|
|
1367
|
+
s += R_pert[i * 3 + k] * R_base[j * 3 + k];
|
|
1368
|
+
}
|
|
1369
|
+
dR[i * 3 + j] = s;
|
|
1370
|
+
}
|
|
1371
|
+
}
|
|
1372
|
+
return [
|
|
1373
|
+
0.5 * (dR[7] - dR[5]),
|
|
1374
|
+
0.5 * (dR[2] - dR[6]),
|
|
1375
|
+
0.5 * (dR[3] - dR[1])
|
|
1376
|
+
];
|
|
1377
|
+
}
|
|
1378
|
+
function solve6x6(A, b, x) {
|
|
1379
|
+
const N = 6;
|
|
1380
|
+
const a = new Float64Array(A);
|
|
1381
|
+
const r = new Float64Array(b);
|
|
1382
|
+
for (let col = 0; col < N; col++) {
|
|
1383
|
+
let maxVal = Math.abs(a[col * N + col]);
|
|
1384
|
+
let maxRow = col;
|
|
1385
|
+
for (let row = col + 1; row < N; row++) {
|
|
1386
|
+
const val = Math.abs(a[row * N + col]);
|
|
1387
|
+
if (val > maxVal) {
|
|
1388
|
+
maxVal = val;
|
|
1389
|
+
maxRow = row;
|
|
1390
|
+
}
|
|
1391
|
+
}
|
|
1392
|
+
if (maxRow !== col) {
|
|
1393
|
+
for (let k = 0; k < N; k++) {
|
|
1394
|
+
const tmp2 = a[col * N + k];
|
|
1395
|
+
a[col * N + k] = a[maxRow * N + k];
|
|
1396
|
+
a[maxRow * N + k] = tmp2;
|
|
1397
|
+
}
|
|
1398
|
+
const tmp = r[col];
|
|
1399
|
+
r[col] = r[maxRow];
|
|
1400
|
+
r[maxRow] = tmp;
|
|
1401
|
+
}
|
|
1402
|
+
const pivot = a[col * N + col];
|
|
1403
|
+
if (Math.abs(pivot) < 1e-12) {
|
|
1404
|
+
x.fill(0);
|
|
1405
|
+
return;
|
|
1406
|
+
}
|
|
1407
|
+
for (let row = col + 1; row < N; row++) {
|
|
1408
|
+
const factor = a[row * N + col] / pivot;
|
|
1409
|
+
for (let k = col; k < N; k++) {
|
|
1410
|
+
a[row * N + k] -= factor * a[col * N + k];
|
|
1411
|
+
}
|
|
1412
|
+
r[row] -= factor * r[col];
|
|
1413
|
+
}
|
|
1414
|
+
}
|
|
1415
|
+
for (let row = N - 1; row >= 0; row--) {
|
|
1416
|
+
let sum = r[row];
|
|
1417
|
+
for (let k = row + 1; k < N; k++) {
|
|
1418
|
+
sum -= a[row * N + k] * x[k];
|
|
1419
|
+
}
|
|
1420
|
+
x[row] = sum / a[row * N + row];
|
|
1421
|
+
}
|
|
1422
|
+
}
|
|
1423
|
+
var _syncMat4 = new THREE11.Matrix4();
|
|
1424
|
+
function syncGizmoToSite(data, siteId, target) {
|
|
1425
|
+
if (siteId === -1) return;
|
|
1426
|
+
const sitePos = data.site_xpos.subarray(siteId * 3, siteId * 3 + 3);
|
|
1427
|
+
const siteMat = data.site_xmat.subarray(siteId * 9, siteId * 9 + 9);
|
|
1428
|
+
target.position.set(sitePos[0], sitePos[1], sitePos[2]);
|
|
1429
|
+
_syncMat4.set(
|
|
1430
|
+
siteMat[0],
|
|
1431
|
+
siteMat[1],
|
|
1432
|
+
siteMat[2],
|
|
1433
|
+
0,
|
|
1434
|
+
siteMat[3],
|
|
1435
|
+
siteMat[4],
|
|
1436
|
+
siteMat[5],
|
|
1437
|
+
0,
|
|
1438
|
+
siteMat[6],
|
|
1439
|
+
siteMat[7],
|
|
1440
|
+
siteMat[8],
|
|
1441
|
+
0,
|
|
1442
|
+
0,
|
|
1443
|
+
0,
|
|
1444
|
+
0,
|
|
1445
|
+
1
|
|
1446
|
+
);
|
|
1447
|
+
target.quaternion.setFromRotationMatrix(_syncMat4);
|
|
1448
|
+
}
|
|
1449
|
+
function IkControllerImpl({
|
|
1450
|
+
config,
|
|
1451
|
+
children
|
|
1452
|
+
}) {
|
|
1453
|
+
const { mjModelRef, mjDataRef, mujocoRef, configRef, resetCallbacks, status } = useMujocoSim();
|
|
1454
|
+
const ikEnabledRef = useRef(false);
|
|
1455
|
+
const ikCalculatingRef = useRef(false);
|
|
1456
|
+
const ikTargetRef = useRef(new THREE11.Group());
|
|
1457
|
+
const siteIdRef = useRef(-1);
|
|
1458
|
+
const genericIkRef = useRef(new GenericIK(mujocoRef.current));
|
|
1459
|
+
const firstIkEnableRef = useRef(true);
|
|
1460
|
+
const needsInitialSync = useRef(true);
|
|
1461
|
+
const gizmoAnimRef = useRef({
|
|
1462
|
+
active: false,
|
|
1463
|
+
startPos: new THREE11.Vector3(),
|
|
1464
|
+
endPos: new THREE11.Vector3(),
|
|
1465
|
+
startRot: new THREE11.Quaternion(),
|
|
1466
|
+
endRot: new THREE11.Quaternion(),
|
|
1467
|
+
startTime: 0,
|
|
1468
|
+
duration: 1e3
|
|
1469
|
+
});
|
|
1470
|
+
useEffect(() => {
|
|
1471
|
+
const model = mjModelRef.current;
|
|
1472
|
+
if (!model || status !== "ready") {
|
|
1473
|
+
siteIdRef.current = -1;
|
|
1474
|
+
return;
|
|
1475
|
+
}
|
|
1476
|
+
siteIdRef.current = findSiteByName(model, config.siteName);
|
|
1477
|
+
const data = mjDataRef.current;
|
|
1478
|
+
if (data && ikTargetRef.current) {
|
|
1479
|
+
syncGizmoToSite(data, siteIdRef.current, ikTargetRef.current);
|
|
1480
|
+
}
|
|
1481
|
+
}, [config.siteName, status, mjModelRef, mjDataRef]);
|
|
1482
|
+
const ikSolveFn = useCallback(
|
|
1483
|
+
(pos, quat, currentQ) => {
|
|
1484
|
+
if (config.ikSolveFn) return config.ikSolveFn(pos, quat, currentQ);
|
|
1485
|
+
const model = mjModelRef.current;
|
|
1486
|
+
const data = mjDataRef.current;
|
|
1487
|
+
if (!model || !data || siteIdRef.current === -1) return null;
|
|
1488
|
+
return genericIkRef.current.solve(
|
|
1489
|
+
model,
|
|
1490
|
+
data,
|
|
1491
|
+
siteIdRef.current,
|
|
1492
|
+
config.numJoints,
|
|
1493
|
+
pos,
|
|
1494
|
+
quat,
|
|
1495
|
+
currentQ,
|
|
1496
|
+
{
|
|
1497
|
+
damping: config.damping,
|
|
1498
|
+
maxIterations: config.maxIterations
|
|
1499
|
+
}
|
|
1500
|
+
);
|
|
1501
|
+
},
|
|
1502
|
+
[config.ikSolveFn, config.numJoints, config.damping, config.maxIterations, mjModelRef, mjDataRef]
|
|
1503
|
+
);
|
|
1504
|
+
const ikSolveFnRef = useRef(ikSolveFn);
|
|
1505
|
+
ikSolveFnRef.current = ikSolveFn;
|
|
1506
|
+
useFrame(() => {
|
|
1507
|
+
if (needsInitialSync.current && siteIdRef.current !== -1) {
|
|
1508
|
+
const data = mjDataRef.current;
|
|
1509
|
+
if (data && ikTargetRef.current) {
|
|
1510
|
+
syncGizmoToSite(data, siteIdRef.current, ikTargetRef.current);
|
|
1511
|
+
needsInitialSync.current = false;
|
|
1512
|
+
}
|
|
1513
|
+
}
|
|
1514
|
+
const ga = gizmoAnimRef.current;
|
|
1515
|
+
const target = ikTargetRef.current;
|
|
1516
|
+
if (!ga.active || !target) return;
|
|
1517
|
+
const now = performance.now();
|
|
1518
|
+
const elapsed = now - ga.startTime;
|
|
1519
|
+
const t = Math.min(elapsed / ga.duration, 1);
|
|
1520
|
+
const ease = 1 - Math.pow(1 - t, 3);
|
|
1521
|
+
target.position.lerpVectors(ga.startPos, ga.endPos, ease);
|
|
1522
|
+
target.quaternion.slerpQuaternions(ga.startRot, ga.endRot, ease);
|
|
1523
|
+
if (t >= 1) ga.active = false;
|
|
1524
|
+
});
|
|
1525
|
+
useBeforePhysicsStep((model, data) => {
|
|
1526
|
+
if (!ikEnabledRef.current) {
|
|
1527
|
+
ikCalculatingRef.current = false;
|
|
1528
|
+
return;
|
|
1529
|
+
}
|
|
1530
|
+
const target = ikTargetRef.current;
|
|
1531
|
+
if (!target) return;
|
|
1532
|
+
ikCalculatingRef.current = true;
|
|
1533
|
+
const numJoints = config.numJoints;
|
|
1534
|
+
const currentQ = [];
|
|
1535
|
+
for (let i = 0; i < numJoints; i++) currentQ.push(data.qpos[i]);
|
|
1536
|
+
const solution = ikSolveFnRef.current(target.position, target.quaternion, currentQ);
|
|
1537
|
+
if (solution) {
|
|
1538
|
+
for (let i = 0; i < numJoints; i++) data.ctrl[i] = solution[i];
|
|
1539
|
+
}
|
|
1540
|
+
});
|
|
1541
|
+
useEffect(() => {
|
|
1542
|
+
const cb = () => {
|
|
1543
|
+
const data = mjDataRef.current;
|
|
1544
|
+
if (data && ikTargetRef.current) {
|
|
1545
|
+
syncGizmoToSite(data, siteIdRef.current, ikTargetRef.current);
|
|
1546
|
+
}
|
|
1547
|
+
gizmoAnimRef.current.active = false;
|
|
1548
|
+
firstIkEnableRef.current = true;
|
|
1549
|
+
ikEnabledRef.current = false;
|
|
1550
|
+
needsInitialSync.current = true;
|
|
1551
|
+
};
|
|
1552
|
+
resetCallbacks.current.add(cb);
|
|
1553
|
+
return () => {
|
|
1554
|
+
resetCallbacks.current.delete(cb);
|
|
1555
|
+
};
|
|
1556
|
+
}, [resetCallbacks, mjDataRef]);
|
|
1557
|
+
const setIkEnabled = useCallback(
|
|
1558
|
+
(enabled) => {
|
|
1559
|
+
ikEnabledRef.current = enabled;
|
|
1560
|
+
const data = mjDataRef.current;
|
|
1561
|
+
if (enabled && data && !gizmoAnimRef.current.active && ikTargetRef.current) {
|
|
1562
|
+
syncGizmoToSite(data, siteIdRef.current, ikTargetRef.current);
|
|
1563
|
+
firstIkEnableRef.current = false;
|
|
1564
|
+
}
|
|
1565
|
+
},
|
|
1566
|
+
[mjDataRef]
|
|
1567
|
+
);
|
|
1568
|
+
const syncTargetToSiteApi = useCallback(() => {
|
|
1569
|
+
const data = mjDataRef.current;
|
|
1570
|
+
const target = ikTargetRef.current;
|
|
1571
|
+
if (data && target) syncGizmoToSite(data, siteIdRef.current, target);
|
|
1572
|
+
}, [mjDataRef]);
|
|
1573
|
+
const solveIK = useCallback(
|
|
1574
|
+
(pos, quat, currentQ) => {
|
|
1575
|
+
return ikSolveFnRef.current(pos, quat, currentQ);
|
|
1576
|
+
},
|
|
1577
|
+
[]
|
|
1578
|
+
);
|
|
1579
|
+
const moveTarget = useCallback(
|
|
1580
|
+
(pos, duration = 0) => {
|
|
1581
|
+
if (!ikEnabledRef.current) setIkEnabled(true);
|
|
1582
|
+
const target = ikTargetRef.current;
|
|
1583
|
+
if (!target) return;
|
|
1584
|
+
const targetPos = pos.clone();
|
|
1585
|
+
const targetRot = new THREE11.Quaternion().setFromEuler(
|
|
1586
|
+
new THREE11.Euler(Math.PI, 0, 0)
|
|
1587
|
+
);
|
|
1588
|
+
if (duration > 0) {
|
|
1589
|
+
const ga = gizmoAnimRef.current;
|
|
1590
|
+
ga.active = true;
|
|
1591
|
+
ga.startPos.copy(target.position);
|
|
1592
|
+
ga.endPos.copy(targetPos);
|
|
1593
|
+
ga.startRot.copy(target.quaternion);
|
|
1594
|
+
ga.endRot.copy(targetRot);
|
|
1595
|
+
ga.startTime = performance.now();
|
|
1596
|
+
ga.duration = duration;
|
|
1597
|
+
} else {
|
|
1598
|
+
gizmoAnimRef.current.active = false;
|
|
1599
|
+
target.position.copy(targetPos);
|
|
1600
|
+
target.quaternion.copy(targetRot);
|
|
1601
|
+
}
|
|
1602
|
+
},
|
|
1603
|
+
[setIkEnabled]
|
|
1604
|
+
);
|
|
1605
|
+
const getGizmoStats = useCallback(
|
|
1606
|
+
() => {
|
|
1607
|
+
const target = ikTargetRef.current;
|
|
1608
|
+
if (!ikCalculatingRef.current || !target) return null;
|
|
1609
|
+
return {
|
|
1610
|
+
pos: target.position.clone(),
|
|
1611
|
+
rot: new THREE11.Euler().setFromQuaternion(target.quaternion)
|
|
1612
|
+
};
|
|
1613
|
+
},
|
|
1614
|
+
[]
|
|
1615
|
+
);
|
|
1488
1616
|
const contextValue = useMemo(
|
|
1489
1617
|
() => ({
|
|
1490
|
-
api,
|
|
1491
|
-
mjModelRef,
|
|
1492
|
-
mjDataRef,
|
|
1493
|
-
mujocoRef,
|
|
1494
|
-
configRef,
|
|
1495
|
-
siteIdRef,
|
|
1496
|
-
gripperIdRef,
|
|
1497
1618
|
ikEnabledRef,
|
|
1498
1619
|
ikCalculatingRef,
|
|
1499
|
-
pausedRef,
|
|
1500
|
-
speedRef,
|
|
1501
|
-
substepsRef,
|
|
1502
1620
|
ikTargetRef,
|
|
1503
|
-
|
|
1504
|
-
|
|
1505
|
-
|
|
1506
|
-
|
|
1507
|
-
|
|
1508
|
-
|
|
1509
|
-
beforeStepCallbacks,
|
|
1510
|
-
afterStepCallbacks,
|
|
1511
|
-
status
|
|
1621
|
+
siteIdRef,
|
|
1622
|
+
setIkEnabled,
|
|
1623
|
+
moveTarget,
|
|
1624
|
+
syncTargetToSite: syncTargetToSiteApi,
|
|
1625
|
+
solveIK,
|
|
1626
|
+
getGizmoStats
|
|
1512
1627
|
}),
|
|
1513
|
-
[
|
|
1628
|
+
[setIkEnabled, moveTarget, syncTargetToSiteApi, solveIK, getGizmoStats]
|
|
1514
1629
|
);
|
|
1515
|
-
return /* @__PURE__ */ jsx(
|
|
1630
|
+
return /* @__PURE__ */ jsx(IkContext.Provider, { value: contextValue, children });
|
|
1516
1631
|
}
|
|
1517
|
-
var
|
|
1518
|
-
|
|
1519
|
-
|
|
1520
|
-
|
|
1521
|
-
|
|
1522
|
-
|
|
1523
|
-
|
|
1524
|
-
|
|
1525
|
-
|
|
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
|
-
}
|
|
1632
|
+
var IkController = createController(
|
|
1633
|
+
{
|
|
1634
|
+
name: "IkController",
|
|
1635
|
+
defaultConfig: {
|
|
1636
|
+
damping: 0.01,
|
|
1637
|
+
maxIterations: 50
|
|
1638
|
+
}
|
|
1639
|
+
},
|
|
1640
|
+
IkControllerImpl
|
|
1564
1641
|
);
|
|
1565
|
-
var CapsuleGeometry = class extends
|
|
1642
|
+
var CapsuleGeometry = class extends THREE11.BufferGeometry {
|
|
1566
1643
|
parameters;
|
|
1567
1644
|
constructor(radius = 1, length = 1, capSegments = 4, radialSegments = 8) {
|
|
1568
1645
|
super();
|
|
1569
1646
|
this.type = "CapsuleGeometry";
|
|
1570
1647
|
this.parameters = { radius, length, capSegments, radialSegments };
|
|
1571
|
-
const path = new
|
|
1648
|
+
const path = new THREE11.Path();
|
|
1572
1649
|
path.absarc(0, -length / 2, radius, Math.PI * 1.5, 0, false);
|
|
1573
1650
|
path.absarc(0, length / 2, radius, 0, Math.PI * 0.5, false);
|
|
1574
|
-
const latheGeometry = new
|
|
1651
|
+
const latheGeometry = new THREE11.LatheGeometry(path.getPoints(capSegments), radialSegments);
|
|
1575
1652
|
const self = this;
|
|
1576
1653
|
self.setIndex(latheGeometry.getIndex());
|
|
1577
1654
|
self.setAttribute("position", latheGeometry.getAttribute("position"));
|
|
@@ -1579,27 +1656,27 @@ var CapsuleGeometry = class extends THREE.BufferGeometry {
|
|
|
1579
1656
|
self.setAttribute("uv", latheGeometry.getAttribute("uv"));
|
|
1580
1657
|
}
|
|
1581
1658
|
};
|
|
1582
|
-
var Reflector = class extends
|
|
1659
|
+
var Reflector = class extends THREE11.Mesh {
|
|
1583
1660
|
isReflector = true;
|
|
1584
1661
|
camera;
|
|
1585
|
-
reflectorPlane = new
|
|
1586
|
-
normal = new
|
|
1587
|
-
reflectorWorldPosition = new
|
|
1588
|
-
cameraWorldPosition = new
|
|
1589
|
-
rotationMatrix = new
|
|
1590
|
-
lookAtPosition = new
|
|
1591
|
-
clipPlane = new
|
|
1592
|
-
view = new
|
|
1593
|
-
target = new
|
|
1594
|
-
q = new
|
|
1595
|
-
textureMatrix = new
|
|
1662
|
+
reflectorPlane = new THREE11.Plane();
|
|
1663
|
+
normal = new THREE11.Vector3();
|
|
1664
|
+
reflectorWorldPosition = new THREE11.Vector3();
|
|
1665
|
+
cameraWorldPosition = new THREE11.Vector3();
|
|
1666
|
+
rotationMatrix = new THREE11.Matrix4();
|
|
1667
|
+
lookAtPosition = new THREE11.Vector3(0, 0, -1);
|
|
1668
|
+
clipPlane = new THREE11.Vector4();
|
|
1669
|
+
view = new THREE11.Vector3();
|
|
1670
|
+
target = new THREE11.Vector3();
|
|
1671
|
+
q = new THREE11.Vector4();
|
|
1672
|
+
textureMatrix = new THREE11.Matrix4();
|
|
1596
1673
|
virtualCamera;
|
|
1597
1674
|
renderTarget;
|
|
1598
1675
|
constructor(geometry, options = {}) {
|
|
1599
1676
|
super(geometry);
|
|
1600
1677
|
this.type = "Reflector";
|
|
1601
|
-
this.camera = new
|
|
1602
|
-
const color = options.color !== void 0 ? new
|
|
1678
|
+
this.camera = new THREE11.PerspectiveCamera();
|
|
1679
|
+
const color = options.color !== void 0 ? new THREE11.Color(options.color) : new THREE11.Color(8355711);
|
|
1603
1680
|
const textureWidth = options.textureWidth || 512;
|
|
1604
1681
|
const textureHeight = options.textureHeight || 512;
|
|
1605
1682
|
const clipBias = options.clipBias || 0;
|
|
@@ -1607,11 +1684,11 @@ var Reflector = class extends THREE.Mesh {
|
|
|
1607
1684
|
const blendTexture = options.texture || void 0;
|
|
1608
1685
|
const mixStrength = options.mixStrength !== void 0 ? options.mixStrength : 0.25;
|
|
1609
1686
|
this.virtualCamera = this.camera;
|
|
1610
|
-
this.renderTarget = new
|
|
1687
|
+
this.renderTarget = new THREE11.WebGLRenderTarget(textureWidth, textureHeight, {
|
|
1611
1688
|
samples: multisample,
|
|
1612
|
-
type:
|
|
1689
|
+
type: THREE11.HalfFloatType
|
|
1613
1690
|
});
|
|
1614
|
-
this.material = new
|
|
1691
|
+
this.material = new THREE11.MeshPhysicalMaterial({
|
|
1615
1692
|
map: blendTexture,
|
|
1616
1693
|
color,
|
|
1617
1694
|
roughness: 0.5,
|
|
@@ -1737,7 +1814,7 @@ var GeomBuilder = class {
|
|
|
1737
1814
|
const pos = mjModel.geom_pos.subarray(g * 3, g * 3 + 3);
|
|
1738
1815
|
const quat = mjModel.geom_quat.subarray(g * 4, g * 4 + 4);
|
|
1739
1816
|
const matId = mjModel.geom_matid[g];
|
|
1740
|
-
const color = new
|
|
1817
|
+
const color = new THREE11.Color(16777215);
|
|
1741
1818
|
let opacity = 1;
|
|
1742
1819
|
if (matId >= 0) {
|
|
1743
1820
|
const rgba = mjModel.mat_rgba.subarray(matId * 4, matId * 4 + 4);
|
|
@@ -1752,16 +1829,16 @@ var GeomBuilder = class {
|
|
|
1752
1829
|
let geo = null;
|
|
1753
1830
|
const getVal = (v) => v?.value ?? v;
|
|
1754
1831
|
if (type === getVal(MG.mjGEOM_PLANE)) {
|
|
1755
|
-
geo = new
|
|
1832
|
+
geo = new THREE11.PlaneGeometry(size[0] * 2 || 5, size[1] * 2 || 5);
|
|
1756
1833
|
} else if (type === getVal(MG.mjGEOM_SPHERE)) {
|
|
1757
|
-
geo = new
|
|
1834
|
+
geo = new THREE11.SphereGeometry(size[0], 24, 24);
|
|
1758
1835
|
} else if (type === getVal(MG.mjGEOM_CAPSULE)) {
|
|
1759
1836
|
geo = new CapsuleGeometry(size[0], size[1] * 2, 24, 12);
|
|
1760
1837
|
geo.rotateX(Math.PI / 2);
|
|
1761
1838
|
} else if (type === getVal(MG.mjGEOM_BOX)) {
|
|
1762
|
-
geo = new
|
|
1839
|
+
geo = new THREE11.BoxGeometry(size[0] * 2, size[1] * 2, size[2] * 2);
|
|
1763
1840
|
} else if (type === getVal(MG.mjGEOM_CYLINDER)) {
|
|
1764
|
-
geo = new
|
|
1841
|
+
geo = new THREE11.CylinderGeometry(size[0], size[0], size[1] * 2, 24);
|
|
1765
1842
|
geo.rotateX(Math.PI / 2);
|
|
1766
1843
|
} else if (type === getVal(MG.mjGEOM_MESH)) {
|
|
1767
1844
|
const mId = mjModel.geom_dataid[g];
|
|
@@ -1769,8 +1846,8 @@ var GeomBuilder = class {
|
|
|
1769
1846
|
const vNum = mjModel.mesh_vertnum[mId];
|
|
1770
1847
|
const fAdr = mjModel.mesh_faceadr[mId];
|
|
1771
1848
|
const fNum = mjModel.mesh_facenum[mId];
|
|
1772
|
-
geo = new
|
|
1773
|
-
geo.setAttribute("position", new
|
|
1849
|
+
geo = new THREE11.BufferGeometry();
|
|
1850
|
+
geo.setAttribute("position", new THREE11.Float32BufferAttribute(mjModel.mesh_vert.subarray(vAdr * 3, (vAdr + vNum) * 3), 3));
|
|
1774
1851
|
geo.setIndex(Array.from(mjModel.mesh_face.subarray(fAdr * 3, (fAdr + fNum) * 3)));
|
|
1775
1852
|
geo.computeVertexNormals();
|
|
1776
1853
|
}
|
|
@@ -1785,7 +1862,7 @@ var GeomBuilder = class {
|
|
|
1785
1862
|
mixStrength: 0.25
|
|
1786
1863
|
});
|
|
1787
1864
|
} else {
|
|
1788
|
-
mesh = new
|
|
1865
|
+
mesh = new THREE11.Mesh(geo, new THREE11.MeshStandardMaterial({
|
|
1789
1866
|
color,
|
|
1790
1867
|
transparent: opacity < 1,
|
|
1791
1868
|
opacity,
|
|
@@ -1810,10 +1887,11 @@ function SceneRenderer() {
|
|
|
1810
1887
|
const bodyRefs = useRef([]);
|
|
1811
1888
|
const prevModelRef = useRef(null);
|
|
1812
1889
|
const geomBuilder = useMemo(() => {
|
|
1890
|
+
if (status !== "ready") return null;
|
|
1813
1891
|
return new GeomBuilder(mujocoRef.current);
|
|
1814
|
-
}, [mujocoRef
|
|
1892
|
+
}, [status, mujocoRef]);
|
|
1815
1893
|
useEffect(() => {
|
|
1816
|
-
if (status !== "ready") return;
|
|
1894
|
+
if (status !== "ready" || !geomBuilder) return;
|
|
1817
1895
|
const model = mjModelRef.current;
|
|
1818
1896
|
const group = groupRef.current;
|
|
1819
1897
|
if (!model || !group) return;
|
|
@@ -1824,7 +1902,7 @@ function SceneRenderer() {
|
|
|
1824
1902
|
}
|
|
1825
1903
|
const refs = [];
|
|
1826
1904
|
for (let i = 0; i < model.nbody; i++) {
|
|
1827
|
-
const bodyGroup = new
|
|
1905
|
+
const bodyGroup = new THREE11.Group();
|
|
1828
1906
|
bodyGroup.userData.bodyID = i;
|
|
1829
1907
|
for (let g = 0; g < model.ngeom; g++) {
|
|
1830
1908
|
if (model.geom_bodyid[g] === i) {
|
|
@@ -1867,31 +1945,25 @@ function SceneRenderer() {
|
|
|
1867
1945
|
while (obj && obj.userData.bodyID === void 0 && obj.parent) {
|
|
1868
1946
|
obj = obj.parent;
|
|
1869
1947
|
}
|
|
1870
|
-
|
|
1948
|
+
const bodyID = obj?.userData.bodyID;
|
|
1949
|
+
if (typeof bodyID === "number" && bodyID > 0) {
|
|
1871
1950
|
const model = mjModelRef.current;
|
|
1872
|
-
if (model && onSelectionRef.current) {
|
|
1873
|
-
const name = getName(model, model.name_bodyadr[
|
|
1874
|
-
onSelectionRef.current(
|
|
1951
|
+
if (model && bodyID < model.nbody && onSelectionRef.current) {
|
|
1952
|
+
const name = getName(model, model.name_bodyadr[bodyID]);
|
|
1953
|
+
onSelectionRef.current(bodyID, name);
|
|
1875
1954
|
}
|
|
1876
1955
|
}
|
|
1877
1956
|
}
|
|
1878
1957
|
}
|
|
1879
1958
|
);
|
|
1880
1959
|
}
|
|
1881
|
-
var _mat4 = new
|
|
1882
|
-
var _pos = new
|
|
1883
|
-
var _quat = new
|
|
1884
|
-
var _scale = new
|
|
1960
|
+
var _mat4 = new THREE11.Matrix4();
|
|
1961
|
+
var _pos = new THREE11.Vector3();
|
|
1962
|
+
var _quat = new THREE11.Quaternion();
|
|
1963
|
+
var _scale = new THREE11.Vector3(1, 1, 1);
|
|
1885
1964
|
function IkGizmo({ siteName, scale = 0.18, onDrag }) {
|
|
1886
|
-
const {
|
|
1887
|
-
|
|
1888
|
-
mjModelRef,
|
|
1889
|
-
mjDataRef,
|
|
1890
|
-
siteIdRef,
|
|
1891
|
-
api,
|
|
1892
|
-
ikEnabledRef,
|
|
1893
|
-
status
|
|
1894
|
-
} = useMujocoSim();
|
|
1965
|
+
const { mjModelRef, mjDataRef, status } = useMujocoSim();
|
|
1966
|
+
const { ikTargetRef, siteIdRef, ikEnabledRef, setIkEnabled } = useIk();
|
|
1895
1967
|
const wrapperRef = useRef(null);
|
|
1896
1968
|
const pivotRef = useRef(null);
|
|
1897
1969
|
const draggingRef = useRef(false);
|
|
@@ -1899,19 +1971,15 @@ function IkGizmo({ siteName, scale = 0.18, onDrag }) {
|
|
|
1899
1971
|
const { controls } = useThree();
|
|
1900
1972
|
useEffect(() => {
|
|
1901
1973
|
const model = mjModelRef.current;
|
|
1902
|
-
if (!model || status !== "ready") {
|
|
1974
|
+
if (!model || status !== "ready" || !siteName) {
|
|
1903
1975
|
localSiteIdRef.current = -1;
|
|
1904
1976
|
return;
|
|
1905
1977
|
}
|
|
1906
|
-
|
|
1907
|
-
|
|
1908
|
-
} else {
|
|
1909
|
-
localSiteIdRef.current = siteIdRef.current;
|
|
1910
|
-
}
|
|
1911
|
-
}, [siteName, status, mjModelRef, siteIdRef]);
|
|
1978
|
+
localSiteIdRef.current = findSiteByName(model, siteName);
|
|
1979
|
+
}, [siteName, status, mjModelRef]);
|
|
1912
1980
|
useFrame(() => {
|
|
1913
1981
|
const data = mjDataRef.current;
|
|
1914
|
-
const sid = localSiteIdRef.current;
|
|
1982
|
+
const sid = siteName ? localSiteIdRef.current : siteIdRef.current;
|
|
1915
1983
|
if (!data || sid < 0 || !wrapperRef.current) return;
|
|
1916
1984
|
if (!draggingRef.current) {
|
|
1917
1985
|
const p = data.site_xpos;
|
|
@@ -1956,7 +2024,7 @@ function IkGizmo({ siteName, scale = 0.18, onDrag }) {
|
|
|
1956
2024
|
onDragStart: () => {
|
|
1957
2025
|
draggingRef.current = true;
|
|
1958
2026
|
if (!onDrag) {
|
|
1959
|
-
if (!ikEnabledRef.current)
|
|
2027
|
+
if (!ikEnabledRef.current) setIkEnabled(true);
|
|
1960
2028
|
}
|
|
1961
2029
|
if (controls) controls.enabled = false;
|
|
1962
2030
|
},
|
|
@@ -1984,11 +2052,11 @@ function IkGizmo({ siteName, scale = 0.18, onDrag }) {
|
|
|
1984
2052
|
}
|
|
1985
2053
|
) });
|
|
1986
2054
|
}
|
|
1987
|
-
var _dummy = new
|
|
2055
|
+
var _dummy = new THREE11.Object3D();
|
|
1988
2056
|
function ContactMarkers({
|
|
1989
2057
|
maxContacts = 100,
|
|
1990
|
-
radius =
|
|
1991
|
-
color = "#
|
|
2058
|
+
radius = 8e-3,
|
|
2059
|
+
color = "#22d3ee",
|
|
1992
2060
|
visible = true
|
|
1993
2061
|
} = {}) {
|
|
1994
2062
|
const { mjDataRef, status } = useMujocoSim();
|
|
@@ -2003,43 +2071,33 @@ function ContactMarkers({
|
|
|
2003
2071
|
const ncon = data.ncon;
|
|
2004
2072
|
const count = Math.min(ncon, maxContacts);
|
|
2005
2073
|
for (let i = 0; i < count; i++) {
|
|
2006
|
-
|
|
2007
|
-
|
|
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 {
|
|
2074
|
+
const c = getContact(data, i);
|
|
2075
|
+
if (!c) {
|
|
2013
2076
|
mesh.count = i;
|
|
2014
2077
|
mesh.instanceMatrix.needsUpdate = true;
|
|
2015
2078
|
return;
|
|
2016
2079
|
}
|
|
2080
|
+
_dummy.position.set(c.pos[0], c.pos[1], c.pos[2]);
|
|
2081
|
+
_dummy.updateMatrix();
|
|
2082
|
+
mesh.setMatrixAt(i, _dummy.matrix);
|
|
2017
2083
|
}
|
|
2018
2084
|
mesh.count = count;
|
|
2019
2085
|
mesh.instanceMatrix.needsUpdate = true;
|
|
2020
2086
|
});
|
|
2021
2087
|
if (status !== "ready") return null;
|
|
2022
|
-
return /* @__PURE__ */ jsxs("instancedMesh", { ref: meshRef, args: [void 0, void 0, maxContacts], children: [
|
|
2088
|
+
return /* @__PURE__ */ jsxs("instancedMesh", { ref: meshRef, args: [void 0, void 0, maxContacts], frustumCulled: false, renderOrder: 999, children: [
|
|
2023
2089
|
/* @__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
|
-
)
|
|
2090
|
+
/* @__PURE__ */ jsx("meshBasicMaterial", { color, depthTest: false })
|
|
2033
2091
|
] });
|
|
2034
2092
|
}
|
|
2035
2093
|
var _force = new Float64Array(3);
|
|
2036
2094
|
var _torque = new Float64Array(3);
|
|
2037
2095
|
var _point = new Float64Array(3);
|
|
2038
|
-
var _bodyPos = new
|
|
2039
|
-
var _bodyQuat = new
|
|
2040
|
-
var _worldHit = new
|
|
2041
|
-
var _raycaster = new
|
|
2042
|
-
var _mouse = new
|
|
2096
|
+
var _bodyPos = new THREE11.Vector3();
|
|
2097
|
+
var _bodyQuat = new THREE11.Quaternion();
|
|
2098
|
+
var _worldHit = new THREE11.Vector3();
|
|
2099
|
+
var _raycaster = new THREE11.Raycaster();
|
|
2100
|
+
var _mouse = new THREE11.Vector2();
|
|
2043
2101
|
function DragInteraction({
|
|
2044
2102
|
stiffness = 250,
|
|
2045
2103
|
showArrow = true
|
|
@@ -2049,16 +2107,16 @@ function DragInteraction({
|
|
|
2049
2107
|
const draggingRef = useRef(false);
|
|
2050
2108
|
const bodyIdRef = useRef(-1);
|
|
2051
2109
|
const grabDistanceRef = useRef(0);
|
|
2052
|
-
const localHitRef = useRef(new
|
|
2053
|
-
const grabWorldRef = useRef(new
|
|
2054
|
-
const mouseWorldRef = useRef(new
|
|
2110
|
+
const localHitRef = useRef(new THREE11.Vector3());
|
|
2111
|
+
const grabWorldRef = useRef(new THREE11.Vector3());
|
|
2112
|
+
const mouseWorldRef = useRef(new THREE11.Vector3());
|
|
2055
2113
|
const arrowRef = useRef(null);
|
|
2056
2114
|
const groupRef = useRef(null);
|
|
2057
2115
|
useEffect(() => {
|
|
2058
2116
|
if (!showArrow || !groupRef.current) return;
|
|
2059
|
-
const arrow = new
|
|
2060
|
-
new
|
|
2061
|
-
new
|
|
2117
|
+
const arrow = new THREE11.ArrowHelper(
|
|
2118
|
+
new THREE11.Vector3(0, 1, 0),
|
|
2119
|
+
new THREE11.Vector3(),
|
|
2062
2120
|
0.1,
|
|
2063
2121
|
16729156
|
|
2064
2122
|
);
|
|
@@ -2186,7 +2244,7 @@ function DragInteraction({
|
|
|
2186
2244
|
if (!arrow) return;
|
|
2187
2245
|
if (draggingRef.current && bodyIdRef.current > 0) {
|
|
2188
2246
|
arrow.visible = true;
|
|
2189
|
-
const dir = mouseWorldRef.current
|
|
2247
|
+
const dir = _bodyPos.copy(mouseWorldRef.current).sub(grabWorldRef.current);
|
|
2190
2248
|
const len = dir.length();
|
|
2191
2249
|
if (len > 1e-3) {
|
|
2192
2250
|
dir.normalize();
|
|
@@ -2201,7 +2259,7 @@ function DragInteraction({
|
|
|
2201
2259
|
if (status !== "ready") return null;
|
|
2202
2260
|
return /* @__PURE__ */ jsx("group", { ref: groupRef });
|
|
2203
2261
|
}
|
|
2204
|
-
function
|
|
2262
|
+
function useSceneLights(intensity = 1) {
|
|
2205
2263
|
const { mjModelRef, status } = useMujocoSim();
|
|
2206
2264
|
const { scene } = useThree();
|
|
2207
2265
|
const lightsRef = useRef([]);
|
|
@@ -2229,7 +2287,7 @@ function SceneLights({ intensity = 1 }) {
|
|
|
2229
2287
|
const dr = model.light_diffuse ? model.light_diffuse[3 * i] : 1;
|
|
2230
2288
|
const dg = model.light_diffuse ? model.light_diffuse[3 * i + 1] : 1;
|
|
2231
2289
|
const db = model.light_diffuse ? model.light_diffuse[3 * i + 2] : 1;
|
|
2232
|
-
const color = new
|
|
2290
|
+
const color = new THREE11.Color(dr, dg, db);
|
|
2233
2291
|
const px = model.light_pos[3 * i];
|
|
2234
2292
|
const py = model.light_pos[3 * i + 1];
|
|
2235
2293
|
const pz = model.light_pos[3 * i + 2];
|
|
@@ -2237,7 +2295,7 @@ function SceneLights({ intensity = 1 }) {
|
|
|
2237
2295
|
const dy = model.light_dir[3 * i + 1];
|
|
2238
2296
|
const dz = model.light_dir[3 * i + 2];
|
|
2239
2297
|
if (isDirectional) {
|
|
2240
|
-
const light = new
|
|
2298
|
+
const light = new THREE11.DirectionalLight(color, finalIntensity);
|
|
2241
2299
|
light.position.set(px, py, pz);
|
|
2242
2300
|
light.target.position.set(px + dx, py + dy, pz + dz);
|
|
2243
2301
|
light.castShadow = castShadow;
|
|
@@ -2260,7 +2318,7 @@ function SceneLights({ intensity = 1 }) {
|
|
|
2260
2318
|
const cutoff = model.light_cutoff ? model.light_cutoff[i] : 45;
|
|
2261
2319
|
const exponent = model.light_exponent ? model.light_exponent[i] : 10;
|
|
2262
2320
|
const angle = cutoff * Math.PI / 180;
|
|
2263
|
-
const light = new
|
|
2321
|
+
const light = new THREE11.SpotLight(color, finalIntensity, 0, angle, exponent / 128);
|
|
2264
2322
|
light.position.set(px, py, pz);
|
|
2265
2323
|
light.target.position.set(px + dx, py + dy, pz + dz);
|
|
2266
2324
|
light.castShadow = castShadow;
|
|
@@ -2290,6 +2348,11 @@ function SceneLights({ intensity = 1 }) {
|
|
|
2290
2348
|
targetsRef.current = [];
|
|
2291
2349
|
};
|
|
2292
2350
|
}, [status, mjModelRef, scene, intensity]);
|
|
2351
|
+
}
|
|
2352
|
+
|
|
2353
|
+
// src/components/SceneLights.tsx
|
|
2354
|
+
function SceneLights({ intensity = 1 }) {
|
|
2355
|
+
useSceneLights(intensity);
|
|
2293
2356
|
return null;
|
|
2294
2357
|
}
|
|
2295
2358
|
var JOINT_COLORS = {
|
|
@@ -2302,6 +2365,12 @@ var JOINT_COLORS = {
|
|
|
2302
2365
|
3: 16776960
|
|
2303
2366
|
// hinge - yellow
|
|
2304
2367
|
};
|
|
2368
|
+
var _v3a = new THREE11.Vector3();
|
|
2369
|
+
new THREE11.Vector3();
|
|
2370
|
+
var _quat2 = new THREE11.Quaternion();
|
|
2371
|
+
var _contactPos = new THREE11.Vector3();
|
|
2372
|
+
var _contactNormal = new THREE11.Vector3();
|
|
2373
|
+
var MAX_CONTACT_ARROWS = 50;
|
|
2305
2374
|
function Debug({
|
|
2306
2375
|
showGeoms = false,
|
|
2307
2376
|
showSites = false,
|
|
@@ -2328,21 +2397,21 @@ function Debug({
|
|
|
2328
2397
|
let geometry = null;
|
|
2329
2398
|
switch (type) {
|
|
2330
2399
|
case 2:
|
|
2331
|
-
geometry = new
|
|
2400
|
+
geometry = new THREE11.SphereGeometry(s[3 * i], 12, 8);
|
|
2332
2401
|
break;
|
|
2333
2402
|
case 3:
|
|
2334
|
-
geometry = new
|
|
2403
|
+
geometry = new THREE11.CapsuleGeometry(s[3 * i], s[3 * i + 1] * 2, 6, 8);
|
|
2335
2404
|
break;
|
|
2336
2405
|
case 5:
|
|
2337
|
-
geometry = new
|
|
2406
|
+
geometry = new THREE11.CylinderGeometry(s[3 * i], s[3 * i], s[3 * i + 1] * 2, 12);
|
|
2338
2407
|
break;
|
|
2339
2408
|
case 6:
|
|
2340
|
-
geometry = new
|
|
2409
|
+
geometry = new THREE11.BoxGeometry(s[3 * i] * 2, s[3 * i + 1] * 2, s[3 * i + 2] * 2);
|
|
2341
2410
|
break;
|
|
2342
2411
|
}
|
|
2343
2412
|
if (geometry) {
|
|
2344
|
-
const mat = new
|
|
2345
|
-
const mesh = new
|
|
2413
|
+
const mat = new THREE11.MeshBasicMaterial({ color: 65280, wireframe: true, transparent: true, opacity: 0.3 });
|
|
2414
|
+
const mesh = new THREE11.Mesh(geometry, mat);
|
|
2346
2415
|
mesh.userData.geomId = i;
|
|
2347
2416
|
mesh.userData.bodyId = model.geom_bodyid[i];
|
|
2348
2417
|
geoms.push(mesh);
|
|
@@ -2350,35 +2419,88 @@ function Debug({
|
|
|
2350
2419
|
}
|
|
2351
2420
|
}
|
|
2352
2421
|
if (showSites) {
|
|
2422
|
+
const siteSize = model.site_size;
|
|
2353
2423
|
for (let i = 0; i < model.nsite; i++) {
|
|
2354
|
-
|
|
2355
|
-
|
|
2356
|
-
|
|
2424
|
+
let radius = 8e-3;
|
|
2425
|
+
if (siteSize) {
|
|
2426
|
+
radius = Math.max(siteSize[3 * i] * 0.5, 4e-3);
|
|
2427
|
+
} else {
|
|
2428
|
+
const bodyId = model.site_bodyid[i];
|
|
2429
|
+
let maxGeomSize = 0;
|
|
2430
|
+
for (let g = 0; g < model.ngeom; g++) {
|
|
2431
|
+
if (model.geom_bodyid[g] === bodyId) {
|
|
2432
|
+
maxGeomSize = Math.max(maxGeomSize, model.geom_size[3 * g]);
|
|
2433
|
+
}
|
|
2434
|
+
}
|
|
2435
|
+
if (maxGeomSize > 0) radius = maxGeomSize * 0.15;
|
|
2436
|
+
}
|
|
2437
|
+
const geometry = new THREE11.OctahedronGeometry(radius);
|
|
2438
|
+
const mat = new THREE11.MeshBasicMaterial({ color: 16711935, depthTest: false });
|
|
2439
|
+
const mesh = new THREE11.Mesh(geometry, mat);
|
|
2440
|
+
mesh.renderOrder = 999;
|
|
2441
|
+
mesh.frustumCulled = false;
|
|
2357
2442
|
mesh.userData.siteId = i;
|
|
2443
|
+
const canvas = document.createElement("canvas");
|
|
2444
|
+
canvas.width = 256;
|
|
2445
|
+
canvas.height = 64;
|
|
2446
|
+
const ctx = canvas.getContext("2d");
|
|
2447
|
+
ctx.fillStyle = "#ff00ff";
|
|
2448
|
+
ctx.font = "bold 36px monospace";
|
|
2449
|
+
ctx.textAlign = "center";
|
|
2450
|
+
ctx.fillText(getName(model, model.name_siteadr[i]), 128, 42);
|
|
2451
|
+
const tex = new THREE11.CanvasTexture(canvas);
|
|
2452
|
+
const spriteMat = new THREE11.SpriteMaterial({ map: tex, depthTest: false, transparent: true });
|
|
2453
|
+
const sprite = new THREE11.Sprite(spriteMat);
|
|
2454
|
+
const labelScale = radius * 15;
|
|
2455
|
+
sprite.scale.set(labelScale, labelScale * 0.25, 1);
|
|
2456
|
+
sprite.position.y = radius * 2;
|
|
2457
|
+
sprite.renderOrder = 999;
|
|
2458
|
+
mesh.add(sprite);
|
|
2358
2459
|
sites.push(mesh);
|
|
2359
2460
|
}
|
|
2360
2461
|
}
|
|
2361
2462
|
if (showJoints) {
|
|
2463
|
+
const jntPos = model.jnt_pos;
|
|
2464
|
+
const jntAxis = model.jnt_axis;
|
|
2362
2465
|
for (let i = 0; i < model.njnt; i++) {
|
|
2363
2466
|
const type = model.jnt_type[i];
|
|
2364
2467
|
const color = JOINT_COLORS[type] ?? 16777215;
|
|
2365
|
-
const
|
|
2366
|
-
|
|
2367
|
-
|
|
2368
|
-
|
|
2468
|
+
const bodyId = model.jnt_bodyid[i];
|
|
2469
|
+
let maxGeomSize = 0;
|
|
2470
|
+
for (let g = 0; g < model.ngeom; g++) {
|
|
2471
|
+
if (model.geom_bodyid[g] === bodyId) {
|
|
2472
|
+
maxGeomSize = Math.max(maxGeomSize, model.geom_size[3 * g]);
|
|
2473
|
+
}
|
|
2474
|
+
}
|
|
2475
|
+
const arrowLen = Math.max(maxGeomSize * 0.8, 0.05);
|
|
2476
|
+
const arrow = new THREE11.ArrowHelper(
|
|
2477
|
+
new THREE11.Vector3(0, 0, 1),
|
|
2478
|
+
new THREE11.Vector3(),
|
|
2479
|
+
arrowLen,
|
|
2369
2480
|
color,
|
|
2370
|
-
0.
|
|
2371
|
-
|
|
2481
|
+
arrowLen * 0.25,
|
|
2482
|
+
arrowLen * 0.12
|
|
2372
2483
|
);
|
|
2484
|
+
arrow.renderOrder = 999;
|
|
2485
|
+
arrow.frustumCulled = false;
|
|
2486
|
+
arrow.line.material = new THREE11.LineBasicMaterial({ color, depthTest: false });
|
|
2487
|
+
arrow.cone.material.depthTest = false;
|
|
2488
|
+
arrow.line.renderOrder = 999;
|
|
2489
|
+
arrow.line.frustumCulled = false;
|
|
2490
|
+
arrow.cone.renderOrder = 999;
|
|
2491
|
+
arrow.cone.frustumCulled = false;
|
|
2373
2492
|
arrow.userData.jointId = i;
|
|
2493
|
+
arrow.userData.bodyId = bodyId;
|
|
2494
|
+
arrow.userData.hasJntPos = !!jntPos;
|
|
2495
|
+
arrow.userData.hasJntAxis = !!jntAxis;
|
|
2374
2496
|
joints.push(arrow);
|
|
2375
2497
|
}
|
|
2376
2498
|
}
|
|
2377
2499
|
if (showCOM) {
|
|
2378
2500
|
for (let i = 1; i < model.nbody; i++) {
|
|
2379
|
-
const geometry = new
|
|
2380
|
-
const mat = new
|
|
2381
|
-
const mesh = new
|
|
2501
|
+
const geometry = new THREE11.SphereGeometry(5e-3, 6, 6);
|
|
2502
|
+
const mat = new THREE11.MeshBasicMaterial({ color: 16711680 });
|
|
2503
|
+
const mesh = new THREE11.Mesh(geometry, mat);
|
|
2382
2504
|
mesh.userData.bodyId = i;
|
|
2383
2505
|
comMarkers.push(mesh);
|
|
2384
2506
|
}
|
|
@@ -2406,6 +2528,8 @@ function Debug({
|
|
|
2406
2528
|
const model = mjModelRef.current;
|
|
2407
2529
|
const data = mjDataRef.current;
|
|
2408
2530
|
if (!model || !data || !debugGeometry) return;
|
|
2531
|
+
const jntPos = model.jnt_pos;
|
|
2532
|
+
const jntAxis = model.jnt_axis;
|
|
2409
2533
|
for (const mesh of debugGeometry.geoms) {
|
|
2410
2534
|
const bid = mesh.userData.bodyId;
|
|
2411
2535
|
const i3 = bid * 3;
|
|
@@ -2419,7 +2543,8 @@ function Debug({
|
|
|
2419
2543
|
);
|
|
2420
2544
|
const gid = mesh.userData.geomId;
|
|
2421
2545
|
const gp = model.geom_pos;
|
|
2422
|
-
|
|
2546
|
+
_v3a.set(gp[3 * gid], gp[3 * gid + 1], gp[3 * gid + 2]).applyQuaternion(mesh.quaternion);
|
|
2547
|
+
mesh.position.add(_v3a);
|
|
2423
2548
|
}
|
|
2424
2549
|
for (const mesh of debugGeometry.sites) {
|
|
2425
2550
|
const sid = mesh.userData.siteId;
|
|
@@ -2429,6 +2554,28 @@ function Debug({
|
|
|
2429
2554
|
data.site_xpos[3 * sid + 2]
|
|
2430
2555
|
);
|
|
2431
2556
|
}
|
|
2557
|
+
for (const obj of debugGeometry.joints) {
|
|
2558
|
+
const arrow = obj;
|
|
2559
|
+
const jid = arrow.userData.jointId;
|
|
2560
|
+
const bid = arrow.userData.bodyId;
|
|
2561
|
+
const i3 = bid * 3;
|
|
2562
|
+
const i4 = bid * 4;
|
|
2563
|
+
_quat2.set(
|
|
2564
|
+
data.xquat[i4 + 1],
|
|
2565
|
+
data.xquat[i4 + 2],
|
|
2566
|
+
data.xquat[i4 + 3],
|
|
2567
|
+
data.xquat[i4]
|
|
2568
|
+
);
|
|
2569
|
+
arrow.position.set(data.xpos[i3], data.xpos[i3 + 1], data.xpos[i3 + 2]);
|
|
2570
|
+
if (jntPos) {
|
|
2571
|
+
_v3a.set(jntPos[3 * jid], jntPos[3 * jid + 1], jntPos[3 * jid + 2]).applyQuaternion(_quat2);
|
|
2572
|
+
arrow.position.add(_v3a);
|
|
2573
|
+
}
|
|
2574
|
+
if (jntAxis) {
|
|
2575
|
+
_v3a.set(jntAxis[3 * jid], jntAxis[3 * jid + 1], jntAxis[3 * jid + 2]).applyQuaternion(_quat2).normalize();
|
|
2576
|
+
arrow.setDirection(_v3a);
|
|
2577
|
+
}
|
|
2578
|
+
}
|
|
2432
2579
|
for (const mesh of debugGeometry.comMarkers) {
|
|
2433
2580
|
const bid = mesh.userData.bodyId;
|
|
2434
2581
|
const i3 = bid * 3;
|
|
@@ -2436,34 +2583,61 @@ function Debug({
|
|
|
2436
2583
|
}
|
|
2437
2584
|
});
|
|
2438
2585
|
const contactGroupRef = useRef(null);
|
|
2439
|
-
const
|
|
2586
|
+
const contactPoolRef = useRef([]);
|
|
2587
|
+
const contactPoolInitRef = useRef(false);
|
|
2588
|
+
useEffect(() => {
|
|
2589
|
+
const group = contactGroupRef.current;
|
|
2590
|
+
if (!group || contactPoolInitRef.current) return;
|
|
2591
|
+
contactPoolInitRef.current = true;
|
|
2592
|
+
const pool = [];
|
|
2593
|
+
for (let i = 0; i < MAX_CONTACT_ARROWS; i++) {
|
|
2594
|
+
const arrow = new THREE11.ArrowHelper(
|
|
2595
|
+
new THREE11.Vector3(0, 1, 0),
|
|
2596
|
+
new THREE11.Vector3(),
|
|
2597
|
+
0.1,
|
|
2598
|
+
16729156,
|
|
2599
|
+
0.03,
|
|
2600
|
+
0.015
|
|
2601
|
+
);
|
|
2602
|
+
arrow.visible = false;
|
|
2603
|
+
group.add(arrow);
|
|
2604
|
+
pool.push(arrow);
|
|
2605
|
+
}
|
|
2606
|
+
contactPoolRef.current = pool;
|
|
2607
|
+
return () => {
|
|
2608
|
+
for (const arrow of pool) {
|
|
2609
|
+
group.remove(arrow);
|
|
2610
|
+
arrow.dispose();
|
|
2611
|
+
}
|
|
2612
|
+
contactPoolRef.current = [];
|
|
2613
|
+
contactPoolInitRef.current = false;
|
|
2614
|
+
};
|
|
2615
|
+
}, [showContacts]);
|
|
2440
2616
|
useFrame(() => {
|
|
2441
2617
|
if (!showContacts) return;
|
|
2442
|
-
const model = mjModelRef.current;
|
|
2443
2618
|
const data = mjDataRef.current;
|
|
2444
|
-
const
|
|
2445
|
-
if (!
|
|
2446
|
-
for (const arrow of contactArrowsRef.current) {
|
|
2447
|
-
group.remove(arrow);
|
|
2448
|
-
arrow.dispose();
|
|
2449
|
-
}
|
|
2450
|
-
contactArrowsRef.current = [];
|
|
2619
|
+
const pool = contactPoolRef.current;
|
|
2620
|
+
if (!data || pool.length === 0) return;
|
|
2451
2621
|
const ncon = data.ncon;
|
|
2452
|
-
|
|
2453
|
-
|
|
2454
|
-
|
|
2455
|
-
|
|
2456
|
-
|
|
2457
|
-
|
|
2458
|
-
|
|
2459
|
-
|
|
2460
|
-
|
|
2461
|
-
|
|
2462
|
-
|
|
2463
|
-
|
|
2464
|
-
|
|
2465
|
-
|
|
2466
|
-
|
|
2622
|
+
let arrowIdx = 0;
|
|
2623
|
+
for (let i = 0; i < Math.min(ncon, MAX_CONTACT_ARROWS); i++) {
|
|
2624
|
+
const c = getContact(data, i);
|
|
2625
|
+
if (!c) break;
|
|
2626
|
+
_contactPos.set(c.pos[0], c.pos[1], c.pos[2]);
|
|
2627
|
+
_contactNormal.set(c.frame[0], c.frame[1], c.frame[2]);
|
|
2628
|
+
const force = Math.abs(c.dist) * 100;
|
|
2629
|
+
const length = Math.min(force * 0.01, 0.1);
|
|
2630
|
+
if (length > 1e-3 && arrowIdx < pool.length) {
|
|
2631
|
+
const arrow = pool[arrowIdx];
|
|
2632
|
+
arrow.position.copy(_contactPos);
|
|
2633
|
+
arrow.setDirection(_contactNormal);
|
|
2634
|
+
arrow.setLength(length, length * 0.3, length * 0.15);
|
|
2635
|
+
arrow.visible = true;
|
|
2636
|
+
arrowIdx++;
|
|
2637
|
+
}
|
|
2638
|
+
}
|
|
2639
|
+
for (let i = arrowIdx; i < pool.length; i++) {
|
|
2640
|
+
pool[i].visible = false;
|
|
2467
2641
|
}
|
|
2468
2642
|
});
|
|
2469
2643
|
if (status !== "ready") return null;
|
|
@@ -2472,30 +2646,75 @@ function Debug({
|
|
|
2472
2646
|
showContacts && /* @__PURE__ */ jsx("group", { ref: contactGroupRef })
|
|
2473
2647
|
] });
|
|
2474
2648
|
}
|
|
2475
|
-
var DEFAULT_TENDON_COLOR = new
|
|
2649
|
+
var DEFAULT_TENDON_COLOR = new THREE11.Color(0.3, 0.3, 0.8);
|
|
2476
2650
|
var DEFAULT_TENDON_WIDTH = 2e-3;
|
|
2651
|
+
new THREE11.Vector3();
|
|
2477
2652
|
function TendonRenderer() {
|
|
2478
2653
|
const { mjModelRef, mjDataRef, status } = useMujocoSim();
|
|
2479
2654
|
const groupRef = useRef(null);
|
|
2480
2655
|
const meshesRef = useRef([]);
|
|
2481
|
-
|
|
2656
|
+
const curvesRef = useRef([]);
|
|
2657
|
+
const materialRef = useRef(null);
|
|
2658
|
+
useEffect(() => {
|
|
2482
2659
|
const model = mjModelRef.current;
|
|
2483
2660
|
const data = mjDataRef.current;
|
|
2484
2661
|
const group = groupRef.current;
|
|
2485
2662
|
if (!model || !data || !group) return;
|
|
2486
2663
|
const ntendon = model.ntendon ?? 0;
|
|
2487
2664
|
if (ntendon === 0) return;
|
|
2488
|
-
|
|
2489
|
-
|
|
2490
|
-
|
|
2491
|
-
|
|
2665
|
+
const material = new THREE11.MeshStandardMaterial({
|
|
2666
|
+
color: DEFAULT_TENDON_COLOR,
|
|
2667
|
+
roughness: 0.6,
|
|
2668
|
+
metalness: 0.1
|
|
2669
|
+
});
|
|
2670
|
+
materialRef.current = material;
|
|
2671
|
+
const meshes = [];
|
|
2672
|
+
const curves = [];
|
|
2673
|
+
for (let t = 0; t < ntendon; t++) {
|
|
2674
|
+
const wrapNum = model.ten_wrapnum[t];
|
|
2675
|
+
if (wrapNum < 2) {
|
|
2676
|
+
meshes.push(null);
|
|
2677
|
+
curves.push(null);
|
|
2678
|
+
continue;
|
|
2679
|
+
}
|
|
2680
|
+
const points = Array.from({ length: wrapNum }, () => new THREE11.Vector3());
|
|
2681
|
+
const curve = new THREE11.CatmullRomCurve3(points, false);
|
|
2682
|
+
const segments = Math.max(wrapNum * 2, 4);
|
|
2683
|
+
const geometry = new THREE11.TubeGeometry(curve, segments, DEFAULT_TENDON_WIDTH, 6, false);
|
|
2684
|
+
const mesh = new THREE11.Mesh(geometry, material);
|
|
2685
|
+
mesh.frustumCulled = false;
|
|
2686
|
+
group.add(mesh);
|
|
2687
|
+
meshes.push(mesh);
|
|
2688
|
+
curves.push(curve);
|
|
2492
2689
|
}
|
|
2493
|
-
meshesRef.current =
|
|
2690
|
+
meshesRef.current = meshes;
|
|
2691
|
+
curvesRef.current = curves;
|
|
2692
|
+
return () => {
|
|
2693
|
+
for (const mesh of meshes) {
|
|
2694
|
+
if (!mesh) continue;
|
|
2695
|
+
group.remove(mesh);
|
|
2696
|
+
mesh.geometry.dispose();
|
|
2697
|
+
}
|
|
2698
|
+
material.dispose();
|
|
2699
|
+
meshesRef.current = [];
|
|
2700
|
+
curvesRef.current = [];
|
|
2701
|
+
materialRef.current = null;
|
|
2702
|
+
};
|
|
2703
|
+
}, [status, mjModelRef, mjDataRef]);
|
|
2704
|
+
useFrame(() => {
|
|
2705
|
+
const model = mjModelRef.current;
|
|
2706
|
+
const data = mjDataRef.current;
|
|
2707
|
+
if (!model || !data) return;
|
|
2708
|
+
const ntendon = model.ntendon ?? 0;
|
|
2709
|
+
const meshes = meshesRef.current;
|
|
2710
|
+
const curves = curvesRef.current;
|
|
2494
2711
|
for (let t = 0; t < ntendon; t++) {
|
|
2712
|
+
const mesh = meshes[t];
|
|
2713
|
+
const curve = curves[t];
|
|
2714
|
+
if (!mesh || !curve) continue;
|
|
2495
2715
|
const wrapAdr = model.ten_wrapadr[t];
|
|
2496
2716
|
const wrapNum = model.ten_wrapnum[t];
|
|
2497
|
-
|
|
2498
|
-
const points = [];
|
|
2717
|
+
let validCount = 0;
|
|
2499
2718
|
for (let w = 0; w < wrapNum; w++) {
|
|
2500
2719
|
const idx = (wrapAdr + w) * 3;
|
|
2501
2720
|
if (data.wrap_xpos && idx + 2 < data.wrap_xpos.length) {
|
|
@@ -2503,27 +2722,32 @@ function TendonRenderer() {
|
|
|
2503
2722
|
const y = data.wrap_xpos[idx + 1];
|
|
2504
2723
|
const z = data.wrap_xpos[idx + 2];
|
|
2505
2724
|
if (x !== 0 || y !== 0 || z !== 0) {
|
|
2506
|
-
|
|
2725
|
+
if (validCount < curve.points.length) {
|
|
2726
|
+
curve.points[validCount].set(x, y, z);
|
|
2727
|
+
}
|
|
2728
|
+
validCount++;
|
|
2507
2729
|
}
|
|
2508
2730
|
}
|
|
2509
2731
|
}
|
|
2510
|
-
if (
|
|
2511
|
-
|
|
2512
|
-
|
|
2732
|
+
if (validCount < 2) {
|
|
2733
|
+
mesh.visible = false;
|
|
2734
|
+
continue;
|
|
2735
|
+
}
|
|
2736
|
+
if (curve.points.length !== validCount) {
|
|
2737
|
+
curve.points.length = validCount;
|
|
2738
|
+
while (curve.points.length < validCount) {
|
|
2739
|
+
curve.points.push(new THREE11.Vector3());
|
|
2740
|
+
}
|
|
2741
|
+
}
|
|
2742
|
+
mesh.geometry.dispose();
|
|
2743
|
+
mesh.geometry = new THREE11.TubeGeometry(
|
|
2513
2744
|
curve,
|
|
2514
|
-
Math.max(
|
|
2745
|
+
Math.max(validCount * 2, 4),
|
|
2515
2746
|
DEFAULT_TENDON_WIDTH,
|
|
2516
2747
|
6,
|
|
2517
2748
|
false
|
|
2518
2749
|
);
|
|
2519
|
-
|
|
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);
|
|
2750
|
+
mesh.visible = true;
|
|
2527
2751
|
}
|
|
2528
2752
|
});
|
|
2529
2753
|
if (status !== "ready") return null;
|
|
@@ -2543,24 +2767,24 @@ function FlexRenderer() {
|
|
|
2543
2767
|
const vertAdr = model.flex_vertadr[f];
|
|
2544
2768
|
const vertNum = model.flex_vertnum[f];
|
|
2545
2769
|
if (vertNum === 0) continue;
|
|
2546
|
-
const geometry = new
|
|
2770
|
+
const geometry = new THREE11.BufferGeometry();
|
|
2547
2771
|
const positions = new Float32Array(vertNum * 3);
|
|
2548
|
-
geometry.setAttribute("position", new
|
|
2772
|
+
geometry.setAttribute("position", new THREE11.BufferAttribute(positions, 3));
|
|
2549
2773
|
geometry.computeVertexNormals();
|
|
2550
|
-
let color = new
|
|
2774
|
+
let color = new THREE11.Color(0.5, 0.5, 0.5);
|
|
2551
2775
|
if (model.flex_rgba) {
|
|
2552
|
-
color = new
|
|
2776
|
+
color = new THREE11.Color(
|
|
2553
2777
|
model.flex_rgba[4 * f],
|
|
2554
2778
|
model.flex_rgba[4 * f + 1],
|
|
2555
2779
|
model.flex_rgba[4 * f + 2]
|
|
2556
2780
|
);
|
|
2557
2781
|
}
|
|
2558
|
-
const material = new
|
|
2782
|
+
const material = new THREE11.MeshStandardMaterial({
|
|
2559
2783
|
color,
|
|
2560
2784
|
roughness: 0.7,
|
|
2561
|
-
side:
|
|
2785
|
+
side: THREE11.DoubleSide
|
|
2562
2786
|
});
|
|
2563
|
-
const mesh = new
|
|
2787
|
+
const mesh = new THREE11.Mesh(geometry, material);
|
|
2564
2788
|
mesh.userData.flexId = f;
|
|
2565
2789
|
mesh.userData.vertAdr = vertAdr;
|
|
2566
2790
|
mesh.userData.vertNum = vertNum;
|
|
@@ -2595,22 +2819,45 @@ function FlexRenderer() {
|
|
|
2595
2819
|
if (status !== "ready") return null;
|
|
2596
2820
|
return /* @__PURE__ */ jsx("group", { ref: groupRef });
|
|
2597
2821
|
}
|
|
2822
|
+
var geomNameCacheByModel = /* @__PURE__ */ new WeakMap();
|
|
2823
|
+
function getGeomNameCached(model, geomId) {
|
|
2824
|
+
let perModel = geomNameCacheByModel.get(model);
|
|
2825
|
+
if (!perModel) {
|
|
2826
|
+
perModel = /* @__PURE__ */ new Map();
|
|
2827
|
+
geomNameCacheByModel.set(model, perModel);
|
|
2828
|
+
}
|
|
2829
|
+
let name = perModel.get(geomId);
|
|
2830
|
+
if (name === void 0) {
|
|
2831
|
+
name = getName(model, model.name_geomadr[geomId]);
|
|
2832
|
+
perModel.set(geomId, name);
|
|
2833
|
+
}
|
|
2834
|
+
return name;
|
|
2835
|
+
}
|
|
2598
2836
|
function useContacts(bodyName, callback) {
|
|
2599
|
-
const { mjModelRef } = useMujocoSim();
|
|
2837
|
+
const { mjModelRef, status } = useMujocoSim();
|
|
2600
2838
|
const contactsRef = useRef([]);
|
|
2601
2839
|
const bodyIdRef = useRef(-1);
|
|
2840
|
+
const bodyResolvedRef = useRef(false);
|
|
2602
2841
|
const callbackRef = useRef(callback);
|
|
2603
2842
|
callbackRef.current = callback;
|
|
2604
2843
|
useEffect(() => {
|
|
2605
2844
|
if (!bodyName) {
|
|
2606
2845
|
bodyIdRef.current = -1;
|
|
2846
|
+
bodyResolvedRef.current = true;
|
|
2607
2847
|
return;
|
|
2608
2848
|
}
|
|
2849
|
+
bodyResolvedRef.current = false;
|
|
2850
|
+
if (status !== "ready") return;
|
|
2609
2851
|
const model = mjModelRef.current;
|
|
2610
2852
|
if (!model) return;
|
|
2611
2853
|
bodyIdRef.current = findBodyByName(model, bodyName);
|
|
2612
|
-
|
|
2854
|
+
bodyResolvedRef.current = true;
|
|
2855
|
+
}, [bodyName, status, mjModelRef]);
|
|
2613
2856
|
useAfterPhysicsStep((model, data) => {
|
|
2857
|
+
if (bodyName && !bodyResolvedRef.current) {
|
|
2858
|
+
bodyIdRef.current = findBodyByName(model, bodyName);
|
|
2859
|
+
bodyResolvedRef.current = true;
|
|
2860
|
+
}
|
|
2614
2861
|
const ncon = data.ncon;
|
|
2615
2862
|
if (ncon === 0) {
|
|
2616
2863
|
if (contactsRef.current.length > 0) contactsRef.current = [];
|
|
@@ -2620,24 +2867,21 @@ function useContacts(bodyName, callback) {
|
|
|
2620
2867
|
const contacts = [];
|
|
2621
2868
|
const filterBody = bodyIdRef.current;
|
|
2622
2869
|
for (let i = 0; i < ncon; i++) {
|
|
2623
|
-
|
|
2624
|
-
|
|
2625
|
-
|
|
2626
|
-
|
|
2627
|
-
|
|
2628
|
-
|
|
2629
|
-
|
|
2630
|
-
|
|
2631
|
-
|
|
2632
|
-
|
|
2633
|
-
|
|
2634
|
-
|
|
2635
|
-
|
|
2636
|
-
|
|
2637
|
-
|
|
2638
|
-
} catch {
|
|
2639
|
-
break;
|
|
2640
|
-
}
|
|
2870
|
+
const c = getContact(data, i);
|
|
2871
|
+
if (!c) break;
|
|
2872
|
+
if (filterBody >= 0) {
|
|
2873
|
+
const b1 = model.geom_bodyid[c.geom1];
|
|
2874
|
+
const b2 = model.geom_bodyid[c.geom2];
|
|
2875
|
+
if (b1 !== filterBody && b2 !== filterBody) continue;
|
|
2876
|
+
}
|
|
2877
|
+
contacts.push({
|
|
2878
|
+
geom1: c.geom1,
|
|
2879
|
+
geom1Name: getGeomNameCached(model, c.geom1),
|
|
2880
|
+
geom2: c.geom2,
|
|
2881
|
+
geom2Name: getGeomNameCached(model, c.geom2),
|
|
2882
|
+
pos: [c.pos[0], c.pos[1], c.pos[2]],
|
|
2883
|
+
depth: c.dist
|
|
2884
|
+
});
|
|
2641
2885
|
}
|
|
2642
2886
|
contactsRef.current = contacts;
|
|
2643
2887
|
callbackRef.current?.(contacts);
|
|
@@ -2771,28 +3015,28 @@ function TrajectoryPlayer({
|
|
|
2771
3015
|
onFrame
|
|
2772
3016
|
}) {
|
|
2773
3017
|
const player = useTrajectoryPlayer(trajectory, { fps, loop });
|
|
3018
|
+
const onFrameRef = useRef(onFrame);
|
|
3019
|
+
onFrameRef.current = onFrame;
|
|
3020
|
+
const lastReportedFrameRef = useRef(-1);
|
|
2774
3021
|
useEffect(() => {
|
|
2775
3022
|
if (playing) {
|
|
2776
3023
|
player.play();
|
|
2777
3024
|
} else {
|
|
2778
3025
|
player.pause();
|
|
2779
3026
|
}
|
|
2780
|
-
}, [playing]);
|
|
2781
|
-
|
|
2782
|
-
if (
|
|
2783
|
-
|
|
2784
|
-
|
|
2785
|
-
|
|
2786
|
-
|
|
3027
|
+
}, [playing, player]);
|
|
3028
|
+
useFrame(() => {
|
|
3029
|
+
if (!onFrameRef.current) return;
|
|
3030
|
+
const currentFrame = player.frame;
|
|
3031
|
+
if (currentFrame !== lastReportedFrameRef.current && player.playing) {
|
|
3032
|
+
lastReportedFrameRef.current = currentFrame;
|
|
3033
|
+
onFrameRef.current(currentFrame);
|
|
2787
3034
|
}
|
|
2788
|
-
}
|
|
3035
|
+
});
|
|
2789
3036
|
return null;
|
|
2790
3037
|
}
|
|
2791
|
-
function
|
|
2792
|
-
|
|
2793
|
-
color = "#ff4444",
|
|
2794
|
-
emissiveIntensity = 0.3
|
|
2795
|
-
}) {
|
|
3038
|
+
function useSelectionHighlight(bodyId, options = {}) {
|
|
3039
|
+
const { color = "#ff4444", emissiveIntensity = 0.3 } = options;
|
|
2796
3040
|
const { scene } = useThree();
|
|
2797
3041
|
const prevMeshesRef = useRef([]);
|
|
2798
3042
|
useEffect(() => {
|
|
@@ -2805,7 +3049,7 @@ function SelectionHighlight({
|
|
|
2805
3049
|
}
|
|
2806
3050
|
prevMeshesRef.current = [];
|
|
2807
3051
|
if (bodyId === null || bodyId < 0) return;
|
|
2808
|
-
const highlightColor = new
|
|
3052
|
+
const highlightColor = new THREE11.Color(color);
|
|
2809
3053
|
scene.traverse((obj) => {
|
|
2810
3054
|
if (obj.userData.bodyID === bodyId && obj.isMesh) {
|
|
2811
3055
|
const mesh = obj;
|
|
@@ -2832,6 +3076,15 @@ function SelectionHighlight({
|
|
|
2832
3076
|
prevMeshesRef.current = [];
|
|
2833
3077
|
};
|
|
2834
3078
|
}, [bodyId, color, emissiveIntensity, scene]);
|
|
3079
|
+
}
|
|
3080
|
+
|
|
3081
|
+
// src/components/SelectionHighlight.tsx
|
|
3082
|
+
function SelectionHighlight({
|
|
3083
|
+
bodyId,
|
|
3084
|
+
color = "#ff4444",
|
|
3085
|
+
emissiveIntensity = 0.3
|
|
3086
|
+
}) {
|
|
3087
|
+
useSelectionHighlight(bodyId, { color, emissiveIntensity });
|
|
2835
3088
|
return null;
|
|
2836
3089
|
}
|
|
2837
3090
|
function useActuators() {
|
|
@@ -2852,12 +3105,12 @@ function useActuators() {
|
|
|
2852
3105
|
return actuators;
|
|
2853
3106
|
}, [status, mjModelRef]);
|
|
2854
3107
|
}
|
|
2855
|
-
var _mat42 = new
|
|
3108
|
+
var _mat42 = new THREE11.Matrix4();
|
|
2856
3109
|
function useSitePosition(siteName) {
|
|
2857
3110
|
const { mjModelRef, mjDataRef, status } = useMujocoSim();
|
|
2858
3111
|
const siteIdRef = useRef(-1);
|
|
2859
|
-
const positionRef = useRef(new
|
|
2860
|
-
const quaternionRef = useRef(new
|
|
3112
|
+
const positionRef = useRef(new THREE11.Vector3());
|
|
3113
|
+
const quaternionRef = useRef(new THREE11.Quaternion());
|
|
2861
3114
|
useEffect(() => {
|
|
2862
3115
|
const model = mjModelRef.current;
|
|
2863
3116
|
if (!model || status !== "ready") {
|
|
@@ -2986,6 +3239,8 @@ function useJointState(name) {
|
|
|
2986
3239
|
const dofDimRef = useRef(1);
|
|
2987
3240
|
const positionRef = useRef(0);
|
|
2988
3241
|
const velocityRef = useRef(0);
|
|
3242
|
+
const posBufferRef = useRef(null);
|
|
3243
|
+
const velBufferRef = useRef(null);
|
|
2989
3244
|
useEffect(() => {
|
|
2990
3245
|
const model = mjModelRef.current;
|
|
2991
3246
|
if (!model || status !== "ready") return;
|
|
@@ -3005,6 +3260,13 @@ function useJointState(name) {
|
|
|
3005
3260
|
qposDimRef.current = 1;
|
|
3006
3261
|
dofDimRef.current = 1;
|
|
3007
3262
|
}
|
|
3263
|
+
if (qposDimRef.current > 1) {
|
|
3264
|
+
posBufferRef.current = new Float64Array(qposDimRef.current);
|
|
3265
|
+
velBufferRef.current = new Float64Array(dofDimRef.current);
|
|
3266
|
+
} else {
|
|
3267
|
+
posBufferRef.current = null;
|
|
3268
|
+
velBufferRef.current = null;
|
|
3269
|
+
}
|
|
3008
3270
|
return;
|
|
3009
3271
|
}
|
|
3010
3272
|
}
|
|
@@ -3018,8 +3280,12 @@ function useJointState(name) {
|
|
|
3018
3280
|
positionRef.current = data.qpos[qa];
|
|
3019
3281
|
velocityRef.current = data.qvel[da];
|
|
3020
3282
|
} else {
|
|
3021
|
-
|
|
3022
|
-
|
|
3283
|
+
const posBuf = posBufferRef.current;
|
|
3284
|
+
const velBuf = velBufferRef.current;
|
|
3285
|
+
posBuf.set(data.qpos.subarray(qa, qa + qposDimRef.current));
|
|
3286
|
+
velBuf.set(data.qvel.subarray(da, da + dofDimRef.current));
|
|
3287
|
+
positionRef.current = posBuf;
|
|
3288
|
+
velocityRef.current = velBuf;
|
|
3023
3289
|
}
|
|
3024
3290
|
});
|
|
3025
3291
|
return { position: positionRef, velocity: velocityRef };
|
|
@@ -3027,10 +3293,10 @@ function useJointState(name) {
|
|
|
3027
3293
|
function useBodyState(name) {
|
|
3028
3294
|
const { mjModelRef, status } = useMujocoSim();
|
|
3029
3295
|
const bodyIdRef = useRef(-1);
|
|
3030
|
-
const position = useRef(new
|
|
3031
|
-
const quaternion = useRef(new
|
|
3032
|
-
const linearVelocity = useRef(new
|
|
3033
|
-
const angularVelocity = useRef(new
|
|
3296
|
+
const position = useRef(new THREE11.Vector3());
|
|
3297
|
+
const quaternion = useRef(new THREE11.Quaternion());
|
|
3298
|
+
const linearVelocity = useRef(new THREE11.Vector3());
|
|
3299
|
+
const angularVelocity = useRef(new THREE11.Vector3());
|
|
3034
3300
|
useEffect(() => {
|
|
3035
3301
|
const model = mjModelRef.current;
|
|
3036
3302
|
if (!model || status !== "ready") return;
|
|
@@ -3367,10 +3633,97 @@ function useCtrlNoise(config = {}) {
|
|
|
3367
3633
|
}
|
|
3368
3634
|
});
|
|
3369
3635
|
}
|
|
3636
|
+
function useCameraAnimation() {
|
|
3637
|
+
const { camera } = useThree();
|
|
3638
|
+
const orbitTargetRef = useRef(new THREE11.Vector3(0, 0, 0));
|
|
3639
|
+
const cameraAnimRef = useRef({
|
|
3640
|
+
active: false,
|
|
3641
|
+
startPos: new THREE11.Vector3(),
|
|
3642
|
+
endPos: new THREE11.Vector3(),
|
|
3643
|
+
startRot: new THREE11.Quaternion(),
|
|
3644
|
+
endRot: new THREE11.Quaternion(),
|
|
3645
|
+
startTarget: new THREE11.Vector3(),
|
|
3646
|
+
endTarget: new THREE11.Vector3(),
|
|
3647
|
+
startTime: 0,
|
|
3648
|
+
duration: 0,
|
|
3649
|
+
resolve: null
|
|
3650
|
+
});
|
|
3651
|
+
useFrame((state) => {
|
|
3652
|
+
const ca = cameraAnimRef.current;
|
|
3653
|
+
if (!ca.active) return;
|
|
3654
|
+
const now = performance.now();
|
|
3655
|
+
const progress = Math.min((now - ca.startTime) / ca.duration, 1);
|
|
3656
|
+
const ease = progress < 0.5 ? 4 * progress * progress * progress : 1 - Math.pow(-2 * progress + 2, 3) / 2;
|
|
3657
|
+
camera.position.lerpVectors(ca.startPos, ca.endPos, ease);
|
|
3658
|
+
camera.quaternion.slerpQuaternions(ca.startRot, ca.endRot, ease);
|
|
3659
|
+
orbitTargetRef.current.lerpVectors(ca.startTarget, ca.endTarget, ease);
|
|
3660
|
+
const orbitControls = state.controls;
|
|
3661
|
+
if (orbitControls?.target) {
|
|
3662
|
+
orbitControls.target.copy(orbitTargetRef.current);
|
|
3663
|
+
}
|
|
3664
|
+
if (progress >= 1) {
|
|
3665
|
+
ca.active = false;
|
|
3666
|
+
camera.position.copy(ca.endPos);
|
|
3667
|
+
camera.quaternion.copy(ca.endRot);
|
|
3668
|
+
orbitTargetRef.current.copy(ca.endTarget);
|
|
3669
|
+
ca.resolve?.();
|
|
3670
|
+
ca.resolve = null;
|
|
3671
|
+
}
|
|
3672
|
+
});
|
|
3673
|
+
const getCameraState = useCallback(
|
|
3674
|
+
() => ({
|
|
3675
|
+
position: camera.position.clone(),
|
|
3676
|
+
target: orbitTargetRef.current.clone()
|
|
3677
|
+
}),
|
|
3678
|
+
[camera]
|
|
3679
|
+
);
|
|
3680
|
+
const moveCameraTo = useCallback(
|
|
3681
|
+
(position, target, durationMs) => {
|
|
3682
|
+
return new Promise((resolve) => {
|
|
3683
|
+
const ca = cameraAnimRef.current;
|
|
3684
|
+
ca.active = true;
|
|
3685
|
+
ca.startTime = performance.now();
|
|
3686
|
+
ca.duration = durationMs;
|
|
3687
|
+
ca.startPos.copy(camera.position);
|
|
3688
|
+
ca.startRot.copy(camera.quaternion);
|
|
3689
|
+
ca.startTarget.copy(orbitTargetRef.current);
|
|
3690
|
+
ca.endPos.copy(position);
|
|
3691
|
+
ca.endTarget.copy(target);
|
|
3692
|
+
const dummyCam = camera.clone();
|
|
3693
|
+
dummyCam.position.copy(position);
|
|
3694
|
+
dummyCam.lookAt(target);
|
|
3695
|
+
ca.endRot.copy(dummyCam.quaternion);
|
|
3696
|
+
ca.resolve = resolve;
|
|
3697
|
+
setTimeout(resolve, durationMs + 100);
|
|
3698
|
+
});
|
|
3699
|
+
},
|
|
3700
|
+
[camera]
|
|
3701
|
+
);
|
|
3702
|
+
return { getCameraState, moveCameraTo };
|
|
3703
|
+
}
|
|
3370
3704
|
/**
|
|
3371
3705
|
* @license
|
|
3372
3706
|
* SPDX-License-Identifier: Apache-2.0
|
|
3373
3707
|
*/
|
|
3708
|
+
/**
|
|
3709
|
+
* @license
|
|
3710
|
+
* SPDX-License-Identifier: Apache-2.0
|
|
3711
|
+
*
|
|
3712
|
+
* createController — typed factory for BYOC (Bring Your Own Controller) plugins.
|
|
3713
|
+
*/
|
|
3714
|
+
/**
|
|
3715
|
+
* @license
|
|
3716
|
+
* SPDX-License-Identifier: Apache-2.0
|
|
3717
|
+
*
|
|
3718
|
+
* IkContext — React context for the IK controller plugin.
|
|
3719
|
+
*/
|
|
3720
|
+
/**
|
|
3721
|
+
* @license
|
|
3722
|
+
* SPDX-License-Identifier: Apache-2.0
|
|
3723
|
+
*
|
|
3724
|
+
* IkController — composable IK controller plugin.
|
|
3725
|
+
* Extracts all IK logic from MujocoSimProvider into an opt-in component.
|
|
3726
|
+
*/
|
|
3374
3727
|
/**
|
|
3375
3728
|
* @license
|
|
3376
3729
|
* SPDX-License-Identifier: Apache-2.0
|
|
@@ -3384,6 +3737,14 @@ function useCtrlNoise(config = {}) {
|
|
|
3384
3737
|
* Fixed from original: reads data.ncon first, accesses contact via .get(i),
|
|
3385
3738
|
* limits to maxContacts to avoid WASM heap OOM.
|
|
3386
3739
|
*/
|
|
3740
|
+
/**
|
|
3741
|
+
* @license
|
|
3742
|
+
* SPDX-License-Identifier: Apache-2.0
|
|
3743
|
+
*
|
|
3744
|
+
* useSceneLights — hook form of SceneLights (spec 6.3)
|
|
3745
|
+
*
|
|
3746
|
+
* Auto-creates Three.js lights from MJCF <light> elements.
|
|
3747
|
+
*/
|
|
3387
3748
|
/**
|
|
3388
3749
|
* @license
|
|
3389
3750
|
* SPDX-License-Identifier: Apache-2.0
|
|
@@ -3446,6 +3807,15 @@ function useCtrlNoise(config = {}) {
|
|
|
3446
3807
|
*
|
|
3447
3808
|
* TrajectoryPlayer — component form of trajectory playback (spec 13.2)
|
|
3448
3809
|
*/
|
|
3810
|
+
/**
|
|
3811
|
+
* @license
|
|
3812
|
+
* SPDX-License-Identifier: Apache-2.0
|
|
3813
|
+
*
|
|
3814
|
+
* useSelectionHighlight — hook form of SelectionHighlight (spec 6.5)
|
|
3815
|
+
*
|
|
3816
|
+
* Applies emissive highlight to all meshes belonging to a body.
|
|
3817
|
+
* Restores original emissive when bodyId changes or hook unmounts.
|
|
3818
|
+
*/
|
|
3449
3819
|
/**
|
|
3450
3820
|
* @license
|
|
3451
3821
|
* SPDX-License-Identifier: Apache-2.0
|
|
@@ -3512,7 +3882,13 @@ function useCtrlNoise(config = {}) {
|
|
|
3512
3882
|
*
|
|
3513
3883
|
* useCtrlNoise — control noise / perturbation hook (spec 3.2)
|
|
3514
3884
|
*/
|
|
3885
|
+
/**
|
|
3886
|
+
* @license
|
|
3887
|
+
* SPDX-License-Identifier: Apache-2.0
|
|
3888
|
+
*
|
|
3889
|
+
* useCameraAnimation — composable camera animation hook.
|
|
3890
|
+
*/
|
|
3515
3891
|
|
|
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 };
|
|
3892
|
+
export { ContactListener, ContactMarkers, Debug, DragInteraction, FlexRenderer, IkController, IkGizmo, MujocoCanvas, MujocoProvider, MujocoSimProvider, SceneLights, SceneRenderer, SelectionHighlight, TendonRenderer, TrajectoryPlayer, createController, findActuatorByName, findBodyByName, findGeomByName, findJointByName, findKeyframeByName, findSensorByName, findSiteByName, findTendonByName, getContact, getName, loadScene, useActuators, useAfterPhysicsStep, useBeforePhysicsStep, useBodyState, useCameraAnimation, useContactEvents, useContacts, useCtrl, useCtrlNoise, useGamepad, useGravityCompensation, useIk, useJointState, useKeyboardTeleop, useMujoco, useMujocoSim, usePolicy, useSceneLights, useSelectionHighlight, useSensor, useSensors, useSitePosition, useTrajectoryPlayer, useTrajectoryRecorder, useVideoRecorder };
|
|
3517
3893
|
//# sourceMappingURL=index.js.map
|
|
3518
3894
|
//# sourceMappingURL=index.js.map
|