mujoco-react 10.8.0 → 11.0.0
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- package/README.md +15 -0
- package/dist/{chunk-5WFYM73E.js → chunk-UCQ5MJP4.js} +2 -2
- package/dist/chunk-UCQ5MJP4.js.map +1 -0
- package/dist/index.d.ts +23 -23
- package/dist/index.js +566 -415
- package/dist/index.js.map +1 -1
- package/dist/onnx.d.ts +1 -1
- package/dist/spark.d.ts +1 -1
- package/dist/spark.js +1 -1
- package/dist/{types-Bu8Aki_j.d.ts → types-BP8iiYei.d.ts} +48 -5
- package/package.json +6 -5
- package/src/components/FlexRenderer.tsx +13 -14
- package/src/core/GenericIK.ts +251 -214
- package/src/core/MujocoCanvas.tsx +2 -0
- package/src/core/MujocoPhysics.tsx +3 -0
- package/src/core/MujocoProvider.tsx +23 -9
- package/src/core/MujocoSimProvider.tsx +31 -34
- package/src/core/SceneLoader.ts +63 -30
- package/src/core/engineState.ts +58 -0
- package/src/hooks/useActuators.ts +3 -13
- package/src/hooks/useControlGroup.ts +2 -2
- package/src/hooks/useCtrl.ts +7 -6
- package/src/hooks/useGamepad.ts +3 -3
- package/src/hooks/useKeyboardTeleop.ts +2 -2
- package/src/index.ts +2 -0
- package/src/rendering/flexTopology.ts +24 -0
- package/src/types.ts +42 -4
- package/dist/chunk-5WFYM73E.js.map +0 -1
package/src/core/GenericIK.ts
CHANGED
|
@@ -10,9 +10,12 @@ export interface GenericIKOptions {
|
|
|
10
10
|
maxIterations: number;
|
|
11
11
|
damping: number;
|
|
12
12
|
tolerance: number;
|
|
13
|
+
/** @deprecated Analytic Jacobians do not require perturbations. */
|
|
13
14
|
epsilon: number;
|
|
14
15
|
posWeight: number;
|
|
15
16
|
rotWeight: number;
|
|
17
|
+
/** Per-iteration |Δq| cap in radians; bounds near-singular DLS steps. */
|
|
18
|
+
maxStepRad: number;
|
|
16
19
|
jointLimits?: ReadonlyArray<readonly [number, number] | null | undefined>;
|
|
17
20
|
}
|
|
18
21
|
|
|
@@ -26,6 +29,7 @@ const DEFAULTS: Required<Omit<GenericIKOptions, 'jointLimits'>> = {
|
|
|
26
29
|
epsilon: 1e-6,
|
|
27
30
|
posWeight: 1.0,
|
|
28
31
|
rotWeight: 0.3,
|
|
32
|
+
maxStepRad: 0.5,
|
|
29
33
|
};
|
|
30
34
|
|
|
31
35
|
function resolveOptions(opts?: Partial<GenericIKOptions>): ResolvedGenericIKOptions {
|
|
@@ -36,193 +40,218 @@ function resolveOptions(opts?: Partial<GenericIKOptions>): ResolvedGenericIKOpti
|
|
|
36
40
|
epsilon: opts?.epsilon ?? DEFAULTS.epsilon,
|
|
37
41
|
posWeight: opts?.posWeight ?? DEFAULTS.posWeight,
|
|
38
42
|
rotWeight: opts?.rotWeight ?? DEFAULTS.rotWeight,
|
|
43
|
+
maxStepRad: opts?.maxStepRad ?? DEFAULTS.maxStepRad,
|
|
39
44
|
jointLimits: opts?.jointLimits,
|
|
40
45
|
};
|
|
41
46
|
}
|
|
42
47
|
|
|
43
|
-
/**
|
|
44
|
-
* Generic Damped Least-Squares IK solver.
|
|
45
|
-
* Uses finite-difference Jacobian via MuJoCo's mj_forward.
|
|
46
|
-
* Works for any MuJoCo model — no robot-specific parameters.
|
|
47
|
-
*/
|
|
48
|
+
/** Analytic damped least-squares IK in MuJoCo velocity coordinates. */
|
|
48
49
|
export class GenericIK {
|
|
49
|
-
private mujoco: MujocoModule
|
|
50
|
-
|
|
51
|
-
constructor(mujoco: MujocoModule) {
|
|
52
|
-
this.mujoco = mujoco;
|
|
53
|
-
}
|
|
50
|
+
constructor(private mujoco: MujocoModule) {}
|
|
54
51
|
|
|
55
|
-
/**
|
|
56
|
-
* Solve IK for a target 6-DOF pose.
|
|
57
|
-
* @param model MuJoCo model
|
|
58
|
-
* @param data MuJoCo data (qpos will be temporarily modified, then restored)
|
|
59
|
-
* @param siteId Index of the end-effector site to control
|
|
60
|
-
* @param qposAdr qpos addresses for scalar joints in solve order
|
|
61
|
-
* @param targetPos Target position in world frame
|
|
62
|
-
* @param targetQuat Target orientation in world frame
|
|
63
|
-
* @param currentQ Current joint angles matching qposAdr order
|
|
64
|
-
* @param opts Optional solver parameters
|
|
65
|
-
* @returns Joint angles array, or null if solver diverged
|
|
66
|
-
*/
|
|
52
|
+
/** Scalar hinge/slide interface. Returns values in qposAdr order. */
|
|
67
53
|
solve(
|
|
68
54
|
model: MujocoModel,
|
|
69
55
|
data: MujocoData,
|
|
70
56
|
siteId: number,
|
|
71
57
|
qposAdr: ArrayLike<number>,
|
|
72
|
-
targetPos: THREE.Vector3,
|
|
73
|
-
targetQuat: THREE.Quaternion,
|
|
58
|
+
targetPos: Pick<THREE.Vector3, 'x' | 'y' | 'z'>,
|
|
59
|
+
targetQuat: Pick<THREE.Quaternion, 'x' | 'y' | 'z' | 'w'>,
|
|
74
60
|
currentQ: ArrayLike<number>,
|
|
75
|
-
opts?: Partial<GenericIKOptions
|
|
61
|
+
opts?: Partial<GenericIKOptions>,
|
|
76
62
|
): number[] | null {
|
|
77
|
-
|
|
78
|
-
const
|
|
79
|
-
|
|
80
|
-
|
|
81
|
-
const savedQpos = new Float64Array(data.qpos.length);
|
|
82
|
-
savedQpos.set(data.qpos);
|
|
83
|
-
|
|
84
|
-
// Build target rotation matrix (3x3 row-major)
|
|
85
|
-
const R_target = quatToMat3(targetQuat);
|
|
86
|
-
|
|
87
|
-
// Working joint angles — start from current configuration
|
|
88
|
-
const q = new Float64Array(n);
|
|
89
|
-
for (let i = 0; i < n; i++) q[i] = currentQ[i];
|
|
90
|
-
|
|
91
|
-
// Pre-allocate work arrays
|
|
92
|
-
const J = new Float64Array(6 * n); // 6×n Jacobian (row-major)
|
|
93
|
-
const JJt = new Float64Array(36); // 6×6
|
|
94
|
-
const rhs = new Float64Array(6); // right-hand side
|
|
95
|
-
const x = new Float64Array(6); // solve result
|
|
96
|
-
const dq = new Float64Array(n); // joint update
|
|
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
|
-
|
|
102
|
-
let bestQ: number[] | null = null;
|
|
103
|
-
let bestErr = Infinity;
|
|
104
|
-
// Stop early once the error stops improving. The solver always returns the
|
|
105
|
-
// best configuration it has seen, so when it stalls or diverges (an
|
|
106
|
-
// unreachable target, e.g. a near-singular wrist orientation) further
|
|
107
|
-
// iterations can't beat bestQ — they only burn (1 + n) mj_forward calls
|
|
108
|
-
// each and feed jitter into ctrl. This is what bounds the cost of dragging
|
|
109
|
-
// the target into an unreachable region.
|
|
110
|
-
const patience = 4;
|
|
111
|
-
let noImprove = 0;
|
|
112
|
-
|
|
113
|
-
if (n === 0) return null;
|
|
114
|
-
|
|
115
|
-
for (let iter = 0; iter < o.maxIterations; iter++) {
|
|
116
|
-
// Set joints and run FK
|
|
117
|
-
for (let i = 0; i < n; i++) data.qpos[qposAdr[i]] = q[i];
|
|
118
|
-
this.mujoco.mj_forward(model, data);
|
|
119
|
-
|
|
120
|
-
// Read current site pose
|
|
121
|
-
const sp = data.site_xpos;
|
|
122
|
-
const sm = data.site_xmat;
|
|
123
|
-
const off3 = siteId * 3;
|
|
124
|
-
const off9 = siteId * 9;
|
|
125
|
-
for (let i = 0; i < 3; i++) baseSitePos[i] = sp[off3 + i];
|
|
126
|
-
for (let i = 0; i < 9; i++) baseSiteMat[i] = sm[off9 + i];
|
|
127
|
-
|
|
128
|
-
// Compute 6D error
|
|
129
|
-
const posErr0 = targetPos.x - baseSitePos[0];
|
|
130
|
-
const posErr1 = targetPos.y - baseSitePos[1];
|
|
131
|
-
const posErr2 = targetPos.z - baseSitePos[2];
|
|
132
|
-
const rotErr = orientationError(baseSiteMat, R_target);
|
|
133
|
-
|
|
134
|
-
const error = [
|
|
135
|
-
posErr0 * o.posWeight,
|
|
136
|
-
posErr1 * o.posWeight,
|
|
137
|
-
posErr2 * o.posWeight,
|
|
138
|
-
rotErr[0] * o.rotWeight,
|
|
139
|
-
rotErr[1] * o.rotWeight,
|
|
140
|
-
rotErr[2] * o.rotWeight,
|
|
141
|
-
];
|
|
142
|
-
|
|
143
|
-
const errNorm = Math.sqrt(
|
|
144
|
-
error[0] * error[0] + error[1] * error[1] + error[2] * error[2] +
|
|
145
|
-
error[3] * error[3] + error[4] * error[4] + error[5] * error[5]
|
|
63
|
+
if (currentQ.length !== qposAdr.length) throw new Error('IK currentQ must match qposAdr length');
|
|
64
|
+
const jointIds = Array.from(qposAdr, (adr) => {
|
|
65
|
+
const id = Array.from({ length: model.njnt }, (_, i) => i).find(
|
|
66
|
+
(i) => model.jnt_qposadr[i] === adr,
|
|
146
67
|
);
|
|
147
|
-
|
|
148
|
-
|
|
149
|
-
|
|
150
|
-
|
|
151
|
-
bestQ = Array.from(q);
|
|
152
|
-
noImprove = 0;
|
|
153
|
-
} else {
|
|
154
|
-
noImprove++;
|
|
68
|
+
if (id === undefined || model.jnt_type[id] < 2) {
|
|
69
|
+
throw new Error(
|
|
70
|
+
'Scalar IK requires hinge/slide qpos addresses; use solveJoints for ball/free joints',
|
|
71
|
+
);
|
|
155
72
|
}
|
|
73
|
+
return id;
|
|
74
|
+
});
|
|
75
|
+
const initial = new Float64Array(data.qpos);
|
|
76
|
+
for (let i = 0; i < qposAdr.length; i++) initial[qposAdr[i]] = currentQ[i];
|
|
77
|
+
const result = this.solveJoints(model, data, siteId, jointIds, targetPos, targetQuat, initial, opts);
|
|
78
|
+
return result ? Array.from(qposAdr, (adr) => result[adr]) : null;
|
|
79
|
+
}
|
|
156
80
|
|
|
157
|
-
|
|
158
|
-
|
|
159
|
-
|
|
160
|
-
|
|
161
|
-
|
|
162
|
-
|
|
163
|
-
|
|
164
|
-
|
|
165
|
-
|
|
166
|
-
|
|
167
|
-
|
|
81
|
+
/**
|
|
82
|
+
* Solve selected joint IDs, including ball/free joints. Initial and returned
|
|
83
|
+
* configurations have model.nq entries, with MuJoCo wxyz quaternions.
|
|
84
|
+
* Jacobian columns use nv/dof addresses; mj_integratePos updates quaternions.
|
|
85
|
+
* The caller's qpos is restored even if the solve throws.
|
|
86
|
+
*/
|
|
87
|
+
solveJoints(
|
|
88
|
+
model: MujocoModel,
|
|
89
|
+
data: MujocoData,
|
|
90
|
+
siteId: number,
|
|
91
|
+
jointIds: ArrayLike<number>,
|
|
92
|
+
targetPos: Pick<THREE.Vector3, 'x' | 'y' | 'z'>,
|
|
93
|
+
targetQuat: Pick<THREE.Quaternion, 'x' | 'y' | 'z' | 'w'>,
|
|
94
|
+
currentQpos: ArrayLike<number> = data.qpos,
|
|
95
|
+
opts?: Partial<GenericIKOptions>,
|
|
96
|
+
): number[] | null {
|
|
97
|
+
const o = resolveOptions(opts);
|
|
98
|
+
const targetRotation = new THREE.Quaternion(targetQuat.x, targetQuat.y, targetQuat.z, targetQuat.w);
|
|
99
|
+
if (!Number.isInteger(siteId) || siteId < 0 || siteId >= model.nsite)
|
|
100
|
+
throw new Error('Invalid IK site ID');
|
|
101
|
+
if (currentQpos.length !== model.nq || !Array.from(currentQpos).every(Number.isFinite))
|
|
102
|
+
throw new Error('IK configuration must contain nq finite values');
|
|
103
|
+
if (
|
|
104
|
+
![
|
|
105
|
+
targetPos.x,
|
|
106
|
+
targetPos.y,
|
|
107
|
+
targetPos.z,
|
|
108
|
+
targetQuat.x,
|
|
109
|
+
targetQuat.y,
|
|
110
|
+
targetQuat.z,
|
|
111
|
+
targetQuat.w,
|
|
112
|
+
].every(Number.isFinite) ||
|
|
113
|
+
targetRotation.lengthSq() === 0
|
|
114
|
+
)
|
|
115
|
+
throw new Error('IK target must be finite with a nonzero quaternion');
|
|
116
|
+
if (
|
|
117
|
+
!(o.damping > 0) ||
|
|
118
|
+
!(o.maxStepRad > 0) ||
|
|
119
|
+
!Number.isFinite(o.maxStepRad) ||
|
|
120
|
+
!Number.isInteger(o.maxIterations) ||
|
|
121
|
+
o.maxIterations < 1
|
|
122
|
+
)
|
|
123
|
+
throw new Error('Invalid IK solver options');
|
|
124
|
+
const ids = Array.from(jointIds);
|
|
125
|
+
if (
|
|
126
|
+
new Set(ids).size !== ids.length ||
|
|
127
|
+
ids.some((id) => !Number.isInteger(id) || id < 0 || id >= model.njnt)
|
|
128
|
+
)
|
|
129
|
+
throw new Error('IK joint IDs must be unique and valid');
|
|
130
|
+
const dofs = ids.flatMap((id) =>
|
|
131
|
+
Array.from(
|
|
132
|
+
{ length: model.jnt_type[id] === 0 ? 6 : model.jnt_type[id] === 1 ? 3 : 1 },
|
|
133
|
+
(_, i) => model.jnt_dofadr[id] + i,
|
|
134
|
+
),
|
|
135
|
+
);
|
|
136
|
+
const n = dofs.length;
|
|
137
|
+
if (!n) return null;
|
|
138
|
+
const savedQpos = new Float64Array(data.qpos);
|
|
139
|
+
const buffers: { delete(): void }[] = [];
|
|
140
|
+
try {
|
|
141
|
+
const alloc = (size: number) => {
|
|
142
|
+
const b = new this.mujoco.DoubleBuffer(size);
|
|
143
|
+
buffers.push(b);
|
|
144
|
+
return b;
|
|
145
|
+
};
|
|
146
|
+
const jacp = alloc(3 * model.nv),
|
|
147
|
+
jacr = alloc(3 * model.nv),
|
|
148
|
+
qbuffer = alloc(model.nq);
|
|
149
|
+
qbuffer.GetView().set(currentQpos);
|
|
150
|
+
const target = quatToMat3(targetRotation.normalize());
|
|
151
|
+
const J = new Float64Array(6 * n),
|
|
152
|
+
JJt = new Float64Array(36),
|
|
153
|
+
rhs = new Float64Array(6),
|
|
154
|
+
x = new Float64Array(6);
|
|
155
|
+
const velocity = new Array<number>(model.nv).fill(0);
|
|
156
|
+
let best: number[] | null = null,
|
|
157
|
+
bestErr = Infinity,
|
|
158
|
+
noImprove = 0;
|
|
159
|
+
const clamp = () => {
|
|
160
|
+
const q = qbuffer.GetView();
|
|
161
|
+
ids.forEach((id, i) => {
|
|
162
|
+
const adr = model.jnt_qposadr[id],
|
|
163
|
+
type = model.jnt_type[id];
|
|
164
|
+
if (type >= 2) {
|
|
165
|
+
const limits =
|
|
166
|
+
o.jointLimits?.[i] ??
|
|
167
|
+
(model.jnt_limited[id]
|
|
168
|
+
? [model.jnt_range[2 * id], model.jnt_range[2 * id + 1]]
|
|
169
|
+
: null);
|
|
170
|
+
if (limits)
|
|
171
|
+
q[adr] = Math.max(Math.min(...limits), Math.min(Math.max(...limits), q[adr]));
|
|
172
|
+
} else {
|
|
173
|
+
const offset = adr + (type === 0 ? 3 : 0);
|
|
174
|
+
const quat = new THREE.Quaternion(
|
|
175
|
+
q[offset + 1],
|
|
176
|
+
q[offset + 2],
|
|
177
|
+
q[offset + 3],
|
|
178
|
+
q[offset],
|
|
179
|
+
).normalize();
|
|
180
|
+
if (type === 1 && model.jnt_limited[id]) {
|
|
181
|
+
if (quat.w < 0) {
|
|
182
|
+
quat.x *= -1;
|
|
183
|
+
quat.y *= -1;
|
|
184
|
+
quat.z *= -1;
|
|
185
|
+
quat.w *= -1;
|
|
186
|
+
}
|
|
187
|
+
const angle = 2 * Math.acos(Math.min(1, Math.max(-1, quat.w)));
|
|
188
|
+
const limit = model.jnt_range[2 * id + 1];
|
|
189
|
+
if (angle > limit)
|
|
190
|
+
quat.identity().slerp(
|
|
191
|
+
new THREE.Quaternion(
|
|
192
|
+
q[offset + 1],
|
|
193
|
+
q[offset + 2],
|
|
194
|
+
q[offset + 3],
|
|
195
|
+
q[offset],
|
|
196
|
+
).normalize(),
|
|
197
|
+
limit / angle,
|
|
198
|
+
);
|
|
199
|
+
}
|
|
200
|
+
q.set([quat.w, quat.x, quat.y, quat.z], offset);
|
|
201
|
+
}
|
|
202
|
+
});
|
|
203
|
+
};
|
|
204
|
+
clamp();
|
|
205
|
+
for (let iter = 0; iter < o.maxIterations; iter++) {
|
|
206
|
+
data.qpos.set(qbuffer.GetView());
|
|
168
207
|
this.mujoco.mj_forward(model, data);
|
|
169
|
-
|
|
170
|
-
|
|
171
|
-
|
|
172
|
-
|
|
173
|
-
|
|
174
|
-
|
|
175
|
-
|
|
176
|
-
|
|
177
|
-
|
|
178
|
-
|
|
179
|
-
|
|
180
|
-
|
|
181
|
-
|
|
182
|
-
|
|
183
|
-
|
|
184
|
-
|
|
185
|
-
|
|
186
|
-
|
|
187
|
-
|
|
188
|
-
|
|
189
|
-
|
|
190
|
-
|
|
191
|
-
|
|
192
|
-
// Damped least squares: Δq = Jᵀ (J Jᵀ + λI)⁻¹ error
|
|
193
|
-
// 1. Compute JJᵀ (6×6)
|
|
194
|
-
for (let r = 0; r < 6; r++) {
|
|
195
|
-
for (let c = 0; c < 6; c++) {
|
|
196
|
-
let sum = 0;
|
|
197
|
-
for (let k = 0; k < n; k++) {
|
|
198
|
-
sum += J[r * n + k] * J[c * n + k];
|
|
208
|
+
const pos = data.site_xpos.subarray(3 * siteId, 3 * siteId + 3);
|
|
209
|
+
const rot = orientationError(data.site_xmat.subarray(9 * siteId, 9 * siteId + 9), target);
|
|
210
|
+
const error = [
|
|
211
|
+
(targetPos.x - pos[0]) * o.posWeight,
|
|
212
|
+
(targetPos.y - pos[1]) * o.posWeight,
|
|
213
|
+
(targetPos.z - pos[2]) * o.posWeight,
|
|
214
|
+
...rot.map((v) => v * o.rotWeight),
|
|
215
|
+
];
|
|
216
|
+
const norm = Math.hypot(...error);
|
|
217
|
+
if (!Number.isFinite(norm)) break;
|
|
218
|
+
if (norm < bestErr - 1e-9) {
|
|
219
|
+
bestErr = norm;
|
|
220
|
+
best = Array.from(qbuffer.GetView());
|
|
221
|
+
noImprove = 0;
|
|
222
|
+
} else noImprove++;
|
|
223
|
+
if (norm < o.tolerance || noImprove >= 4) break;
|
|
224
|
+
this.mujoco.mj_jacSite(model, data, jacp, jacr, siteId);
|
|
225
|
+
const jp = jacp.GetView(),
|
|
226
|
+
jr = jacr.GetView();
|
|
227
|
+
for (let r = 0; r < 3; r++)
|
|
228
|
+
for (let j = 0; j < n; j++) {
|
|
229
|
+
J[r * n + j] = jp[r * model.nv + dofs[j]] * o.posWeight;
|
|
230
|
+
J[(r + 3) * n + j] = jr[r * model.nv + dofs[j]] * o.rotWeight;
|
|
199
231
|
}
|
|
200
|
-
|
|
201
|
-
|
|
202
|
-
|
|
203
|
-
|
|
204
|
-
|
|
205
|
-
|
|
206
|
-
|
|
207
|
-
|
|
208
|
-
|
|
209
|
-
|
|
210
|
-
|
|
211
|
-
|
|
212
|
-
|
|
232
|
+
for (let r = 0; r < 6; r++)
|
|
233
|
+
for (let c = 0; c < 6; c++) {
|
|
234
|
+
let sum = 0;
|
|
235
|
+
for (let k = 0; k < n; k++) sum += J[r * n + k] * J[c * n + k];
|
|
236
|
+
JJt[r * 6 + c] = sum + (r === c ? o.damping : 0);
|
|
237
|
+
}
|
|
238
|
+
rhs.set(error);
|
|
239
|
+
solve6x6(JJt, rhs, x);
|
|
240
|
+
velocity.fill(0);
|
|
241
|
+
for (let j = 0; j < n; j++) {
|
|
242
|
+
let step = 0;
|
|
243
|
+
for (let r = 0; r < 6; r++) step += J[r * n + j] * x[r];
|
|
244
|
+
velocity[dofs[j]] = Math.max(-o.maxStepRad, Math.min(o.maxStepRad, step));
|
|
213
245
|
}
|
|
214
|
-
|
|
246
|
+
this.mujoco.mj_integratePos(model, qbuffer, velocity, 1);
|
|
247
|
+
clamp();
|
|
215
248
|
}
|
|
216
|
-
|
|
217
|
-
|
|
218
|
-
for (
|
|
249
|
+
return best;
|
|
250
|
+
} finally {
|
|
251
|
+
for (const buffer of buffers) buffer.delete();
|
|
252
|
+
data.qpos.set(savedQpos);
|
|
253
|
+
this.mujoco.mj_forward(model, data);
|
|
219
254
|
}
|
|
220
|
-
|
|
221
|
-
// Restore original qpos
|
|
222
|
-
data.qpos.set(savedQpos);
|
|
223
|
-
this.mujoco.mj_forward(model, data);
|
|
224
|
-
|
|
225
|
-
return bestQ;
|
|
226
255
|
}
|
|
227
256
|
}
|
|
228
257
|
|
|
@@ -231,13 +260,28 @@ export class GenericIK {
|
|
|
231
260
|
/** Convert THREE.Quaternion to 3x3 rotation matrix (row-major Float64Array) */
|
|
232
261
|
function quatToMat3(q: THREE.Quaternion): Float64Array {
|
|
233
262
|
const m = new Float64Array(9);
|
|
234
|
-
const x = q.x,
|
|
235
|
-
|
|
236
|
-
|
|
237
|
-
|
|
238
|
-
|
|
239
|
-
|
|
240
|
-
|
|
263
|
+
const x = q.x,
|
|
264
|
+
y = q.y,
|
|
265
|
+
z = q.z,
|
|
266
|
+
w = q.w;
|
|
267
|
+
const xx = x * x,
|
|
268
|
+
yy = y * y,
|
|
269
|
+
zz = z * z;
|
|
270
|
+
const xy = x * y,
|
|
271
|
+
xz = x * z,
|
|
272
|
+
yz = y * z;
|
|
273
|
+
const wx = w * x,
|
|
274
|
+
wy = w * y,
|
|
275
|
+
wz = w * z;
|
|
276
|
+
m[0] = 1 - 2 * (yy + zz);
|
|
277
|
+
m[1] = 2 * (xy - wz);
|
|
278
|
+
m[2] = 2 * (xz + wy);
|
|
279
|
+
m[3] = 2 * (xy + wz);
|
|
280
|
+
m[4] = 1 - 2 * (xx + zz);
|
|
281
|
+
m[5] = 2 * (yz - wx);
|
|
282
|
+
m[6] = 2 * (xz - wy);
|
|
283
|
+
m[7] = 2 * (yz + wx);
|
|
284
|
+
m[8] = 1 - 2 * (xx + yy);
|
|
241
285
|
return m;
|
|
242
286
|
}
|
|
243
287
|
|
|
@@ -272,48 +316,34 @@ function orientationError(R_cur: Float64Array, R_tgt: Float64Array): [number, nu
|
|
|
272
316
|
return [0, 0, 0];
|
|
273
317
|
}
|
|
274
318
|
|
|
275
|
-
//
|
|
276
|
-
if (angle > Math.PI - 1e-
|
|
277
|
-
|
|
278
|
-
|
|
279
|
-
|
|
280
|
-
|
|
281
|
-
|
|
319
|
+
// Quaternion extraction is well-conditioned at pi, where the skew vanishes.
|
|
320
|
+
if (angle > Math.PI - 1e-4) {
|
|
321
|
+
const matrix = new THREE.Matrix4().set(
|
|
322
|
+
Re[0],
|
|
323
|
+
Re[1],
|
|
324
|
+
Re[2],
|
|
325
|
+
0,
|
|
326
|
+
Re[3],
|
|
327
|
+
Re[4],
|
|
328
|
+
Re[5],
|
|
329
|
+
0,
|
|
330
|
+
Re[6],
|
|
331
|
+
Re[7],
|
|
332
|
+
Re[8],
|
|
333
|
+
0,
|
|
334
|
+
0,
|
|
335
|
+
0,
|
|
336
|
+
0,
|
|
337
|
+
1,
|
|
338
|
+
);
|
|
339
|
+
const q = new THREE.Quaternion().setFromRotationMatrix(matrix).normalize();
|
|
340
|
+
const scale = (angle / Math.hypot(q.x, q.y, q.z)) * (q.w < 0 ? -1 : 1);
|
|
341
|
+
return [q.x * scale, q.y * scale, q.z * scale];
|
|
282
342
|
}
|
|
283
343
|
|
|
284
344
|
// General case: axis = skew(R_err) / (2 sin(angle)), scaled by angle
|
|
285
345
|
const s = angle / (2 * Math.sin(angle));
|
|
286
|
-
return [
|
|
287
|
-
s * (Re[7] - Re[5]),
|
|
288
|
-
s * (Re[2] - Re[6]),
|
|
289
|
-
s * (Re[3] - Re[1]),
|
|
290
|
-
];
|
|
291
|
-
}
|
|
292
|
-
|
|
293
|
-
/**
|
|
294
|
-
* Compute angular velocity vector from R_base to R_perturbed.
|
|
295
|
-
* Returns the axis-angle of R_perturbed * R_base^T.
|
|
296
|
-
* (Small angle: the rotation caused by the perturbation.)
|
|
297
|
-
*/
|
|
298
|
-
function angularDelta(R_base: Float64Array, R_pert: Float64Array): [number, number, number] {
|
|
299
|
-
// δR = R_pert * R_base^T
|
|
300
|
-
// Small angle approx: ω ≈ 0.5 * [δR[7]-δR[5], δR[2]-δR[6], δR[3]-δR[1]]
|
|
301
|
-
// This is fine because the perturbation epsilon is tiny.
|
|
302
|
-
const dR = new Float64Array(9);
|
|
303
|
-
for (let i = 0; i < 3; i++) {
|
|
304
|
-
for (let j = 0; j < 3; j++) {
|
|
305
|
-
let s = 0;
|
|
306
|
-
for (let k = 0; k < 3; k++) {
|
|
307
|
-
s += R_pert[i * 3 + k] * R_base[j * 3 + k];
|
|
308
|
-
}
|
|
309
|
-
dR[i * 3 + j] = s;
|
|
310
|
-
}
|
|
311
|
-
}
|
|
312
|
-
return [
|
|
313
|
-
0.5 * (dR[7] - dR[5]),
|
|
314
|
-
0.5 * (dR[2] - dR[6]),
|
|
315
|
-
0.5 * (dR[3] - dR[1]),
|
|
316
|
-
];
|
|
346
|
+
return [s * (Re[7] - Re[5]), s * (Re[2] - Re[6]), s * (Re[3] - Re[1])];
|
|
317
347
|
}
|
|
318
348
|
|
|
319
349
|
/**
|
|
@@ -333,15 +363,22 @@ function solve6x6(A: Float64Array, b: Float64Array, x: Float64Array): void {
|
|
|
333
363
|
let maxRow = col;
|
|
334
364
|
for (let row = col + 1; row < N; row++) {
|
|
335
365
|
const val = Math.abs(a[row * N + col]);
|
|
336
|
-
if (val > maxVal) {
|
|
366
|
+
if (val > maxVal) {
|
|
367
|
+
maxVal = val;
|
|
368
|
+
maxRow = row;
|
|
369
|
+
}
|
|
337
370
|
}
|
|
338
371
|
|
|
339
372
|
// Swap rows
|
|
340
373
|
if (maxRow !== col) {
|
|
341
374
|
for (let k = 0; k < N; k++) {
|
|
342
|
-
const tmp = a[col * N + k];
|
|
375
|
+
const tmp = a[col * N + k];
|
|
376
|
+
a[col * N + k] = a[maxRow * N + k];
|
|
377
|
+
a[maxRow * N + k] = tmp;
|
|
343
378
|
}
|
|
344
|
-
const tmp = r[col];
|
|
379
|
+
const tmp = r[col];
|
|
380
|
+
r[col] = r[maxRow];
|
|
381
|
+
r[maxRow] = tmp;
|
|
345
382
|
}
|
|
346
383
|
|
|
347
384
|
const pivot = a[col * N + col];
|
|
@@ -27,6 +27,7 @@ export const MujocoCanvas = forwardRef<MujocoSimAPI, MujocoCanvasProps>(
|
|
|
27
27
|
// Declarative physics config
|
|
28
28
|
gravity,
|
|
29
29
|
timestep,
|
|
30
|
+
integrator,
|
|
30
31
|
substeps,
|
|
31
32
|
paused,
|
|
32
33
|
speed,
|
|
@@ -71,6 +72,7 @@ export const MujocoCanvas = forwardRef<MujocoSimAPI, MujocoCanvasProps>(
|
|
|
71
72
|
onSelection={onSelection}
|
|
72
73
|
gravity={gravity}
|
|
73
74
|
timestep={timestep}
|
|
75
|
+
integrator={integrator}
|
|
74
76
|
substeps={substeps}
|
|
75
77
|
paused={paused}
|
|
76
78
|
speed={speed}
|
|
@@ -8,6 +8,7 @@ import { useMujocoWasm } from './MujocoProvider';
|
|
|
8
8
|
import { MujocoSimProvider } from './MujocoSimProvider';
|
|
9
9
|
import type {
|
|
10
10
|
MujocoSimAPI,
|
|
11
|
+
MujocoIntegrator,
|
|
11
12
|
ReadyCallbackInput,
|
|
12
13
|
SceneConfig,
|
|
13
14
|
SelectionCallbackInput,
|
|
@@ -29,6 +30,8 @@ export interface MujocoPhysicsProps {
|
|
|
29
30
|
gravity?: [number, number, number];
|
|
30
31
|
/** Override model.opt.timestep. */
|
|
31
32
|
timestep?: number;
|
|
33
|
+
/** Override the integrator; omit to preserve the model setting. */
|
|
34
|
+
integrator?: MujocoIntegrator;
|
|
32
35
|
/** mj_step calls per frame. */
|
|
33
36
|
substeps?: number;
|
|
34
37
|
/** Declarative pause. */
|
|
@@ -3,6 +3,8 @@
|
|
|
3
3
|
* SPDX-License-Identifier: Apache-2.0
|
|
4
4
|
*/
|
|
5
5
|
|
|
6
|
+
import { configureModuleThreads } from './engineState';
|
|
7
|
+
|
|
6
8
|
import loadMujoco from '@mujoco/mujoco';
|
|
7
9
|
import defaultMujocoWasmUrl from '@mujoco/mujoco/mujoco.wasm?url';
|
|
8
10
|
import { createContext, useContext, useEffect, useRef, useState } from 'react';
|
|
@@ -46,6 +48,8 @@ export interface MujocoProviderProps {
|
|
|
46
48
|
* uses threaded only when both conditions are satisfied.
|
|
47
49
|
*/
|
|
48
50
|
wasmVariant?: MujocoWasmVariant;
|
|
51
|
+
/** Physics worker count. Requires the threaded build; defaults to 0 (no pool). */
|
|
52
|
+
threadCount?: number;
|
|
49
53
|
/** Timeout in ms for WASM module load. Default: 30000. */
|
|
50
54
|
timeout?: number;
|
|
51
55
|
children: React.ReactNode;
|
|
@@ -88,6 +92,7 @@ export function MujocoProvider({
|
|
|
88
92
|
mtWasmUrl,
|
|
89
93
|
threadedLoader,
|
|
90
94
|
wasmVariant = 'single',
|
|
95
|
+
threadCount = 0,
|
|
91
96
|
timeout = 30000,
|
|
92
97
|
children,
|
|
93
98
|
onError,
|
|
@@ -95,14 +100,20 @@ export function MujocoProvider({
|
|
|
95
100
|
const [status, setStatus] = useState<'loading' | 'ready' | 'error'>('loading');
|
|
96
101
|
const [error, setError] = useState<string | null>(null);
|
|
97
102
|
const moduleRef = useRef<MujocoModule | null>(null);
|
|
98
|
-
const isMounted = useRef(true);
|
|
99
103
|
const onErrorRef = useRef(onError);
|
|
100
104
|
onErrorRef.current = onError;
|
|
101
105
|
|
|
102
106
|
useEffect(() => {
|
|
103
|
-
|
|
107
|
+
let disposed = false;
|
|
108
|
+
moduleRef.current = null;
|
|
109
|
+
setStatus('loading');
|
|
110
|
+
setError(null);
|
|
104
111
|
|
|
105
112
|
const variant = resolveWasmVariant(wasmVariant, threadedLoader, mtWasmUrl);
|
|
113
|
+
if (!Number.isInteger(threadCount) || threadCount < 0 || (threadCount > 0 && variant !== 'threaded') || (variant === 'threaded' && !canUseThreadedWasm())) {
|
|
114
|
+
const err = new Error('threadCount must be a nonnegative integer; threaded physics requires the MT loader and cross-origin isolation');
|
|
115
|
+
setError(err.message); setStatus('error'); onErrorRef.current?.(err); return;
|
|
116
|
+
}
|
|
106
117
|
if (variant === 'threaded' && !threadedLoader) {
|
|
107
118
|
const err = new Error('MujocoProvider wasmVariant="threaded" requires a threadedLoader from @mujoco/mujoco/mt');
|
|
108
119
|
setError(err.message);
|
|
@@ -128,40 +139,43 @@ export function MujocoProvider({
|
|
|
128
139
|
const wasmPromise = load({
|
|
129
140
|
locateFile: (path: string) => path.endsWith('.wasm') ? selectedWasmUrl : path,
|
|
130
141
|
printErr: (text: string) => {
|
|
131
|
-
if (text.includes('Aborted') &&
|
|
142
|
+
if (text.includes('Aborted') && !disposed) {
|
|
132
143
|
setError('Simulation crashed. Reload page.');
|
|
133
144
|
setStatus('error');
|
|
134
145
|
}
|
|
135
146
|
},
|
|
136
147
|
});
|
|
137
148
|
|
|
149
|
+
let timeoutId: ReturnType<typeof setTimeout>;
|
|
138
150
|
const timeoutPromise = new Promise<never>((_, reject) =>
|
|
139
|
-
setTimeout(() => reject(new Error(`WASM module load timed out after ${timeout}ms`)), timeout)
|
|
151
|
+
timeoutId = setTimeout(() => reject(new Error(`WASM module load timed out after ${timeout}ms`)), timeout)
|
|
140
152
|
);
|
|
141
153
|
|
|
142
154
|
Promise.race([wasmPromise, timeoutPromise])
|
|
143
155
|
.then((inst: unknown) => {
|
|
144
|
-
if (
|
|
156
|
+
if (!disposed) {
|
|
145
157
|
if (!isMujocoModule(inst)) {
|
|
146
158
|
throw new Error('MuJoCo WASM module initialized with an unexpected shape');
|
|
147
159
|
}
|
|
160
|
+
configureModuleThreads(inst, threadCount);
|
|
148
161
|
moduleRef.current = inst;
|
|
149
162
|
setStatus('ready');
|
|
150
163
|
}
|
|
151
164
|
})
|
|
152
165
|
.catch((err: Error) => {
|
|
153
|
-
if (
|
|
166
|
+
if (!disposed) {
|
|
154
167
|
const msg = err.message || 'Failed to init spatial simulation';
|
|
155
168
|
setError(msg);
|
|
156
169
|
setStatus('error');
|
|
157
170
|
onErrorRef.current?.(new Error(msg));
|
|
158
171
|
}
|
|
159
|
-
});
|
|
172
|
+
}).finally(() => clearTimeout(timeoutId));
|
|
160
173
|
|
|
161
174
|
return () => {
|
|
162
|
-
|
|
175
|
+
disposed = true;
|
|
176
|
+
clearTimeout(timeoutId);
|
|
163
177
|
};
|
|
164
|
-
}, [wasmUrl, mtWasmUrl, threadedLoader, wasmVariant, timeout]);
|
|
178
|
+
}, [wasmUrl, mtWasmUrl, threadedLoader, wasmVariant, threadCount, timeout]);
|
|
165
179
|
|
|
166
180
|
return (
|
|
167
181
|
<MujocoContext.Provider
|