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

Delete sparsity structures from mjData, use the ones in mjModel

Browse Source 
PiperOrigin-RevId: 795067950
Change-Id: If4f52e40ac49f7c6986b33ef9c8b534199dfd21a
This commit is contained in:
Yuval Tassa
2025-08-14 09:11:47 -07:00
committed by Copybara-Service
parent 7eb13777f8
commit 977f94e9df
25 changed files with 120 additions and 349 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/includes/references.h
-15
View File
@@ -309,21 +309,6 @@ struct mjData_ {
mjtNum* qH; // L'*D*L factorization of modified M (nC x 1)
mjtNum* qHDiagInv; // 1/diag(D) of modified M (nv x 1)
// computed by mj_resetData
int* B_rownnz; // body-dof: non-zeros in each row (nbody x 1)
int* B_rowadr; // body-dof: address of each row in B_colind (nbody x 1)
int* B_colind; // body-dof: column indices of non-zeros (nB x 1)
int* M_rownnz; // reduced inertia: non-zeros in each row (nv x 1)
int* M_rowadr; // reduced inertia: address of each row in M_colind (nv x 1)
int* M_colind; // reduced inertia: column indices of non-zeros (nC x 1)
int* mapM2M; // index mapping from qM to M (nC x 1)
int* D_rownnz; // full inertia: non-zeros in each row (nv x 1)
int* D_rowadr; // full inertia: address of each row in D_colind (nv x 1)
int* D_diag; // full inertia: index of diagonal element (nv x 1)
int* D_colind; // full inertia: column indices of non-zeros (nD x 1)
int* mapM2D; // index mapping from qM to D (nD x 1)
int* mapD2M; // index mapping from D to qM (nM x 1)
// computed by mj_implicit/mj_derivative
mjtNum* qDeriv; // d (passive + actuator - bias) / d qvel (nD x 1)
include/mujoco/mjdata.h
-15
View File
@@ -337,21 +337,6 @@ struct mjData_ {
mjtNum* qH; // L'*D*L factorization of modified M (nC x 1)
mjtNum* qHDiagInv; // 1/diag(D) of modified M (nv x 1)
// computed by mj_resetData
int* B_rownnz; // body-dof: non-zeros in each row (nbody x 1)
int* B_rowadr; // body-dof: address of each row in B_colind (nbody x 1)
int* B_colind; // body-dof: column indices of non-zeros (nB x 1)
int* M_rownnz; // reduced inertia: non-zeros in each row (nv x 1)
int* M_rowadr; // reduced inertia: address of each row in M_colind (nv x 1)
int* M_colind; // reduced inertia: column indices of non-zeros (nC x 1)
int* mapM2M; // index mapping from qM to M (nC x 1)
int* D_rownnz; // full inertia: non-zeros in each row (nv x 1)
int* D_rowadr; // full inertia: address of each row in D_colind (nv x 1)
int* D_diag; // full inertia: index of diagonal element (nv x 1)
int* D_colind; // full inertia: column indices of non-zeros (nD x 1)
int* mapM2D; // index mapping from qM to D (nD x 1)
int* mapD2M; // index mapping from D to qM (nM x 1)
// computed by mj_implicit/mj_derivative
mjtNum* qDeriv; // d (passive + actuator - bias) / d qvel (nD x 1)
include/mujoco/mjxmacro.h
-13
View File
@@ -700,19 +700,6 @@
X ( mjtNum, subtree_angmom, nbody, 3 ) \
XNV ( mjtNum, qH, nC, 1 ) \
X ( mjtNum, qHDiagInv, nv, 1 ) \
XNV ( int, B_rownnz, nbody, 1 ) \
XNV ( int, B_rowadr, nbody, 1 ) \
XNV ( int, B_colind, nB, 1 ) \
XNV ( int, M_rownnz, nv, 1 ) \
XNV ( int, M_rowadr, nv, 1 ) \
XNV ( int, M_colind, nC, 1 ) \
XNV ( int, mapM2M, nC, 1 ) \
XNV ( int, D_rownnz, nv, 1 ) \
XNV ( int, D_rowadr, nv, 1 ) \
XNV ( int, D_diag, nv, 1 ) \
XNV ( int, D_colind, nD, 1 ) \
XNV ( int, mapM2D, nD, 1 ) \
XNV ( int, mapD2M, nM, 1 ) \
XNV ( mjtNum, qDeriv, nD, 1 ) \
XNV ( mjtNum, qLU, nD, 1 ) \
X ( mjtNum, actuator_force, nu, 1 ) \
mjx/mujoco/mjx/_src/io.py
+1 -14
View File
@@ -764,19 +764,6 @@ def _make_data_c(
'ten_velocity': (m.ntendon, float_),
'actuator_velocity': (m.nu, float_),
'plugin_data': (get(m, 'nplugin'), np.uint64),
'B_rownnz': (m.nbody, np.int32),
'B_rowadr': (m.nbody, np.int32),
'B_colind': (m.nB, np.int32),
'M_rownnz': (m.nv, np.int32),
'M_rowadr': (m.nv, np.int32),
'M_colind': (m.nC, np.int32),
'mapM2M': (m.nC, np.int32),
'D_rownnz': (m.nv, np.int32),
'D_rowadr': (m.nv, np.int32),
'D_diag': (m.nv, np.int32),
'D_colind': (m.nD, np.int32),
'mapM2D': (m.nD, np.int32),
'mapD2M': (m.nM, np.int32),
'qDeriv': (m.nD, float_),
'qLU': (m.nD, float_),
'qfrc_spring': (m.nv, float_),
@@ -1497,7 +1484,7 @@ def _get_data_into(
# TODO(taylorhowell): remove mapping once qM is deprecated
# map inertia (sparse) to reduced inertia (compressed sparse) representation
result_i.M[:] = result_i.qM[result_i.mapM2M]
result_i.M[:] = result_i.qM[m.mapM2M]
# recalculate qLD and qLDiagInv as MJX and MuJoCo have different
# representations of the Cholesky decomposition.
mjx/mujoco/mjx/_src/smooth_test.py
+1 -1
View File
@@ -94,7 +94,7 @@ class SmoothTest(absltest.TestCase):
dx = jax.jit(mjx.factor_m)(mx, mjx.put_data(m, d))
qLDLegacy = np.zeros(mx.nM) # pylint:disable=invalid-name
for i in range(m.nC):
qLDLegacy[d.mapM2M[i]] = d.qLD[i]
qLDLegacy[m.mapM2M[i]] = d.qLD[i]
_assert_eq(qLDLegacy, dx._impl.qLD, 'qLD')
_assert_attr_eq(d, dx._impl, 'qLDiagInv')
# com_vel
mjx/mujoco/mjx/_src/types.py
+13 -13
View File
@@ -599,6 +599,19 @@ class ModelC(PyTreeNode):
sensor_plugin: jax.Array
plugin: jax.Array
plugin_stateadr: jax.Array
B_rownnz: jax.Array # pylint:disable=invalid-name
B_rowadr: jax.Array # pylint:disable=invalid-name
B_colind: jax.Array # pylint:disable=invalid-name
M_rownnz: jax.Array # pylint:disable=invalid-name
M_rowadr: jax.Array # pylint:disable=invalid-name
M_colind: jax.Array # pylint:disable=invalid-name
mapM2M: jax.Array # pylint:disable=invalid-name
D_rownnz: jax.Array # pylint:disable=invalid-name
D_rowadr: jax.Array # pylint:disable=invalid-name
D_diag: jax.Array # pylint:disable=invalid-name
D_colind: jax.Array # pylint:disable=invalid-name
mapM2D: jax.Array # pylint:disable=invalid-name
mapD2M: jax.Array # pylint:disable=invalid-name
class ModelJAX(PyTreeNode):
@@ -1020,19 +1033,6 @@ class DataC(PyTreeNode):
plugin_data: jax.Array
qH: jax.Array # pylint:disable=invalid-name
qHDiagInv: jax.Array # pylint:disable=invalid-name
B_rownnz: jax.Array # pylint:disable=invalid-name
B_rowadr: jax.Array # pylint:disable=invalid-name
B_colind: jax.Array # pylint:disable=invalid-name
M_rownnz: jax.Array # pylint:disable=invalid-name
M_rowadr: jax.Array # pylint:disable=invalid-name
M_colind: jax.Array # pylint:disable=invalid-name
mapM2M: jax.Array # pylint:disable=invalid-name
D_rownnz: jax.Array # pylint:disable=invalid-name
D_rowadr: jax.Array # pylint:disable=invalid-name
D_diag: jax.Array # pylint:disable=invalid-name
D_colind: jax.Array # pylint:disable=invalid-name
mapM2D: jax.Array # pylint:disable=invalid-name
mapD2M: jax.Array # pylint:disable=invalid-name
qDeriv: jax.Array # pylint:disable=invalid-name
qLU: jax.Array # pylint:disable=invalid-name
qfrc_spring: jax.Array
mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py
Vendored
+6 -9
View File
@@ -142,9 +142,6 @@ def put_model(mjm: mujoco.MjModel) -> types.Model:
# calculate some fields that cannot be easily computed inline
nlsp = mjm.opt.ls_iterations # TODO(team): how to set nlsp?
# unfortunately we must create Data in order to get some model fields like M_rownnz
mjd = mujoco.MjData(mjm)
# dof lower triangle row and column indices (used in solver)
dof_tri_row, dof_tri_col = np.tril_indices(mjm.nv)
@@ -182,11 +179,11 @@ def put_model(mjm: mujoco.MjModel) -> types.Model:
for k in range(mjm.nv):
# skip diagonal rows
if mjd.M_rownnz[k] == 1:
if mjm.M_rownnz[k] == 1:
continue
dof_depth[k] = dof_depth[mjm.dof_parentid[k]] + 1
i = mjm.dof_parentid[k]
diag_k = mjd.M_rowadr[k] + mjd.M_rownnz[k] - 1
diag_k = mjm.M_rowadr[k] + mjm.M_rownnz[k] - 1
Madr_ki = diag_k - 1
while i > -1:
qLD_updates.setdefault(dof_depth[i], []).append((i, k, Madr_ki))
@@ -487,10 +484,10 @@ def put_model(mjm: mujoco.MjModel) -> types.Model:
qM_mulm_j=wp.array(qM_mulm_j, dtype=int),
qM_madr_ij=wp.array(qM_madr_ij, dtype=int),
qLD_updates=qLD_updates,
M_rownnz=wp.array(mjd.M_rownnz, dtype=int),
M_rowadr=wp.array(mjd.M_rowadr, dtype=int),
M_colind=wp.array(mjd.M_colind, dtype=int),
mapM2M=wp.array(mjd.mapM2M, dtype=int),
M_rownnz=wp.array(mjm.M_rownnz, dtype=int),
M_rowadr=wp.array(mjm.M_rowadr, dtype=int),
M_colind=wp.array(mjm.M_colind, dtype=int),
mapM2M=wp.array(mjm.mapM2M, dtype=int),
qM_tiles=qM_tiles,
body_tree=body_tree,
body_parentid=wp.array(mjm.body_parentid, dtype=int),
python/mujoco/introspect/structs.py
-104
View File
@@ -5770,110 +5770,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([
doc='1/diag(D) of modified M',
array_extent=('nv',),
),
StructFieldDecl(
name='B_rownnz',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='body-dof: non-zeros in each row',
array_extent=('nbody',),
),
StructFieldDecl(
name='B_rowadr',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='body-dof: address of each row in B_colind',
array_extent=('nbody',),
),
StructFieldDecl(
name='B_colind',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='body-dof: column indices of non-zeros',
array_extent=('nB',),
),
StructFieldDecl(
name='M_rownnz',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='reduced inertia: non-zeros in each row',
array_extent=('nv',),
),
StructFieldDecl(
name='M_rowadr',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='reduced inertia: address of each row in M_colind',
array_extent=('nv',),
),
StructFieldDecl(
name='M_colind',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='reduced inertia: column indices of non-zeros',
array_extent=('nC',),
),
StructFieldDecl(
name='mapM2M',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='index mapping from qM to M',
array_extent=('nC',),
),
StructFieldDecl(
name='D_rownnz',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='full inertia: non-zeros in each row',
array_extent=('nv',),
),
StructFieldDecl(
name='D_rowadr',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='full inertia: address of each row in D_colind',
array_extent=('nv',),
),
StructFieldDecl(
name='D_diag',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='full inertia: index of diagonal element',
array_extent=('nv',),
),
StructFieldDecl(
name='D_colind',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='full inertia: column indices of non-zeros',
array_extent=('nD',),
),
StructFieldDecl(
name='mapM2D',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='index mapping from qM to D',
array_extent=('nD',),
),
StructFieldDecl(
name='mapD2M',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='index mapping from D to qM',
array_extent=('nM',),
),
StructFieldDecl(
name='qDeriv',
type=PointerType(
src/engine/engine_core_constraint.c
+6 -6
View File
@@ -2037,7 +2037,7 @@ void mj_projectConstraint(const mjModel* m, mjData* d) {
// inverse square root of D from inertia LDL decomposition
mjtNum* sqrtInvD = mjSTACKALLOC(d, nv, mjtNum);
for (int i=0; i < nv; i++) {
int diag = d->M_rowadr[i] + d->M_rownnz[i] - 1;
int diag = m->M_rowadr[i] + m->M_rownnz[i] - 1;
sqrtInvD[i] = 1 / mju_sqrt(d->qLD[diag]);
}
@@ -2074,10 +2074,10 @@ void mj_projectConstraint(const mjModel* m, mjData* d) {
}
// traverse row j of C, marking new unique nonzeros
int nnzC = d->M_rownnz[j];
int adrC = d->M_rowadr[j];
int nnzC = m->M_rownnz[j];
int adrC = m->M_rowadr[j];
for (int k=0; k < nnzC; k++) {
int c = d->M_colind[adrC + k];
int c = m->M_colind[adrC + k];
if (marker[c] != r) {
marker[c] = r;
nnz++;
@@ -2157,10 +2157,10 @@ void mj_projectConstraint(const mjModel* m, mjData* d) {
continue;
}
int j = B_colind[i];
int adrC = d->M_rowadr[j];
int adrC = m->M_rowadr[j];
mju_addToSclSparseInc(B + adrB, d->qLD + adrC,
nnzB, B_colind + adrB,
d->M_rownnz[j]-1, d->M_colind + adrC, -b);
m->M_rownnz[j]-1, m->M_colind + adrC, -b);
}
// B(r,:) <- sqrt(inv(D)) * B(r,:)
src/engine/engine_core_smooth.c
+11 -11
View File
@@ -1472,9 +1472,9 @@ void mj_transmission(const mjModel* m, mjData* d) {
// add tendon armature to M
void mj_tendonArmature(const mjModel* m, mjData* d) {
int nv = m->nv, ntendon = m->ntendon, issparse = mj_isSparse(m);
const int* M_rownnz = d->M_rownnz;
const int* M_rowadr = d->M_rowadr;
const int* M_colind = d->M_colind;
const int* M_rownnz = m->M_rownnz;
const int* M_rowadr = m->M_rowadr;
const int* M_colind = m->M_colind;
for (int k=0; k < ntendon; k++) {
mjtNum armature = m->tendon_armature[k];
@@ -1555,7 +1555,7 @@ void mj_crb(const mjModel* m, mjData* d) {
if (m->dof_simplenum[i]) {
int n = i + m->dof_simplenum[i];
for (; i < n; i++) {
d->M[d->M_rowadr[i]] = m->dof_M0[i];
d->M[m->M_rowadr[i]] = m->dof_M0[i];
}
// finish or else fall through with next row
@@ -1565,7 +1565,7 @@ void mj_crb(const mjModel* m, mjData* d) {
}
// init M(i,i) with armature inertia
int Madr_ij = d->M_rowadr[i] + d->M_rownnz[i] - 1;
int Madr_ij = m->M_rowadr[i] + m->M_rownnz[i] - 1;
d->M[Madr_ij] = m->dof_armature[i];
// precompute buf = crb_body_i * cdof_i
@@ -1585,7 +1585,7 @@ void mj_makeM(const mjModel* m, mjData* d) {
TM_START;
mj_crb(m, d);
mj_tendonArmature(m, d);
mju_scatter(d->qM, d->M, d->mapM2M, m->nC);
mju_scatter(d->qM, d->M, m->mapM2M, m->nC);
TM_END(mjTIMER_POS_INERTIA);
}
@@ -1659,7 +1659,7 @@ void mj_factorI_legacy(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD
void mj_factorM(const mjModel* m, mjData* d) {
TM_START;
mju_copy(d->qLD, d->M, m->nC);
mj_factorI(d->qLD, d->qLDiagInv, m->nv, d->M_rownnz, d->M_rowadr, d->M_colind);
mj_factorI(d->qLD, d->qLDiagInv, m->nv, m->M_rownnz, m->M_rowadr, m->M_colind);
TM_ADD(mjTIMER_POS_INERTIA);
}
@@ -1893,7 +1893,7 @@ void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n) {
mju_copy(x, y, n*m->nv);
}
mj_solveLD(x, d->qLD, d->qLDiagInv, m->nv, n,
d->M_rownnz, d->M_rowadr, d->M_colind);
m->M_rownnz, m->M_rowadr, m->M_colind);
}
@@ -1904,9 +1904,9 @@ void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y,
int nv = m->nv;
// local copies of key variables
const int* rownnz = d->M_rownnz;
const int* rowadr = d->M_rowadr;
const int* colind = d->M_colind;
const int* rownnz = m->M_rownnz;
const int* rowadr = m->M_rowadr;
const int* colind = m->M_colind;
const int* diagnum = m->dof_simplenum;
const mjtNum* qLD = d->qLD;
src/engine/engine_derivative.c
+20 -20
View File
@@ -458,9 +458,9 @@ void mjd_rne_vel_dense(const mjModel* m, mjData* d) {
mju_scl(row, Dcfrcbody + (m->dof_bodyid[i]*6+k)*nv, d->cdof[i*6+k], nv);
// dense to sparse: qDeriv -= row
int end = d->D_rowadr[i] + d->D_rownnz[i];
for (int adr=d->D_rowadr[i]; adr < end; adr++) {
d->qDeriv[adr] -= row[d->D_colind[adr]];
int end = m->D_rowadr[i] + m->D_rownnz[i];
for (int adr=m->D_rowadr[i]; adr < end; adr++) {
d->qDeriv[adr] -= row[m->D_colind[adr]];
}
}
}
@@ -492,7 +492,7 @@ static void copyFromParent(const mjModel* m, mjData* d, mjtNum* mat, int n) {
}
// copy: guaranteed to be at beginning of sparse array, due to sorting
mju_copy(mat + 6*d->B_rowadr[n], mat + 6*d->B_rowadr[m->body_parentid[n]], 6*ndof);
mju_copy(mat + 6*m->B_rowadr[n], mat + 6*m->B_rowadr[m->body_parentid[n]], 6*ndof);
}
@@ -507,10 +507,10 @@ static void addToParent(const mjModel* m, mjData* d, mjtNum* mat, int n) {
// find matching nonzeros
int np = m->body_parentid[n];
int i = 0, ip = 0;
while (i < d->B_rownnz[n] && ip < d->B_rownnz[np]) {
while (i < m->B_rownnz[n] && ip < m->B_rownnz[np]) {
// columns match
if (d->B_colind[d->B_rowadr[n] + i] == d->B_colind[d->B_rowadr[np] + ip]) {
mju_addTo(mat + 6*(d->B_rowadr[np] + ip), mat + 6*(d->B_rowadr[n] + i), 6);
if (m->B_colind[m->B_rowadr[n] + i] == m->B_colind[m->B_rowadr[np] + ip]) {
mju_addTo(mat + 6*(m->B_rowadr[np] + ip), mat + 6*(m->B_rowadr[n] + i), 6);
// advance both
i++;
@@ -518,7 +518,7 @@ static void addToParent(const mjModel* m, mjData* d, mjtNum* mat, int n) {
}
// mismatch columns: advance parent
else if (d->B_colind[d->B_rowadr[n] + i] > d->B_colind[d->B_rowadr[np] + ip]) {
else if (m->B_colind[m->B_rowadr[n] + i] > m->B_colind[m->B_rowadr[np] + ip]) {
ip++;
}
@@ -534,7 +534,7 @@ static void addToParent(const mjModel* m, mjData* d, mjtNum* mat, int n) {
// derivative of cvel, cdof_dot w.r.t qvel
static void mjd_comVel_vel(const mjModel* m, mjData* d, mjtNum* Dcvel, mjtNum* Dcdofdot) {
int nv = m->nv, nbody = m->nbody;
int* Badr = d->B_rowadr, * Dadr = d->D_rowadr;
int* Badr = m->B_rowadr, * Dadr = m->D_rowadr;
mjtNum mat[36], matT[36]; // 6x6 matrices
// forward pass over bodies: accumulate Dcvel, set Dcdofdot
@@ -603,9 +603,9 @@ static void mjd_comVel_vel(const mjModel* m, mjData* d, mjtNum* Dcvel, mjtNum* D
// subtract d qfrc_bias / d qvel from qDeriv
static void mjd_rne_vel(const mjModel* m, mjData* d) {
int nv = m->nv, nbody = m->nbody;
const int* Badr = d->B_rowadr;
const int* Dadr = d->D_rowadr;
const int* Bnnz = d->B_rownnz;
const int* Badr = m->B_rowadr;
const int* Dadr = m->D_rowadr;
const int* Bnnz = m->B_rownnz;
mjtNum mat[36], mat1[36], mat2[36], dmul[36], tmp[6];
@@ -710,10 +710,10 @@ static void addJTBJ(const mjModel* m, mjData* d, const mjtNum* J, const mjtNum*
mju_scl(row, J+j*nv, J[i*nv+k] * B[i*n+j], nv);
// add row to qDeriv(k,:)
int rownnz_k = d->D_rownnz[k];
int rownnz_k = m->D_rownnz[k];
for (int s=0; s < rownnz_k; s++) {
int adr = d->D_rowadr[k] + s;
d->qDeriv[adr] += row[d->D_colind[adr]];
int adr = m->D_rowadr[k] + s;
d->qDeriv[adr] += row[m->D_colind[adr]];
}
}
}
@@ -748,8 +748,8 @@ static void addJTBJSparse(
int colik = J_colind[ik];
// qDeriv(k,:) += J(j,:) * J(i,k)*B(i,j)
mju_addToSclSparseInc(d->qDeriv + d->D_rowadr[colik], J + adr_j,
d->D_rownnz[colik], d->D_colind + d->D_rowadr[colik],
mju_addToSclSparseInc(d->qDeriv + m->D_rowadr[colik], J + adr_j,
m->D_rownnz[colik], m->D_colind + m->D_rowadr[colik],
nnz_j, J_colind + adr_j,
J[ik]*B[i*n+j]);
}
@@ -1433,12 +1433,12 @@ void mjd_passive_vel(const mjModel* m, mjData* d) {
// dof damping
for (int i=0; i < nv; i++) {
int nnz_i = d->D_rownnz[i];
int nnz_i = m->D_rownnz[i];
for (int j=0; j < nnz_i; j++) {
int ij = d->D_rowadr[i] + j;
int ij = m->D_rowadr[i] + j;
// identify diagonal element
if (d->D_colind[ij] == i) {
if (m->D_colind[ij] == i) {
d->qDeriv[ij] -= m->dof_damping[i];
break;
}
src/engine/engine_derivative_fd.c
+5 -5
View File
@@ -204,8 +204,8 @@ void mjd_passive_velFD(const mjModel* m, mjData* d, mjtNum eps) {
// copy to i-th column of qDeriv
for (int j=0; j < nv; j++) {
int adr = d->D_rowadr[j] + cnt[j];
if (cnt[j] < d->D_rownnz[j] && d->D_colind[adr] == i) {
int adr = m->D_rowadr[j] + cnt[j];
if (cnt[j] < m->D_rownnz[j] && m->D_colind[adr] == i) {
d->qDeriv[adr] = fd[j];
cnt[j]++;
}
@@ -263,8 +263,8 @@ void mjd_smooth_velFD(const mjModel* m, mjData* d, mjtNum eps) {
// copy to sparse qDeriv
for (int j=0; j < nv; j++) {
if (cnt[j] < d->D_rownnz[j] && d->D_colind[d->D_rowadr[j]+cnt[j]] == i) {
d->qDeriv[d->D_rowadr[j]+cnt[j]] = fd[j];
if (cnt[j] < m->D_rownnz[j] && m->D_colind[m->D_rowadr[j]+cnt[j]] == i) {
d->qDeriv[m->D_rowadr[j]+cnt[j]] = fd[j];
cnt[j]++;
}
}
@@ -272,7 +272,7 @@ void mjd_smooth_velFD(const mjModel* m, mjData* d, mjtNum eps) {
// make sure final row counters equal rownnz
for (int i=0; i < nv; i++) {
if (cnt[i] != d->D_rownnz[i]) {
if (cnt[i] != m->D_rownnz[i]) {
mjERROR("error in constructing FD sparse derivative");
}
}
src/engine/engine_forward.c
+10 -10
View File
@@ -869,18 +869,18 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) {
// qH = M + h*diag(B)
mju_copy(d->qH, d->M, nC);
for (int i=0; i < nv; i++) {
d->qH[d->M_rowadr[i] + d->M_rownnz[i] - 1] += m->opt.timestep * m->dof_damping[i];
d->qH[m->M_rowadr[i] + m->M_rownnz[i] - 1] += m->opt.timestep * m->dof_damping[i];
}
// factorize in-place
mj_factorI(d->qH, d->qHDiagInv, nv, d->M_rownnz, d->M_rowadr, d->M_colind);
mj_factorI(d->qH, d->qHDiagInv, nv, m->M_rownnz, m->M_rowadr, m->M_colind);
}
// solve
mju_add(qfrc, d->qfrc_smooth, d->qfrc_constraint, nv);
mju_copy(qacc, qfrc, m->nv);
mj_solveLD(qacc, d->qH, d->qHDiagInv, nv, 1,
d->M_rownnz, d->M_rowadr, d->M_colind);
m->M_rownnz, m->M_rowadr, m->M_colind);
}
// advance state and time
@@ -1025,18 +1025,18 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
mjd_smooth_vel(m, d, /* flg_bias = */ 1);
// gather qLU <- qM (lower to full)
mju_gather(d->qLU, d->qM, d->mapM2D, nD);
mju_gather(d->qLU, d->qM, m->mapM2D, nD);
// set qLU = qM - dt*qDeriv
mju_addToScl(d->qLU, d->qDeriv, -m->opt.timestep, m->nD);
// factorize qLU
int* scratch = mjSTACKALLOC(d, nv, int);
mju_factorLUSparse(d->qLU, nv, scratch, d->D_rownnz, d->D_rowadr, d->D_colind);
mju_factorLUSparse(d->qLU, nv, scratch, m->D_rownnz, m->D_rowadr, m->D_colind);
}
// solve for qacc: (qM - dt*qDeriv) * qacc = qfrc
mju_solveLUSparse(qacc, d->qLU, qfrc, nv, d->D_rownnz, d->D_rowadr, d->D_diag, d->D_colind);
mju_solveLUSparse(qacc, d->qLU, qfrc, nv, m->D_rownnz, m->D_rowadr, m->D_diag, m->D_colind);
}
// IMPLICITFAST
@@ -1047,22 +1047,22 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
// modified mass matrix: gather MhB <- qDeriv (full to lower)
mjtNum* MhB = mjSTACKALLOC(d, nM, mjtNum);
mju_gather(MhB, d->qDeriv, d->mapD2M, nM);
mju_gather(MhB, d->qDeriv, m->mapD2M, nM);
// set MhB = M - dt*qDeriv
mju_addScl(MhB, d->qM, MhB, -m->opt.timestep, nM);
// gather qH <- MhB (legacy to CSR)
mju_gather(d->qH, MhB, d->mapM2M, nC);
mju_gather(d->qH, MhB, m->mapM2M, nC);
// factorize in-place
mj_factorI(d->qH, d->qHDiagInv, nv, d->M_rownnz, d->M_rowadr, d->M_colind);
mj_factorI(d->qH, d->qHDiagInv, nv, m->M_rownnz, m->M_rowadr, m->M_colind);
}
// solve for qacc: (qM - dt*qDeriv) * qacc = qfrc
mju_copy(qacc, qfrc, nv);
mj_solveLD(qacc, d->qH, d->qHDiagInv, nv, 1,
d->M_rownnz, d->M_rowadr, d->M_colind);
m->M_rownnz, m->M_rowadr, m->M_colind);
} else {
mjERROR("integrator must be implicit or implicitfast");
src/engine/engine_inverse.c
+3 -3
View File
@@ -116,14 +116,14 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) {
mjd_smooth_vel(m, d, /* flg_bias = */ 1);
// gather qLU <- qM (lower to full)
mju_gather(d->qLU, d->qM, d->mapM2D, nD);
mju_gather(d->qLU, d->qM, m->mapM2D, nD);
// set qLU = qM - dt*qDeriv
mju_addToScl(d->qLU, d->qDeriv, -m->opt.timestep, m->nD);
// set qfrc = qLU * qacc
mju_mulMatVecSparse(qfrc, d->qLU, qacc, nv,
d->D_rownnz, d->D_rowadr, d->D_colind, /*rowsuper=*/NULL);
m->D_rownnz, m->D_rowadr, m->D_colind, /*rowsuper=*/NULL);
break;
case mjINT_IMPLICITFAST:
@@ -137,7 +137,7 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) {
// set M = M - dt*qDeriv (reduced to M nonzeros)
mjtNum* qDerivReduced = mjSTACKALLOC(d, m->nM, mjtNum);
for (int i=0; i < nM; i++) {
qDerivReduced[i] = d->qDeriv[d->mapD2M[i]];
qDerivReduced[i] = d->qDeriv[m->mapD2M[i]];
}
mju_addToScl(d->qM, qDerivReduced, -m->opt.timestep, m->nM);
src/engine/engine_io.c
+6 -34
View File
@@ -1118,18 +1118,18 @@ void mj_makeBSparse(int nv, int nbody, int nB,
// check D and B sparsity for consistency
static void checkDBSparse(const mjModel* m, mjData* d) {
static void checkDBSparse(const mjModel* m) {
// process all dofs
for (int j = 0; j < m->nv; j++) {
// get body for this dof
int i = m->dof_bodyid[j];
// D[row j] and B[row i] should be identical
if (d->D_rownnz[j] != d->B_rownnz[i]) {
if (m->D_rownnz[j] != m->B_rownnz[i]) {
mjERROR("rows have different nnz");
}
for (int k = 0; k < d->D_rownnz[j]; k++) {
if (d->D_colind[d->D_rowadr[j] + k] != d->B_colind[d->B_rowadr[i] + k]) {
for (int k = 0; k < m->D_rownnz[j]; k++) {
if (m->D_colind[m->D_rowadr[j] + k] != m->B_colind[m->B_rowadr[i] + k]) {
mjERROR("rows have different colind");
}
}
@@ -2004,36 +2004,8 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
}
}
// construct sparse matrix representations
if (m->body_dofadr) {
mj_markStack(d);
int* remaining = mjSTACKALLOC(d, m->nv, int);
int* count = mjSTACKALLOC(d, m->nbody, int);
int* M = mjSTACKALLOC(d, m->nM, int);
int* D = mjSTACKALLOC(d, m->nD, int);
// make D
mj_makeDofDofSparse(m->nv, m->nC, m->nD, m->nM, m->dof_parentid, m->dof_simplenum,
d->D_rownnz, d->D_rowadr, d->D_diag, d->D_colind,
/*reduced=*/0, /*upper=*/1, remaining);
// make B, check D and B
mj_makeBSparse(m->nv, m->nbody, m->nB, m->body_dofnum, m->body_parentid,
m->body_dofadr, d->B_rownnz, d->B_rowadr, d->B_colind, count);
checkDBSparse(m, d);
// make C
mj_makeDofDofSparse(m->nv, m->nC, m->nD, m->nM, m->dof_parentid, m->dof_simplenum,
d->M_rownnz, d->M_rowadr, NULL, d->M_colind,
/*reduced=*/1, /*upper=*/0, remaining);
// make index mappings: mapM2D, mapD2M, mapM2C, mapM2M
mj_makeDofDofMaps(m->nv, m->nM, m->nC, m->nD, m->dof_Madr, m->dof_simplenum, m->dof_parentid,
d->D_rownnz, d->D_rowadr, d->D_colind, d->M_rownnz, d->M_rowadr,
d->mapM2D, d->mapD2M, d->mapM2M, remaining, M, D);
mj_freeStack(d);
}
// check consistency of sparse matrix representations
checkDBSparse(m);
// restore pluginstate and plugindata
if (d->nplugin) {
src/engine/engine_island.c
+1 -1
View File
@@ -521,7 +521,7 @@ void mj_island(const mjModel* m, mjData* d) {
// inertia: block-diagonalize both iLD <- qLD and iM <- qM
mju_blockDiagSparse(d->iLD, d->iM_rownnz, d->iM_rowadr, d->iM_colind,
d->qLD, d->M_rownnz, d->M_rowadr, d->M_colind,
d->qLD, m->M_rownnz, m->M_rowadr, m->M_colind,
nidof, nisland,
d->map_idof2dof, d->map_dof2idof,
d->island_idofadr, d->island_idofadr,
src/engine/engine_print.c
+7 -32
View File
@@ -1155,51 +1155,26 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena
d->moment_rowadr, d->moment_colind, fp, float_format);
printArray("CRB", m->nbody, 10, d->crb, fp, float_format);
printInertia("QM", d->qM, m, fp, float_format);
printSparse("M", d->M, m->nv, d->M_rownnz,
d->M_rowadr, d->M_colind, fp, float_format);
printSparse("QLD", d->qLD, m->nv, d->M_rownnz,
d->M_rowadr, d->M_colind, fp, float_format);
printSparse("M", d->M, m->nv, m->M_rownnz,
m->M_rowadr, m->M_colind, fp, float_format);
printSparse("QLD", d->qLD, m->nv, m->M_rownnz,
m->M_rowadr, m->M_colind, fp, float_format);
printArray("QLDIAGINV", m->nv, 1, d->qLDiagInv, fp, float_format);
if (!mju_isZero(d->qHDiagInv, m->nv)) {
printSparse("QH", d->qH, m->nv, d->M_rownnz, d->M_rowadr, d->M_colind, fp, float_format);
printSparse("QH", d->qH, m->nv, m->M_rownnz, m->M_rowadr, m->M_colind, fp, float_format);
printArray("QHDIAGINV", m->nv, 1, d->qHDiagInv, fp, float_format);
}
// B sparse structure
mj_printSparsity("B: body-dof matrix", m->nbody, m->nv, d->B_rowadr, NULL, d->B_rownnz, NULL,
d->B_colind, fp);
printArrayInt("B_ROWNNZ", 1, m->nbody, d->B_rownnz, fp);
printArrayInt("B_ROWADR", 1, m->nbody, d->B_rowadr, fp);
printArrayInt("B_COLIND", 1, m->nB, d->B_colind, fp);
// M sparse structure
mj_printSparsity("M: reduced inertia matrix", m->nv, m->nv, d->M_rowadr, NULL, d->M_rownnz,
NULL, d->M_colind, fp);
printArrayInt("M_ROWNNZ", 1, m->nv, d->M_rownnz, fp);
printArrayInt("M_ROWADR", 1, m->nv, d->M_rowadr, fp);
printArrayInt("M_COLIND", 1, m->nC, d->M_colind, fp);
printArrayInt("MAPM2M", 1, m->nC, d->mapM2M, fp);
// D sparse structure
mj_printSparsity("D: dof-dof matrix", m->nv, m->nv,
d->D_rowadr, d->D_diag, d->D_rownnz, NULL, d->D_colind, fp);
printArrayInt("D_ROWNNZ", 1, m->nv, d->D_rownnz, fp);
printArrayInt("D_ROWADR", 1, m->nv, d->D_rowadr, fp);
printArrayInt("D_COLIND", 1, m->nD, d->D_colind, fp);
printArrayInt("MAPM2D", 1, m->nD, d->mapM2D, fp);
printArrayInt("MAPD2M", 1, m->nM, d->mapD2M, fp);
// print qDeriv
if (!mju_isZero(d->qDeriv, m->nD)) {
printSparse("QDERIV", d->qDeriv, m->nv, d->D_rownnz, d->D_rowadr, d->D_colind,
printSparse("QDERIV", d->qDeriv, m->nv, m->D_rownnz, m->D_rowadr, m->D_colind,
fp, float_format);
}
// print qLU
if (!mju_isZero(d->qLU, m->nD)) {
printSparse("QLU", d->qLU, m->nv, d->D_rownnz, d->D_rowadr, d->D_colind, fp, float_format);
printSparse("QLU", d->qLU, m->nv, m->D_rownnz, m->D_rowadr, m->D_colind, fp, float_format);
}
// contact
src/engine/engine_solver.c
+3 -3
View File
@@ -885,9 +885,9 @@ static void CGpointers(const mjModel* m, const mjData* d, mjCGContext* ctx, int
ctx->qacc = d->qacc;
// inertia
ctx->M_rownnz = d->M_rownnz;
ctx->M_rowadr = d->M_rowadr;
ctx->M_colind = d->M_colind;
ctx->M_rownnz = m->M_rownnz;
ctx->M_rowadr = m->M_rowadr;
ctx->M_colind = m->M_colind;
ctx->M = d->M;
ctx->qLD = d->qLD;
ctx->qLDiagInv = d->qLDiagInv;
src/engine/engine_support.c
+5 -5
View File
@@ -1058,14 +1058,14 @@ void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec)
// non-simple: add off-diagonals
if (!m->dof_simplenum[i]) {
int adr = d->M_rowadr[i];
res[i] += mju_dotSparse(qLD+adr, vec, d->M_rownnz[i] - 1, d->M_colind+adr);
int adr = m->M_rowadr[i];
res[i] += mju_dotSparse(qLD+adr, vec, m->M_rownnz[i] - 1, m->M_colind+adr);
}
}
// res *= sqrt(D)
for (int i=0; i < nv; i++) {
int diag = d->M_rowadr[i] + d->M_rownnz[i] - 1;
int diag = m->M_rowadr[i] + m->M_rownnz[i] - 1;
res[i] *= mju_sqrt(qLD[diag]);
}
}
@@ -1084,7 +1084,7 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst,
int* buf_ind = mjSTACKALLOC(d, nv, int);
mju_addToMatSparse(dst, rownnz, rowadr, colind, nv,
d->M, d->M_rownnz, d->M_rowadr, d->M_colind,
d->M, m->M_rownnz, m->M_rowadr, m->M_colind,
buf_val, buf_ind);
mj_freeStack(d);
@@ -1092,7 +1092,7 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst,
// dense
else {
mju_addToSymSparse(dst, d->M, nv, d->M_rownnz, d->M_rowadr, d->M_colind, /*flg_upper=*/ 1);
mju_addToSymSparse(dst, d->M, nv, m->M_rownnz, m->M_rowadr, m->M_colind, /*flg_upper=*/ 1);
}
}
test/benchmark/factorI_benchmark_test.cc
+2 -2
View File
@@ -46,7 +46,7 @@ static void BM_factorI(benchmark::State& state, bool legacy, bool coil) {
// M: mass matrix in CSR format
mjtNum* M = mj_stackAllocNum(d, m->nC);
mju_gather(M, d->qM, d->mapM2M, m->nC);
mju_gather(M, d->qM, m->mapM2M, m->nC);
// LDlegacy: legacy LD matrix (size nM)
mjtNum* LDlegacy = mj_stackAllocNum(d, m->nM);
@@ -59,7 +59,7 @@ static void BM_factorI(benchmark::State& state, bool legacy, bool coil) {
} else {
mju_copy(d->qLD, M, m->nC);
mj_factorI(d->qLD, d->qLDiagInv, m->nv,
d->M_rownnz, d->M_rowadr, d->M_colind);
m->M_rownnz, m->M_rowadr, m->M_colind);
}
}
}
test/benchmark/inertia_benchmark_test.cc
+3 -3
View File
@@ -48,7 +48,7 @@ static void BM_solve(benchmark::State& state, SolveType type) {
// M: mass matrix in CSR format
mjtNum* M = mj_stackAllocNum(d, m->nC);
mju_gather(M, d->qM, d->mapM2M, m->nC);
mju_gather(M, d->qM, m->mapM2M, m->nC);
// LDlegacy: legacy LD matrix (size nM)
mjtNum* LDlegacy = mj_stackAllocNum(d, m->nM);
@@ -73,9 +73,9 @@ static void BM_solve(benchmark::State& state, SolveType type) {
case SolveType::kCsr:
mju_copy(d->qLD, M, m->nC);
mj_factorI(d->qLD, d->qLDiagInv, m->nv,
d->M_rownnz, d->M_rowadr, d->M_colind);
m->M_rownnz, m->M_rowadr, m->M_colind);
mj_solveLD(res, d->qLD, d->qLDiagInv, m->nv, 1,
d->M_rownnz, d->M_rowadr, d->M_colind);
m->M_rownnz, m->M_rowadr, m->M_colind);
}
}
}
test/benchmark/solveLD_benchmark_test.cc
+2 -2
View File
@@ -54,7 +54,7 @@ static void BM_solveLD(benchmark::State& state, bool featherstone, bool coil) {
// scatter into legacy matrix
mjtNum* LDlegacy = mj_stackAllocNum(d, m->nM);
mju_zero(LDlegacy, m->nM);
mju_scatter(LDlegacy, d->qLD, d->mapM2M, m->nC);
mju_scatter(LDlegacy, d->qLD, m->mapM2M, m->nC);
// benchmark
while (state.KeepRunningBatch(kNumBenchmarkSteps)) {
@@ -64,7 +64,7 @@ static void BM_solveLD(benchmark::State& state, bool featherstone, bool coil) {
mj_solveLD_legacy(m, res, 1, LDlegacy, d->qLDiagInv);
} else {
mj_solveLD(res, d->qLD, d->qLDiagInv, m->nv, 1,
d->M_rownnz, d->M_rowadr, d->M_colind);
m->M_rownnz, m->M_rowadr, m->M_colind);
}
}
}
test/engine/engine_core_smooth_test.cc
+13 -13
View File
@@ -236,7 +236,7 @@ TEST_F(CoreSmoothTest, TendonArmature) {
// put only CRB inertia in M2
mj_crb(m, d);
mju_scatter(d->qM, d->M, d->mapM2M, m->nC);
mju_scatter(d->qM, d->M, m->mapM2M, m->nC);
vector<mjtNum> M2(nv*nv);
mj_fullM(m, M2.data(), d->qM);
@@ -649,7 +649,7 @@ TEST_F(CoreSmoothTest, FactorI) {
int nv = model->nv;
vector<mjtNum> Ldense(nv*nv, 0);
mju_sparse2dense(Ldense.data(), data->qLD, nv, nv,
data->M_rownnz, data->M_rowadr, data->M_colind);
model->M_rownnz, model->M_rowadr, model->M_colind);
for (int i=0; i < nv; i++) {
// set diagonal to 1
Ldense[i*nv+i] = 1;
@@ -658,7 +658,7 @@ TEST_F(CoreSmoothTest, FactorI) {
// dense D matrix
vector<mjtNum> Ddense(nv*nv);
mju_sparse2dense(Ddense.data(), data->qLD, nv, nv,
data->M_rownnz, data->M_rowadr, data->M_colind);
model->M_rownnz, model->M_rowadr, model->M_colind);
for (int i=0; i < nv; i++) {
for (int j=0; j < nv; j++) {
// zero everything except the diagonal
@@ -698,12 +698,12 @@ TEST_F(CoreSmoothTest, SolveLDs) {
// copy M into LD: Legacy format
vector<mjtNum> LDlegacy(nM, 0);
mju_scatter(LDlegacy.data(), d->qLD, d->mapM2M, nC);
mju_scatter(LDlegacy.data(), d->qLD, m->mapM2M, nC);
// compare LD and LDs densified matrices
vector<mjtNum> LDdense(nv*nv);
mju_sparse2dense(LDdense.data(), d->qLD, nv, nv,
d->M_rownnz, d->M_rowadr, d->M_colind);
m->M_rownnz, m->M_rowadr, m->M_colind);
vector<mjtNum> LDdense2(nv*nv);
mj_fullM(m, LDdense2.data(), LDlegacy.data());
@@ -722,7 +722,7 @@ TEST_F(CoreSmoothTest, SolveLDs) {
mj_solveLD_legacy(m, vec.data(), 1, LDlegacy.data(), d->qLDiagInv);
mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, 1,
d->M_rownnz, d->M_rowadr, d->M_colind);
m->M_rownnz, m->M_rowadr, m->M_colind);
// expect vectors to match up to floating point precision
for (int i=0; i < nv; i++) {
@@ -746,7 +746,7 @@ TEST_F(CoreSmoothTest, SolveLDmultipleVectors) {
// copy LD into LDlegacy: Legacy format
vector<mjtNum> LDlegacy(m->nM, 0);
mju_scatter(LDlegacy.data(), d->qLD, d->mapM2M, m->nC);
mju_scatter(LDlegacy.data(), d->qLD, m->mapM2M, m->nC);
// compare n LD and LDs vector solve
int n = 3;
@@ -757,7 +757,7 @@ TEST_F(CoreSmoothTest, SolveLDmultipleVectors) {
mj_solveLD_legacy(m, vec.data(), n, LDlegacy.data(), d->qLDiagInv);
mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, n,
d->M_rownnz, d->M_rowadr, d->M_colind);
m->M_rownnz, m->M_rowadr, m->M_colind);
// expect vectors to match up to floating point precision
for (int i=0; i < nv*n; i++) {
@@ -781,7 +781,7 @@ TEST_F(CoreSmoothTest, SolveM2) {
int nv = m->nv;
vector<mjtNum> sqrtInvD(nv);
for (int i=0; i < nv; i++) {
int diag = d->M_rowadr[i] + d->M_rownnz[i] - 1;
int diag = m->M_rowadr[i] + m->M_rownnz[i] - 1;
sqrtInvD[i] = 1 / mju_sqrt(d->qLD[diag]);
}
@@ -795,7 +795,7 @@ TEST_F(CoreSmoothTest, SolveM2) {
mj_solveM2(m, d, res.data(), vec.data(), sqrtInvD.data(), n);
mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, n,
d->M_rownnz, d->M_rowadr, d->M_colind);
m->M_rownnz, m->M_rowadr, m->M_colind);
// expect equality of dot(v, M^-1 * v) and dot(M^-1/2 * v, M^-1/2 * v)
for (int i=0; i < n; i++) {
@@ -824,17 +824,17 @@ TEST_F(CoreSmoothTest, FactorIs) {
// copy qLDlegacy into qLDexpected: CSR format
vector<mjtNum> qLDexpected(nC);
mju_gather(qLDexpected.data(), qLDlegacy.data(), d->mapM2M, nC);
mju_gather(qLDexpected.data(), qLDlegacy.data(), m->mapM2M, nC);
// copy qM into qLD: CSR format
vector<mjtNum> qLD(nC);
mju_gather(qLD.data(), d->qM, d->mapM2M, nC);
mju_gather(qLD.data(), d->qM, m->mapM2M, nC);
vector<mjtNum> qLDiagInvExpected(d->qLDiagInv, d->qLDiagInv + nv);
vector<mjtNum> qLDiagInv(nv, 0);
mj_factorI(qLD.data(), qLDiagInv.data(), nv,
d->M_rownnz, d->M_rowadr, d->M_colind);
m->M_rownnz, m->M_rowadr, m->M_colind);
// expect outputs to match to floating point precision
EXPECT_THAT(qLD, Pointwise(DoubleNear(1e-12), qLDexpected));
test/engine/engine_derivative_test.cc
+2 -2
View File
@@ -436,7 +436,7 @@ static void LinearSystem(const mjModel* m, mjData* d, mjtNum* A, mjtNum* B) {
Ac[nv*nv + i*nv + i] = -m->dof_damping[i];
}
mj_solveLD(Ac, d->qH, d->qHDiagInv, nv, 2*nv,
d->M_rownnz, d->M_rowadr, d->M_colind);
m->M_rownnz, m->M_rowadr, m->M_colind);
// A = [dt*Ac; Ac]
mju_transpose(A, Ac, 2*nv, nv);
@@ -464,7 +464,7 @@ static void LinearSystem(const mjModel* m, mjData* d, mjtNum* A, mjtNum* B) {
mju_sparse2dense(Bc, d->actuator_moment, nu, nv, d->moment_rownnz,
d->moment_rowadr, d->moment_colind);
mj_solveLD(Bc, d->qH, d->qHDiagInv, nv, nu,
d->M_rownnz, d->M_rowadr, d->M_colind);
m->M_rownnz, m->M_rowadr, m->M_colind);
mju_transpose(BcT, Bc, nu, nv);
mju_scl(B, BcT, dt*dt, nu*nv);
mju_scl(B+nu*nv, BcT, dt, nu*nv);
unity/Runtime/Bindings/MjBindings.cs
-13
View File
@@ -4983,19 +4983,6 @@ public unsafe struct mjData_ {
public double* subtree_angmom;
public double* qH;
public double* qHDiagInv;
public int* B_rownnz;
public int* B_rowadr;
public int* B_colind;
public int* M_rownnz;
public int* M_rowadr;
public int* M_colind;
public int* mapM2M;
public int* D_rownnz;
public int* D_rowadr;
public int* D_diag;
public int* D_colind;
public int* mapM2D;
public int* mapD2M;
public double* qDeriv;
public double* qLU;
public double* actuator_force;