Skip to content

Commit ea02b61

Browse files
xela-95xela-95
authored
Merge pull request #228 from martinaxgloria/feat/addCoupledPendulum
Add coupled_pendulum tutorial and tests
2 parents cc6df8b + 731d88c commit ea02b61

14 files changed

+1379
-2709
lines changed

‎pixi.lock

+464-2,709
Some generated files are not rendered by default. Learn more about customizing how changed files appear on GitHub.

‎pixi.toml

+2
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,7 @@ configure = { cmd = [
3030
"-DCMAKE_BUILD_TYPE=Release",
3131
"-DCMAKE_INSTALL_PREFIX=$CMAKE_INSTALL_PREFIX",
3232
"-DBUILD_TESTING:BOOL=ON",
33+
"-DGZ_SIM_YARP_PLUGINS_ENABLE_TESTS_WITH_ICUB_MAIN:BOOL=OFF",
3334
# Use the cross-platform Ninja generator
3435
"-G",
3536
"Ninja",
@@ -57,6 +58,7 @@ compilers = "*"
5758
gtest = "*"
5859
fd-find = "*"
5960
clang-format = "18.*"
61+
icub-main = "*"
6062

6163
[target.linux-64.dependencies]
6264
libgl-devel = "*"

‎tests/CMakeLists.txt

+9
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,13 @@
11
find_package(GTest REQUIRED)
2+
# Only if the GZ_SIM_YARP_PLUGINS_ENABLE_TESTS_WITH_ICUB_MAIN variable is not defined,
3+
# we look for ICUB, to set its default value
4+
if(NOT DEFINED GZ_SIM_YARP_PLUGINS_ENABLE_TESTS_WITH_ICUB_MAIN)
5+
find_package(ICUB QUIET)
6+
endif()
7+
option(GZ_SIM_YARP_PLUGINS_ENABLE_TESTS_WITH_ICUB_MAIN "Enable tests that require the installation of the icub-main package" ICUB_FOUND)
8+
if (GZ_SIM_YARP_PLUGINS_ENABLE_TESTS_WITH_ICUB_MAIN)
9+
find_package(ICUB REQUIRED)
10+
endif()
211

312
add_subdirectory(forcetorque)
413
add_subdirectory(imu)

‎tests/controlboard/ControlBoardPositionControlTest.cc

+155
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,5 @@
11
#include <Common.hh>
2+
#include <ControlBoardDriver.hh>
23
#include <DeviceRegistry.hh>
34

45
#include <gtest/gtest.h>
@@ -118,6 +119,92 @@ class ControlBoardPositionFixture : public testing::Test
118119
yarp::dev::IEncoders* iEncoders = nullptr;
119120
};
120121

122+
class ControlBoardPositionCoupledPendulumFixture : public ::testing::Test
123+
{
124+
protected:
125+
// void SetUp() override
126+
ControlBoardPositionCoupledPendulumFixture()
127+
: testFixture{(std::filesystem::path(CMAKE_CURRENT_SOURCE_DIR)
128+
/ "coupled_pendulum_two_joints_coupled.sdf")
129+
.string()}
130+
{
131+
gz::common::Console::SetVerbosity(4);
132+
133+
testFixture.
134+
// Use configure callback to get values at startup
135+
OnConfigure([&](const gz::sim::Entity& _worldEntity,
136+
const std::shared_ptr<const sdf::Element>& /*_sdf*/,
137+
gz::sim::EntityComponentManager& _ecm,
138+
gz::sim::EventManager& /*_eventMgr*/) {
139+
std::cerr << "========== Configuring test" << std::endl;
140+
141+
gz::sim::World world(_worldEntity);
142+
143+
// Get model
144+
auto modelEntity = world.ModelByName(_ecm, "coupled_pendulum");
145+
modelEntity = world.ModelByName(_ecm, "coupled_pendulum");
146+
EXPECT_NE(gz::sim::kNullEntity, modelEntity);
147+
model = gz::sim::Model(modelEntity);
148+
149+
auto devicesKeys = DeviceRegistry::getHandler()->getDevicesKeys(_ecm);
150+
std::cerr << "Number of Devices: " << devicesKeys.size() << std::endl;
151+
auto cbKey = devicesKeys.at(0);
152+
EXPECT_TRUE(DeviceRegistry::getHandler()->getDevice(_ecm, devicesKeys[0], driver));
153+
std::cerr << "Driver key: " << cbKey << std::endl;
154+
ASSERT_TRUE(driver != nullptr);
155+
iPositionControl = nullptr;
156+
ASSERT_TRUE(driver->view(iPositionControl));
157+
iControlMode = nullptr;
158+
ASSERT_TRUE(driver->view(iControlMode));
159+
iEncoders = nullptr;
160+
ASSERT_TRUE(driver->view(iEncoders));
161+
162+
// Get joint1
163+
auto jointEntity0 = model.JointByName(_ecm, "upper_joint");
164+
EXPECT_NE(gz::sim::kNullEntity, jointEntity0);
165+
joint0 = gz::sim::Joint(jointEntity0);
166+
167+
// Get joint2
168+
auto jointEntity1 = model.JointByName(_ecm, "lower_joint");
169+
EXPECT_NE(gz::sim::kNullEntity, jointEntity1);
170+
joint1 = gz::sim::Joint(jointEntity1);
171+
172+
// Set joint in torque control mode
173+
ASSERT_TRUE(iControlMode->setControlMode(0, VOCAB_CM_POSITION));
174+
ASSERT_TRUE(iControlMode->setControlMode(1, VOCAB_CM_POSITION));
175+
176+
// Print number of joint configured
177+
int nJointsConfigured{};
178+
ASSERT_TRUE(iPositionControl->getAxes(&nJointsConfigured));
179+
std::cerr << "Number of joints configured: " << nJointsConfigured << std::endl;
180+
181+
configured = true;
182+
std::cerr << "========== Test configured" << std::endl;
183+
});
184+
}
185+
186+
// Get SDF model name from test parameter
187+
gz::sim::TestFixture testFixture;
188+
double linkMass{1};
189+
double linkLength{1.0};
190+
double linkInertiaAtLinkEnd{0.3352}; // Computed with parallel axis theorem
191+
int plannedIterations{5000};
192+
int iterations{0};
193+
std::vector<std::vector<double>> trackingErrors{2};
194+
double acceptedTolerance{5e-2};
195+
bool configured{false};
196+
gz::math::Vector3d gravity;
197+
gz::sim::Entity modelEntity;
198+
gz::sim::Model model;
199+
gz::sim::Joint joint0;
200+
gz::sim::Joint joint1;
201+
yarp::dev::PolyDriver* driver;
202+
yarp::dev::IPositionControl* iPositionControl = nullptr;
203+
yarp::dev::IControlMode* iControlMode = nullptr;
204+
yarp::dev::IEncoders* iEncoders = nullptr;
205+
};
206+
207+
121208
TEST_F(ControlBoardPositionFixture, CheckPositionTrackingWithTrajectoryGenerationUsingPendulumModel)
122209
{
123210
auto refPosition{90.0};
@@ -167,5 +254,73 @@ TEST_F(ControlBoardPositionFixture, CheckPositionTrackingWithTrajectoryGeneratio
167254
ASSERT_LT(jointPosError, acceptedTolerance);
168255
}
169256

257+
TEST_F(ControlBoardPositionCoupledPendulumFixture, CheckPositionTrackingWithTrajectoryGenerationUsingPendulumModel)
258+
{
259+
auto refPosition{90.0};
260+
bool motionDone0{false};
261+
bool motionDone1{false};
262+
yarp::sig::Vector jointPosition{0.0, 0.0};
263+
yarp::sig::Vector jointPosError{0.0, 0.0};
264+
265+
testFixture
266+
.OnPostUpdate(
267+
[&](const gz::sim::UpdateInfo& _info, const gz::sim::EntityComponentManager& _ecm) {
268+
// std::cerr << "========== Iteration: " << iterations << std::endl;
269+
270+
iEncoders->getEncoders(jointPosition.data());
271+
iPositionControl->checkMotionDone(0, &motionDone0);
272+
iPositionControl->checkMotionDone(1, &motionDone1);
273+
274+
// std::cerr << "ref position: " << refTrajectory[iterations] << std::endl;
275+
// std::cerr << "joint position: " << jointPosition << std::endl;
276+
277+
iterations++;
278+
})
279+
.
280+
// The moment we finalize, the configure callback is called
281+
Finalize();
282+
283+
int modeSet0{};
284+
int modeSet1{};
285+
iControlMode->getControlMode(0, &modeSet0);
286+
iControlMode->getControlMode(1, &modeSet1);
287+
ASSERT_TRUE(modeSet0 == VOCAB_CM_POSITION);
288+
ASSERT_TRUE(modeSet1 == VOCAB_CM_POSITION);
289+
290+
// Set reference position
291+
iPositionControl->positionMove(0, refPosition);
292+
293+
// Setup simulation server, this will call the post-update callbacks.
294+
// It also calls pre-update and update callbacks if those are being used.
295+
while (!motionDone0)
296+
{
297+
std::cerr << "Running server" << std::endl;
298+
testFixture.Server()->Run(true, plannedIterations, false);
299+
jointPosError[0] = abs(refPosition - jointPosition[0]);
300+
std::cerr << "Joint 0 position error: " << jointPosError[0] << std::endl;
301+
}
302+
303+
std::cerr << "Final tracking error for joint 0: " << jointPosError[0] << std::endl;
304+
ASSERT_LT(jointPosError[0], acceptedTolerance);
305+
306+
iPositionControl->positionMove(1, refPosition);
307+
while (!motionDone1)
308+
{
309+
std::cerr << "Running server" << std::endl;
310+
testFixture.Server()->Run(true, plannedIterations, false);
311+
jointPosError[1] = abs(refPosition - jointPosition[1]);
312+
std::cerr << "Joint 1 position error: " << jointPosError[1] << std::endl;
313+
}
314+
315+
// Final assertions
316+
ASSERT_TRUE(configured);
317+
ASSERT_TRUE(motionDone0);
318+
ASSERT_TRUE(motionDone1);
319+
320+
// Verify that the final error is within the accepted tolerance
321+
std::cerr << "Final tracking error for joint 1: " << jointPosError[1] << std::endl;
322+
ASSERT_LT(jointPosError[1], acceptedTolerance);
323+
}
324+
170325
} // namespace test
171326
} // namespace gzyarp

