From 098adf70f9b6e5bf9e6919234e14000893aeebaf Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Mon, 25 Jan 2021 10:47:01 +0100 Subject: [PATCH] First draft of gazebo_yarp_robotinterface plugin --- CMakeLists.txt | 15 +- .../include/GazeboYarpPlugins/Handler.hh | 17 +- libraries/singleton/src/Handler.cc | 11 ++ plugins/CMakeLists.txt | 4 + plugins/robotinterface/CMakeLists.txt | 12 ++ plugins/robotinterface/README.md | 5 + .../gazebo/GazeboYarpRobotInterface.hh | 37 ++++ .../src/GazeboYarpRobotInterface.cc | 106 ++++++++++ tests/CMakeLists.txt | 6 +- tests/robotinterface/CMakeLists.txt | 14 ++ .../RobotInterfaceControlBoardConfig.ini | 41 ++++ tests/robotinterface/RobotInterfaceTest.cc | 110 +++++++++++ tests/robotinterface/RobotInterfaceTest.world | 186 ++++++++++++++++++ tests/robotinterface/RobotInterfaceTest.xml | 21 ++ 14 files changed, 577 insertions(+), 8 deletions(-) create mode 100644 plugins/robotinterface/CMakeLists.txt create mode 100644 plugins/robotinterface/README.md create mode 100644 plugins/robotinterface/include/gazebo/GazeboYarpRobotInterface.hh create mode 100644 plugins/robotinterface/src/GazeboYarpRobotInterface.cc create mode 100644 tests/robotinterface/CMakeLists.txt create mode 100644 tests/robotinterface/RobotInterfaceControlBoardConfig.ini create mode 100644 tests/robotinterface/RobotInterfaceTest.cc create mode 100644 tests/robotinterface/RobotInterfaceTest.world create mode 100644 tests/robotinterface/RobotInterfaceTest.xml diff --git a/CMakeLists.txt b/CMakeLists.txt index 44708dc63..8f9425aa8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,7 +8,7 @@ PROJECT(GazeboYARPPlugins) # Project version set(${PROJECT_NAME}_MAJOR_VERSION 3) set(${PROJECT_NAME}_MINOR_VERSION 5) -set(${PROJECT_NAME}_PATCH_VERSION 1) +set(${PROJECT_NAME}_PATCH_VERSION 100) set(${PROJECT_NAME}_VERSION ${${PROJECT_NAME}_MAJOR_VERSION}.${${PROJECT_NAME}_MINOR_VERSION}.${${PROJECT_NAME}_PATCH_VERSION}) @@ -17,6 +17,12 @@ set(${PROJECT_NAME}_VERSION # See https://cmake.org/cmake/help/latest/module/GNUInstallDirs.html include(GNUInstallDirs) +# Build all the plugins in the same directory to simplify running tests +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}") + + # option(BUILD_TESTING "Create tests using CMake" OFF) if(BUILD_TESTING) @@ -26,6 +32,7 @@ endif() # Finding dependencies find_package(OpenCV QUIET) option(GAZEBO_YARP_PLUGINS_HAS_OPENCV "Compile plugins that depend on OpenCV" ${OpenCV_FOUND}) +option(GAZEBO_YARP_PLUGINS_HAS_YARP_ROBOTINTERFACE "Compile plugins that depend on libYARP_robotinterface" ON) if(GAZEBO_YARP_PLUGINS_HAS_OPENCV) find_package(OpenCV REQUIRED) @@ -34,7 +41,11 @@ else() set(YARP_ADDITIONAL_COMPONENTS_REQUIRED "") endif() -find_package(YARP 3.2.102 REQUIRED COMPONENTS os sig dev math ${YARP_ADDITIONAL_COMPONENTS_REQUIRED}) +if(GAZEBO_YARP_PLUGINS_HAS_YARP_ROBOTINTERFACE) + list(APPEND YARP_ADDITIONAL_COMPONENTS_REQUIRED "robotinterface") +endif() + +find_package(YARP 3.4 REQUIRED COMPONENTS os sig dev math robotinterface ${YARP_ADDITIONAL_COMPONENTS_REQUIRED}) find_package(Gazebo REQUIRED) if (Gazebo_VERSION_MAJOR LESS 7.0) message(status "Gazebo version : " ${Gazebo_VERSION_MAJOR}.${Gazebo_VERSION_MINOR}.${Gazebo_VERSION_PATCH}) diff --git a/libraries/singleton/include/GazeboYarpPlugins/Handler.hh b/libraries/singleton/include/GazeboYarpPlugins/Handler.hh index 73ca7dd9c..6aabf5250 100644 --- a/libraries/singleton/include/GazeboYarpPlugins/Handler.hh +++ b/libraries/singleton/include/GazeboYarpPlugins/Handler.hh @@ -37,11 +37,8 @@ namespace gazebo } } -namespace yarp { - namespace dev { - class PolyDriver; - } -} +#include +#include namespace GazeboYarpPlugins { @@ -121,6 +118,16 @@ public: */ void removeDevice(const std::string& deviceName); + /** + * \brief Returns a list of the opened devices + * \note This list acts just as a view of the available devices, + * and it does not transfer or share ownership of the devices. + * The consumer code needs to make sure that the driver lifetime + * is longer then the consumer lifetime. + */ + void getDevicesAsPolyDriverList(yarp::dev::PolyDriverList& list); + + /** Destructor */ ~Handler(); diff --git a/libraries/singleton/src/Handler.cc b/libraries/singleton/src/Handler.cc index e5340994e..f53735896 100644 --- a/libraries/singleton/src/Handler.cc +++ b/libraries/singleton/src/Handler.cc @@ -228,4 +228,15 @@ void Handler::removeDevice(const std::string& deviceName) return; } +void Handler::getDevicesAsPolyDriverList(yarp::dev::PolyDriverList& list) +{ + for(auto&& devicesMapElem: m_devicesMap) { + yDebug() << "DEBUG TO REMOVE: Add device to " << devicesMapElem.first; + list.push(devicesMapElem.second.object(), devicesMapElem.first.c_str()); + } + + return; +} + + } diff --git a/plugins/CMakeLists.txt b/plugins/CMakeLists.txt index 3a8e6489a..11ee008a4 100644 --- a/plugins/CMakeLists.txt +++ b/plugins/CMakeLists.txt @@ -40,3 +40,7 @@ add_subdirectory(contactloadcellarray) add_subdirectory(modelposepublisher) add_subdirectory(basestate) add_subdirectory(configurationoverride) + +if(GAZEBO_YARP_PLUGINS_HAS_YARP_ROBOTINTERFACE) + add_subdirectory(robotinterface) +endif() \ No newline at end of file diff --git a/plugins/robotinterface/CMakeLists.txt b/plugins/robotinterface/CMakeLists.txt new file mode 100644 index 000000000..ef2245c6b --- /dev/null +++ b/plugins/robotinterface/CMakeLists.txt @@ -0,0 +1,12 @@ +# Copyright (C) Fondazione Istituto Italiano di Tecnologia +# CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + + +include(AddGazeboYarpPluginTarget) + +add_gazebo_yarp_plugin_target(LIBRARY_NAME robotinterface + INCLUDE_DIRS include/gazebo + SYSTEM_INCLUDE_DIRS ${GAZEBO_YARP_COMMON_HEADERS} ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${SDFORMAT_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIRS} + LINKED_LIBRARIES gazebo_yarp_lib_common gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} + HEADERS include/gazebo/GazeboYarpRobotInterface.hh + SOURCES src/GazeboYarpRobotInterface.cc) diff --git a/plugins/robotinterface/README.md b/plugins/robotinterface/README.md new file mode 100644 index 000000000..049ab6ea6 --- /dev/null +++ b/plugins/robotinterface/README.md @@ -0,0 +1,5 @@ +gazebo_yarp_robotinterface +========================== + +The `gazebo_yarp_robotinterface` plugin permits to load several [YARP devices]() that can be attached to YARP devices +already opened by other Gazebo-YARP plugins using the same XML format used by the [`yarprobotinterface`](http://www.yarp.it/git-master/yarprobotinterface.html) tool and the [`libYARP_robotinterface` C++ library](https://github.com/robotology/yarp/tree/master/src/libYARP_robotinterface). \ No newline at end of file diff --git a/plugins/robotinterface/include/gazebo/GazeboYarpRobotInterface.hh b/plugins/robotinterface/include/gazebo/GazeboYarpRobotInterface.hh new file mode 100644 index 000000000..c00264fbf --- /dev/null +++ b/plugins/robotinterface/include/gazebo/GazeboYarpRobotInterface.hh @@ -0,0 +1,37 @@ +/* + * Copyright (C) Fondazione Istituto Italiano di Tecnologia + * CopyPolicy: Released under the terms of the LGPLv2.1 or any later version, see LGPL.TXT or LGPL3.TXT + */ + +#ifndef GAZEBOYARP_ROBOTINTERFACEPLUGIN_HH +#define GAZEBOYARP_ROBOTINTERFACEPLUGIN_HH + +#include +#include + +#include + +namespace gazebo +{ + class GazeboYarpRobotInterface; +} + +/** + * See gazebo-yarp-plugins/plugins/robotinterface/README.md for documentation on this plugin. + * + */ +class gazebo::GazeboYarpRobotInterface : public gazebo::ModelPlugin +{ +public: + GazeboYarpRobotInterface(); + virtual ~GazeboYarpRobotInterface(); + + void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf); + +private: + yarp::robotinterface::experimental::XMLReader m_xmlRobotInterfaceReader; + yarp::robotinterface::experimental::XMLReaderResult m_xmlRobotInterfaceResult; +}; + + +#endif diff --git a/plugins/robotinterface/src/GazeboYarpRobotInterface.cc b/plugins/robotinterface/src/GazeboYarpRobotInterface.cc new file mode 100644 index 000000000..e90de0c50 --- /dev/null +++ b/plugins/robotinterface/src/GazeboYarpRobotInterface.cc @@ -0,0 +1,106 @@ +/* + * Copyright (C) 2013-2015 Fondazione Istituto Italiano di Tecnologia RBCS & iCub Facility & ADVR + * Authors: see AUTHORS file. + * CopyPolicy: Released under the terms of the LGPLv2.1 or any later version, see LGPL.TXT or LGPL3.TXT + */ + +#include "GazeboYarpRobotInterface.hh" +#include +#include + +#include + +#include +#include + +namespace gazebo +{ + +GazeboYarpRobotInterface::GazeboYarpRobotInterface() +{ + std::cerr << "GazeboYarpRobotInterface constructor" << std::endl; +} + +GazeboYarpRobotInterface::~GazeboYarpRobotInterface() +{ + // Close robotinterface + bool ok = m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::experimental::ActionPhaseInterrupt1); + if (!ok) { + yError() << "GazeboYarpRobotInterface: impossible to run phase ActionPhaseInterrupt1 robotinterface"; + } + ok = m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::experimental::ActionPhaseShutdown); + if (!ok) { + yError() << "GazeboYarpRobotInterface: impossible to run phase ActionPhaseShutdown in robotinterface"; + } + + yarp::os::Network::fini(); +} + +void GazeboYarpRobotInterface::Load(physics::ModelPtr _parentModel, sdf::ElementPtr _sdf) +{ + yarp::os::Network::init(); + + if (!yarp::os::Network::checkNetwork(GazeboYarpPlugins::yarpNetworkInitializationTimeout)) { + yError() << "GazeboYarpRobotInterface : yarp network does not seem to be available, is the yarpserver running?"; + return; + } + + if (!_parentModel) { + gzerr << "GazeboYarpRobotInterface plugin requires a parent.\n"; + return; + } + + GazeboYarpPlugins::Handler::getHandler()->setRobot(get_pointer(_parentModel)); + + // Getting .xml and loading configuration file from sdf + bool loaded_configuration = false; + if (_sdf->HasElement("yarpRobotInterfaceConfigurationFile")) + { + std::string robotinterface_file_name = _sdf->Get("yarpRobotInterfaceConfigurationFile"); + std::string robotinterface_file_path = gazebo::common::SystemPaths::Instance()->FindFileURI(robotinterface_file_name); + + if (robotinterface_file_name == "") { + yError() << "GazeboYarpRobotInterface error: failure in finding robotinterface configuration for model" << _parentModel->GetName() << "\n" + << "GazeboYarpRobotInterface error: yarpRobotInterfaceConfigurationFile : " << robotinterface_file_name << "\n" + << "GazeboYarpRobotInterface error: yarpRobotInterfaceConfigurationFile absolute path : " << robotinterface_file_path; + loaded_configuration = false; + } else { + m_xmlRobotInterfaceResult = m_xmlRobotInterfaceReader.getRobotFromFile(robotinterface_file_path); + + if (m_xmlRobotInterfaceResult.parsingIsSuccessful) { + loaded_configuration = true; + } else { + yError() << "GazeboYarpRobotInterface error: failure in loading robotinterface configuration for model" << _parentModel->GetName() << "\n" + << "GazeboYarpRobotInterface error: yarpRobotInterfaceConfigurationFile : " << robotinterface_file_name << "\n" + << "GazeboYarpRobotInterface error: yarpRobotInterfaceConfigurationFile absolute path : " << robotinterface_file_path; + loaded_configuration = false; + } + } + } + + if (!loaded_configuration) { + yError() << "GazeboYarpRobotInterface : xml file specified in yarpRobotInterfaceConfigurationFile not found or not loaded."; + return; + } + + // Extract externalDriverList of devices from the one that have been already opened in the Gazebo model by other gazebo_yarp plugins + yarp::dev::PolyDriverList externalDriverList; + GazeboYarpPlugins::Handler::getHandler()->getDevicesAsPolyDriverList(externalDriverList); + + // Set external devices from the one that have been already opened in the Gazebo model by other gazebo_yarp plugins + bool ok = m_xmlRobotInterfaceResult.robot.setExternalDevices(externalDriverList); + if (!ok) { + yError() << "GazeboYarpRobotInterface : impossible to set external devices"; + } + + // Start robotinterface + ok = m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::experimental::ActionPhaseStartup); + if (!ok) { + yError() << "GazeboYarpRobotInterface : impossible to start robotinterface"; + } +} + + +// Register this plugin with the simulator +GZ_REGISTER_MODEL_PLUGIN(GazeboYarpRobotInterface) +} diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 4d2bdfa13..314471426 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -33,4 +33,8 @@ target_compile_definitions(ControlBoardControlTest PRIVATE -DCMAKE_CURRENT_BINAR target_compile_definitions(ControlBoardControlTest PRIVATE -DGAZEBO_YARP_PLUGINS_DIR="$") add_test(NAME ControlBoardControlTest COMMAND ControlBoardControlTest) # Ensure that YARP devices can be found -set_property(TEST ControlBoardControlTest PROPERTY ENVIRONMENT YARP_DATA_DIRS=${YARP_DATA_INSTALL_DIR_FULL}) \ No newline at end of file +set_property(TEST ControlBoardControlTest PROPERTY ENVIRONMENT YARP_DATA_DIRS=${YARP_DATA_INSTALL_DIR_FULL}) + +if(GAZEBO_YARP_PLUGINS_HAS_YARP_ROBOTINTERFACE) + add_subdirectory(robotinterface) +endif() \ No newline at end of file diff --git a/tests/robotinterface/CMakeLists.txt b/tests/robotinterface/CMakeLists.txt new file mode 100644 index 000000000..d6102faa4 --- /dev/null +++ b/tests/robotinterface/CMakeLists.txt @@ -0,0 +1,14 @@ +# Copyright (C) 2020 Istituto Italiano di Tecnologia +# CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + + +add_executable(RobotInterfaceTest RobotInterfaceTest.cc) +target_include_directories(RobotInterfaceTest PUBLIC ${GAZEBO_INCLUDE_DIRS}) +target_link_libraries(RobotInterfaceTest PUBLIC ${GAZEBO_LIBRARIES} ${GAZEBO_TEST_LIB} gyp-gazebo-classic-gtest YARP::YARP_dev YARP::YARP_os ${Boost_LIBRARIES} ${PROTOBUF_LIBRARIES} gazebo_yarp_lib_common gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES}) +target_compile_definitions(RobotInterfaceTest PRIVATE -DCMAKE_CURRENT_SOURCE_DIR="${CMAKE_CURRENT_SOURCE_DIR}") +target_compile_definitions(RobotInterfaceTest PRIVATE -DCMAKE_CURRENT_BINARY_DIR="${CMAKE_CURRENT_BINARY_DIR}") +target_compile_definitions(RobotInterfaceTest PRIVATE -DGAZEBO_YARP_PLUGINS_DIR="$") +add_test(NAME RobotInterfaceTest COMMAND RobotInterfaceTest) +# Ensure that YARP devices can be found +# Disable use of online model database +set_property(TEST RobotInterfaceTest PROPERTY ENVIRONMENT YARP_DATA_DIRS=${YARP_DATA_INSTALL_DIR_FULL};GAZEBO_MODEL_DATABASE_URI=) \ No newline at end of file diff --git a/tests/robotinterface/RobotInterfaceControlBoardConfig.ini b/tests/robotinterface/RobotInterfaceControlBoardConfig.ini new file mode 100644 index 000000000..6d719c3d4 --- /dev/null +++ b/tests/robotinterface/RobotInterfaceControlBoardConfig.ini @@ -0,0 +1,41 @@ +[WRAPPER] +# name of the wrapper device to be instatiated by the factory +device controlboardwrapper2 +# rate of output streaming from ports in ms +period 10 +# output port name +name /pendulumGazebo/body +# Total number of joints +joints 1 +# list of MotorControl device to use +networks ( pendulum_controlboard ) +# for each network specify the joint map +pendulum 0 0 0 0 +# Verbose output (on if present, off if commented out) +# verbose + +# Specify configuration of MotorControl devices +[pendulum_controlboard] +# name of the device to be instatiated by the factory +device gazebo_controlboard +#jointNames list +jointNames upper_joint +name pendulum + +#PIDs: + +[POSITION_CONTROL] +controlUnits metric_units +controlLaw joint_pid_gazebo_v1 +kp 20.0 +kd 0.122 +ki 0.003 +maxInt 9999 +maxOutput 9999 +shift 0.0 +ko 0.0 +stictionUp 0.0 +stictionDwn 0.0 + +[VELOCITY_CONTROL] +velocityControlImplementationType integrator_and_position_pid diff --git a/tests/robotinterface/RobotInterfaceTest.cc b/tests/robotinterface/RobotInterfaceTest.cc new file mode 100644 index 000000000..3bfff2613 --- /dev/null +++ b/tests/robotinterface/RobotInterfaceTest.cc @@ -0,0 +1,110 @@ +/* + * Copyright (C) 2020 Fondazione Istituto Italiano di Tecnologia + * + * Licensed under either the GNU Lesser General Public License v3.0 : + * https://www.gnu.org/licenses/lgpl-3.0.html + * or the GNU Lesser General Public License v2.1 : + * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html + * at your option. + */ + +#include +#include + +#include +#include +#include +#include +#include +#include + +class RobotInterfaceTest : public gazebo::ServerFixture, + public testing::WithParamInterface +{ + struct PluginTestHelperOptions + { + }; + + public: gazebo::event::ConnectionPtr updateConnection; + + public: void PluginTest(const std::string &_physicsEngine); + public: void PluginTestHelper(const std::string &_physicsEngine, + const std::string &worldName, + const PluginTestHelperOptions& options); +}; + +void RobotInterfaceTest::PluginTestHelper(const std::string &_physicsEngine, + const std::string &worldName, + const PluginTestHelperOptions& options) +{ + bool worldPaused = true; + std::string worldAbsPath = CMAKE_CURRENT_SOURCE_DIR"/" + worldName; + Load(worldAbsPath, worldPaused, _physicsEngine); + + gazebo::physics::WorldPtr world = gazebo::physics::get_world("default"); + ASSERT_TRUE(world != NULL); + + // Verify physics engine type + gazebo::physics::PhysicsEnginePtr physics = world->Physics(); + ASSERT_TRUE(physics != NULL); + EXPECT_EQ(physics->GetType(), _physicsEngine); + + gzdbg << "RobotInterfaceTest: testing world " << worldName << " with physics engine " << _physicsEngine << std::endl; + + // Run a few step of simulation to ensure that YARP plugin start correctly + world->Step(10); + + // Try to connect to the model controlboard using the wrapper opened by the + // gazebo_yarp_robotinterface plugin + yarp::os::Property option; + yarp::dev::PolyDriver driver; + yarp::dev::IEncoders *ienc = 0; + option.put("device","remote_controlboard"); + option.put("remote","/pendulumGazebo/openedByTheRobotInterface"); + option.put("local","/RobotInterfaceTest"); + + ASSERT_TRUE(driver.open(option)); + + // open the views + ASSERT_TRUE(driver.view(ienc)); + + // retrieve number of axes + int nAxes = 0; + ienc->getAxes(&nAxes); + EXPECT_EQ(nAxes, 1); + + // Unload the simulation + Unload(); +} + +///////////////////////////////////////////////////////////////////// +void RobotInterfaceTest::PluginTest(const std::string &_physicsEngine) +{ + // Make sure that the YARP network does not require yarpserver running + yarp::os::NetworkBase::setLocalMode(true); + + // Defined by CMake + std::string pluginDir = GAZEBO_YARP_PLUGINS_DIR; + gazebo::common::SystemPaths::Instance()->AddPluginPaths(pluginDir); + std::string modelDir = CMAKE_CURRENT_SOURCE_DIR; + gazebo::common::SystemPaths::Instance()->AddModelPaths(modelDir); + + PluginTestHelperOptions options; + this->PluginTestHelper(_physicsEngine, "RobotInterfaceTest.world", options); +} + +TEST_P(RobotInterfaceTest, PluginTest) +{ + PluginTest(GetParam()); +} + +// Only test for ode +INSTANTIATE_TEST_CASE_P(PhysicsEngines, RobotInterfaceTest, ::testing::Values("ode")); + +///////////////////////////////////////////////// +/// Main +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/tests/robotinterface/RobotInterfaceTest.world b/tests/robotinterface/RobotInterfaceTest.world new file mode 100644 index 000000000..79e1984db --- /dev/null +++ b/tests/robotinterface/RobotInterfaceTest.world @@ -0,0 +1,186 @@ + + + + + model://ground_plane + + + model://sun + + + + + + 100 + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + + + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + + + + 0 0 2.1 -1.5708 0 0 + 0 + + 0 0 0.5 0 0 0 + + 0.01 + 0 + 0 + 0.01 + 0 + 0.01 + + 1.0 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + base + upper_link + + 1.0 0 0 + + -100 + 100 + 1000.0 + 50 + + + 4 + 0 + 0 + 0 + + + + + + + + model://RobotInterfaceControlBoardConfig.ini + 0.0 + + + + model://RobotInterfaceTest.xml + + + + diff --git a/tests/robotinterface/RobotInterfaceTest.xml b/tests/robotinterface/RobotInterfaceTest.xml new file mode 100644 index 000000000..b8af4a8c0 --- /dev/null +++ b/tests/robotinterface/RobotInterfaceTest.xml @@ -0,0 +1,21 @@ + + + + + + 10 + /pendulumGazebo/openedByTheRobotInterface + + ( 0 0 0 0 ) + + 1 + + + + pendulum_with_base::pendulum + + + + + +