Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add test for forcetorque plugin #30

Merged
merged 17 commits into from
Aug 11, 2023
Merged

Add test for forcetorque plugin #30

Show file tree
Hide file tree
Changes from 9 commits
Commits
Show all changes
17 commits
Select commit Hold shift + click to select a range
5589283
FT test
lucapa17 Aug 9, 2023
374f667
Create README.md
lucapa17 Aug 9, 2023
f01adbc
changes in CMakeLists files
lucapa17 Aug 10, 2023
dbcc747
Merge branch 'main' into FT-tests
lucapa17 Aug 10, 2023
3d7a04e
set env variable GZ_SIM_SYSTEM_PLUGIN_PATH in CMakeList file for FT test
lucapa17 Aug 10, 2023
9f4a0ec
use multipleanalogsensorsclient instead of yarp port for test
lucapa17 Aug 11, 2023
222a77c
add YARP_DATA_DIRS
GiacomoBisio Aug 11, 2023
fd28795
increase timeout and running time
GiacomoBisio Aug 11, 2023
6697372
Fix use of cache in apt workflow
traversaro Aug 11, 2023
2864978
Update apt.yml
traversaro Aug 11, 2023
f305c97
Update apt.yml
traversaro Aug 11, 2023
5a0f38c
check successful read of getSixAxisForceTorqueSensorMeasure
GiacomoBisio Aug 11, 2023
2b18712
Test with getSixAxisForceTorqueSensorStatus
GiacomoBisio Aug 11, 2023
1b97714
Delete README.md
GiacomoBisio Aug 11, 2023
9f13744
Changes in tutorial sdf files
GiacomoBisio Aug 11, 2023
234c357
Suppress tests on Windows
GiacomoBisio Aug 11, 2023
0715cd4
Update conda-forge.yml
GiacomoBisio Aug 11, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions .github/workflows/apt.yml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,7 @@ jobs:
key: source-deps-${{ runner.os }}-os-${{ matrix.os }}-build-type-${{ matrix.build_type }}-ycm-${{ matrix.ycm_tag }}-yarp-${{ matrix.yarp_tag }}

- name: Source-based Dependencies
if: steps.cache-source-deps.outputs.cache-hit != 'true'
run: |
# YCM
git clone https://github.com/robotology/ycm
Expand Down
9 changes: 9 additions & 0 deletions CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,9 +12,18 @@ set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR})
gz_find_package(gz-sim7 REQUIRED)
set(GZ_SIM_VER ${gz-sim7_VERSION_MAJOR})

set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_BINDIR}")
set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}")
set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}")

# Support shared libraries on Windows
set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)

add_subdirectory(libraries)
add_subdirectory(plugins)
option(BUILD_TESTING "Create tests using CMake" ON)
if(BUILD_TESTING)
include(CTest)
add_subdirectory(tests)
endif()

2 changes: 2 additions & 0 deletions tests/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
add_subdirectory(forcetorque)

26 changes: 26 additions & 0 deletions tests/forcetorque/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,26 @@
# Fetch and configure GTest
include(FetchContent)
FetchContent_Declare(
googletest
URL https://github.com/google/googletest/archive/609281088cfefc76f9d0ce82e1ff6c30cc3591e5.zip
)
set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
FetchContent_MakeAvailable(googletest)

add_executable(ForceTorqueTest ForceTorqueTest.cc)
target_link_libraries(ForceTorqueTest
gtest_main
gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER}
${YARP_LIBRARIES}
)
add_test(NAME ForceTorqueTest
COMMAND ForceTorqueTest)

set_property(TEST ForceTorqueTest PROPERTY ENVIRONMENT YARP_DATA_DIRS=${YARP_DATA_INSTALL_DIR_FULL})

set(_env_vars)
list(APPEND _env_vars "GZ_SIM_SYSTEM_PLUGIN_PATH=$<TARGET_FILE_DIR:GazeboYarpForceTorque>")

set_tests_properties(ForceTorqueTest PROPERTIES
ENVIRONMENT "${_env_vars}")
49 changes: 49 additions & 0 deletions tests/forcetorque/ForceTorqueTest.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,49 @@
#include <gtest/gtest.h>
#include <yarp/dev/PolyDriver.h>
#include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
#include <yarp/os/Network.h>
#include <gz/sim/TestFixture.hh>

TEST(ForceTorqueTest, PluginTest)
{
yarp::os::NetworkBase::setLocalMode(true);

// Maximum verbosity helps with debugging
gz::common::Console::SetVerbosity(4);

// Instantiate test fixture
gz::sim::TestFixture fixture("../../../tests/forcetorque/model.sdf");

int iterations = 5000;
fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false);

yarp::os::Property option;
option.put("device","multipleanalogsensorsclient");
option.put("remote","/forcetorque");
option.put("timeout",5.0);
option.put("local", "/ForceTorqueTest");
yarp::dev::PolyDriver driver;

ASSERT_TRUE(driver.open(option));

yarp::dev::ISixAxisForceTorqueSensors* isixaxis = nullptr;

ASSERT_TRUE(driver.view(isixaxis));

fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false);
yarp::sig::Vector measure(6);
std::string sensorName;
double timestamp;

isixaxis->getSixAxisForceTorqueSensorName(0, sensorName);
isixaxis->getSixAxisForceTorqueSensorMeasure(0, measure, timestamp);
Copy link
Member

@traversaro traversaro Aug 11, 2023 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I suggest:

