Merge pull request #265 from robotology/fix/getRefPosition

Nicogene · web-flow · commit 72df0137b5fd · 2025-07-01T16:44:54.000+02:00
ControlBoardPositionDirectControlTest: add check of getRefPositions
diff --git a/tests/controlboard/CMakeLists.txt b/tests/controlboard/CMakeLists.txt
@@ -9,7 +9,9 @@ set(TESTS
 
 foreach(TEST ${TESTS})
   add_executable(${TEST} ${TEST}.cc)
-
+  if (GZ_SIM_YARP_PLUGINS_ENABLE_TESTS_WITH_ICUB_MAIN)
+    target_compile_definitions(${TEST} PRIVATE GZ_SIM_YARP_PLUGINS_ENABLE_TESTS_WITH_ICUB_MAIN)
+  endif()
   target_link_libraries(${TEST}
     PRIVATE GTest::gtest_main
             test-helpers
diff --git a/tests/controlboard/ControlBoardPositionControlTest.cc b/tests/controlboard/ControlBoardPositionControlTest.cc
@@ -254,13 +254,16 @@ TEST_F(ControlBoardPositionFixture, CheckPositionTrackingWithTrajectoryGeneratio
     ASSERT_LT(jointPosError, acceptedTolerance);
 }
 
+#if defined GZ_SIM_YARP_PLUGINS_ENABLE_TESTS_WITH_ICUB_MAIN
+
 TEST_F(ControlBoardPositionCoupledPendulumFixture, CheckPositionTrackingWithTrajectoryGenerationUsingPendulumModel)
 {
     auto refPosition{90.0};
     bool motionDone0{false};
     bool motionDone1{false};
-    yarp::sig::Vector jointPosition{0.0, 0.0};
-    yarp::sig::Vector jointPosError{0.0, 0.0};
+    bool motionDone2{false};
+    yarp::sig::Vector jointPosition{0.0, 0.0, 0.0};
+    yarp::sig::Vector jointPosError{0.0, 0.0, 0.0};
 
     testFixture
         .OnPostUpdate(
@@ -270,6 +273,7 @@ TEST_F(ControlBoardPositionCoupledPendulumFixture, CheckPositionTrackingWithTraj
                 iEncoders->getEncoders(jointPosition.data());
                 iPositionControl->checkMotionDone(0, &motionDone0);
                 iPositionControl->checkMotionDone(1, &motionDone1);
+                iPositionControl->checkMotionDone(2, &motionDone2);
 
                 // std::cerr << "ref position: " << refTrajectory[iterations] << std::endl;
                 // std::cerr << "joint position: " << jointPosition << std::endl;
@@ -282,45 +286,52 @@ TEST_F(ControlBoardPositionCoupledPendulumFixture, CheckPositionTrackingWithTraj
 
     int modeSet0{};
     int modeSet1{};
+    int modeSet2{};
     iControlMode->getControlMode(0, &modeSet0);
     iControlMode->getControlMode(1, &modeSet1);
+    iControlMode->getControlMode(2, &modeSet2);
     ASSERT_TRUE(modeSet0 == VOCAB_CM_POSITION);
     ASSERT_TRUE(modeSet1 == VOCAB_CM_POSITION);
+    ASSERT_TRUE(modeSet2 == VOCAB_CM_POSITION);
 
     // Set reference position
-    iPositionControl->positionMove(0, refPosition);
+    iPositionControl->positionMove(1, refPosition);
 
     // Setup simulation server, this will call the post-update callbacks.
     // It also calls pre-update and update callbacks if those are being used.
-    while (!motionDone0)
+    while (!motionDone1)
     {
         std::cerr << "Running server" << std::endl;
         testFixture.Server()->Run(true, plannedIterations, false);
-        jointPosError[0] = abs(refPosition - jointPosition[0]);
-        std::cerr << "Joint 0 position error: " << jointPosError[0] << std::endl;
+        jointPosError[1] = abs(refPosition - jointPosition[1]);
+        std::cerr << "Joint 0 position error: " << jointPosError[1] << std::endl;
     }
 
     std::cerr << "Final tracking error for joint 0: " << jointPosError[0] << std::endl;
     ASSERT_LT(jointPosError[0], acceptedTolerance);
 
-    iPositionControl->positionMove(1, refPosition);
-    while (!motionDone1)
+    iPositionControl->positionMove(2, refPosition);
+    while (!motionDone2)
     {
         std::cerr << "Running server" << std::endl;
         testFixture.Server()->Run(true, plannedIterations, false);
-        jointPosError[1] = abs(refPosition - jointPosition[1]);
-        std::cerr << "Joint 1 position error: " << jointPosError[1] << std::endl;
+        jointPosError[1] = abs(refPosition - jointPosition[2]);
+        std::cerr << "Joint 1 position error: " << jointPosError[2] << std::endl;
     }
 
     // Final assertions
     ASSERT_TRUE(configured);
     ASSERT_TRUE(motionDone0);
     ASSERT_TRUE(motionDone1);
+    ASSERT_TRUE(motionDone2);
 
     // Verify that the final error is within the accepted tolerance
     std::cerr << "Final tracking error for joint 1: " << jointPosError[1] << std::endl;
     ASSERT_LT(jointPosError[1], acceptedTolerance);
+    std::cerr << "Final tracking error for joint 2: " << jointPosError[2] << std::endl;
+    ASSERT_LT(jointPosError[2], acceptedTolerance);
 }
+#endif // GZ_SIM_YARP_PLUGINS_ENABLE_TESTS_WITH_ICUB_MAIN
 
 } // namespace test
 } // namespace gzyarp
diff --git a/tests/controlboard/ControlBoardPositionDirectControlTest.cc b/tests/controlboard/ControlBoardPositionDirectControlTest.cc
@@ -127,15 +127,13 @@ class ControlBoardPositionDirectCoupledPendulumFixture : public ::testing::Test
 protected:
     // void SetUp() override
     ControlBoardPositionDirectCoupledPendulumFixture()
-        : testFixture{(std::filesystem::path(CMAKE_CURRENT_SOURCE_DIR)
-                       / "coupled_pendulum_two_joints_coupled.sdf")
-                          .string()}
     {
+        yarp::os::NetworkBase::setLocalMode(true);
+        testFixture = std::make_unique<gz::sim::TestFixture>(
+            (std::filesystem::path(CMAKE_CURRENT_SOURCE_DIR) / "coupled_pendulum_two_joints_coupled.sdf").string());
         gz::common::Console::SetVerbosity(4);
-
-        testFixture.
-            // Use configure callback to get values at startup
-            OnConfigure([&](const gz::sim::Entity& _worldEntity,
+        // Use configure callback to get values at startup
+        testFixture->OnConfigure([&](const gz::sim::Entity& _worldEntity,
                             const std::shared_ptr<const sdf::Element>& /*_sdf*/,
                             gz::sim::EntityComponentManager& _ecm,
                             gz::sim::EventManager& /*_eventMgr*/) {
@@ -162,6 +160,25 @@ class ControlBoardPositionDirectCoupledPendulumFixture : public ::testing::Test
                 iEncoders = nullptr;
                 ASSERT_TRUE(driver->view(iEncoders));
 
+                yarp::os::Property prop;
+                prop.put("device", "remotecontrolboardremapper");
+
+                prop.addGroup("axesNames");
+                auto& botAxesList = prop.findGroup("axesNames").addList();
+                botAxesList.addString("fixed_base");
+                botAxesList.addString("upper_joint");
+                botAxesList.addString("lower_joint");
+
+                prop.addGroup("remoteControlBoards");
+                auto& botRCBList = prop.findGroup("remoteControlBoards").addList();
+                botRCBList.addString("/coupledPendulumGazebo/body");
+
+                prop.put("localPortPrefix","/gz-sim-posdir-test");
+
+                ASSERT_TRUE(remote_controlboard_remapper_driver.open(prop));
+                ASSERT_TRUE(remote_controlboard_remapper_driver.view(posDirect_remapper));
+                ASSERT_TRUE(posDirect_remapper != nullptr);
+
                 // Get joint1
                 auto jointEntity0 = model.JointByName(_ecm, "upper_joint");
                 EXPECT_NE(gz::sim::kNullEntity, jointEntity0);
@@ -175,6 +192,7 @@ class ControlBoardPositionDirectCoupledPendulumFixture : public ::testing::Test
                 // Set joint in torque control mode
                 ASSERT_TRUE(iControlMode->setControlMode(0, VOCAB_CM_POSITION_DIRECT));
                 ASSERT_TRUE(iControlMode->setControlMode(1, VOCAB_CM_POSITION_DIRECT));
+                ASSERT_TRUE(iControlMode->setControlMode(2, VOCAB_CM_POSITION_DIRECT));
 
                 // Print number of joint configured
                 int nJointsConfigured{};
@@ -187,21 +205,23 @@ class ControlBoardPositionDirectCoupledPendulumFixture : public ::testing::Test
     }
 
     // Get SDF model name from test parameter
-    gz::sim::TestFixture testFixture;
+    std::unique_ptr<gz::sim::TestFixture> testFixture;
     double linkMass{1};
     double linkLength{1.0};
     double linkInertiaAtLinkEnd{0.3352}; // Computed with parallel axis theorem
     int plannedIterations{5000};
     int iterations{0};
     std::vector<std::vector<double>> trackingErrors{2};
-    double acceptedTolerance{5e-2};
+    double acceptedTolerance{5e-1};
     bool configured{false};
     gz::math::Vector3d gravity;
     gz::sim::Entity modelEntity;
     gz::sim::Model model;
     gz::sim::Joint joint0;
     gz::sim::Joint joint1;
     yarp::dev::PolyDriver* driver;
+    yarp::dev::PolyDriver remote_controlboard_remapper_driver;
+    yarp::dev::IPositionDirect* posDirect_remapper = nullptr;
     yarp::dev::IPositionDirect* iPositionDirectControl = nullptr;
     yarp::dev::IControlMode* iControlMode = nullptr;
     yarp::dev::IEncoders* iEncoders = nullptr;
@@ -219,19 +239,21 @@ TEST_F(ControlBoardPositionDirectFixture, CheckPositionTrackingUsingPendulumMode
         refTrajectory[i] = value;
     }
 
-    double jointPosition;
+    double jointPosition, jointRefPosition;
 
     testFixture
         .OnPreUpdate([&](const gz::sim::UpdateInfo& _info, gz::sim::EntityComponentManager& _ecm) {
             // Set ref position
             iPositionDirectControl->setPosition(0, refTrajectory[iterations]);
+            iPositionDirectControl->getRefPosition(0, &jointRefPosition);
+            EXPECT_EQ(refTrajectory[iterations], jointRefPosition);
+
         })
         .OnPostUpdate(
             [&](const gz::sim::UpdateInfo& _info, const gz::sim::EntityComponentManager& _ecm) {
                 // std::cerr << "========== Iteration: " << iterations << std::endl;
 
                 iEncoders->getEncoder(0, &jointPosition);
-
                 // std::cerr << "ref position: " << refTrajectory[iterations] << std::endl;
                 // std::cerr << "joint position: " << jointPosition << std::endl;
 
@@ -264,7 +286,7 @@ TEST_F(ControlBoardPositionDirectFixture, CheckPositionTrackingUsingPendulumMode
     std::cerr << "Average tracking error: " << avgTrackgingError << std::endl;
     EXPECT_LT(avgTrackgingError, acceptedTolerance);
 }
-
+#if defined GZ_SIM_YARP_PLUGINS_ENABLE_TESTS_WITH_ICUB_MAIN
 TEST_F(ControlBoardPositionDirectCoupledPendulumFixture, CheckPositionTrackingUsingCoupledPendulumModel)
 {
     // Generate ref trajectory
@@ -277,27 +299,32 @@ TEST_F(ControlBoardPositionDirectCoupledPendulumFixture, CheckPositionTrackingUs
         refTrajectory[i] = value;
     }
 
-    yarp::sig::Vector jointPositions{0.0, 0.0};
+    yarp::sig::Vector jointPositions{0.0, 0.0, 0.0}, jointRefPositions{0.0, 0.0, 0.0}, jointRefPositionsRemapper{0.0, 0.0, 0.0};
 
-    testFixture
-        .OnPreUpdate([&](const gz::sim::UpdateInfo& _info, gz::sim::EntityComponentManager& _ecm) {
+    testFixture->OnPreUpdate([&](const gz::sim::UpdateInfo& _info, gz::sim::EntityComponentManager& _ecm) {
             // Set ref position
-            iPositionDirectControl->setPosition(0, refTrajectory[iterations]);
             iPositionDirectControl->setPosition(1, refTrajectory[iterations]);
+            iPositionDirectControl->setPosition(2, refTrajectory[iterations]);
+            iPositionDirectControl->getRefPositions(jointRefPositions.data());
+            posDirect_remapper->getRefPositions(jointRefPositionsRemapper.data());
+            EXPECT_EQ(refTrajectory[iterations], jointRefPositions[1]);
+            EXPECT_EQ(refTrajectory[iterations], jointRefPositions[2]);
+            EXPECT_EQ(refTrajectory[iterations], jointRefPositionsRemapper[1]);
+            EXPECT_EQ(refTrajectory[iterations], jointRefPositionsRemapper[2]);
         })
         .OnPostUpdate(
             [&](const gz::sim::UpdateInfo& _info, const gz::sim::EntityComponentManager& _ecm) {
                 // std::cerr << "========== Iteration: " << iterations << std::endl;
 
                 iEncoders->getEncoders(jointPositions.data());
-                
+
                 // std::cerr << "ref position: " << refTrajectory[iterations] << std::endl;
                 // std::cerr << "joint position: " << jointPosition << std::endl;
 
                 // Tracking error
                 // EXPECT_NEAR(jointPosition, refTrajectory[iterations], acceptedTolerance);
-                trackingErrors[0].push_back(abs(refTrajectory[iterations] - jointPositions[0]));
-                trackingErrors[1].push_back(abs(refTrajectory[iterations] - jointPositions[1]));
+                trackingErrors[0].push_back(abs(refTrajectory[iterations] - jointPositions[1]));
+                trackingErrors[1].push_back(abs(refTrajectory[iterations] - jointPositions[2]));
 
                 iterations++;
             })
@@ -314,7 +341,7 @@ TEST_F(ControlBoardPositionDirectCoupledPendulumFixture, CheckPositionTrackingUs
 
     // Setup simulation server, this will call the post-update callbacks.
     // It also calls pre-update and update callbacks if those are being used.
-    testFixture.Server()->Run(true, plannedIterations, false);
+    testFixture->Server()->Run(true, plannedIterations, false);
     std::cerr << "Simulation completed" << std::endl;
     // Final assertions
     EXPECT_TRUE(configured);
@@ -332,5 +359,7 @@ TEST_F(ControlBoardPositionDirectCoupledPendulumFixture, CheckPositionTrackingUs
     EXPECT_LT(avgTrackgingError1, acceptedTolerance);
 }
 
+#endif // GZ_SIM_YARP_PLUGINS_ENABLE_TESTS_WITH_ICUB_MAIN
+
 } // namespace test
 } // namespace gzyarp
diff --git a/tests/controlboard/conf/coupled_pendulum_nws.xml b/tests/controlboard/conf/coupled_pendulum_nws.xml
@@ -0,0 +1,17 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<robot name="coupled_pendulum" portprefix="coupled_pendulum" build="0" xmlns:xi="http://www.w3.org/2001/XInclude">
+    <devices>
+        <device name="coupled_pendulum_nws_yarp" type="controlBoard_nws_yarp">
+            <!-- See https://www.yarp.it/latest/classControlBoard__nws__yarp.html for parameter documentation -->
+            <param name="name"> /coupledPendulumGazebo/body </param>
+            <param name="period"> 0.01 </param>
+            <action phase="startup" level="5" type="attach">
+                <!-- This is the same name that we passed with the yarpDeviceName to the gazebo_controlboard plugin -->
+                <param name="device"> controlboard_plugin_device </param>
+            </action>
+            <action phase="shutdown" level="5" type="detach" />
+        </device>
+    </devices>
+</robot>
diff --git a/tests/controlboard/conf/gazebo_controlboard_multiple_joints.ini b/tests/controlboard/conf/gazebo_controlboard_multiple_joints.ini
@@ -20,15 +20,15 @@ stictionDwn   (0.0      0.0)
 controlUnits  metric_units
 controlLaw    joint_pid_gazebo_v1
 velocityControlImplementationType integrator_and_position_pid
-kp            (500.0)
-kd            (2.0)
-ki            (0.1)
-maxInt        (9999)
-maxOutput     (9999)
-shift         (0.0)
-ko            (0.0)
-stictionUp    (0.0)
-stictionDwn   (0.0)
+kp            (500.0 500.0)
+kd            (2.0 2.0)
+ki            (0.1 0.1)
+maxInt        (9999 9999)
+maxOutput     (9999 9999)
+shift         (0.0 0.0)
+ko            (0.0 0.0)
+stictionUp    (0.0 0.0)
+stictionDwn   (0.0 0.0)
 
 [LIMITS]
 jntPosMax (200.0 10.0)
diff --git a/tests/controlboard/coupled_pendulum_two_controlboards.sdf b/tests/controlboard/coupled_pendulum_two_controlboards.sdf
@@ -171,9 +171,9 @@
         </yarpConfigurationFile>
         <initialConfiguration>0.0</initialConfiguration>
       </plugin>
-      <!-- <plugin name='robotinterface' filename='libgazebo_yarp_robotinterface.so'>
-      <yarpRobotInterfaceConfigurationFile>model://coupled_pendulum/conf/coupled_pendulum_nws.xml</yarpRobotInterfaceConfigurationFile>
-      </plugin> -->
+      <plugin name='robotinterface' filename='libgazebo_yarp_robotinterface.so'>
+      <yarpRobotInterfaceConfigurationFile>model://coupled_pendulum/conf/coupled_pendulum_nwsdafsads.xml</yarpRobotInterfaceConfigurationFile>
+      </plugin>
 
       <frame name='fake_base_fixed_joint' attached_to='base_link'>
         <pose>0 0 0 0 0 0</pose>
diff --git a/tests/controlboard/coupled_pendulum_two_joints_coupled.sdf b/tests/controlboard/coupled_pendulum_two_joints_coupled.sdf
@@ -46,6 +46,24 @@
     </model>
 
     <model name="coupled_pendulum">
+      <joint name="fixed_base" type="revolute">
+          <parent>world</parent>
+          <child>base_link</child>
+          <axis>
+              <xyz>1 0 0</xyz>
+              <limit>
+                  <lower>-5</lower>
+                  <upper>5</upper>
+                  <effort>100</effort>
+                  <velocity>100</velocity>
+              </limit>
+              <dynamics>
+                  <damping>0.0</damping>
+                  <spring_reference>0</spring_reference>
+                  <spring_stiffness>0.0</spring_stiffness>
+              </dynamics>
+          </axis>
+      </joint>
       <link name='base_link'>
         <inertial>
           <pose>0 0 0 0 0 0</pose>
@@ -161,13 +179,13 @@
 
       <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
         <yarpConfigurationFile>
-          model://conf/gazebo_controlboard_multiple_joints.ini
+          model://conf/gazebo_controlboard_coupling.ini
         </yarpConfigurationFile>
         <initialConfiguration>0.0</initialConfiguration>
       </plugin>
-      <!-- <plugin name='robotinterface' filename='libgazebo_yarp_robotinterface.so'>
-      <yarpRobotInterfaceConfigurationFile>model://coupled_pendulum/conf/coupled_pendulum_nws.xml</yarpRobotInterfaceConfigurationFile>
-      </plugin> -->
+      <plugin name="gzyarp::RobotInterface" filename="gz-sim-yarp-robotinterface-system">
+      <yarpRobotInterfaceConfigurationFile>model://conf/coupled_pendulum_nws.xml</yarpRobotInterfaceConfigurationFile>
+      </plugin>
 
       <frame name='fake_base_fixed_joint' attached_to='base_link'>
         <pose>0 0 0 0 0 0</pose>
