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::Create( - const mjModel* m, mjData* d, int instance) { +std::optional 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 -#include #include #include @@ -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 {