From 9a9dcd9b1b013f2777de90c7f62ed76cfcfd5e3d Mon Sep 17 00:00:00 2001 From: VX792 Date: Tue, 1 Mar 2022 13:04:35 +0100 Subject: [PATCH] Port four bar linkage transmission loader from ROS1 --- .../hardware_interface/hardware_info.hpp | 1 + hardware_interface/src/component_parser.cpp | 4 +- transmission_interface/CMakeLists.txt | 10 + .../four_bar_linkage_transmission_loader.hpp | 50 ++ .../ros2_control_plugins.xml | 8 + .../four_bar_linkage_transmission_loader.cpp | 61 +++ ...r_bar_linkage_transmission_loader_test.cpp | 487 ++++++++++++++++++ 7 files changed, 620 insertions(+), 1 deletion(-) create mode 100644 transmission_interface/include/transmission_interface/four_bar_linkage_transmission_loader.hpp create mode 100644 transmission_interface/src/four_bar_linkage_transmission_loader.cpp create mode 100644 transmission_interface/test/four_bar_linkage_transmission_loader_test.cpp diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index ae57c9b8bfb..c02925bd0a8 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -84,6 +84,7 @@ struct ActuatorInfo std::string name; std::vector interfaces; std::string role; + double mechanical_reduction = 1.0; double offset = 0.0; }; diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 34a89906c59..832ac76904c 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -385,6 +385,8 @@ ActuatorInfo parse_transmission_actuator_from_xml(const tinyxml2::XMLElement * e ActuatorInfo actuator_info; actuator_info.name = get_attribute_value(element_it, kNameAttribute, element_it->Name()); actuator_info.role = get_attribute_value(element_it, kRoleAttribute, element_it->Name()); + actuator_info.mechanical_reduction = + get_parameter_value_or(element_it->FirstChildElement(), kReductionAttribute, 1.0); actuator_info.offset = get_parameter_value_or(element_it->FirstChildElement(), kOffsetAttribute, 0.0); return actuator_info; @@ -474,7 +476,7 @@ void auto_fill_transmission_interfaces(HardwareInfo & hardware) } transmission.actuators.push_back( - ActuatorInfo{"actuator1", transmission.joints[0].interfaces, "actuator1", 0.0}); + ActuatorInfo{"actuator1", transmission.joints[0].interfaces, "actuator1", 1.0, 0.0}); } } } diff --git a/transmission_interface/CMakeLists.txt b/transmission_interface/CMakeLists.txt index 91fc480f5b6..12eb1ced80d 100644 --- a/transmission_interface/CMakeLists.txt +++ b/transmission_interface/CMakeLists.txt @@ -19,7 +19,10 @@ install( add_library(transmission_interface SHARED include/transmission_interface/transmission_loader.hpp include/transmission_interface/simple_transmission_loader.hpp + include/transmission_interface/four_bar_linkage_transmission_loader.hpp + src/simple_transmission_loader.cpp + src/four_bar_linkage_transmission_loader.cpp ) target_include_directories(transmission_interface PUBLIC include) @@ -76,6 +79,13 @@ if(BUILD_TESTING) ) target_include_directories(test_simple_transmission_loader PUBLIC include hardware_interface) ament_target_dependencies(test_simple_transmission_loader hardware_interface) + + ament_add_gmock( + test_four_bar_linkage_transmission_loader + test/four_bar_linkage_transmission_loader_test.cpp + ) + target_include_directories(test_four_bar_linkage_transmission_loader PUBLIC include hardware_interface) + ament_target_dependencies(test_four_bar_linkage_transmission_loader hardware_interface) endif() ament_export_include_directories( diff --git a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission_loader.hpp b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission_loader.hpp new file mode 100644 index 00000000000..7fd955fbcfa --- /dev/null +++ b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission_loader.hpp @@ -0,0 +1,50 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2022, PAL Robotics S.L. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics S.L. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +#ifndef TRANSMISSION_INTERFACE__FOUR_BAR_LINKAGE_TRANSMISSION_LOADER_HPP_ +#define TRANSMISSION_INTERFACE__FOUR_BAR_LINKAGE_TRANSMISSION_LOADER_HPP_ + +#include + +#include "transmission_interface/transmission.hpp" +#include "transmission_interface/transmission_loader.hpp" + +namespace transmission_interface +{ +/** + * \brief Class for loading a four-bar linkage transmission instance from configuration data. + */ +class FourBarLinkageTransmissionLoader : public TransmissionLoader +{ +public: + std::shared_ptr load( + const hardware_interface::TransmissionInfo & transmission_info) override; +}; + +} // namespace transmission_interface + +#endif // TRANSMISSION_INTERFACE__FOUR_BAR_LINKAGE_TRANSMISSION_LOADER_HPP_ diff --git a/transmission_interface/ros2_control_plugins.xml b/transmission_interface/ros2_control_plugins.xml index 4bf22d1a989..97ee352efa9 100644 --- a/transmission_interface/ros2_control_plugins.xml +++ b/transmission_interface/ros2_control_plugins.xml @@ -7,4 +7,12 @@ Load configuration of a simple transmission from a URDF description. + + + + Load configuration of a four bar linkage transmission from a URDF description. + + diff --git a/transmission_interface/src/four_bar_linkage_transmission_loader.cpp b/transmission_interface/src/four_bar_linkage_transmission_loader.cpp new file mode 100644 index 00000000000..2f8add7c39e --- /dev/null +++ b/transmission_interface/src/four_bar_linkage_transmission_loader.cpp @@ -0,0 +1,61 @@ +// Copyright 2022 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "transmission_interface/four_bar_linkage_transmission_loader.hpp" + +#include + +#include "hardware_interface/component_parser.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/rclcpp.hpp" +#include "transmission_interface/four_bar_linkage_transmission.hpp" + +namespace transmission_interface +{ +std::shared_ptr FourBarLinkageTransmissionLoader::load( + const hardware_interface::TransmissionInfo & transmission_info) +{ + try + { + const auto act_reduction1 = transmission_info.actuators.at(0).mechanical_reduction; + const auto act_reduction2 = transmission_info.actuators.at(1).mechanical_reduction; + + const auto jnt_reduction1 = transmission_info.joints.at(0).mechanical_reduction; + const auto jnt_reduction2 = transmission_info.joints.at(1).mechanical_reduction; + + const auto jnt_offset1 = transmission_info.joints.at(0).offset; + const auto jnt_offset2 = transmission_info.joints.at(1).offset; + + std::shared_ptr transmission(new FourBarLinkageTransmission( + {act_reduction1, act_reduction2}, {jnt_reduction1, jnt_reduction2}, + {jnt_offset1, jnt_offset2})); + return transmission; + } + catch (const Exception & ex) + { + RCLCPP_ERROR( + rclcpp::get_logger("four_bar_linkage_transmission_loader"), + "Failed to construct transmission '%s'", ex.what()); + return std::shared_ptr(); + } +} + +} // namespace transmission_interface + +PLUGINLIB_EXPORT_CLASS( + transmission_interface::FourBarLinkageTransmissionLoader, + transmission_interface::TransmissionLoader) diff --git a/transmission_interface/test/four_bar_linkage_transmission_loader_test.cpp b/transmission_interface/test/four_bar_linkage_transmission_loader_test.cpp new file mode 100644 index 00000000000..7a7c8f6ad26 --- /dev/null +++ b/transmission_interface/test/four_bar_linkage_transmission_loader_test.cpp @@ -0,0 +1,487 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2022, PAL Robotics S.L. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics S.L. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +#include +#include +#include +#include +#include + +#include "hardware_interface/component_parser.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "pluginlib/class_loader.hpp" +#include "transmission_interface/four_bar_linkage_transmission.hpp" +#include "transmission_interface/four_bar_linkage_transmission_loader.hpp" +#include "transmission_interface/transmission_loader.hpp" + +using testing::SizeIs; + +class TransmissionPluginLoader +{ +public: + std::shared_ptr create(const std::string & type) + { + try + { + return class_loader_.createUniqueInstance(type); + } + catch (std::exception & ex) + { + std::cerr << ex.what() << std::endl; + return std::shared_ptr(); + } + } + +private: + // must keep it alive because instance destroyers need it + pluginlib::ClassLoader class_loader_ = { + "transmission_interface", "transmission_interface::TransmissionLoader"}; +}; + +TEST(FourBarLinkageTransmissionLoaderTest, FullSpec) +{ + // Parse transmission info + std::string urdf_to_test = R"( + + + + + + -0.5 + 0.5 + + + + + + -1 + 1 + + + + + transmission_interface/FourBarLinkageTransmission + + 50 + + + -50 + + + 0.5 + 2.0 + + + -0.5 + -2.0 + + + + + )"; + + std::vector infos = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(infos[0].transmissions, SizeIs(1)); + + // Transmission loader + TransmissionPluginLoader loader; + std::shared_ptr transmission_loader = + loader.create(infos[0].transmissions[0].type); + ASSERT_TRUE(nullptr != transmission_loader); + + std::shared_ptr transmission; + const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0]; + transmission = transmission_loader->load(info); + ASSERT_TRUE(nullptr != transmission); + + // Validate transmission + transmission_interface::FourBarLinkageTransmission * four_bar_linkage_transmission = + dynamic_cast(transmission.get()); + ASSERT_TRUE(nullptr != four_bar_linkage_transmission); + + const std::vector & actuator_reduction = + four_bar_linkage_transmission->get_actuator_reduction(); + EXPECT_EQ(50.0, actuator_reduction[0]); + EXPECT_EQ(-50.0, actuator_reduction[1]); + + const std::vector & joint_reduction = + four_bar_linkage_transmission->get_joint_reduction(); + EXPECT_EQ(2.0, joint_reduction[0]); + EXPECT_EQ(-2.0, joint_reduction[1]); + + const std::vector & joint_offset = four_bar_linkage_transmission->get_joint_offset(); + EXPECT_EQ(0.5, joint_offset[0]); + EXPECT_EQ(-0.5, joint_offset[1]); +} + +TEST(FourBarLinkageTransmissionLoaderTest, only_mech_red_specified) +{ + // Parse transmission info + std::string urdf_to_test = R"( + + + + + + -0.5 + 0.5 + + + + + + -1 + 1 + + + + + transmission_interface/FourBarLinkageTransmission + + 50 + + + -50 + + + 1.0 + + + 1.0 + + + + + )"; + std::vector infos = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(infos[0].transmissions, SizeIs(1)); + + // Transmission loader + TransmissionPluginLoader loader; + std::shared_ptr transmission_loader = + loader.create(infos[0].transmissions[0].type); + ASSERT_TRUE(nullptr != transmission_loader); + + std::shared_ptr transmission = nullptr; + const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0]; + transmission = transmission_loader->load(info); + ASSERT_TRUE(nullptr != transmission); + + // Validate transmission + transmission_interface::FourBarLinkageTransmission * four_bar_linkage_transmission = + dynamic_cast(transmission.get()); + + const std::vector & actuator_reduction = + four_bar_linkage_transmission->get_actuator_reduction(); + EXPECT_EQ(50.0, actuator_reduction[0]); + EXPECT_EQ(-50.0, actuator_reduction[1]); + + const std::vector & joint_reduction = + four_bar_linkage_transmission->get_joint_reduction(); + EXPECT_EQ(1.0, joint_reduction[0]); + EXPECT_EQ(1.0, joint_reduction[1]); + + const std::vector & joint_offset = four_bar_linkage_transmission->get_joint_offset(); + EXPECT_EQ(0.0, joint_offset[0]); + EXPECT_EQ(0.0, joint_offset[1]); +} + +TEST(SimpleTransmissionLoaderTest, offset_and_mech_red_not_specified) +{ + // Parse transmission info + std::string urdf_to_test = R"( + + + + + + -0.5 + 0.5 + + + + + + -1 + 1 + + + + + transmission_interface/FourBarLinkageTransmission + + + + + + + + )"; + std::vector infos = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(infos[0].transmissions, SizeIs(1)); + + // Transmission loader + TransmissionPluginLoader loader; + std::shared_ptr transmission_loader = + loader.create(infos[0].transmissions[0].type); + ASSERT_TRUE(nullptr != transmission_loader); + + std::shared_ptr transmission = nullptr; + const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0]; + transmission = transmission_loader->load(info); + ASSERT_TRUE(nullptr != transmission); + + // Validate transmission + transmission_interface::FourBarLinkageTransmission * four_bar_linkage_transmission = + dynamic_cast(transmission.get()); + + const std::vector & actuator_reduction = + four_bar_linkage_transmission->get_actuator_reduction(); + EXPECT_EQ(1.0, actuator_reduction[0]); + EXPECT_EQ(1.0, actuator_reduction[1]); + + const std::vector & joint_reduction = + four_bar_linkage_transmission->get_joint_reduction(); + EXPECT_EQ(1.0, joint_reduction[0]); + EXPECT_EQ(1.0, joint_reduction[1]); + + const std::vector & joint_offset = four_bar_linkage_transmission->get_joint_offset(); + EXPECT_EQ(0.0, joint_offset[0]); + EXPECT_EQ(0.0, joint_offset[1]); +} + +TEST(FourBarLinkageTransmissionLoaderTest, mechanical_reduction_not_a_number) +{ + // Parse transmission info + std::string urdf_to_test = R"( + + + + + + -0.5 + 0.5 + + + + + + -1 + 1 + + + + + transmission_interface/FourBarLinkageTransmission + + one + + + two + + + three + + + four + + + + + )"; + std::vector infos = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(infos[0].transmissions, SizeIs(1)); + + // Transmission loader + TransmissionPluginLoader loader; + std::shared_ptr transmission_loader = + loader.create(infos[0].transmissions[0].type); + ASSERT_TRUE(nullptr != transmission_loader); + + std::shared_ptr transmission = nullptr; + const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0]; + transmission = transmission_loader->load(info); + ASSERT_TRUE(nullptr != transmission); + + // Validate transmission + transmission_interface::FourBarLinkageTransmission * four_bar_linkage_transmission = + dynamic_cast(transmission.get()); + + // default kicks in for ill-defined values + const std::vector & actuator_reduction = + four_bar_linkage_transmission->get_actuator_reduction(); + EXPECT_EQ(1.0, actuator_reduction[0]); + EXPECT_EQ(1.0, actuator_reduction[1]); + + const std::vector & joint_reduction = + four_bar_linkage_transmission->get_joint_reduction(); + EXPECT_EQ(1.0, joint_reduction[0]); + EXPECT_EQ(1.0, joint_reduction[1]); + + const std::vector & joint_offset = four_bar_linkage_transmission->get_joint_offset(); + EXPECT_EQ(0.0, joint_offset[0]); + EXPECT_EQ(0.0, joint_offset[1]); +} + +TEST(FourBarLinkageTransmissionLoaderTest, offset_ill_defined) +{ + // Parse transmission info + std::string urdf_to_test = R"( + + + + + + -0.5 + 0.5 + + + + + + -1 + 1 + + + + + transmission_interface/FourBarLinkageTransmission + + 50 + + + -50 + + + two + 2.0 + + + three + -2.0 + + + + + )"; + std::vector infos = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(infos[0].transmissions, SizeIs(1)); + + // Transmission loader + TransmissionPluginLoader loader; + std::shared_ptr transmission_loader = + loader.create(infos[0].transmissions[0].type); + ASSERT_TRUE(nullptr != transmission_loader); + + std::shared_ptr transmission = nullptr; + const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0]; + transmission = transmission_loader->load(info); + ASSERT_TRUE(nullptr != transmission); + + // Validate transmission + transmission_interface::FourBarLinkageTransmission * four_bar_linkage_transmission = + dynamic_cast(transmission.get()); + + // default kicks in for ill-defined values + const std::vector & actuator_reduction = + four_bar_linkage_transmission->get_actuator_reduction(); + EXPECT_EQ(50.0, actuator_reduction[0]); + EXPECT_EQ(-50.0, actuator_reduction[1]); + + const std::vector & joint_reduction = + four_bar_linkage_transmission->get_joint_reduction(); + EXPECT_EQ(2.0, joint_reduction[0]); + EXPECT_EQ(-2.0, joint_reduction[1]); + + // default kicks in for ill-defined values + const std::vector & joint_offset = four_bar_linkage_transmission->get_joint_offset(); + EXPECT_EQ(0.0, joint_offset[0]); + EXPECT_EQ(0.0, joint_offset[1]); +} + +TEST(FourBarLinkageTransmissionLoaderTest, mech_red_invalid_value) +{ + // Parse transmission info + std::string urdf_to_test = R"( + + + + + + -0.5 + 0.5 + + + + + + -1 + 1 + + + + + transmission_interface/FourBarLinkageTransmission + + 0 + + + 0 + + + 2 + 0 + + + 3 + 0 + + + + + )"; + std::vector infos = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(infos[0].transmissions, SizeIs(1)); + + // Transmission loader + TransmissionPluginLoader loader; + std::shared_ptr transmission_loader = + loader.create(infos[0].transmissions[0].type); + ASSERT_TRUE(nullptr != transmission_loader); + + std::shared_ptr transmission = nullptr; + const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0]; + transmission = transmission_loader->load(info); + ASSERT_TRUE(nullptr == transmission); +}