From e6dc5ed1764e1847ed9d8fe01fb23b08e8a9ea24 Mon Sep 17 00:00:00 2001
From: Google DeepMind <noreply@deepmind.com>
Date: Thu, 2 Nov 2023 03:01:34 -0700
Subject: [PATCH] Add inner radius as a parameter for the sdf gear plugin.

PiperOrigin-RevId: 578789045
Change-Id: Id69418d6b1adcce404927de4ed68c94a5ec1f66f
---
 plugin/sdf/gear.cc | 9 ++++++---
 plugin/sdf/gear.h  | 8 ++++----
 2 files changed, 10 insertions(+), 7 deletions(-)

diff --git a/plugin/sdf/gear.cc b/plugin/sdf/gear.cc
index 0546b1d0..67f4a8be 100644
--- a/plugin/sdf/gear.cc
+++ b/plugin/sdf/gear.cc
@@ -82,6 +82,9 @@ static mjtNum distance2D(const mjtNum p[3],
   mjtNum h = 2.2 / Pd;
 
   mjtNum innerR = Ro - h - 0.14*D;
+  if (attributes[4] >= 0.0) {
+    innerR = attributes[4] / 2.0;
+  }
 
   // Early exit
   if (innerR - rho > 0.0)
@@ -144,10 +147,10 @@ static mjtNum distance(const mjtNum p[3],
 }  // namespace
 
 // factory function
-std::optional<Gear> Gear::Create(
-  const mjModel* m, mjData* d, int instance) {
+std::optional<Gear> Gear::Create(const mjModel* m, mjData* d, int instance) {
   if (CheckAttr("alpha", m, instance) && CheckAttr("diameter", m, instance) &&
-      CheckAttr("teeth", m, instance)) {
+      CheckAttr("teeth", m, instance) &&
+      CheckAttr("innerdiameter", m, instance)) {
       return Gear(m, d, instance);
   } else {
       mju_warning("Invalid parameter specification in Gear plugin");
diff --git a/plugin/sdf/gear.h b/plugin/sdf/gear.h
index f6e49ebc..f1f63a75 100644
--- a/plugin/sdf/gear.h
+++ b/plugin/sdf/gear.h
@@ -16,7 +16,6 @@
 #define MUJOCO_PLUGIN_SDF_GEAR_H_
 
 #include <optional>
-#include <vector>
 
 #include <mujoco/mjdata.h>
 #include <mujoco/mjmodel.h>
@@ -27,10 +26,11 @@
 namespace mujoco::plugin::sdf {
 
 struct GearAttribute {
-  static constexpr int nattribute = 4;
+  static constexpr int nattribute = 5;
   static constexpr char const* names[nattribute] = {"alpha", "diameter",
-                                                    "teeth", "thickness"};
-  static constexpr mjtNum defaults[nattribute] = { 0, 2.8, 25, .2 };
+                                                    "teeth", "thickness",
+                                                    "innerdiameter"};
+  static constexpr mjtNum defaults[nattribute] = { 0, 2.8, 25, .2 , -1};
 };
 
 class Gear {