chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge pull request #3276 from dparikh79:fix/3275-boxqp-symmetric-lower-matvec

Browse Source 
PiperOrigin-RevId: 917279467
Change-Id: If4117b1e2147679336becb558e0f87621555dfc7
This commit is contained in:
Copybara-Service
2026-05-18 09:36:01 -07:00
parent 71d1014e70 71072c025d
commit a1056676a0
2 changed files with 89 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_util_solve.c
+15 -2
View File
@@ -1407,6 +1407,19 @@ static mjtNum mulVecMatVecSym(const mjtNum* vec, const mjtNum* mat, int n) {
}
// multiply symmetric matrix with vector: res = mat*vec
// assumes symmetry of mat, ignores upper triangle, res must not alias vec
static void mulSymVec(mjtNum* restrict res, const mjtNum* mat, const mjtNum* vec, int n) {
for (int i=0; i < n; i++) {
// diagonal + strict lower triangle: res[i] = sum_{j<=i} mat[i,j] * vec[j]
res[i] = mat[n*i+i]*vec[i] + mju_dot(mat+n*i, vec, i);
// strict upper mirror contribution: res[k] += mat[i,k] * vec[i] for k < i
mju_addToScl(res, mat+n*i, vec[i], i);
}
}
// minimize 0.5*x'*H*x + x'*g s.t. lower <= x <=upper, explicit options
// additional arguments to mju_boxQP (see mju_boxQP documentation):
// maxiter maximum number of iterations
@@ -1512,7 +1525,7 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index, // outputs
oldvalue = value;
// compute gradient
mju_mulMatVec(grad, H, res, n, n);
mulSymVec(grad, H, res, n);
mju_addTo(grad, g, n);
// find clamped dimensions
@@ -1555,7 +1568,7 @@ int mju_boxQPoption(mjtNum* res, mjtNum* R, int* index, // outputs
for (int i=0; i < n; i++) {
temp[i] = clamped[i] ? res[i] : 0;
}
mju_mulMatVec(search, H, temp, n, n);
mulSymVec(search, H, temp, n);
mju_addTo(search, g, n);
// search = compress_free(search)