From d1fc97fca95e37db157280b761cacf4bb62c77dc Mon Sep 17 00:00:00 2001 From: Alessandro Croci Date: Tue, 19 Mar 2024 10:25:19 +0100 Subject: [PATCH 1/2] Add unit test to check management of joint position limits for multiple joints from yarp configuration file --- tests/controlboard/ControlBoardCommonsTest.cc | 43 +++++ .../gazebo_controlboard_multiple_joints.ini | 25 +++ ...endulum_two_joints_single_controlboard.sdf | 181 ++++++++++++++++++ 3 files changed, 249 insertions(+) create mode 100644 tests/controlboard/conf/gazebo_controlboard_multiple_joints.ini create mode 100644 tests/controlboard/coupled_pendulum_two_joints_single_controlboard.sdf diff --git a/tests/controlboard/ControlBoardCommonsTest.cc b/tests/controlboard/ControlBoardCommonsTest.cc index 3942519..0f4f42f 100644 --- a/tests/controlboard/ControlBoardCommonsTest.cc +++ b/tests/controlboard/ControlBoardCommonsTest.cc @@ -10,6 +10,8 @@ #include #include +#include +#include #include #include #include @@ -72,5 +74,46 @@ TEST(ControlBoardCommonsTest, ConfigureMultipleControlBoards) ASSERT_TRUE(std::find(keys.begin(), keys.end(), cb2Key) != keys.end()); } +// Check that joint position limits are read correctly from yarp configuration +TEST(ControlBoardCommonsTest, JointPositionLimitsForMultipleJoints) +{ + std::string modelSdfName = "coupled_pendulum_two_joints_single_controlboard.sdf"; + std::string sdfPath = std::string("../../../tests/controlboard/") + modelSdfName; + std::string deviceScopedName = "model/coupled_pendulum/controlboard_plugin_device"; + yarp::dev::PolyDriver* driver; + yarp::dev::IControlLimits* iControlLimits; + + gz::sim::TestFixture testFixture{sdfPath}; + gz::common::Console::SetVerbosity(4); + + testFixture.Finalize(); + + driver = gzyarp::Handler::getHandler()->getDevice(deviceScopedName); + ASSERT_TRUE(driver != nullptr); + ASSERT_TRUE(driver->view(iControlLimits)); + + auto singleton = ControlBoardDataSingleton::getControlBoardHandler(); + auto keys = singleton->getControlBoardKeys(); + + std::cerr << "ControlBoard singleton keys vector size: " << keys.size() << std::endl; + ASSERT_EQ(keys.size(), 1); + + std::cerr << "ControlBoard ids: " << keys[0] << std::endl; + ASSERT_TRUE(std::find(keys.begin(), keys.end(), deviceScopedName) != keys.end()); + + auto controlBoardData = singleton->getControlBoardData(deviceScopedName); + + auto expectedJointMaxLimits = std::vector{200.0, 10.0}; + auto expectedJointMinLimits = std::vector{-200.0, -10.0}; + + for (int i = 0; i < expectedJointMaxLimits.size(); i++) + { + double maxLimit, minLimit; + iControlLimits->getLimits(i, &minLimit, &maxLimit); + EXPECT_DOUBLE_EQ(maxLimit, expectedJointMaxLimits[i]); + EXPECT_DOUBLE_EQ(minLimit, expectedJointMinLimits[i]); + } +} + } // namespace test } // namespace gzyarp diff --git a/tests/controlboard/conf/gazebo_controlboard_multiple_joints.ini b/tests/controlboard/conf/gazebo_controlboard_multiple_joints.ini new file mode 100644 index 0000000..46a5610 --- /dev/null +++ b/tests/controlboard/conf/gazebo_controlboard_multiple_joints.ini @@ -0,0 +1,25 @@ +yarpDeviceName controlboard_plugin_device +jointNames upper_joint lower_joint + +#PIDs: +# this information is used to set the PID values in simulation for GAZEBO, we need only the first three values +[POSITION_CONTROL] +controlUnits metric_units +controlLaw joint_pid_gazebo_v1 +kp 3000.0 3000.0 +kd 2.0 2.0 +ki 0.1 0.1 +maxInt 99999 99999 +maxOutput 99999 99999 +shift 0.0 0.0 +ko 0.0 0.0 +stictionUp 0.0 0.0 +stictionDwn 0.0 0.0 + +[GAZEBO_VELOCITY_PIDS] +#Torso +Pid0 500.0 2.0 0.1 9999 9999 9 9 + +[LIMITS] +jntPosMax 200.0 10.0 +jntPosMin -200.0 -10.0 diff --git a/tests/controlboard/coupled_pendulum_two_joints_single_controlboard.sdf b/tests/controlboard/coupled_pendulum_two_joints_single_controlboard.sdf new file mode 100644 index 0000000..fa155d4 --- /dev/null +++ b/tests/controlboard/coupled_pendulum_two_joints_single_controlboard.sdf @@ -0,0 +1,181 @@ + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + + 0 0 0 0 0 0 + 1000 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + 0 0 1 0 0 0 + + + 0.14999999999999999 0.14999999999999999 2.1499999999999999 + + + + + 0 0 1 0 0 0 + + + 0.14999999999999999 0.14999999999999999 2.1499999999999999 + + + + + + 0.14999999999999999 0 1.75 3.1415000000000002 0 0 + base_link + link2 + + 1 0 0 + + -5 + 5 + 100 + 100 + + + 0 + 0 + + + + + 0 0 0 0 0 0 + + 0 0 -0.90000000000000002 -0.30000000000000004 0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + 0 0 -0.5 0 0 0 + + + 0.14999999999999999 0.14999999999999999 1 + + + + + + -0.14999999999999999 0 1.75 3.1415000000000002 0 0 + base_link + link1 + + 1 0 0 + + -5 + 5 + 100 + 100 + + + 0 + 0 + + + + + 0 0 0 0 0 0 + + 0 0 -0.90000000000000002 0 0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + 0 0 -0.5 0 0 0 + + + 0.14999999999999999 0.14999999999999999 1 + + + + + + + + ../../../tests/controlboard/conf/gazebo_controlboard_multiple_joints.ini + + 0.0 + + + + + 0 0 0 0 0 0 + + + 0 0 0 0 0 0 + + + + + From 24b2f7ff48a39ffc2728cb7603c3e99342917f93 Mon Sep 17 00:00:00 2001 From: Alessandro Croci Date: Tue, 19 Mar 2024 10:26:33 +0100 Subject: [PATCH 2/2] Fix handling of joint poisiton limits for controlboard with multiple joints --- plugins/controlboard/src/ControlBoard.cpp | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/plugins/controlboard/src/ControlBoard.cpp b/plugins/controlboard/src/ControlBoard.cpp index 6900e01..9ae7e73 100644 --- a/plugins/controlboard/src/ControlBoard.cpp +++ b/plugins/controlboard/src/ControlBoard.cpp @@ -646,18 +646,20 @@ bool ControlBoard::initializeJointPositionLimits(const gz::sim::EntityComponentM size_t numberOfJoints = m_controlBoardData.joints.size(); Bottle limitMinGroup, limitMaxGroup; - for (auto& joint : m_controlBoardData.joints) + if (!(tryGetGroup(limitsGroup, limitMinGroup, "jntPosMin", "", numberOfJoints + 1) + && tryGetGroup(limitsGroup, limitMaxGroup, "jntPosMax", "", numberOfJoints + 1))) + { + yError() << "Error while reading joint position limits from plugin configuration"; + return false; + } + + for (size_t i = 0; i < numberOfJoints; ++i) { // TODO: access gazebo joint position limits and use them to check if software limits // ([LIMITS] group) are consistent. In case they are not defined set them as sw limits. - if (!(tryGetGroup(limitsGroup, limitMinGroup, "jntPosMin", "", numberOfJoints + 1) - && tryGetGroup(limitsGroup, limitMaxGroup, "jntPosMax", "", numberOfJoints + 1))) - { - yError() << "Error while reading joint position limits from plugin parameters"; - return false; - } - joint.positionLimitMin = limitMinGroup.get(1).asFloat64(); - joint.positionLimitMax = limitMaxGroup.get(1).asFloat64(); + auto& joint = m_controlBoardData.joints[i]; + joint.positionLimitMin = limitMinGroup.get(i + 1).asFloat64(); + joint.positionLimitMax = limitMaxGroup.get(i + 1).asFloat64(); } return true;