Add missing term in mj_jacDot

PiperOrigin-RevId: 735686836 Change-Id: I813ca46f71e368ddc97a6d2e540102137b31dbcf
2025-03-11 03:00:25 -07:00
parent 3b27f30827
commit de48f4178f
4 changed files with 88 additions and 42 deletions
@@ -1893,7 +1893,7 @@ void mj_comVel(const mjModel* m, mjData* d) {

    // assign cvel, cdofdot
    mju_copy(d->cvel+6*i, cvel, 6);
-    mju_copy(d->cdof_dot+6*bda, cdofdot, 6*m->body_dofnum[i]);
+    mju_copy(d->cdof_dot+6*bda, cdofdot, 6*dofnum);
  }
 }