‎tests/controlboard/ControlBoardPositionDirectControlTest.cc

+152
Original file line numberDiff line numberDiff line change
@@ -122,6 +122,91 @@ class ControlBoardPositionDirectFixture : public ::testing::Test
122122
yarp::dev::IEncoders* iEncoders = nullptr;
123123
};
124124

125+
class ControlBoardPositionDirectCoupledPendulumFixture : public ::testing::Test
126+
{
127+
protected:
128+
// void SetUp() override
129+
ControlBoardPositionDirectCoupledPendulumFixture()
130+
: testFixture{(std::filesystem::path(CMAKE_CURRENT_SOURCE_DIR)
131+
/ "coupled_pendulum_two_joints_coupled.sdf")
132+
.string()}
133+
{
134+
gz::common::Console::SetVerbosity(4);
135+
136+
testFixture.
137+
// Use configure callback to get values at startup
138+
OnConfigure([&](const gz::sim::Entity& _worldEntity,
139+
const std::shared_ptr<const sdf::Element>& /*_sdf*/,
140+
gz::sim::EntityComponentManager& _ecm,
141+
gz::sim::EventManager& /*_eventMgr*/) {
142+
std::cerr << "========== Configuring test" << std::endl;
143+
144+
gz::sim::World world(_worldEntity);
145+
146+
// Get model
147+
auto modelEntity = world.ModelByName(_ecm, "coupled_pendulum");
148+
modelEntity = world.ModelByName(_ecm, "coupled_pendulum");
149+
EXPECT_NE(gz::sim::kNullEntity, modelEntity);
150+
model = gz::sim::Model(modelEntity);
151+
152+
auto devicesKeys = DeviceRegistry::getHandler()->getDevicesKeys(_ecm);
153+
std::cerr << "Number of Devices: " << devicesKeys.size() << std::endl;
154+
auto cbKey = devicesKeys.at(0);
155+
EXPECT_TRUE(DeviceRegistry::getHandler()->getDevice(_ecm, devicesKeys[0], driver));
156+
std::cerr << "Driver key: " << cbKey << std::endl;
157+
ASSERT_TRUE(driver != nullptr);
158+
iPositionDirectControl = nullptr;
159+
ASSERT_TRUE(driver->view(iPositionDirectControl));
160+
iControlMode = nullptr;
161+
ASSERT_TRUE(driver->view(iControlMode));
162+
iEncoders = nullptr;
163+
ASSERT_TRUE(driver->view(iEncoders));
164+
165+
// Get joint1
166+
auto jointEntity0 = model.JointByName(_ecm, "upper_joint");
167+
EXPECT_NE(gz::sim::kNullEntity, jointEntity0);
168+
joint0 = gz::sim::Joint(jointEntity0);
169+
170+
// Get joint2
171+
auto jointEntity1 = model.JointByName(_ecm, "lower_joint");
172+
EXPECT_NE(gz::sim::kNullEntity, jointEntity1);
173+
joint1 = gz::sim::Joint(jointEntity1);
174+
175+
// Set joint in torque control mode
176+
ASSERT_TRUE(iControlMode->setControlMode(0, VOCAB_CM_POSITION_DIRECT));
177+
ASSERT_TRUE(iControlMode->setControlMode(1, VOCAB_CM_POSITION_DIRECT));
178+
179+
// Print number of joint configured
180+
int nJointsConfigured{};
181+
ASSERT_TRUE(iPositionDirectControl->getAxes(&nJointsConfigured));
182+
std::cerr << "Number of joints configured: " << nJointsConfigured << std::endl;
183+
184+
configured = true;
185+
std::cerr << "========== Test configured" << std::endl;
186+
});
187+
}
188+
189+
// Get SDF model name from test parameter
190+
gz::sim::TestFixture testFixture;
191+
double linkMass{1};
192+
double linkLength{1.0};
193+
double linkInertiaAtLinkEnd{0.3352}; // Computed with parallel axis theorem
194+
int plannedIterations{5000};
195+
int iterations{0};
196+
std::vector<std::vector<double>> trackingErrors{2};
197+
double acceptedTolerance{5e-2};
198+
bool configured{false};
199+
gz::math::Vector3d gravity;
200+
gz::sim::Entity modelEntity;
201+
gz::sim::Model model;
202+
gz::sim::Joint joint0;
203+
gz::sim::Joint joint1;
204+
yarp::dev::PolyDriver* driver;
205+
yarp::dev::IPositionDirect* iPositionDirectControl = nullptr;
206+
yarp::dev::IControlMode* iControlMode = nullptr;
207+
yarp::dev::IEncoders* iEncoders = nullptr;
208+
};
209+
125210
TEST_F(ControlBoardPositionDirectFixture, CheckPositionTrackingUsingPendulumModel)
126211
{
127212
// Generate ref trajectory
@@ -180,5 +265,72 @@ TEST_F(ControlBoardPositionDirectFixture, CheckPositionTrackingUsingPendulumMode
180265
EXPECT_LT(avgTrackgingError, acceptedTolerance);
181266
}
182267

268+
TEST_F(ControlBoardPositionDirectCoupledPendulumFixture, CheckPositionTrackingUsingCoupledPendulumModel)
269+
{
270+
// Generate ref trajectory
271+
std::vector<double> refTrajectory(plannedIterations);
272+
double amplitude = 100.0; // deg
273+
for (int i = 0; i < plannedIterations; i++)
274+
{
275+
double t = static_cast<double>(i) / plannedIterations;
276+
double value = amplitude * std::sin(gzyarp::pi * t);
277+
refTrajectory[i] = value;
278+
}
279+
280+
yarp::sig::Vector jointPositions{0.0, 0.0};
281+
282+
testFixture
283+
.OnPreUpdate([&](const gz::sim::UpdateInfo& _info, gz::sim::EntityComponentManager& _ecm) {
284+
// Set ref position
285+
iPositionDirectControl->setPosition(0, refTrajectory[iterations]);
286+
iPositionDirectControl->setPosition(1, refTrajectory[iterations]);
287+
})
288+
.OnPostUpdate(
289+
[&](const gz::sim::UpdateInfo& _info, const gz::sim::EntityComponentManager& _ecm) {
290+
// std::cerr << "========== Iteration: " << iterations << std::endl;
291+
292+
iEncoders->getEncoders(jointPositions.data());
293+
294+
// std::cerr << "ref position: " << refTrajectory[iterations] << std::endl;
295+
// std::cerr << "joint position: " << jointPosition << std::endl;
296+
297+
// Tracking error
298+
// EXPECT_NEAR(jointPosition, refTrajectory[iterations], acceptedTolerance);
299+
trackingErrors[0].push_back(abs(refTrajectory[iterations] - jointPositions[0]));
300+
trackingErrors[1].push_back(abs(refTrajectory[iterations] - jointPositions[1]));
301+
302+
iterations++;
303+
})
304+
.
305+
// The moment we finalize, the configure callback is called
306+
Finalize();
307+
308+
int modeSet0{};
309+
int modeSet1{};
310+
ASSERT_TRUE(iControlMode->getControlMode(0, &modeSet0));
311+
ASSERT_TRUE(iControlMode->getControlMode(1, &modeSet1));
312+
ASSERT_TRUE(modeSet0 == VOCAB_CM_POSITION_DIRECT);
313+
ASSERT_TRUE(modeSet1 == VOCAB_CM_POSITION_DIRECT);
314+
315+
// Setup simulation server, this will call the post-update callbacks.
316+
// It also calls pre-update and update callbacks if those are being used.
317+
testFixture.Server()->Run(true, plannedIterations, false);
318+
std::cerr << "Simulation completed" << std::endl;
319+
// Final assertions
320+
EXPECT_TRUE(configured);
321+
// Verify that the post update function was called 1000 times
322+
EXPECT_EQ(plannedIterations, iterations);
323+
// Verify that the average tracking error is within the accepted tolerance
324+
std::cerr << "Tracking error vector size: " << trackingErrors.size() << std::endl;
325+
auto avgTrackgingError0 = std::accumulate(trackingErrors[0].begin(), trackingErrors[0].end(), 0.0)
326+
/ trackingErrors[0].size();
327+
auto avgTrackgingError1 = std::accumulate(trackingErrors[1].begin(), trackingErrors[1].end(), 0.0)
328+
/ trackingErrors[1].size();
329+
std::cerr << "Average tracking error joint 0: " << avgTrackgingError0 << std::endl;
330+
std::cerr << "Average tracking error joint 1: " << avgTrackgingError1 << std::endl;
331+
EXPECT_LT(avgTrackgingError0, acceptedTolerance);
332+
EXPECT_LT(avgTrackgingError1, acceptedTolerance);
333+
}
334+
183335
} // namespace test
184336
} // namespace gzyarp

0 commit comments

Comments
 (0)