From 5dd7bf861371bfa82663d5e7192dca4a01a21ffc Mon Sep 17 00:00:00 2001
From: Nimrod Gileadi <nimrod@google.com>
Date: Thu, 11 Aug 2022 15:26:32 -0700
Subject: [PATCH] Verify that body_parentid and similar properties always go up
 the kinematic chain.

PiperOrigin-RevId: 467063831
Change-Id: I591c6a6f78178e9c2196cc94d7e622eba93d8191
---
 src/engine/engine_io.c | 16 ++++++++++++++++
 1 file changed, 16 insertions(+)

diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c
index 6c74d0a7..1482e8cb 100644
--- a/src/engine/engine_io.c
+++ b/src/engine/engine_io.c
@@ -1298,6 +1298,17 @@ const char* mj_validateReferences(const mjModel* m) {
 #undef MJMODEL_REFERENCES
 
   // special logic that doesn't fit in the macro:
+  for (int i=0; i<m->nbody; i++) {
+    if (i > 0 && m->body_parentid[i] >= i) {
+      return "Invalid model: bad body_parentid.";
+    }
+    if (m->body_rootid[i] > i) {
+      return "Invalid model: bad body_rootid.";
+    }
+    if (m->body_weldid[i] > i) {
+      return "Invalid model: bad body_weldid.";
+    }
+  }
   for (int i=0; i<m->njnt; i++) {
     if (m->jnt_type[i] >= 4 || m->jnt_type[i] < 0) {
       return "Invalid model: jnt_type out of bounds.";
@@ -1311,6 +1322,11 @@ const char* mj_validateReferences(const mjModel* m) {
       return "Invalid model: jnt_dofadr out of bounds.";
     }
   }
+  for (int i=0; i<m->nv; i++) {
+    if (m->dof_parentid[i] >= i) {
+      return "Invalid model: bad dof_parentid.";
+    }
+  }
   for (int i=0; i<m->ngeom; i++) {
     if (m->geom_condim[i] > 6 || m->geom_condim[i] < 0) {
       return "Invalid model: geom_condim out of bounds.";