Skip to content

Commit

Permalink
FT test
Browse files Browse the repository at this point in the history
  • Loading branch information
@lucapa17
lucapa17 committed Aug 9, 2023
1 parent c3aa2dc commit 5589283
Show file tree
Hide file tree
Showing 6 changed files with 244 additions and 0 deletions.
1 change: 1 addition & 0 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,4 +15,5 @@ set(GZ_SIM_VER ${gz-sim7_VERSION_MAJOR})

add_subdirectory(libraries)
add_subdirectory(plugins)
add_subdirectory(tests)

2 changes: 2 additions & 0 deletions tests/CMakeLists.txt
View file
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
Original file line number Diff line number Diff line change
@@ -0,0 +1,26 @@
project(GTestSetup)

# Find Gazebo
find_package(gz-sim7 REQUIRED)
set(GZ_SIM_VER ${gz-sim7_VERSION_MAJOR})

# 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)

enable_testing()

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}
)
include(GoogleTest)
gtest_discover_tests(ForceTorqueTest)
44 changes: 44 additions & 0 deletions tests/forcetorque/ForceTorqueTest.cc
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,44 @@
#include <gtest/gtest.h>
#include <yarp/os/Bottle.h>
#include <yarp/os/BufferedPort.h>
#include <yarp/os/LogStream.h>
#include <yarp/os/Network.h>
#include <gz/sim/TestFixture.hh>

TEST(gz_yarp_tests, ft_test)
{
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 = 1000;
fixture.Server()->Run(true, iterations, false);

yarp::os::Port p;
p.open("/read");
yarp::os::Network::connect("/forcetorque/measures:o","/read");
yarp::os::Bottle b;
p.read(b);
std::stringstream ss(b.get(5).toString());

char parenthesis;
double forceX, forceY, forceZ, torqueX, torqueY, torqueZ, simTime;

ss >> parenthesis >> parenthesis >>
forceX >> forceY >> forceZ >>
torqueX >> torqueY >> torqueZ >>
parenthesis >> simTime ;


EXPECT_NEAR(forceX, 0.0, 1e-2);
EXPECT_NEAR(forceY, 0.0, 1e-2);
EXPECT_NEAR(forceZ, -9.8*10, 1e-2);
EXPECT_NEAR(torqueX, 0.0, 1e-2);
EXPECT_NEAR(torqueY, 0.0, 1e-2);
EXPECT_NEAR(torqueZ, 0.0, 1e-2);
EXPECT_GT(simTime, 0.0);
}
15 changes: 15 additions & 0 deletions tests/forcetorque/forcetorque_nws.xml
View file
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
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="plugins/forcetorque/libGazeboYarpForceTorque.so">
<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="plugins/robotinterface/libGazeboYarpRobotInterface.so">
<yarpRobotInterfaceConfigurationFile>../../../tests/forcetorque/forcetorque_nws.xml</yarpRobotInterfaceConfigurationFile>
</plugin>
</model>
</world>
</sdf>

0 comments on commit 5589283

Please sign in to comment.