Skip to content

Commit 236a5e8

Browse files
saikishorsaikishor
committed
Update tests for joint axis
Signed-off-by: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>
1 parent 4fcdccb commit 236a5e8

1 file changed

Lines changed: 161 additions & 0 deletions

File tree

‎urdf_parser/test/urdf_schema_v1_0_legacy_test.cpp‎

Lines changed: 161 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1126,6 +1126,167 @@ TEST(URDF_V1_0_LEGACY, joint_without_type_attr_fails)
11261126
)urdf";
11271127
EXPECT_EQ(nullptr, urdf::parseURDF(urdf_str));
11281128
}
1129+
1130+
/// @note Joint axis
1131+
TEST(URDF_V1_0_LEGACY, joint_axis_default_is_1_0_0)
1132+
{
1133+
// When <axis> is absent, default is (1,0,0).
1134+
std::string urdf_str = R"urdf(
1135+
<robot name="r" version="1.0">
1136+
<link name="l1"/>
1137+
<link name="l2"/>
1138+
<joint name="j1" type="revolute">
1139+
<parent link="l1"/>
1140+
<child link="l2"/>
1141+
<limit lower="-1" upper="1" effort="10" velocity="1"/>
1142+
</joint>
1143+
</robot>
1144+
)urdf";
1145+
urdf::ModelInterfaceSharedPtr model = urdf::parseURDF(urdf_str);
1146+
ASSERT_NE(nullptr, model);
1147+
const auto & ax = model->getJoint("j1")->axis;
1148+
EXPECT_DOUBLE_EQ(1.0, ax.x);
1149+
EXPECT_DOUBLE_EQ(0.0, ax.y);
1150+
EXPECT_DOUBLE_EQ(0.0, ax.z);
1151+
}
1152+
1153+
TEST(URDF_V1_0_LEGACY, joint_axis_custom_value)
1154+
{
1155+
std::string urdf_str = R"urdf(
1156+
<robot name="r" version="1.0">
1157+
<link name="l1"/>
1158+
<link name="l2"/>
1159+
<joint name="j1" type="revolute">
1160+
<parent link="l1"/>
1161+
<child link="l2"/>
1162+
<axis xyz="0 1 0"/>
1163+
<limit lower="-1" upper="1" effort="10" velocity="1"/>
1164+
</joint>
1165+
</robot>
1166+
)urdf";
1167+
urdf::ModelInterfaceSharedPtr model = urdf::parseURDF(urdf_str);
1168+
ASSERT_NE(nullptr, model);
1169+
const auto & ax = model->getJoint("j1")->axis;
1170+
EXPECT_DOUBLE_EQ(0.0, ax.x);
1171+
EXPECT_DOUBLE_EQ(1.0, ax.y);
1172+
EXPECT_DOUBLE_EQ(0.0, ax.z);
1173+
}
1174+
1175+
TEST(URDF_V1_0_LEGACY, joint_axis_negative_components)
1176+
{
1177+
std::string urdf_str = R"urdf(
1178+
<robot name="r" version="1.0">
1179+
<link name="l1"/>
1180+
<link name="l2"/>
1181+
<joint name="j1" type="continuous">
1182+
<parent link="l1"/>
1183+
<child link="l2"/>
1184+
<axis xyz="0 0 -1"/>
1185+
</joint>
1186+
</robot>
1187+
)urdf";
1188+
urdf::ModelInterfaceSharedPtr model = urdf::parseURDF(urdf_str);
1189+
ASSERT_NE(nullptr, model);
1190+
const auto & ax = model->getJoint("j1")->axis;
1191+
EXPECT_DOUBLE_EQ( 0.0, ax.x);
1192+
EXPECT_DOUBLE_EQ( 0.0, ax.y);
1193+
EXPECT_DOUBLE_EQ(-1.0, ax.z);
1194+
}
1195+
1196+
TEST(URDF_V1_0_LEGACY, joint_axis_zero_vector_allowed_v1_0)
1197+
{
1198+
// v1.0 does not reject a zero-length axis — it is stored verbatim.
1199+
std::string urdf_str = R"urdf(
1200+
<robot name="r" version="1.0">
1201+
<link name="l1"/>
1202+
<link name="l2"/>
1203+
<joint name="j1" type="continuous">
1204+
<parent link="l1"/>
1205+
<child link="l2"/>
1206+
<axis xyz="0 0 0"/>
1207+
</joint>
1208+
</robot>
1209+
)urdf";
1210+
urdf::ModelInterfaceSharedPtr model = urdf::parseURDF(urdf_str);
1211+
ASSERT_NE(nullptr, model);
1212+
const auto & ax = model->getJoint("j1")->axis;
1213+
EXPECT_DOUBLE_EQ(0.0, ax.x);
1214+
EXPECT_DOUBLE_EQ(0.0, ax.y);
1215+
EXPECT_DOUBLE_EQ(0.0, ax.z);
1216+
}
1217+
1218+
TEST(URDF_V1_0_LEGACY, joint_axis_non_unit_length_allowed_v1_0)
1219+
{
1220+
// v1.0 does not normalise the axis — non-unit vectors are stored as-is.
1221+
std::string urdf_str = R"urdf(
1222+
<robot name="r" version="1.0">
1223+
<link name="l1"/>
1224+
<link name="l2"/>
1225+
<joint name="j1" type="revolute">
1226+
<parent link="l1"/>
1227+
<child link="l2"/>
1228+
<axis xyz="2 0 0"/>
1229+
<limit lower="-1" upper="1" effort="10" velocity="1"/>
1230+
</joint>
1231+
</robot>
1232+
)urdf";
1233+
urdf::ModelInterfaceSharedPtr model = urdf::parseURDF(urdf_str);
1234+
ASSERT_NE(nullptr, model);
1235+
const auto & ax = model->getJoint("j1")->axis;
1236+
EXPECT_DOUBLE_EQ(2.0, ax.x);
1237+
EXPECT_DOUBLE_EQ(0.0, ax.y);
1238+
EXPECT_DOUBLE_EQ(0.0, ax.z);
1239+
}
1240+
1241+
TEST(URDF_V1_0_LEGACY, joint_axis_non_principal_unit_vector_allowed)
1242+
{
1243+
// A unit vector not aligned with X/Y/Z is valid and stored verbatim.
1244+
// 1/sqrt(3) ≈ 0.577350269...
1245+
const double c = 1.0 / std::sqrt(3.0);
1246+
std::string urdf_str = R"urdf(
1247+
<robot name="r" version="1.0">
1248+
<link name="l1"/>
1249+
<link name="l2"/>
1250+
<joint name="j1" type="revolute">
1251+
<parent link="l1"/>
1252+
<child link="l2"/>
1253+
<axis xyz="0.57735026919 0.57735026919 0.57735026919"/>
1254+
<limit lower="-1" upper="1" effort="10" velocity="1"/>
1255+
</joint>
1256+
</robot>
1257+
)urdf";
1258+
urdf::ModelInterfaceSharedPtr model = urdf::parseURDF(urdf_str);
1259+
ASSERT_NE(nullptr, model);
1260+
const auto & ax = model->getJoint("j1")->axis;
1261+
EXPECT_NEAR(c, ax.x, 1e-8);
1262+
EXPECT_NEAR(c, ax.y, 1e-8);
1263+
EXPECT_NEAR(c, ax.z, 1e-8);
1264+
EXPECT_NEAR(1.0, std::sqrt(ax.x * ax.x + ax.y * ax.y + ax.z * ax.z), 1e-7);
1265+
}
1266+
1267+
TEST(URDF_V1_0_LEGACY, joint_axis_diagonal_unit_vector_xy_plane)
1268+
{
1269+
// 45-degree axis in the XY plane: (1/sqrt(2), 1/sqrt(2), 0).
1270+
const double c = 1.0 / std::sqrt(2.0);
1271+
std::string urdf_str = R"urdf(
1272+
<robot name="r" version="1.0">
1273+
<link name="l1"/>
1274+
<link name="l2"/>
1275+
<joint name="j1" type="continuous">
1276+
<parent link="l1"/>
1277+
<child link="l2"/>
1278+
<axis xyz="0.70710678118 0.70710678118 0"/>
1279+
</joint>
1280+
</robot>
1281+
)urdf";
1282+
urdf::ModelInterfaceSharedPtr model = urdf::parseURDF(urdf_str);
1283+
ASSERT_NE(nullptr, model);
1284+
const auto & ax = model->getJoint("j1")->axis;
1285+
EXPECT_NEAR(c, ax.x, 1e-8);
1286+
EXPECT_NEAR(c, ax.y, 1e-8);
1287+
EXPECT_NEAR(0.0, ax.z, 1e-8);
1288+
EXPECT_NEAR(1.0, std::sqrt(ax.x * ax.x + ax.y * ax.y + ax.z * ax.z), 1e-7);
1289+
}
11291290
int main(int argc, char **argv)
11301291
{
11311292
::testing::InitGoogleTest(&argc, argv);

0 commit comments

Comments
 (0)