From 26f71e2f6c738135624138eec3583ba24e1fe03d Mon Sep 17 00:00:00 2001
From: Alessio Quaglino <quaglino@google.com>
Date: Wed, 30 Nov 2022 02:17:09 -0800
Subject: [PATCH] Check that spacing is greater than geometry size for
 Composite particles and grid.

PiperOrigin-RevId: 491858371
Change-Id: I03a669286c283a72947705463426badcfc9b0bfd
---
 src/user/user_composite.cc       | 11 ++++++++-
 test/user/user_composite_test.cc | 40 +++++++++++++++++++++++++++++++-
 2 files changed, 49 insertions(+), 2 deletions(-)

diff --git a/src/user/user_composite.cc b/src/user/user_composite.cc
index 1fc18bd6..1041600b 100644
--- a/src/user/user_composite.cc
+++ b/src/user/user_composite.cc
@@ -241,11 +241,20 @@ bool mjCComposite::Make(mjCModel* model, mjCBody* body, char* error, int error_s
   }
 
   // check spacing
+  if (type==mjCOMPTYPE_GRID || type==mjCOMPTYPE_PARTICLE) {
+    if (spacing < mju_max(def[0].geom.size[0],
+                  mju_max(def[0].geom.size[1], def[0].geom.size[2]))) {
+      return comperr(error, "Spacing must be larger than geometry size",
+                     error_sz);
+    }
+  }
+
+  // check cable sizes are nonzero if vertices are not prescribed
   if (mjuu_dot3(size, size)<mjMINVAL && uservert.empty()) {
     return comperr(error, "Positive spacing or length expected in composite", error_sz);
   }
 
-  // check either spacing or length
+  // check spacing is not used by cable
   if (spacing && type==mjCOMPTYPE_CABLE) {
     return comperr(error, "Spacing is not supported by cable composite", error_sz);
   }
diff --git a/test/user/user_composite_test.cc b/test/user/user_composite_test.cc
index 9110b386..fe013e75 100644
--- a/test/user/user_composite_test.cc
+++ b/test/user/user_composite_test.cc
@@ -20,6 +20,7 @@
 #include <gmock/gmock.h>
 #include <gtest/gtest.h>
 #include <absl/strings/str_format.h>
+#include <mujoco/mjdata.h>
 #include <mujoco/mjmodel.h>
 #include <mujoco/mujoco.h>
 #include "test/fixture.h"
@@ -51,7 +52,8 @@ TEST_F(UserCompositeTest, MultipleJointsNotAllowedUnlessParticle) {
   std::array<char, 1024> error;
   mjModel* m = LoadModelFromString(xml, error.data(), error.size());
   EXPECT_THAT(m, IsNull());
-  EXPECT_THAT(error.data(), HasSubstr("Only particles are allowed to have multiple joints"));
+  EXPECT_THAT(error.data(),
+              HasSubstr("Only particles are allowed to have multiple joints"));
 }
 
 TEST_F(UserCompositeTest, StretchAndTwistAllowed) {
@@ -76,5 +78,41 @@ TEST_F(UserCompositeTest, StretchAndTwistAllowed) {
   mj_deleteModel(m);
 }
 
+TEST_F(UserCompositeTest, SpacingGreaterThanGeometry) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+  <worldbody>
+    <composite type="grid" count="282 2">
+      <geom size="8"/>
+    </composite>
+  </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* m = LoadModelFromString(xml, error.data(), error.size());
+  EXPECT_THAT(m, IsNull()) << error.data();
+  EXPECT_THAT(error.data(),
+              HasSubstr("Spacing must be larger than geometry size"));
+}
+
+TEST_F(UserCompositeTest, SpacingEqualToGeometry) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+  <worldbody>
+    <composite type="grid" count="282 2" spacing="8">
+      <geom size="8"/>
+    </composite>
+  </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* m = LoadModelFromString(xml, error.data(), error.size());
+  EXPECT_THAT(m, NotNull()) << error.data();
+  mjData* d = mj_makeData(m);
+  mj_step(m, d);
+  mj_deleteData(d);
+  mj_deleteModel(m);
+}
+
 }  // namespace
 }  // namespace mujoco