Refactor mj_crb and mj_tendonArmature to use CSR M.

PiperOrigin-RevId: 774077388 Change-Id: Id57118a5d66c074766f5c69fd3b889ebc625cbf7
2025-06-21 04:48:07 -07:00
parent 14dc7c2a60
commit 2707eadc00
3 changed files with 45 additions and 35 deletions
@@ -1990,6 +1990,9 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
  mju_zero(d->mocap_pos, 3*m->nmocap);
  mju_zero(d->mocap_quat, 4*m->nmocap);

+  // zero out qM, special case because scattring from M skips simple body off-diagonals
+  mju_zero(d->qM, m->nM);
+
  // copy qpos0 from model
  if (m->qpos0) {
    memcpy(d->qpos, m->qpos0, m->nq*sizeof(mjtNum));