Add mjData.nl: number of active limit constraints.

PiperOrigin-RevId: 561012367
Change-Id: I9a01fec55502f8a90d7435972e477455ae29df15

src/engine/engine_core_constraint.c

@@ -288,6 +288,8 @@ int mj_addConstraint(const mjModel* m, mjData* d,
     d->ne += size;
   } else if (type == mjCNSTR_FRICTION_DOF || type == mjCNSTR_FRICTION_TENDON) {
     d->nf += size;
+  } else if (type == mjCNSTR_LIMIT_JOINT || type == mjCNSTR_LIMIT_TENDON) {
+    d->nl += size;
   }
 
   return 0;
@@ -1658,7 +1660,7 @@ static inline int mj_nc(const mjModel* m, mjData* d, int* nnz) {
 // driver: call all functions above
 void mj_makeConstraint(const mjModel* m, mjData* d) {
   // clear sizes
-  d->ne = d->nf = d->nefc = d->nnzJ = 0;
+  d->ne = d->nf = d->nl = d->nefc = d->nnzJ = 0;
 
   // disabled or Jacobian not allocated: return
   if (mjDISABLED(mjDSBL_CONSTRAINT)) {
@@ -1669,7 +1671,8 @@ void mj_makeConstraint(const mjModel* m, mjData* d) {
   int *nnz = mj_isSparse(m) ? &(d->nnzJ) : NULL;
   int ne_allocated = mj_ne(m, d, nnz);
   int nf_allocated = mj_nf(m, d, nnz);
-  int nefc_allocated = ne_allocated + nf_allocated + mj_nl(m, d, nnz) + mj_nc(m, d, nnz);
+  int nl_allocated = mj_nl(m, d, nnz);
+  int nefc_allocated = ne_allocated + nf_allocated + nl_allocated + mj_nc(m, d, nnz);
   if (!mj_isSparse(m)) {
     d->nnzJ = nefc_allocated * m->nv;
   }
@@ -1704,6 +1707,10 @@ void mj_makeConstraint(const mjModel* m, mjData* d) {
       mjERROR("nf mis-allocation: found nf=%d but allocated %d", d->nf, nf_allocated);
     }
 
+    if (d->nl != nl_allocated) {
+      mjERROR("nl mis-allocation: found nl=%d but allocated %d", d->nl, nl_allocated);
+    }
+
     // check that nefc was computed correctly
     if (d->nefc != nefc_allocated) {
       mjERROR("nefc mis-allocation: found nefc=%d but allocated %d", d->nefc, nefc_allocated);

src/engine/engine_io.c

@@ -1329,6 +1329,7 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
   // clear variable sizes
   d->ne = 0;
   d->nf = 0;
+  d->nl = 0;
   d->nefc = 0;
   d->nnzJ = 0;
   d->ncon = 0;