Skip to content

Commit 818a382

Browse files
saikishorsaikishor
committed
Add tests for joint limits
1 parent 236a5e8 commit 818a382

1 file changed

Lines changed: 284 additions & 0 deletions

File tree

‎urdf_parser/test/urdf_schema_v1_0_legacy_test.cpp‎

Lines changed: 284 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1287,6 +1287,290 @@ TEST(URDF_V1_0_LEGACY, joint_axis_diagonal_unit_vector_xy_plane)
12871287
EXPECT_NEAR(0.0, ax.z, 1e-8);
12881288
EXPECT_NEAR(1.0, std::sqrt(ax.x * ax.x + ax.y * ax.y + ax.z * ax.z), 1e-7);
12891289
}
1290+
1291+
/// @note Joint limits
1292+
TEST(URDF_V1_0_LEGACY, limits_negative_effort_allowed_v1_0)
1293+
{
1294+
// v1.0 does not reject negative effort.
1295+
std::string urdf_str = R"urdf(
1296+
<robot name="r" version="1.0">
1297+
<link name="l1"/>
1298+
<link name="l2"/>
1299+
<joint name="j1" type="fixed">
1300+
<parent link="l1"/>
1301+
<child link="l2"/>
1302+
<limit effort="-5.0" velocity="1.0"/>
1303+
</joint>
1304+
</robot>
1305+
)urdf";
1306+
urdf::ModelInterfaceSharedPtr model = urdf::parseURDF(urdf_str);
1307+
ASSERT_NE(nullptr, model);
1308+
EXPECT_DOUBLE_EQ(-5.0, model->getJoint("j1")->limits->effort);
1309+
}
1310+
1311+
TEST(URDF_V1_0_LEGACY, limits_negative_velocity_allowed_v1_0)
1312+
{
1313+
std::string urdf_str = R"urdf(
1314+
<robot name="r" version="1.0">
1315+
<link name="l1"/>
1316+
<link name="l2"/>
1317+
<joint name="j1" type="fixed">
1318+
<parent link="l1"/>
1319+
<child link="l2"/>
1320+
<limit effort="10.0" velocity="-3.0"/>
1321+
</joint>
1322+
</robot>
1323+
)urdf";
1324+
urdf::ModelInterfaceSharedPtr model = urdf::parseURDF(urdf_str);
1325+
ASSERT_NE(nullptr, model);
1326+
EXPECT_DOUBLE_EQ(-3.0, model->getJoint("j1")->limits->velocity);
1327+
}
1328+
1329+
TEST(URDF_V1_0_LEGACY, limits_zero_effort_and_velocity_allowed_v1_0)
1330+
{
1331+
std::string urdf_str = R"urdf(
1332+
<robot name="r" version="1.0">
1333+
<link name="l1"/>
1334+
<link name="l2"/>
1335+
<joint name="j1" type="fixed">
1336+
<parent link="l1"/>
1337+
<child link="l2"/>
1338+
<limit effort="0.0" velocity="0.0"/>
1339+
</joint>
1340+
</robot>
1341+
)urdf";
1342+
urdf::ModelInterfaceSharedPtr model = urdf::parseURDF(urdf_str);
1343+
ASSERT_NE(nullptr, model);
1344+
EXPECT_DOUBLE_EQ(0.0, model->getJoint("j1")->limits->effort);
1345+
EXPECT_DOUBLE_EQ(0.0, model->getJoint("j1")->limits->velocity);
1346+
}
1347+
1348+
TEST(URDF_V1_0_LEGACY, limits_missing_effort_fails_v1_0)
1349+
{
1350+
std::string urdf_str = R"urdf(
1351+
<robot name="r" version="1.0">
1352+
<link name="l1"/>
1353+
<link name="l2"/>
1354+
<joint name="j1" type="fixed">
1355+
<parent link="l1"/>
1356+
<child link="l2"/>
1357+
<limit velocity="1.0"/>
1358+
</joint>
1359+
</robot>
1360+
)urdf";
1361+
EXPECT_EQ(nullptr, urdf::parseURDF(urdf_str));
1362+
}
1363+
1364+
TEST(URDF_V1_0_LEGACY, limits_missing_velocity_fails_v1_0)
1365+
{
1366+
std::string urdf_str = R"urdf(
1367+
<robot name="r" version="1.0">
1368+
<link name="l1"/>
1369+
<link name="l2"/>
1370+
<joint name="j1" type="fixed">
1371+
<parent link="l1"/>
1372+
<child link="l2"/>
1373+
<limit effort="10.0"/>
1374+
</joint>
1375+
</robot>
1376+
)urdf";
1377+
EXPECT_EQ(nullptr, urdf::parseURDF(urdf_str));
1378+
}
1379+
1380+
TEST(URDF_V1_0_LEGACY, limits_large_positive_values_v1_0)
1381+
{
1382+
std::string urdf_str = R"urdf(
1383+
<robot name="r" version="1.0">
1384+
<link name="l1"/>
1385+
<link name="l2"/>
1386+
<joint name="j1" type="revolute">
1387+
<parent link="l1"/>
1388+
<child link="l2"/>
1389+
<limit lower="-3.14159" upper="3.14159"
1390+
effort="9999.9" velocity="100.0"/>
1391+
</joint>
1392+
</robot>
1393+
)urdf";
1394+
urdf::ModelInterfaceSharedPtr model = urdf::parseURDF(urdf_str);
1395+
ASSERT_NE(nullptr, model);
1396+
EXPECT_NEAR(-3.14159, model->getJoint("j1")->limits->lower, 1e-5);
1397+
EXPECT_NEAR( 3.14159, model->getJoint("j1")->limits->upper, 1e-5);
1398+
EXPECT_NEAR( 9999.9, model->getJoint("j1")->limits->effort, 1e-5);
1399+
EXPECT_NEAR( 100.0, model->getJoint("j1")->limits->velocity, 1e-9);
1400+
}
1401+
1402+
TEST(URDF_V1_0_LEGACY, limits_acceleration_deceleration_jerk_default_to_inf_v1_0)
1403+
{
1404+
// These three fields are not a v1.0 concept — they always default to +inf.
1405+
std::string urdf_str = R"urdf(
1406+
<robot name="r" version="1.0">
1407+
<link name="l1"/>
1408+
<link name="l2"/>
1409+
<joint name="j1" type="fixed">
1410+
<parent link="l1"/>
1411+
<child link="l2"/>
1412+
<limit effort="10.0" velocity="1.0"/>
1413+
</joint>
1414+
</robot>
1415+
)urdf";
1416+
urdf::ModelInterfaceSharedPtr model = urdf::parseURDF(urdf_str);
1417+
ASSERT_NE(nullptr, model);
1418+
EXPECT_TRUE(std::isinf(model->getJoint("j1")->limits->acceleration));
1419+
EXPECT_TRUE(std::isinf(model->getJoint("j1")->limits->deceleration));
1420+
EXPECT_TRUE(std::isinf(model->getJoint("j1")->limits->jerk));
1421+
}
1422+
1423+
TEST(URDF_V1_0_LEGACY, limits_invalid_effort_string_fails_v1_0)
1424+
{
1425+
std::string urdf_str = R"urdf(
1426+
<robot name="r" version="1.0">
1427+
<link name="l1"/>
1428+
<link name="l2"/>
1429+
<joint name="j1" type="fixed">
1430+
<parent link="l1"/>
1431+
<child link="l2"/>
1432+
<limit effort="not_a_number" velocity="1.0"/>
1433+
</joint>
1434+
</robot>
1435+
)urdf";
1436+
EXPECT_EQ(nullptr, urdf::parseURDF(urdf_str));
1437+
}
1438+
1439+
TEST(URDF_V1_0_LEGACY, limits_invalid_velocity_string_fails_v1_0)
1440+
{
1441+
std::string urdf_str = R"urdf(
1442+
<robot name="r" version="1.0">
1443+
<link name="l1"/>
1444+
<link name="l2"/>
1445+
<joint name="j1" type="fixed">
1446+
<parent link="l1"/>
1447+
<child link="l2"/>
1448+
<limit effort="10.0" velocity="fast"/>
1449+
</joint>
1450+
</robot>
1451+
)urdf";
1452+
EXPECT_EQ(nullptr, urdf::parseURDF(urdf_str));
1453+
}
1454+
1455+
TEST(URDF_V1_0_LEGACY, revolute_joint_limits_lower_greater_than_upper_allowed_v1_0)
1456+
{
1457+
// In v1.0 the lower > upper check is not performed — parsed without error.
1458+
std::string urdf_str = R"urdf(
1459+
<robot name="r" version="1.0">
1460+
<link name="l1"/>
1461+
<link name="l2"/>
1462+
<joint name="j1" type="revolute">
1463+
<parent link="l1"/>
1464+
<child link="l2"/>
1465+
<limit lower="1.57" upper="-1.57" effort="10.0" velocity="1.0"/>
1466+
</joint>
1467+
</robot>
1468+
)urdf";
1469+
urdf::ModelInterfaceSharedPtr model = urdf::parseURDF(urdf_str);
1470+
ASSERT_NE(nullptr, model);
1471+
ASSERT_NE(nullptr, model->getJoint("j1")->limits);
1472+
EXPECT_DOUBLE_EQ( 1.57, model->getJoint("j1")->limits->lower);
1473+
EXPECT_DOUBLE_EQ(-1.57, model->getJoint("j1")->limits->upper);
1474+
}
1475+
1476+
TEST(URDF_V1_0_LEGACY, prismatic_joint_limits_lower_greater_than_upper_allowed_v1_0)
1477+
{
1478+
std::string urdf_str = R"urdf(
1479+
<robot name="r" version="1.0">
1480+
<link name="l1"/>
1481+
<link name="l2"/>
1482+
<joint name="j1" type="prismatic">
1483+
<parent link="l1"/>
1484+
<child link="l2"/>
1485+
<limit lower="0.5" upper="-0.5" effort="50.0" velocity="0.5"/>
1486+
</joint>
1487+
</robot>
1488+
)urdf";
1489+
urdf::ModelInterfaceSharedPtr model = urdf::parseURDF(urdf_str);
1490+
ASSERT_NE(nullptr, model);
1491+
ASSERT_NE(nullptr, model->getJoint("j1")->limits);
1492+
EXPECT_DOUBLE_EQ( 0.5, model->getJoint("j1")->limits->lower);
1493+
EXPECT_DOUBLE_EQ(-0.5, model->getJoint("j1")->limits->upper);
1494+
}
1495+
1496+
TEST(URDF_V1_0_LEGACY, revolute_joint_negative_effort_allowed_v1_0)
1497+
{
1498+
// v1.0 does not validate the sign of effort on revolute joints.
1499+
std::string urdf_str = R"urdf(
1500+
<robot name="r" version="1.0">
1501+
<link name="l1"/>
1502+
<link name="l2"/>
1503+
<joint name="j1" type="revolute">
1504+
<parent link="l1"/>
1505+
<child link="l2"/>
1506+
<limit lower="-1.57" upper="1.57" effort="-10.0" velocity="1.0"/>
1507+
</joint>
1508+
</robot>
1509+
)urdf";
1510+
urdf::ModelInterfaceSharedPtr model = urdf::parseURDF(urdf_str);
1511+
ASSERT_NE(nullptr, model);
1512+
ASSERT_NE(nullptr, model->getJoint("j1")->limits);
1513+
EXPECT_DOUBLE_EQ(-10.0, model->getJoint("j1")->limits->effort);
1514+
}
1515+
1516+
TEST(URDF_V1_0_LEGACY, revolute_joint_negative_velocity_allowed_v1_0)
1517+
{
1518+
std::string urdf_str = R"urdf(
1519+
<robot name="r" version="1.0">
1520+
<link name="l1"/>
1521+
<link name="l2"/>
1522+
<joint name="j1" type="revolute">
1523+
<parent link="l1"/>
1524+
<child link="l2"/>
1525+
<limit lower="-1.57" upper="1.57" effort="10.0" velocity="-2.0"/>
1526+
</joint>
1527+
</robot>
1528+
)urdf";
1529+
urdf::ModelInterfaceSharedPtr model = urdf::parseURDF(urdf_str);
1530+
ASSERT_NE(nullptr, model);
1531+
ASSERT_NE(nullptr, model->getJoint("j1")->limits);
1532+
EXPECT_DOUBLE_EQ(-2.0, model->getJoint("j1")->limits->velocity);
1533+
}
1534+
1535+
TEST(URDF_V1_0_LEGACY, prismatic_joint_negative_effort_and_velocity_allowed_v1_0)
1536+
{
1537+
std::string urdf_str = R"urdf(
1538+
<robot name="r" version="1.0">
1539+
<link name="l1"/>
1540+
<link name="l2"/>
1541+
<joint name="j1" type="prismatic">
1542+
<parent link="l1"/>
1543+
<child link="l2"/>
1544+
<limit lower="-0.5" upper="0.5" effort="-50.0" velocity="-0.5"/>
1545+
</joint>
1546+
</robot>
1547+
)urdf";
1548+
urdf::ModelInterfaceSharedPtr model = urdf::parseURDF(urdf_str);
1549+
ASSERT_NE(nullptr, model);
1550+
ASSERT_NE(nullptr, model->getJoint("j1")->limits);
1551+
EXPECT_DOUBLE_EQ(-50.0, model->getJoint("j1")->limits->effort);
1552+
EXPECT_DOUBLE_EQ( -0.5, model->getJoint("j1")->limits->velocity);
1553+
}
1554+
1555+
TEST(URDF_V1_0_LEGACY, revolute_joint_zero_effort_and_velocity_allowed_v1_0)
1556+
{
1557+
std::string urdf_str = R"urdf(
1558+
<robot name="r" version="1.0">
1559+
<link name="l1"/>
1560+
<link name="l2"/>
1561+
<joint name="j1" type="revolute">
1562+
<parent link="l1"/>
1563+
<child link="l2"/>
1564+
<limit lower="-1.0" upper="1.0" effort="0.0" velocity="0.0"/>
1565+
</joint>
1566+
</robot>
1567+
)urdf";
1568+
urdf::ModelInterfaceSharedPtr model = urdf::parseURDF(urdf_str);
1569+
ASSERT_NE(nullptr, model);
1570+
ASSERT_NE(nullptr, model->getJoint("j1")->limits);
1571+
EXPECT_DOUBLE_EQ(0.0, model->getJoint("j1")->limits->effort);
1572+
EXPECT_DOUBLE_EQ(0.0, model->getJoint("j1")->limits->velocity);
1573+
}
12901574
int main(int argc, char **argv)
12911575
{
12921576
::testing::InitGoogleTest(&argc, argv);

0 commit comments

Comments
 (0)