Support spherical joint in URDF.

PiperOrigin-RevId: 642326650
Change-Id: Iec766800bc9ab7e022bd586fc5881df576d01331

doc/changelog.rst

@@ -21,13 +21,13 @@ General
    :ref:`camera/orthographic<body-camera-orthographic>` and :ref:`global/orthographic<visual-global-orthographic>`
    attributes, respectively.
 3. Added :ref:`maxhullvert<asset-mesh-maxhullvert>`, the maximum number of vertices in a mesh's convex hull.
-
+4. Add support for ``ball`` joints in the URDF parser.
 
 MJX
 ~~~
-4. Added support for :ref:`elliptic friction cones<option-cone>`.
+5. Added support for :ref:`elliptic friction cones<option-cone>`.
-5. Fixed a bug that resulted in less-optimal linesearch solutions for some difficult constraint settings.
+6. Fixed a bug that resulted in less-optimal linesearch solutions for some difficult constraint settings.
-6. Fixed a bug in the Newton solver that sometimes resulted in less-optimal gradients.
+7. Fixed a bug in the Newton solver that sometimes resulted in less-optimal gradients.
 
 Version 3.1.6 (Jun 3, 2024)
 ---------------------------

src/xml/xml_urdf.cc

@@ -32,14 +32,15 @@
 using tinyxml2::XMLElement;
 
 // URDF joint type
-static const int urJoint_sz = 6;
+static const int urJoint_sz = 7;
 static const mjMap urJoint_map[urJoint_sz] = {
   {"revolute",    0},
   {"continuous",  1},
   {"prismatic",   2},
   {"fixed",       3},
   {"floating",    4},
-  {"planar",      5}
+  {"planar",      5},
+  {"spherical",   6}  // Bullet physics supports ball joints (non-standard URDF)
 };
 
 
@@ -474,6 +475,14 @@ void mjXURDF::Joint(XMLElement* joint_elem) {
     pjoint2->type = mjJNT_HINGE;
     mjuu_setvec(pjoint2->pos, 0, 0, 0);
     mjuu_copyvec(pjoint2->axis, axis, 3);
+    break;
+
+  case 6:  // ball joint
+    pjoint = mjs_addJoint(pbody, 0);
+    mjs_setString(pjoint->name, jntname.c_str());
+    pjoint->type = mjJNT_BALL;
+    mjuu_setvec(pjoint->pos, 0, 0, 0);
+    mjuu_copyvec(pjoint->axis, axis, 3);
   }
 
   // dynamics element

test/xml/xml_urdf_test.cc

@@ -14,9 +14,11 @@
 
 // Tests for xml/xml_api.cc.
 
+#include <array>
 #include <cstddef>
 #include <cstring>
 #include <string>
+#include <vector>
 
 #include <gmock/gmock.h>
 #include <gtest/gtest.h>
@@ -160,5 +162,95 @@ TEST_F(MujocoTest, CanLoadUrdfWithNonUniqueNamesVisualBeforeCollision) {
   mj_deleteModel(model);
 }
 
+TEST_F(MujocoTest, ReadsJointTypes) {
+  static constexpr char urdf[] = R"(
+  <robot name="">
+  <mujoco>
+    <compiler discardvisual="false"/>
+  </mujoco>
+
+  <link name="world"/>
+  <joint type="floating" name="floating">
+      <parent link="world"/>
+      <child link="link1"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+  </joint>
+  <link name="link1">
+      <collision>
+        <geometry>
+          <box size="0.1 0.2 0.3"/>
+        </geometry>
+      </collision>
+  </link>
+
+  <joint type="revolute" name="revolute">
+      <parent link="world"/>
+      <child link="link2"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <axis xyz="1.0 0.0 -1.0"/>
+  </joint>
+  <link name="link2">
+      <collision>
+        <geometry>
+          <box size="0.1 0.2 0.3"/>
+        </geometry>
+      </collision>
+  </link>
+
+  <joint type="spherical" name="spherical">
+      <parent link="world"/>
+      <child link="link3"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+  </joint>
+  <link name="link3">
+      <collision>
+        <geometry>
+          <box size="0.1 0.2 0.3"/>
+        </geometry>
+      </collision>
+  </link>
+
+  <joint type="prismatic" name="prismatic">
+      <parent link="world"/>
+      <child link="link4"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <axis xyz="0.0 0.0 1.0"/>
+  </joint>
+  <link name="link4">
+      <collision>
+        <geometry>
+          <box size="0.1 0.2 0.3"/>
+        </geometry>
+      </collision>
+  </link>
+
+  </robot>
+  )";
+  std::array<char, 1000> error;
+  mjModel* model = LoadModelFromString(urdf, error.data(), error.size());
+  ASSERT_THAT(model, NotNull()) << error.data();
+
+  constexpr float eps = 1e-6;
+
+  std::vector<std::string> joint_names = {"floating", "revolute", "spherical",
+                                          "prismatic"};
+  std::vector<mjtJoint> expected_joint_types = {
+      mjtJoint::mjJNT_FREE, mjtJoint::mjJNT_HINGE, mjtJoint::mjJNT_BALL,
+      mjtJoint::mjJNT_SLIDE};
+  std::vector<std::vector<float>> expected_axis = {
+    {0.0, 0.0, 1.0}, {0.707107, 0.0, -0.707107}, {0.0, 0.0, 1.0},
+    {0.0, 0.0, 1.0}
+  };
+  for (int i = 0; i < joint_names.size(); ++i) {
+    int id = mj_name2id(model, mjtObj::mjOBJ_JOINT, joint_names[i].c_str());
+    EXPECT_EQ(model->jnt_type[id], expected_joint_types[i]);
+    EXPECT_NEAR(model->jnt_axis[3 * id], expected_axis[i][0], eps);
+    EXPECT_NEAR(model->jnt_axis[3 * id + 1], expected_axis[i][1], eps);
+    EXPECT_NEAR(model->jnt_axis[3 * id + 2], expected_axis[i][2], eps);
+  }
+
+  mj_deleteModel(model);
+}
+
 }  // namespace
 }  // namespace mujoco