From a283fcf2716d2060d6fa4bc92456c1c72ad61bdf Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 4 Nov 2024 16:10:45 +0100 Subject: [PATCH 1/5] migrate the transmission_interface to the new variants changes --- .../transmission_interface/accessor.hpp | 6 +- .../differential_transmission.hpp | 66 +++++++++---------- .../four_bar_linkage_transmission.hpp | 52 +++++++-------- .../include/transmission_interface/handle.hpp | 4 ++ .../simple_transmission.hpp | 57 +++++++++------- .../transmission_interface/transmission.hpp | 24 ++++++- 6 files changed, 124 insertions(+), 85 deletions(-) diff --git a/transmission_interface/include/transmission_interface/accessor.hpp b/transmission_interface/include/transmission_interface/accessor.hpp index a72b0f39b0..5d938fb607 100644 --- a/transmission_interface/include/transmission_interface/accessor.hpp +++ b/transmission_interface/include/transmission_interface/accessor.hpp @@ -47,7 +47,7 @@ std::vector get_names(const std::vector & handles) std::set names; std::transform( handles.cbegin(), handles.cend(), std::inserter(names, names.end()), - [](const auto & handle) { return handle.get_prefix_name(); }); + [](const auto & handle) { return handle->get_prefix_name(); }); return std::vector(names.begin(), names.end()); } @@ -63,8 +63,8 @@ std::vector get_ordered_handles( unordered_handles.cbegin(), unordered_handles.cend(), std::back_inserter(result), [&](const auto & handle) { - return (handle.get_prefix_name() == name) && - (handle.get_interface_name() == interface_type) && handle; + return (handle->get_prefix_name() == name) && + (handle->get_interface_name() == interface_type) && handle; }); } return result; diff --git a/transmission_interface/include/transmission_interface/differential_transmission.hpp b/transmission_interface/include/transmission_interface/differential_transmission.hpp index b6b4353dd8..3710c4123c 100644 --- a/transmission_interface/include/transmission_interface/differential_transmission.hpp +++ b/transmission_interface/include/transmission_interface/differential_transmission.hpp @@ -125,8 +125,8 @@ class DifferentialTransmission : public Transmission * \pre Handles are valid and matching in size */ void configure( - const std::vector & joint_handles, - const std::vector & actuator_handles) override; + const std::vector & joint_handles, + const std::vector & actuator_handles) override; /// Transform variables from actuator to joint space. /** @@ -159,13 +159,13 @@ class DifferentialTransmission : public Transmission std::vector joint_reduction_; std::vector joint_offset_; - std::vector joint_position_; - std::vector joint_velocity_; - std::vector joint_effort_; + std::vector joint_position_; + std::vector joint_velocity_; + std::vector joint_effort_; - std::vector actuator_position_; - std::vector actuator_velocity_; - std::vector actuator_effort_; + std::vector actuator_position_; + std::vector actuator_velocity_; + std::vector actuator_effort_; }; inline DifferentialTransmission::DifferentialTransmission( @@ -191,8 +191,8 @@ inline DifferentialTransmission::DifferentialTransmission( } void DifferentialTransmission::configure( - const std::vector & joint_handles, - const std::vector & actuator_handles) + const std::vector & joint_handles, + const std::vector & actuator_handles) { if (joint_handles.empty()) { @@ -264,11 +264,11 @@ inline void DifferentialTransmission::actuator_to_joint() { assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); - joint_pos[0].set_value( - (act_pos[0].get_value() / ar[0] + act_pos[1].get_value() / ar[1]) / (2.0 * jr[0]) + + joint_pos[0]->set_value( + (act_pos[0]->get_value() / ar[0] + act_pos[1]->get_value() / ar[1]) / (2.0 * jr[0]) + joint_offset_[0]); - joint_pos[1].set_value( - (act_pos[0].get_value() / ar[0] - act_pos[1].get_value() / ar[1]) / (2.0 * jr[1]) + + joint_pos[1]->set_value( + (act_pos[0]->get_value() / ar[0] - act_pos[1]->get_value() / ar[1]) / (2.0 * jr[1]) + joint_offset_[1]); } @@ -278,10 +278,10 @@ inline void DifferentialTransmission::actuator_to_joint() { assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); - joint_vel[0].set_value( - (act_vel[0].get_value() / ar[0] + act_vel[1].get_value() / ar[1]) / (2.0 * jr[0])); - joint_vel[1].set_value( - (act_vel[0].get_value() / ar[0] - act_vel[1].get_value() / ar[1]) / (2.0 * jr[1])); + joint_vel[0]->set_value( + (act_vel[0]->get_value() / ar[0] + act_vel[1]->get_value() / ar[1]) / (2.0 * jr[0])); + joint_vel[1]->set_value( + (act_vel[0]->get_value() / ar[0] - act_vel[1]->get_value() / ar[1]) / (2.0 * jr[1])); } auto & act_eff = actuator_effort_; @@ -290,10 +290,10 @@ inline void DifferentialTransmission::actuator_to_joint() { assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); - joint_eff[0].set_value( - jr[0] * (act_eff[0].get_value() * ar[0] + act_eff[1].get_value() * ar[1])); - joint_eff[1].set_value( - jr[1] * (act_eff[0].get_value() * ar[0] - act_eff[1].get_value() * ar[1])); + joint_eff[0]->set_value( + jr[0] * (act_eff[0]->get_value() * ar[0] + act_eff[1]->get_value() * ar[1])); + joint_eff[1]->set_value( + jr[1] * (act_eff[0]->get_value() * ar[0] - act_eff[1]->get_value() * ar[1])); } } @@ -309,10 +309,10 @@ inline void DifferentialTransmission::joint_to_actuator() assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); double joints_offset_applied[2] = { - joint_pos[0].get_value() - joint_offset_[0], joint_pos[1].get_value() - joint_offset_[1]}; - act_pos[0].set_value( + joint_pos[0]->get_value() - joint_offset_[0], joint_pos[1]->get_value() - joint_offset_[1]}; + act_pos[0]->set_value( (joints_offset_applied[0] * jr[0] + joints_offset_applied[1] * jr[1]) * ar[0]); - act_pos[1].set_value( + act_pos[1]->set_value( (joints_offset_applied[0] * jr[0] - joints_offset_applied[1] * jr[1]) * ar[1]); } @@ -322,10 +322,10 @@ inline void DifferentialTransmission::joint_to_actuator() { assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); - act_vel[0].set_value( - (joint_vel[0].get_value() * jr[0] + joint_vel[1].get_value() * jr[1]) * ar[0]); - act_vel[1].set_value( - (joint_vel[0].get_value() * jr[0] - joint_vel[1].get_value() * jr[1]) * ar[1]); + act_vel[0]->set_value( + (joint_vel[0]->get_value() * jr[0] + joint_vel[1]->get_value() * jr[1]) * ar[0]); + act_vel[1]->set_value( + (joint_vel[0]->get_value() * jr[0] - joint_vel[1]->get_value() * jr[1]) * ar[1]); } auto & act_eff = actuator_effort_; @@ -334,10 +334,10 @@ inline void DifferentialTransmission::joint_to_actuator() { assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); - act_eff[0].set_value( - (joint_eff[0].get_value() / jr[0] + joint_eff[1].get_value() / jr[1]) / (2.0 * ar[0])); - act_eff[1].set_value( - (joint_eff[0].get_value() / jr[0] - joint_eff[1].get_value() / jr[1]) / (2.0 * ar[1])); + act_eff[0]->set_value( + (joint_eff[0]->get_value() / jr[0] + joint_eff[1]->get_value() / jr[1]) / (2.0 * ar[0])); + act_eff[1]->set_value( + (joint_eff[0]->get_value() / jr[0] - joint_eff[1]->get_value() / jr[1]) / (2.0 * ar[1])); } } diff --git a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp index 107133128c..52e3955476 100644 --- a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp +++ b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp @@ -123,8 +123,8 @@ class FourBarLinkageTransmission : public Transmission * \pre Handles are valid and matching in size */ void configure( - const std::vector & joint_handles, - const std::vector & actuator_handles) override; + const std::vector & joint_handles, + const std::vector & actuator_handles) override; /// Transform variables from actuator to joint space. /** @@ -157,13 +157,13 @@ class FourBarLinkageTransmission : public Transmission std::vector joint_reduction_; std::vector joint_offset_; - std::vector joint_position_; - std::vector joint_velocity_; - std::vector joint_effort_; + std::vector joint_position_; + std::vector joint_velocity_; + std::vector joint_effort_; - std::vector actuator_position_; - std::vector actuator_velocity_; - std::vector actuator_effort_; + std::vector actuator_position_; + std::vector actuator_velocity_; + std::vector actuator_effort_; }; inline FourBarLinkageTransmission::FourBarLinkageTransmission( @@ -188,8 +188,8 @@ inline FourBarLinkageTransmission::FourBarLinkageTransmission( } void FourBarLinkageTransmission::configure( - const std::vector & joint_handles, - const std::vector & actuator_handles) + const std::vector & joint_handles, + const std::vector & actuator_handles) { if (joint_handles.empty()) { @@ -262,9 +262,9 @@ inline void FourBarLinkageTransmission::actuator_to_joint() { assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); - joint_pos[0].set_value(act_pos[0].get_value() / (jr[0] * ar[0]) + joint_offset_[0]); - joint_pos[1].set_value( - (act_pos[1].get_value() / ar[1] - act_pos[0].get_value() / (jr[0] * ar[0])) / jr[1] + + joint_pos[0]->set_value(act_pos[0]->get_value() / (jr[0] * ar[0]) + joint_offset_[0]); + joint_pos[1]->set_value( + (act_pos[1]->get_value() / ar[1] - act_pos[0]->get_value() / (jr[0] * ar[0])) / jr[1] + joint_offset_[1]); } @@ -275,9 +275,9 @@ inline void FourBarLinkageTransmission::actuator_to_joint() { assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); - joint_vel[0].set_value(act_vel[0].get_value() / (jr[0] * ar[0])); - joint_vel[1].set_value( - (act_vel[1].get_value() / ar[1] - act_vel[0].get_value() / (jr[0] * ar[0])) / jr[1]); + joint_vel[0]->set_value(act_vel[0]->get_value() / (jr[0] * ar[0])); + joint_vel[1]->set_value( + (act_vel[1]->get_value() / ar[1] - act_vel[0]->get_value() / (jr[0] * ar[0])) / jr[1]); } // effort @@ -287,9 +287,9 @@ inline void FourBarLinkageTransmission::actuator_to_joint() { assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); - joint_eff[0].set_value(jr[0] * act_eff[0].get_value() * ar[0]); - joint_eff[1].set_value( - jr[1] * (act_eff[1].get_value() * ar[1] - jr[0] * act_eff[0].get_value() * ar[0])); + joint_eff[0]->set_value(jr[0] * act_eff[0]->get_value() * ar[0]); + joint_eff[1]->set_value( + jr[1] * (act_eff[1]->get_value() * ar[1] - jr[0] * act_eff[0]->get_value() * ar[0])); } } @@ -306,9 +306,9 @@ inline void FourBarLinkageTransmission::joint_to_actuator() assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); double joints_offset_applied[2] = { - joint_pos[0].get_value() - joint_offset_[0], joint_pos[1].get_value() - joint_offset_[1]}; - act_pos[0].set_value(joints_offset_applied[0] * jr[0] * ar[0]); - act_pos[1].set_value((joints_offset_applied[0] + joints_offset_applied[1] * jr[1]) * ar[1]); + joint_pos[0]->get_value() - joint_offset_[0], joint_pos[1]->get_value() - joint_offset_[1]}; + act_pos[0]->set_value(joints_offset_applied[0] * jr[0] * ar[0]); + act_pos[1]->set_value((joints_offset_applied[0] + joints_offset_applied[1] * jr[1]) * ar[1]); } // velocity @@ -318,8 +318,8 @@ inline void FourBarLinkageTransmission::joint_to_actuator() { assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); - act_vel[0].set_value(joint_vel[0].get_value() * jr[0] * ar[0]); - act_vel[1].set_value((joint_vel[0].get_value() + joint_vel[1].get_value() * jr[1]) * ar[1]); + act_vel[0]->set_value(joint_vel[0]->get_value() * jr[0] * ar[0]); + act_vel[1]->set_value((joint_vel[0]->get_value() + joint_vel[1]->get_value() * jr[1]) * ar[1]); } // effort @@ -329,8 +329,8 @@ inline void FourBarLinkageTransmission::joint_to_actuator() { assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); - act_eff[0].set_value(joint_eff[0].get_value() / (ar[0] * jr[0])); - act_eff[1].set_value((joint_eff[0].get_value() + joint_eff[1].get_value() / jr[1]) / ar[1]); + act_eff[0]->set_value(joint_eff[0]->get_value() / (ar[0] * jr[0])); + act_eff[1]->set_value((joint_eff[0]->get_value() + joint_eff[1]->get_value() / jr[1]) / ar[1]); } } diff --git a/transmission_interface/include/transmission_interface/handle.hpp b/transmission_interface/include/transmission_interface/handle.hpp index 4c40189648..207af0f35a 100644 --- a/transmission_interface/include/transmission_interface/handle.hpp +++ b/transmission_interface/include/transmission_interface/handle.hpp @@ -15,6 +15,8 @@ #ifndef TRANSMISSION_INTERFACE__HANDLE_HPP_ #define TRANSMISSION_INTERFACE__HANDLE_HPP_ +#include + #include "hardware_interface/handle.hpp" namespace transmission_interface @@ -24,6 +26,7 @@ class ActuatorHandle : public hardware_interface::Handle { public: using hardware_interface::Handle::Handle; + using SharedPtr = std::shared_ptr; }; /** A handle used to get and set a value on a given joint interface. */ @@ -31,6 +34,7 @@ class JointHandle : public hardware_interface::Handle { public: using hardware_interface::Handle::Handle; + using SharedPtr = std::shared_ptr; }; } // namespace transmission_interface diff --git a/transmission_interface/include/transmission_interface/simple_transmission.hpp b/transmission_interface/include/transmission_interface/simple_transmission.hpp index 809472ab75..0947e3485a 100644 --- a/transmission_interface/include/transmission_interface/simple_transmission.hpp +++ b/transmission_interface/include/transmission_interface/simple_transmission.hpp @@ -93,14 +93,14 @@ class SimpleTransmission : public Transmission /// Set up the data the transmission operates on. /** - * \param[in] joint_handles Vector of interface handles of one joint - * \param[in] actuator_handles Vector of interface handles of one actuator + * \param[in] joint_handles Vector of interface handles shared pointers of one joint + * \param[in] actuator_handles Vector of interface handles shared pointers of one actuator * \pre Vectors are nonzero. * \pre Joint handles share the same joint name and actuator handles share the same actuator name. */ void configure( - const std::vector & joint_handles, - const std::vector & actuator_handles) override; + const std::vector & joint_handles, + const std::vector & actuator_handles) override; /// Transform variables from actuator to joint space. /** @@ -128,13 +128,13 @@ class SimpleTransmission : public Transmission double reduction_; double jnt_offset_; - JointHandle joint_position_ = {"", "", nullptr}; - JointHandle joint_velocity_ = {"", "", nullptr}; - JointHandle joint_effort_ = {"", "", nullptr}; + JointHandle::SharedPtr joint_position_; + JointHandle::SharedPtr joint_velocity_; + JointHandle::SharedPtr joint_effort_; - ActuatorHandle actuator_position_ = {"", "", nullptr}; - ActuatorHandle actuator_velocity_ = {"", "", nullptr}; - ActuatorHandle actuator_effort_ = {"", "", nullptr}; + ActuatorHandle::SharedPtr actuator_position_; + ActuatorHandle::SharedPtr actuator_velocity_; + ActuatorHandle::SharedPtr actuator_effort_; }; inline SimpleTransmission::SimpleTransmission( @@ -152,11 +152,11 @@ HandleType get_by_interface( const std::vector & handles, const std::string & interface_name) { const auto result = std::find_if( - handles.cbegin(), handles.cend(), - [&interface_name](const auto handle) { return handle.get_interface_name() == interface_name; }); + handles.cbegin(), handles.cend(), [&interface_name](const auto handle) + { return handle->get_interface_name() == interface_name; }); if (result == handles.cend()) { - return HandleType(handles.cbegin()->get_prefix_name(), interface_name, nullptr); + return nullptr; } return *result; } @@ -167,13 +167,13 @@ bool are_names_identical(const std::vector & handles) std::vector names; std::transform( handles.cbegin(), handles.cend(), std::back_inserter(names), - [](const auto & handle) { return handle.get_prefix_name(); }); + [](const auto & handle) { return handle->get_prefix_name(); }); return std::equal(names.cbegin() + 1, names.cend(), names.cbegin()); } inline void SimpleTransmission::configure( - const std::vector & joint_handles, - const std::vector & actuator_handles) + const std::vector & joint_handles, + const std::vector & actuator_handles) { if (joint_handles.empty()) { @@ -185,6 +185,19 @@ inline void SimpleTransmission::configure( throw Exception("No actuator handles were passed in"); } + if (std::any_of( + joint_handles.cbegin(), joint_handles.cend(), [](const auto & handle) { return !handle; })) + { + throw Exception("Null pointers in joint handles"); + } + + if (std::any_of( + actuator_handles.cbegin(), actuator_handles.cend(), + [](const auto & handle) { return !handle; })) + { + throw Exception("Null pointers in actuator handles"); + } + if (!are_names_identical(joint_handles)) { throw Exception("Joint names given to transmissions should be identical"); @@ -218,17 +231,17 @@ inline void SimpleTransmission::actuator_to_joint() { if (joint_effort_ && actuator_effort_) { - joint_effort_.set_value(actuator_effort_.get_value() * reduction_); + joint_effort_->set_value(actuator_effort_->get_value() * reduction_); } if (joint_velocity_ && actuator_velocity_) { - joint_velocity_.set_value(actuator_velocity_.get_value() / reduction_); + joint_velocity_->set_value(actuator_velocity_->get_value() / reduction_); } if (joint_position_ && actuator_position_) { - joint_position_.set_value(actuator_position_.get_value() / reduction_ + jnt_offset_); + joint_position_->set_value(actuator_position_->get_value() / reduction_ + jnt_offset_); } } @@ -236,17 +249,17 @@ inline void SimpleTransmission::joint_to_actuator() { if (joint_effort_ && actuator_effort_) { - actuator_effort_.set_value(joint_effort_.get_value() / reduction_); + actuator_effort_->set_value(joint_effort_->get_value() / reduction_); } if (joint_velocity_ && actuator_velocity_) { - actuator_velocity_.set_value(joint_velocity_.get_value() * reduction_); + actuator_velocity_->set_value(joint_velocity_->get_value() * reduction_); } if (joint_position_ && actuator_position_) { - actuator_position_.set_value((joint_position_.get_value() - jnt_offset_) * reduction_); + actuator_position_->set_value((joint_position_->get_value() - jnt_offset_) * reduction_); } } diff --git a/transmission_interface/include/transmission_interface/transmission.hpp b/transmission_interface/include/transmission_interface/transmission.hpp index 6275f37711..afe54767a3 100644 --- a/transmission_interface/include/transmission_interface/transmission.hpp +++ b/transmission_interface/include/transmission_interface/transmission.hpp @@ -47,9 +47,31 @@ class Transmission public: virtual ~Transmission() = default; + [[deprecated("Use the configure() function with shared pointers instead.")]] virtual void configure( const std::vector & joint_handles, - const std::vector & actuator_handles) = 0; + const std::vector & actuator_handles) + { + std::vector joint_handles_shared_ptr; + joint_handles_shared_ptr.reserve(joint_handles.size()); + for (const auto & joint_handle : joint_handles) + { + joint_handles_shared_ptr.push_back(std::make_shared(joint_handle)); + } + + std::vector actuator_handles_shared_ptr; + actuator_handles_shared_ptr.reserve(actuator_handles.size()); + for (const auto & actuator_handle : actuator_handles) + { + actuator_handles_shared_ptr.push_back(std::make_shared(actuator_handle)); + } + + configure(joint_handles_shared_ptr, actuator_handles_shared_ptr); + } + + virtual void configure( + const std::vector & joint_handles, + const std::vector & actuator_handles) = 0; /// Transform \e effort variables from actuator to joint space. /** From 0edcd82f58113d5ec9f1cf34968f2f69288ed2f9 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 4 Nov 2024 16:10:52 +0100 Subject: [PATCH 2/5] Fix the utils test --- transmission_interface/test/utils_test.cpp | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/transmission_interface/test/utils_test.cpp b/transmission_interface/test/utils_test.cpp index 9afaccaa47..e3715af0c7 100644 --- a/transmission_interface/test/utils_test.cpp +++ b/transmission_interface/test/utils_test.cpp @@ -27,7 +27,24 @@ TEST(UtilsTest, AccessorTest) const std::string NAME = "joint"; double joint_value = 0.0; const JointHandle joint_handle(NAME, HW_IF_POSITION, &joint_value); - const std::vector joint_handles = {joint_handle}; + const std::vector joint_handles = { + std::make_shared(joint_handle)}; + + ASSERT_EQ(transmission_interface::get_names(joint_handles), std::vector{NAME}); + ASSERT_EQ( + transmission_interface::get_ordered_handles(joint_handles, {NAME}, HW_IF_POSITION), + joint_handles); +} + +TEST(UtilsTest, AccessorTestSharedPtr) +{ + const std::string NAME = "joint"; + double joint_value = 0.0; + hardware_interface::InterfaceInfo interface_info; + interface_info.name = HW_IF_POSITION; + const JointHandle::SharedPtr joint_handle = + std::make_shared(hardware_interface::InterfaceDescription(NAME, interface_info)); + const std::vector joint_handles = {joint_handle}; ASSERT_EQ(transmission_interface::get_names(joint_handles), std::vector{NAME}); ASSERT_EQ( From 7f7e6087d479c7777501072aa5973bd6499dd232 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 4 Nov 2024 16:11:04 +0100 Subject: [PATCH 3/5] Fix the simple transmission test --- .../test/simple_transmission_test.cpp | 144 ++++++++++-------- 1 file changed, 84 insertions(+), 60 deletions(-) diff --git a/transmission_interface/test/simple_transmission_test.cpp b/transmission_interface/test/simple_transmission_test.cpp index d687fbfd21..583cde6dbe 100644 --- a/transmission_interface/test/simple_transmission_test.cpp +++ b/transmission_interface/test/simple_transmission_test.cpp @@ -61,12 +61,17 @@ TEST(PreconditionsTest, AccessorValidation) TEST(PreconditionsTest, ConfigureFailsWithInvalidHandles) { SimpleTransmission trans(2.0, -1.0); - double dummy; - auto actuator_handle = ActuatorHandle("act1", HW_IF_POSITION, &dummy); - auto actuator2_handle = ActuatorHandle("act2", HW_IF_POSITION, &dummy); - auto joint_handle = JointHandle("joint1", HW_IF_POSITION, &dummy); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, &dummy); + hardware_interface::InterfaceInfo interface_info; + interface_info.name = HW_IF_POSITION; + std::shared_ptr actuator_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + std::shared_ptr actuator2_handle = std::make_shared( + hardware_interface::InterfaceDescription("act2", interface_info)); + std::shared_ptr joint_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + std::shared_ptr joint2_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint2", interface_info)); EXPECT_THROW(trans.configure({}, {}), transmission_interface::Exception); EXPECT_THROW(trans.configure({joint_handle}, {}), transmission_interface::Exception); @@ -79,34 +84,22 @@ TEST(PreconditionsTest, ConfigureFailsWithInvalidHandles) trans.configure({joint_handle, joint2_handle}, {actuator_handle}), transmission_interface::Exception); - auto invalid_actuator_handle = ActuatorHandle("act1", HW_IF_VELOCITY, nullptr); - auto invalid_joint_handle = JointHandle("joint1", HW_IF_VELOCITY, nullptr); + auto invalid_actuator_handle = nullptr; + auto invalid_joint_handle = nullptr; EXPECT_THROW( trans.configure({invalid_joint_handle}, {invalid_actuator_handle}), transmission_interface::Exception); EXPECT_THROW( trans.configure({}, {actuator_handle, invalid_actuator_handle}), transmission_interface::Exception); + EXPECT_THROW(trans.configure({}, {actuator_handle}), transmission_interface::Exception); + EXPECT_THROW(trans.configure({joint_handle}, {}), transmission_interface::Exception); EXPECT_THROW( trans.configure({invalid_joint_handle}, {actuator_handle}), transmission_interface::Exception); } -class TransmissionSetup -{ -protected: - // Input/output transmission data - double a_val = 0.0; - double j_val = 0.0; - - void reset_values() - { - a_val = 0.0; - j_val = 0.0; - } -}; - /// Exercises the actuator->joint->actuator roundtrip, which should yield the identity map. -class BlackBoxTest : public TransmissionSetup, public ::testing::TestWithParam +class BlackBoxTest : public ::testing::TestWithParam { protected: /** @@ -120,15 +113,21 @@ class BlackBoxTest : public TransmissionSetup, public ::testing::TestWithParam( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto joint_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + const std::vector actuator_handles({actuator_handle}); + const std::vector joint_handles({joint_handle}); + trans.configure(joint_handles, actuator_handles); + + actuator_handle->set_value(ref_val); trans.actuator_to_joint(); trans.joint_to_actuator(); - EXPECT_THAT(ref_val, DoubleNear(a_val, EPS)); + EXPECT_THAT(ref_val, DoubleNear(actuator_handle->get_value(), EPS)); } } }; @@ -140,23 +139,15 @@ TEST_P(BlackBoxTest, IdentityMap) // Test transmission for positive, zero, and negative inputs testIdentityMap(trans, HW_IF_POSITION, 1.0); - reset_values(); testIdentityMap(trans, HW_IF_POSITION, 0.0); - reset_values(); testIdentityMap(trans, HW_IF_POSITION, -1.0); - reset_values(); testIdentityMap(trans, HW_IF_VELOCITY, 1.0); - reset_values(); testIdentityMap(trans, HW_IF_VELOCITY, 0.0); - reset_values(); testIdentityMap(trans, HW_IF_VELOCITY, -1.0); - reset_values(); testIdentityMap(trans, HW_IF_EFFORT, 1.0); - reset_values(); testIdentityMap(trans, HW_IF_EFFORT, 0.0); - reset_values(); testIdentityMap(trans, HW_IF_EFFORT, -1.0); } @@ -167,7 +158,7 @@ INSTANTIATE_TEST_SUITE_P( SimpleTransmission(10.0, -1.0), SimpleTransmission(-10.0, 1.0), SimpleTransmission(-10.0, -1.0))); -class WhiteBoxTest : public TransmissionSetup, public ::testing::Test +class WhiteBoxTest : public ::testing::Test { }; @@ -179,53 +170,86 @@ TEST_F(WhiteBoxTest, MoveJoint) SimpleTransmission trans(10.0, 1.0); - a_val = 1.0; - // Effort interface { - auto actuator_handle = ActuatorHandle("act1", HW_IF_EFFORT, &a_val); - auto joint_handle = JointHandle("joint1", HW_IF_EFFORT, &j_val); - trans.configure({joint_handle}, {actuator_handle}); + hardware_interface::InterfaceInfo interface_info; + interface_info.name = HW_IF_EFFORT; + auto actuator_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto joint_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + const std::vector actuator_handles({actuator_handle}); + const std::vector joint_handles({joint_handle}); + + actuator_handle->set_value(1.0); + trans.configure(joint_handles, actuator_handles); trans.actuator_to_joint(); - EXPECT_THAT(10.0, DoubleNear(j_val, EPS)); + EXPECT_THAT(10.0, DoubleNear(joint_handle->get_value(), EPS)); } // Velocity interface { - auto actuator_handle = ActuatorHandle("act1", HW_IF_VELOCITY, &a_val); - auto joint_handle = JointHandle("joint1", HW_IF_VELOCITY, &j_val); - trans.configure({joint_handle}, {actuator_handle}); + hardware_interface::InterfaceInfo interface_info; + interface_info.name = HW_IF_VELOCITY; + auto actuator_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto joint_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + const std::vector actuator_handles({actuator_handle}); + const std::vector joint_handles({joint_handle}); + + actuator_handle->set_value(1.0); + trans.configure(joint_handles, actuator_handles); trans.actuator_to_joint(); - EXPECT_THAT(0.1, DoubleNear(j_val, EPS)); + EXPECT_THAT(0.1, DoubleNear(joint_handle->get_value(), EPS)); } // Position interface { - auto actuator_handle = ActuatorHandle("act1", HW_IF_POSITION, &a_val); - auto joint_handle = JointHandle("joint1", HW_IF_POSITION, &j_val); - trans.configure({joint_handle}, {actuator_handle}); + hardware_interface::InterfaceInfo interface_info; + interface_info.name = HW_IF_POSITION; + auto actuator_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto joint_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + const std::vector actuator_handles({actuator_handle}); + const std::vector joint_handles({joint_handle}); + + actuator_handle->set_value(1.0); + trans.configure(joint_handles, actuator_handles); trans.actuator_to_joint(); - EXPECT_THAT(1.1, DoubleNear(j_val, EPS)); + EXPECT_THAT(1.1, DoubleNear(joint_handle->get_value(), EPS)); } // Mismatched interface is ignored { - double unique_value = 13.37; + hardware_interface::InterfaceInfo interface_info; + interface_info.name = HW_IF_POSITION; + auto actuator_handle_pos = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto joint_handle_pos = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + interface_info.name = HW_IF_VELOCITY; + auto actuator_handle_vel = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto joint_handle_vel = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); - auto actuator_handle = ActuatorHandle("act1", HW_IF_POSITION, &a_val); - auto actuator_handle2 = ActuatorHandle("act1", HW_IF_VELOCITY, &unique_value); - auto joint_handle = JointHandle("joint1", HW_IF_POSITION, &j_val); - auto joint_handle2 = JointHandle("joint1", HW_IF_VELOCITY, &unique_value); + double unique_value = 13.37; + joint_handle_vel->set_value(unique_value); + actuator_handle_vel->set_value(unique_value); - trans.configure({joint_handle, joint_handle2}, {actuator_handle}); + trans.configure({joint_handle_pos, joint_handle_vel}, {actuator_handle_pos}); trans.actuator_to_joint(); - EXPECT_THAT(unique_value, DoubleNear(13.37, EPS)); + EXPECT_THAT(unique_value, DoubleNear(joint_handle_vel->get_value(), EPS)); + EXPECT_THAT(unique_value, DoubleNear(actuator_handle_vel->get_value(), EPS)); - trans.configure({joint_handle}, {actuator_handle, actuator_handle2}); + trans.configure({joint_handle_pos}, {actuator_handle_pos, actuator_handle_vel}); trans.actuator_to_joint(); - EXPECT_THAT(unique_value, DoubleNear(13.37, EPS)); + EXPECT_THAT(unique_value, DoubleNear(joint_handle_vel->get_value(), EPS)); + EXPECT_THAT(unique_value, DoubleNear(actuator_handle_vel->get_value(), EPS)); } } From 85af879d7c32d6f344e0eef39bb23d2bdbebf4f6 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 4 Nov 2024 16:43:40 +0100 Subject: [PATCH 4/5] fix differential_transmission tests --- .../differential_transmission.hpp | 13 + .../test/differential_transmission_test.cpp | 364 ++++++++++++------ 2 files changed, 251 insertions(+), 126 deletions(-) diff --git a/transmission_interface/include/transmission_interface/differential_transmission.hpp b/transmission_interface/include/transmission_interface/differential_transmission.hpp index 3710c4123c..675d2b8efb 100644 --- a/transmission_interface/include/transmission_interface/differential_transmission.hpp +++ b/transmission_interface/include/transmission_interface/differential_transmission.hpp @@ -204,6 +204,19 @@ void DifferentialTransmission::configure( throw Exception("No actuator handles were passed in"); } + if (std::any_of( + joint_handles.cbegin(), joint_handles.cend(), [](const auto & handle) { return !handle; })) + { + throw Exception("Null pointers in joint handles"); + } + + if (std::any_of( + actuator_handles.cbegin(), actuator_handles.cend(), + [](const auto & handle) { return !handle; })) + { + throw Exception("Null pointers in actuator handles"); + } + const auto joint_names = get_names(joint_handles); if (joint_names.size() != 2) { diff --git a/transmission_interface/test/differential_transmission_test.cpp b/transmission_interface/test/differential_transmission_test.cpp index 14ffe968bc..bb860c30f0 100644 --- a/transmission_interface/test/differential_transmission_test.cpp +++ b/transmission_interface/test/differential_transmission_test.cpp @@ -90,16 +90,24 @@ TEST(PreconditionsTest, AccessorValidation) void testConfigureWithBadHandles(std::string interface_name) { DifferentialTransmission trans({1.0, 1.0}, {1.0, 1.0}); - double dummy; - auto a1_handle = ActuatorHandle("act1", interface_name, &dummy); - auto a2_handle = ActuatorHandle("act2", interface_name, &dummy); - auto a3_handle = ActuatorHandle("act3", interface_name, &dummy); - auto j1_handle = JointHandle("joint1", interface_name, &dummy); - auto j2_handle = JointHandle("joint2", interface_name, &dummy); - auto j3_handle = JointHandle("joint3", interface_name, &dummy); - auto invalid_a1_handle = ActuatorHandle("act1", interface_name, nullptr); - auto invalid_j1_handle = JointHandle("joint1", interface_name, nullptr); + hardware_interface::InterfaceInfo interface_info; + interface_info.name = interface_name; + + auto a1_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto a2_handle = std::make_shared( + hardware_interface::InterfaceDescription("act2", interface_info)); + auto a3_handle = std::make_shared( + hardware_interface::InterfaceDescription("act3", interface_info)); + auto j1_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + auto j2_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint2", interface_info)); + auto j3_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint3", interface_info)); + auto invalid_a1_handle = nullptr; + auto invalid_j1_handle = nullptr; EXPECT_THROW(trans.configure({}, {}), Exception); EXPECT_THROW(trans.configure({j1_handle}, {}), Exception); @@ -127,18 +135,8 @@ TEST(ConfigureTest, FailsWithBadHandles) testConfigureWithBadHandles(HW_IF_EFFORT); } -class TransmissionSetup : public ::testing::Test -{ -protected: - // Input/output transmission data - double a_val[2]; - double j_val[2]; - std::vector a_vec = {&a_val[0], &a_val[1]}; - std::vector j_vec = {&j_val[0], &j_val[1]}; -}; - /// \brief Exercises the actuator->joint->actuator roundtrip, which should yield the identity map. -class BlackBoxTest : public TransmissionSetup +class BlackBoxTest : public ::testing::Test { protected: const double EXTREMAL_VALUE = 1337.1337; @@ -150,24 +148,32 @@ class BlackBoxTest : public TransmissionSetup DifferentialTransmission & trans, const std::vector & ref_val, const std::string & interface_name) { + hardware_interface::InterfaceInfo interface_info; + interface_info.name = interface_name; + + auto a1_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto a2_handle = std::make_shared( + hardware_interface::InterfaceDescription("act2", interface_info)); + auto j1_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + auto j2_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint2", interface_info)); + // set actuator values to reference - a_val[0] = ref_val[0]; - a_val[1] = ref_val[1]; - // create handles and configure - auto a1_handle = ActuatorHandle("act1", interface_name, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", interface_name, a_vec[1]); - auto joint1_handle = JointHandle("joint1", interface_name, j_vec[0]); - auto joint2_handle = JointHandle("joint2", interface_name, j_vec[1]); - trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + a1_handle->set_value(ref_val[0]); + a2_handle->set_value(ref_val[1]); + trans.configure({j1_handle, j2_handle}, {a1_handle, a2_handle}); // actuator->joint->actuator roundtrip // but we also set actuator values to an extremal value // to ensure joint_to_actuator is not a no-op trans.actuator_to_joint(); - a_val[0] = a_val[1] = EXTREMAL_VALUE; + a1_handle->set_value(EXTREMAL_VALUE); + a2_handle->set_value(EXTREMAL_VALUE); trans.joint_to_actuator(); - EXPECT_THAT(ref_val[0], DoubleNear(a_val[0], EPS)); - EXPECT_THAT(ref_val[1], DoubleNear(a_val[1], EPS)); + EXPECT_THAT(ref_val[0], DoubleNear(a1_handle->get_value(), EPS)); + EXPECT_THAT(ref_val[1], DoubleNear(a2_handle->get_value(), EPS)); } // Generate a set of transmission instances @@ -224,7 +230,7 @@ TEST_F(BlackBoxTest, IdentityMap) } } -class WhiteBoxTest : public TransmissionSetup +class WhiteBoxTest : public ::testing::Test { }; @@ -235,45 +241,70 @@ TEST_F(WhiteBoxTest, DontMoveJoints) std::vector joint_offset = {1.0, 1.0}; DifferentialTransmission trans(actuator_reduction, joint_reduction, joint_offset); - - // Actuator input (used for effort, velocity and position) - *a_vec[0] = 0.0; - *a_vec[1] = 0.0; + const double a1_value = 0.0; + const double a2_value = 0.0; // Effort interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); - trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + hardware_interface::InterfaceInfo interface_info; + interface_info.name = HW_IF_EFFORT; + auto a1_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto a2_handle = std::make_shared( + hardware_interface::InterfaceDescription("act2", interface_info)); + auto j1_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + auto j2_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint2", interface_info)); + a1_handle->set_value(a1_value); + a2_handle->set_value(a2_value); + + trans.configure({j1_handle, j2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(j1_handle->get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(j2_handle->get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); - trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + hardware_interface::InterfaceInfo interface_info; + interface_info.name = HW_IF_VELOCITY; + auto a1_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto a2_handle = std::make_shared( + hardware_interface::InterfaceDescription("act2", interface_info)); + auto j1_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + auto j2_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint2", interface_info)); + a1_handle->set_value(a1_value); + a2_handle->set_value(a2_value); + + trans.configure({j1_handle, j2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(j1_handle->get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(j2_handle->get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); - trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + hardware_interface::InterfaceInfo interface_info; + interface_info.name = HW_IF_POSITION; + auto a1_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto a2_handle = std::make_shared( + hardware_interface::InterfaceDescription("act2", interface_info)); + auto j1_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + auto j2_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint2", interface_info)); + a1_handle->set_value(a1_value); + a2_handle->set_value(a2_value); + + trans.configure({j1_handle, j2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(joint_offset[0], DoubleNear(j_val[0], EPS)); - EXPECT_THAT(joint_offset[1], DoubleNear(j_val[1], EPS)); + EXPECT_THAT(joint_offset[0], DoubleNear(j1_handle->get_value(), EPS)); + EXPECT_THAT(joint_offset[1], DoubleNear(j2_handle->get_value(), EPS)); } } @@ -285,43 +316,70 @@ TEST_F(WhiteBoxTest, MoveFirstJointOnly) DifferentialTransmission trans(actuator_reduction, joint_reduction); // Actuator input (used for effort, velocity and position) - *a_vec[0] = 10.0; - *a_vec[1] = 10.0; + const double a1_value = 10.0; + const double a2_value = 10.0; // Effort interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); - trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + hardware_interface::InterfaceInfo interface_info; + interface_info.name = HW_IF_EFFORT; + auto a1_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto a2_handle = std::make_shared( + hardware_interface::InterfaceDescription("act2", interface_info)); + auto j1_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + auto j2_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint2", interface_info)); + a1_handle->set_value(a1_value); + a2_handle->set_value(a2_value); + + trans.configure({j1_handle, j2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(400.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(400.0, DoubleNear(j1_handle->get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(j2_handle->get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); - trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + hardware_interface::InterfaceInfo interface_info; + interface_info.name = HW_IF_VELOCITY; + auto a1_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto a2_handle = std::make_shared( + hardware_interface::InterfaceDescription("act2", interface_info)); + auto j1_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + auto j2_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint2", interface_info)); + a1_handle->set_value(a1_value); + a2_handle->set_value(a2_value); + + trans.configure({j1_handle, j2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.5, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.5, DoubleNear(j1_handle->get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(j2_handle->get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); - trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + hardware_interface::InterfaceInfo interface_info; + interface_info.name = HW_IF_POSITION; + auto a1_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto a2_handle = std::make_shared( + hardware_interface::InterfaceDescription("act2", interface_info)); + auto j1_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + auto j2_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint2", interface_info)); + a1_handle->set_value(a1_value); + a2_handle->set_value(a2_value); + + trans.configure({j1_handle, j2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.5, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.5, DoubleNear(j1_handle->get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(j2_handle->get_value(), EPS)); } } @@ -333,43 +391,70 @@ TEST_F(WhiteBoxTest, MoveSecondJointOnly) DifferentialTransmission trans(actuator_reduction, joint_reduction); // Actuator input (used for effort, velocity and position) - *a_vec[0] = 10.0; - *a_vec[1] = -10.0; + const double a1_value = 10.0; + const double a2_value = -10.0; // Effort interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); - trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + hardware_interface::InterfaceInfo interface_info; + interface_info.name = HW_IF_EFFORT; + auto a1_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto a2_handle = std::make_shared( + hardware_interface::InterfaceDescription("act2", interface_info)); + auto j1_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + auto j2_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint2", interface_info)); + a1_handle->set_value(a1_value); + a2_handle->set_value(a2_value); + + trans.configure({j1_handle, j2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(400.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(j1_handle->get_value(), EPS)); + EXPECT_THAT(400.0, DoubleNear(j2_handle->get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); - trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + hardware_interface::InterfaceInfo interface_info; + interface_info.name = HW_IF_VELOCITY; + auto a1_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto a2_handle = std::make_shared( + hardware_interface::InterfaceDescription("act2", interface_info)); + auto j1_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + auto j2_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint2", interface_info)); + a1_handle->set_value(a1_value); + a2_handle->set_value(a2_value); + + trans.configure({j1_handle, j2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.5, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(j1_handle->get_value(), EPS)); + EXPECT_THAT(0.5, DoubleNear(j2_handle->get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); - trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + hardware_interface::InterfaceInfo interface_info; + interface_info.name = HW_IF_POSITION; + auto a1_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto a2_handle = std::make_shared( + hardware_interface::InterfaceDescription("act2", interface_info)); + auto j1_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + auto j2_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint2", interface_info)); + a1_handle->set_value(a1_value); + a2_handle->set_value(a2_value); + + trans.configure({j1_handle, j2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.5, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(j1_handle->get_value(), EPS)); + EXPECT_THAT(0.5, DoubleNear(j2_handle->get_value(), EPS)); } } @@ -386,42 +471,69 @@ TEST_F(WhiteBoxTest, MoveBothJoints) DifferentialTransmission trans(actuator_reduction, joint_reduction, joint_offset); // Actuator input (used for effort, velocity and position) - *a_vec[0] = 3.0; - *a_vec[1] = 5.0; + const double a1_value = 3.0; + const double a2_value = 5.0; // Effort interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); - trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + hardware_interface::InterfaceInfo interface_info; + interface_info.name = HW_IF_EFFORT; + auto a1_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto a2_handle = std::make_shared( + hardware_interface::InterfaceDescription("act2", interface_info)); + auto j1_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + auto j2_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint2", interface_info)); + a1_handle->set_value(a1_value); + a2_handle->set_value(a2_value); + + trans.configure({j1_handle, j2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(140.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(520.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(140.0, DoubleNear(j1_handle->get_value(), EPS)); + EXPECT_THAT(520.0, DoubleNear(j2_handle->get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); - trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + hardware_interface::InterfaceInfo interface_info; + interface_info.name = HW_IF_VELOCITY; + auto a1_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto a2_handle = std::make_shared( + hardware_interface::InterfaceDescription("act2", interface_info)); + auto j1_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + auto j2_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint2", interface_info)); + a1_handle->set_value(a1_value); + a2_handle->set_value(a2_value); + + trans.configure({j1_handle, j2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(-0.01250, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.06875, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(-0.01250, DoubleNear(j1_handle->get_value(), EPS)); + EXPECT_THAT(0.06875, DoubleNear(j2_handle->get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); - trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + hardware_interface::InterfaceInfo interface_info; + interface_info.name = HW_IF_POSITION; + auto a1_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto a2_handle = std::make_shared( + hardware_interface::InterfaceDescription("act2", interface_info)); + auto j1_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + auto j2_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint2", interface_info)); + a1_handle->set_value(a1_value); + a2_handle->set_value(a2_value); + + trans.configure({j1_handle, j2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(-2.01250, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(4.06875, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(-2.01250, DoubleNear(j1_handle->get_value(), EPS)); + EXPECT_THAT(4.06875, DoubleNear(j2_handle->get_value(), EPS)); } } From 9fa9b188c34f2c99dd1bea0c5ec4aeaeb97b560d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 4 Nov 2024 16:58:47 +0100 Subject: [PATCH 5/5] Fix four base linkage transmission tests --- .../four_bar_linkage_transmission.hpp | 13 + .../four_bar_linkage_transmission_test.cpp | 365 +++++++++++------- 2 files changed, 249 insertions(+), 129 deletions(-) diff --git a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp index 52e3955476..381f69b376 100644 --- a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp +++ b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp @@ -201,6 +201,19 @@ void FourBarLinkageTransmission::configure( throw Exception("No actuator handles were passed in"); } + if (std::any_of( + joint_handles.cbegin(), joint_handles.cend(), [](const auto & handle) { return !handle; })) + { + throw Exception("Null pointers in joint handles"); + } + + if (std::any_of( + actuator_handles.cbegin(), actuator_handles.cend(), + [](const auto & handle) { return !handle; })) + { + throw Exception("Null pointers in actuator handles"); + } + const auto joint_names = get_names(joint_handles); if (joint_names.size() != 2) { diff --git a/transmission_interface/test/four_bar_linkage_transmission_test.cpp b/transmission_interface/test/four_bar_linkage_transmission_test.cpp index 08d98ec734..1c3a4cc114 100644 --- a/transmission_interface/test/four_bar_linkage_transmission_test.cpp +++ b/transmission_interface/test/four_bar_linkage_transmission_test.cpp @@ -93,16 +93,23 @@ TEST(PreconditionsTest, AccessorValidation) void testConfigureWithBadHandles(std::string interface_name) { FourBarLinkageTransmission trans({1.0, 1.0}, {1.0, 1.0}); - double dummy; - - auto a1_handle = ActuatorHandle("act1", interface_name, &dummy); - auto a2_handle = ActuatorHandle("act2", interface_name, &dummy); - auto a3_handle = ActuatorHandle("act3", interface_name, &dummy); - auto j1_handle = JointHandle("joint1", interface_name, &dummy); - auto j2_handle = JointHandle("joint2", interface_name, &dummy); - auto j3_handle = JointHandle("joint3", interface_name, &dummy); - auto invalid_a1_handle = ActuatorHandle("act1", interface_name, nullptr); - auto invalid_j1_handle = JointHandle("joint1", interface_name, nullptr); + hardware_interface::InterfaceInfo interface_info; + interface_info.name = interface_name; + + auto a1_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto a2_handle = std::make_shared( + hardware_interface::InterfaceDescription("act2", interface_info)); + auto a3_handle = std::make_shared( + hardware_interface::InterfaceDescription("act3", interface_info)); + auto j1_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + auto j2_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint2", interface_info)); + auto j3_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint3", interface_info)); + auto invalid_a1_handle = nullptr; + auto invalid_j1_handle = nullptr; EXPECT_THROW(trans.configure({}, {}), Exception); EXPECT_THROW(trans.configure({j1_handle}, {}), Exception); @@ -130,18 +137,8 @@ TEST(ConfigureTest, FailsWithBadHandles) testConfigureWithBadHandles(HW_IF_EFFORT); } -class TransmissionSetup : public ::testing::Test -{ -protected: - // Input/output transmission data - double a_val[2]; - double j_val[2]; - std::vector a_vec = {&a_val[0], &a_val[1]}; - std::vector j_vec = {&j_val[0], &j_val[1]}; -}; - /// \brief Exercises the actuator->joint->actuator roundtrip, which should yield the identity map. -class BlackBoxTest : public TransmissionSetup +class BlackBoxTest : public ::testing::Test { protected: const double EXTREMAL_VALUE = 1337.1337; @@ -153,24 +150,32 @@ class BlackBoxTest : public TransmissionSetup FourBarLinkageTransmission & trans, const std::vector & ref_val, const std::string & interface_name) { + hardware_interface::InterfaceInfo interface_info; + interface_info.name = interface_name; + + auto a1_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto a2_handle = std::make_shared( + hardware_interface::InterfaceDescription("act2", interface_info)); + auto j1_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + auto j2_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint2", interface_info)); + // set actuator values to reference - a_val[0] = ref_val[0]; - a_val[1] = ref_val[1]; - // create handles and configure - auto a1_handle = ActuatorHandle("act1", interface_name, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", interface_name, a_vec[1]); - auto joint1_handle = JointHandle("joint1", interface_name, j_vec[0]); - auto joint2_handle = JointHandle("joint2", interface_name, j_vec[1]); - trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + a1_handle->set_value(ref_val[0]); + a2_handle->set_value(ref_val[1]); + trans.configure({j1_handle, j2_handle}, {a1_handle, a2_handle}); // actuator->joint->actuator roundtrip // but we also set actuator values to an extremal value // to ensure joint_to_actuator is not a no-op trans.actuator_to_joint(); - a_val[0] = a_val[1] = EXTREMAL_VALUE; + a1_handle->set_value(EXTREMAL_VALUE); + a2_handle->set_value(EXTREMAL_VALUE); trans.joint_to_actuator(); - EXPECT_THAT(ref_val[0], DoubleNear(a_val[0], EPS)); - EXPECT_THAT(ref_val[1], DoubleNear(a_val[1], EPS)); + EXPECT_THAT(ref_val[0], DoubleNear(a1_handle->get_value(), EPS)); + EXPECT_THAT(ref_val[1], DoubleNear(a2_handle->get_value(), EPS)); } // Generate a set of transmission instances @@ -227,7 +232,7 @@ TEST_F(BlackBoxTest, IdentityMap) } } -class WhiteBoxTest : public TransmissionSetup +class WhiteBoxTest : public ::testing::Test { }; @@ -240,43 +245,70 @@ TEST_F(WhiteBoxTest, DontMoveJoints) FourBarLinkageTransmission trans(actuator_reduction, joint_reduction, joint_offset); // Actuator input (used for effort, velocity and position) - a_val[0] = 0.0; - a_val[1] = 0.0; + const double a1_value = 0.0; + const double a2_value = 0.0; // Effort interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); - trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + hardware_interface::InterfaceInfo interface_info; + interface_info.name = HW_IF_EFFORT; + auto a1_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto a2_handle = std::make_shared( + hardware_interface::InterfaceDescription("act2", interface_info)); + auto j1_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + auto j2_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint2", interface_info)); + a1_handle->set_value(a1_value); + a2_handle->set_value(a2_value); + + trans.configure({j1_handle, j2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(j1_handle->get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(j2_handle->get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); - trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + hardware_interface::InterfaceInfo interface_info; + interface_info.name = HW_IF_VELOCITY; + auto a1_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto a2_handle = std::make_shared( + hardware_interface::InterfaceDescription("act2", interface_info)); + auto j1_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + auto j2_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint2", interface_info)); + a1_handle->set_value(a1_value); + a2_handle->set_value(a2_value); + + trans.configure({j1_handle, j2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(j1_handle->get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(j2_handle->get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); - trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + hardware_interface::InterfaceInfo interface_info; + interface_info.name = HW_IF_POSITION; + auto a1_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto a2_handle = std::make_shared( + hardware_interface::InterfaceDescription("act2", interface_info)); + auto j1_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + auto j2_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint2", interface_info)); + a1_handle->set_value(a1_value); + a2_handle->set_value(a2_value); + + trans.configure({j1_handle, j2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(joint_offset[0], DoubleNear(j_val[0], EPS)); - EXPECT_THAT(joint_offset[1], DoubleNear(j_val[1], EPS)); + EXPECT_THAT(joint_offset[0], DoubleNear(j1_handle->get_value(), EPS)); + EXPECT_THAT(joint_offset[1], DoubleNear(j2_handle->get_value(), EPS)); } } @@ -289,44 +321,65 @@ TEST_F(WhiteBoxTest, MoveFirstJointOnly) // Effort interface { - a_val[0] = 5.0; - a_val[1] = 10.0; - auto a1_handle = ActuatorHandle("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); - trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + hardware_interface::InterfaceInfo interface_info; + interface_info.name = HW_IF_EFFORT; + auto a1_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto a2_handle = std::make_shared( + hardware_interface::InterfaceDescription("act2", interface_info)); + auto j1_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + auto j2_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint2", interface_info)); + a1_handle->set_value(5.0); + a2_handle->set_value(10.0); + + trans.configure({j1_handle, j2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(100.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(100.0, DoubleNear(j1_handle->get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(j2_handle->get_value(), EPS)); } // Velocity interface { - a_val[0] = 10.0; - a_val[1] = 5.0; - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); - trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + hardware_interface::InterfaceInfo interface_info; + interface_info.name = HW_IF_VELOCITY; + auto a1_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto a2_handle = std::make_shared( + hardware_interface::InterfaceDescription("act2", interface_info)); + auto j1_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + auto j2_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint2", interface_info)); + a1_handle->set_value(10.0); + a2_handle->set_value(5.0); + + trans.configure({j1_handle, j2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.5, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.5, DoubleNear(j1_handle->get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(j2_handle->get_value(), EPS)); } // Position interface { - a_val[0] = 10.0; - a_val[1] = 5.0; - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); - trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + hardware_interface::InterfaceInfo interface_info; + interface_info.name = HW_IF_POSITION; + auto a1_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto a2_handle = std::make_shared( + hardware_interface::InterfaceDescription("act2", interface_info)); + auto j1_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + auto j2_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint2", interface_info)); + a1_handle->set_value(10.0); + a2_handle->set_value(5.0); + + trans.configure({j1_handle, j2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.5, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.5, DoubleNear(j1_handle->get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(j2_handle->get_value(), EPS)); } } @@ -338,43 +391,70 @@ TEST_F(WhiteBoxTest, MoveSecondJointOnly) FourBarLinkageTransmission trans(actuator_reduction, joint_reduction); // Actuator input (used for effort, velocity and position) - a_val[0] = 0.0; - a_val[1] = 10.0; + const double a1_value = 0.0; + const double a2_value = 10.0; // Effort interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); - trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + hardware_interface::InterfaceInfo interface_info; + interface_info.name = HW_IF_EFFORT; + auto a1_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto a2_handle = std::make_shared( + hardware_interface::InterfaceDescription("act2", interface_info)); + auto j1_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + auto j2_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint2", interface_info)); + a1_handle->set_value(a1_value); + a2_handle->set_value(a2_value); + + trans.configure({j1_handle, j2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(200.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(j1_handle->get_value(), EPS)); + EXPECT_THAT(200.0, DoubleNear(j2_handle->get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); - trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + hardware_interface::InterfaceInfo interface_info; + interface_info.name = HW_IF_VELOCITY; + auto a1_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto a2_handle = std::make_shared( + hardware_interface::InterfaceDescription("act2", interface_info)); + auto j1_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + auto j2_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint2", interface_info)); + a1_handle->set_value(a1_value); + a2_handle->set_value(a2_value); + + trans.configure({j1_handle, j2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.5, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(j1_handle->get_value(), EPS)); + EXPECT_THAT(0.5, DoubleNear(j2_handle->get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); - trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + hardware_interface::InterfaceInfo interface_info; + interface_info.name = HW_IF_POSITION; + auto a1_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto a2_handle = std::make_shared( + hardware_interface::InterfaceDescription("act2", interface_info)); + auto j1_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + auto j2_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint2", interface_info)); + a1_handle->set_value(a1_value); + a2_handle->set_value(a2_value); + + trans.configure({j1_handle, j2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.5, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(j1_handle->get_value(), EPS)); + EXPECT_THAT(0.5, DoubleNear(j2_handle->get_value(), EPS)); } } @@ -391,42 +471,69 @@ TEST_F(WhiteBoxTest, MoveBothJoints) FourBarLinkageTransmission trans(actuator_reduction, joint_reduction, joint_offset); // Actuator input (used for effort, velocity and position) - a_val[0] = 3.0; - a_val[1] = 5.0; + const double a1_value = 3.0; + const double a2_value = 5.0; // Effort interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); - trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + hardware_interface::InterfaceInfo interface_info; + interface_info.name = HW_IF_EFFORT; + auto a1_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto a2_handle = std::make_shared( + hardware_interface::InterfaceDescription("act2", interface_info)); + auto j1_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + auto j2_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint2", interface_info)); + a1_handle->set_value(a1_value); + a2_handle->set_value(a2_value); + + trans.configure({j1_handle, j2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(-60.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(-160.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(-60.0, DoubleNear(j1_handle->get_value(), EPS)); + EXPECT_THAT(-160.0, DoubleNear(j2_handle->get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); - trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + hardware_interface::InterfaceInfo interface_info; + interface_info.name = HW_IF_VELOCITY; + auto a1_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto a2_handle = std::make_shared( + hardware_interface::InterfaceDescription("act2", interface_info)); + auto j1_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + auto j2_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint2", interface_info)); + a1_handle->set_value(a1_value); + a2_handle->set_value(a2_value); + + trans.configure({j1_handle, j2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(-0.15, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(-0.025, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(-0.15, DoubleNear(j1_handle->get_value(), EPS)); + EXPECT_THAT(-0.025, DoubleNear(j2_handle->get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); - trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + hardware_interface::InterfaceInfo interface_info; + interface_info.name = HW_IF_POSITION; + auto a1_handle = std::make_shared( + hardware_interface::InterfaceDescription("act1", interface_info)); + auto a2_handle = std::make_shared( + hardware_interface::InterfaceDescription("act2", interface_info)); + auto j1_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint1", interface_info)); + auto j2_handle = std::make_shared( + hardware_interface::InterfaceDescription("joint2", interface_info)); + a1_handle->set_value(a1_value); + a2_handle->set_value(a2_value); + + trans.configure({j1_handle, j2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(-2.15, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(3.975, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(-2.15, DoubleNear(j1_handle->get_value(), EPS)); + EXPECT_THAT(3.975, DoubleNear(j2_handle->get_value(), EPS)); } }