Support capsules in URDF import.

PiperOrigin-RevId: 489980760
Change-Id: I1c21e736a86f6637a00e2062727d2e6fd5694c42

doc/changelog.rst

@@ -56,6 +56,7 @@ General
   register one or more MuJoCo plugins on load.
 - Sensors of type :ref:`user<sensor-user>` no longer require :at:`objtype` and :at:`objname`. If unspecified, the
   objtype will be :ref:`mjOBJ_UNKNOWN<mjtObj>`. ``user`` sensors :at:`datatype` default is now :at-val:`"real"`.
+- Add support for capsules in URDF import.
 
 Simulate
 ^^^^^^^^

src/xml/xml_urdf.cc

@@ -470,6 +470,14 @@ mjCGeom* mjXURDF::Geom(XMLElement* geom_elem, mjCBody* pbody, bool collision) {
     ReadAttr(temp, "radius", 1, pgeom->size, text, true, true);
   }
 
+  // capsule
+  else if ((temp = FindSubElem(elem, "capsule"))) {
+    pgeom->type = mjGEOM_CAPSULE;
+    ReadAttr(temp, "radius", 1, pgeom->size, text, true, true);
+    ReadAttr(temp, "length", 1, pgeom->size+1, text, true, true);
+    pgeom->size[1] /= 2;            // MuJoCo uses half-length
+  }
+
   // mesh
   else if ((temp = FindSubElem(elem, "mesh"))) {
     // set geom type and read mesh attributes

test/xml/xml_urdf_test.cc

@@ -0,0 +1,53 @@
+// Copyright 2021 DeepMind Technologies Limited
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// Tests for xml/xml_api.cc.
+
+#include <cstddef>
+#include <cstring>
+#include <string>
+
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+#include <mujoco/mujoco.h>
+#include "test/fixture.h"
+
+namespace mujoco {
+namespace {
+
+using ::testing::NotNull;
+
+// ---------------------------- test capsule --------------------------------
+
+TEST_F(MujocoTest, ReadsCapsule) {
+  static constexpr char urdf[] = R"(
+  <robot name="">
+  <link name="torso">
+    <collision>
+      <origin rpy="-1.57080 0.00000 0.00000" xyz="0.00000 0.00000 0.00000"/>
+      <geometry>
+        <capsule length="0.14000" radius="0.07000"/>
+      </geometry>
+    </collision>
+  </link>
+  </robot>
+  )";
+  std::array<char, 1000> error;
+  mjModel* model = LoadModelFromString(urdf, error.data(), error.size());
+  ASSERT_THAT(model, NotNull()) << error.data();
+  mj_deleteModel(model);
+}
+
+}  // namespace
+}  // namespace mujoco