chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 4 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
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