Suggested change
isixaxis->getSixAxisForceTorqueSensorMeasure(0, measure, timestamp);
size_t maxNrOfReadingAttempts = 20;
bool readSuccessful = false;
for (size_t i=0; (i < maxNrOfReadingAttempts) && !readSuccessful ; i++)
{
readSuccessful = isixaxis->getSixAxisForceTorqueSensorMeasure(0, measure, timestamp);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
ASSERT_TRUE(readSuccessful);

if the failure continue, you can inspect also the value returned by getSixAxisForceTorqueSensorStatus to understand why the output is not the expected one.

//std::cerr << "The measure of FT sensor " << sensorName << " is " << measure.toString() << " at time " << timestamp << std::endl;

EXPECT_NEAR(measure(0), 0.0, 1e-2);
EXPECT_NEAR(measure(1), 0.0, 1e-2);
EXPECT_NEAR(measure(2), -9.8*10, 1e-2);
EXPECT_NEAR(measure(3), 0.0, 1e-2);
EXPECT_NEAR(measure(4), 0.0, 1e-2);
EXPECT_NEAR(measure(5), 0.0, 1e-2);
EXPECT_GT(timestamp, 0.0);
}
8 changes: 8 additions & 0 deletions tests/forcetorque/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,8 @@
## ForceTorque plugin Test

~~~
cd build
export GZ_SIM_SYSTEM_PLUGIN_PATH=`pwd`
cd tests/forcetorque
./ForceTorqueTest
~~~
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

As mentioned in the comment to the PR as an whole, I suggest to remove this and just document in the README how to run tests.

Suggested change
## ForceTorque plugin Test
~~~
cd build
export GZ_SIM_SYSTEM_PLUGIN_PATH=`pwd`
cd tests/forcetorque
./ForceTorqueTest
~~~

15 changes: 15 additions & 0 deletions tests/forcetorque/forcetorque_nws.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,15 @@
<?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="forcetorque" portprefix="forcetorque" build="0" xmlns:xi="http://www.w3.org/2001/XInclude">
<devices>
<device name="forcetorque_nws_yarp" type="multipleanalogsensorsserver">
<param name="name"> /forcetorque </param>
<param name="period"> 100 </param>
<action phase="startup" level="5" type="attach">
<param name="device"> forcetorque_plugin_device </param>
</action>
<action phase="shutdown" level="5" type="detach" />
</device>
</devices>
</robot>
156 changes: 156 additions & 0 deletions tests/forcetorque/model.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,156 @@
<?xml version="1.0" ?>

<sdf version="1.6">
<world name="sensors">
<physics name="1ms" type="ignored">
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>
<plugin
filename="gz-sim-physics-system"
name="gz::sim::systems::Physics">
</plugin>
<plugin
filename="gz-sim-forcetorque-system"
name="gz::sim::systems::ForceTorque">
</plugin>
<plugin
filename="gz-sim-user-commands-system"
name="gz::sim::systems::UserCommands">
</plugin>
<plugin
filename="gz-sim-scene-broadcaster-system"
name="gz::sim::systems::SceneBroadcaster">
</plugin>

<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>

<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>

<model name="force_torque_example">
<plugin name="GazeboYarpForceTorque" filename="GazeboYarpForceTorque">
<yarpConfigurationString>(yarpDeviceName forcetorque_plugin_device) (jointName joint_12) (sensorName force_torque_sensor)</yarpConfigurationString>
</plugin>
<link name="link_1">
<inertial>
<pose>0 0 0.05 0 0 0</pose>
<inertia>
<ixx>0.020000</ixx>
<ixy>0.000000</ixy>
<ixz>0.000000</ixz>
<iyy>0.020000</iyy>
<iyz>0.000000</iyz>
<izz>0.020000</izz>
</inertia>
<mass>10.000000</mass>
</inertial>
<visual name="visual_cylinder">
<pose>0 0 0.05 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.100000</radius>
<length>0.100000</length>
</cylinder>
</geometry>
</visual>
<collision name="collision_cylinder">
<pose>0 0 0.05 0 0 0</pose>
<max_contacts>250</max_contacts>
<geometry>
<cylinder>
<radius>0.100000</radius>
<length>0.100000</length>
</cylinder>
</geometry>
</collision>
</link>
<link name="link_2">
<pose>0 0 0.15 0 0 0</pose>
<inertial>
<pose>0 0 0.0 0 0 0</pose>
<inertia>
<ixx>0.020000</ixx>
<ixy>0.000000</ixy>
<ixz>0.000000</ixz>
<iyy>0.020000</iyy>
<iyz>0.000000</iyz>
<izz>0.020000</izz>
</inertia>
<mass>10.000000</mass>
</inertial>
<visual name="visual_box">
<pose>0 0 0.0 0 0 0</pose>
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</visual>
<collision name="collision_box">
<pose>0 0 0.0 0 0 0</pose>
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</collision>
</link>

<joint name="joint_01" type="fixed">
<parent>world</parent>
<child>link_1</child>
<pose>0 0 0.0 0 0 0</pose>
</joint>
<joint name="joint_12" type="revolute">
<parent>link_1</parent>
<child>link_2</child>
<pose>0 0 0 0 0 0</pose>
<sensor name="force_torque_sensor" type="force_torque">
<always_on>true</always_on>
<visualize>true</visualize>
<update_rate>30</update_rate>
</sensor>
</joint>
<plugin name="GazeboYarpRobotInterface" filename="GazeboYarpRobotInterface">
<yarpRobotInterfaceConfigurationFile>../../../tests/forcetorque/forcetorque_nws.xml</yarpRobotInterfaceConfigurationFile>
</plugin>
</model>
</world>
</sdf>