chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Support spherical joint in URDF.

Browse Source 
PiperOrigin-RevId: 642326650
Change-Id: Iec766800bc9ab7e022bd586fc5881df576d01331
This commit is contained in:
Baruch Tabanpour
2024-06-11 11:07:06 -07:00
committed by Copybara-Service
parent 07fc95ca9a
commit 5ed464b489
3 changed files with 107 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/changelog.rst
+4 -4
View File
@@ -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. 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<option-cone>`.
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)
---------------------------
src/xml/xml_urdf.cc
+11 -2
View File
@@ -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
+92
View File
@@ -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