From 5ed464b489cd74dc6bcc0f7a8d08ee56b64a3f67 Mon Sep 17 00:00:00 2001 From: Baruch Tabanpour Date: Tue, 11 Jun 2024 11:07:06 -0700 Subject: [PATCH] Support spherical joint in URDF. PiperOrigin-RevId: 642326650 Change-Id: Iec766800bc9ab7e022bd586fc5881df576d01331 --- doc/changelog.rst | 8 ++-- src/xml/xml_urdf.cc | 13 +++++- test/xml/xml_urdf_test.cc | 92 +++++++++++++++++++++++++++++++++++++++ 3 files changed, 107 insertions(+), 6 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index f27194b8..72025f13 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -21,13 +21,13 @@ General :ref:`camera/orthographic` and :ref:`global/orthographic` attributes, respectively. 3. Added :ref:`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`. -5. 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. +5. Added support for :ref:`elliptic friction cones`. +6. Fixed a bug that resulted in less-optimal linesearch solutions for some difficult constraint settings. +7. Fixed a bug in the Newton solver that sometimes resulted in less-optimal gradients. Version 3.1.6 (Jun 3, 2024) --------------------------- diff --git a/src/xml/xml_urdf.cc b/src/xml/xml_urdf.cc index cf3b841d..e7d27d88 100644 --- a/src/xml/xml_urdf.cc +++ b/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 diff --git a/test/xml/xml_urdf_test.cc b/test/xml/xml_urdf_test.cc index 3d798a54..50a45da0 100644 --- a/test/xml/xml_urdf_test.cc +++ b/test/xml/xml_urdf_test.cc @@ -14,9 +14,11 @@ // Tests for xml/xml_api.cc. +#include #include #include #include +#include #include #include @@ -160,5 +162,95 @@ TEST_F(MujocoTest, CanLoadUrdfWithNonUniqueNamesVisualBeforeCollision) { mj_deleteModel(model); } +TEST_F(MujocoTest, ReadsJointTypes) { + static constexpr char urdf[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(urdf, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + + constexpr float eps = 1e-6; + + std::vector joint_names = {"floating", "revolute", "spherical", + "prismatic"}; + std::vector expected_joint_types = { + mjtJoint::mjJNT_FREE, mjtJoint::mjJNT_HINGE, mjtJoint::mjJNT_BALL, + mjtJoint::mjJNT_SLIDE}; + std::vector> 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