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

Add mju_mulVecMatVec, mutiplies a square matrix M by a vector x on both sides. Returns x^T * M * x.

Browse Source 
PiperOrigin-RevId: 474292806
Change-Id: I3432469dbe1f02ccf5a13241c7aa12d824cbe034
This commit is contained in:
Yuval Tassa
2022-09-14 08:04:40 -07:00
committed by Copybara-Service
parent ee9eccc992
commit f4e7fa97af
12 changed files with 109 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_solver.c
+2 -3
View File
@@ -190,7 +190,7 @@ static void residual(const mjModel* m, mjData* d, mjtNum* res, int i, int dim, i
// compute cost change
static mjtNum costChange(const mjtNum* A, mjtNum* force, const mjtNum* oldforce,
const mjtNum* res, int dim) {
mjtNum delta[6], v[6], change;
mjtNum delta[6], change;
// compute change
if (dim==1) {
@@ -198,8 +198,7 @@ static mjtNum costChange(const mjtNum* A, mjtNum* force, const mjtNum* oldforce,
change = 0.5*delta[0]*delta[0]*A[0] + delta[0]*res[0];
} else {
mju_sub(delta, force, oldforce, dim);
mju_mulMatVec(v, A, delta, dim, dim);
change = 0.5*mju_dot(delta, v, dim) + mju_dot(delta, res, dim);
change = 0.5*mju_mulVecMatVec(delta, A, delta, dim) + mju_dot(delta, res, dim);
}
// positive change: restore