From 3dd92f3089e59bb89489241aaa07bd56048d9aed Mon Sep 17 00:00:00 2001 From: root Date: Thu, 9 Sep 2021 17:11:09 +0000 Subject: [PATCH 01/14] Add disabling command propagation option. --- .../fake_components/generic_system.hpp | 96 ++++- .../src/fake_components/generic_system.cpp | 407 ++++++++++++++++++ 2 files changed, 500 insertions(+), 3 deletions(-) create mode 100644 hardware_interface/src/fake_components/generic_system.cpp diff --git a/hardware_interface/include/fake_components/generic_system.hpp b/hardware_interface/include/fake_components/generic_system.hpp index 3dfb2b9b70..eb5a927ed1 100644 --- a/hardware_interface/include/fake_components/generic_system.hpp +++ b/hardware_interface/include/fake_components/generic_system.hpp @@ -17,13 +17,103 @@ #ifndef FAKE_COMPONENTS__GENERIC_SYSTEM_HPP_ #define FAKE_COMPONENTS__GENERIC_SYSTEM_HPP_ -#include "mock_components/generic_system.hpp" +#include +#include + +#include "hardware_interface/base_interface.hpp" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_status_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +using hardware_interface::return_type; namespace fake_components { -using GenericSystem [[deprecated]] = mock_components::GenericSystem; +class HARDWARE_INTERFACE_PUBLIC GenericSystem +: public hardware_interface::BaseInterface +{ +public: + return_type configure(const hardware_interface::HardwareInfo & info) override; + + std::vector export_state_interfaces() override; + + std::vector export_command_interfaces() override; + + return_type start() override + { + status_ = hardware_interface::status::STARTED; + return return_type::OK; + } + + return_type stop() override + { + status_ = hardware_interface::status::STOPPED; + return return_type::OK; + } + + return_type read() override; + + return_type write() override { return return_type::OK; } + +protected: + /// Use standard interfaces for joints because they are relevant for dynamic behaviour + /** + * By splitting the standard interfaces from other type, the users are able to inherit this + * class and simply create small "simulation" with desired dynamic behaviour. + * The advantage over using Gazebo is that enables "quick & dirty" tests of robot's URDF and + * controllers. + */ + const std::vector standard_interfaces_ = { + hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, + hardware_interface::HW_IF_ACCELERATION, hardware_interface::HW_IF_EFFORT}; + + const size_t POSITION_INTERFACE_INDEX = 0; + + struct MimicJoint + { + std::size_t joint_index; + std::size_t mimicked_joint_index; + double multiplier = 1.0; + }; + std::vector mimic_joints_; + + /// The size of this vector is (standard_interfaces_.size() x nr_joints) + std::vector> joint_commands_; + std::vector> joint_states_; + + std::vector other_interfaces_; + /// The size of this vector is (other_interfaces_.size() x nr_joints) + std::vector> other_commands_; + std::vector> other_states_; + + std::vector sensor_interfaces_; + /// The size of this vector is (other_interfaces_.size() x nr_joints) + std::vector> sensor_fake_commands_; + std::vector> sensor_states_; + +private: + template + bool get_interface( + const std::string & name, const std::vector & interface_list, + const std::string & interface_name, const size_t vector_index, + std::vector> & values, std::vector & interfaces); + + void initialize_storage_vectors( + std::vector> & commands, std::vector> & states, + const std::vector & interfaces); + + bool fake_sensor_command_interfaces_; + double position_state_following_offset_; + std::string custom_interface_with_following_offset_; + size_t index_custom_interface_with_following_offset_; + bool command_propagation_disabled_; +}; + +typedef GenericSystem GenericRobot; -using GenericSystem [[deprecated]] = mock_components::GenericRobot; } // namespace fake_components #endif // FAKE_COMPONENTS__GENERIC_SYSTEM_HPP_ diff --git a/hardware_interface/src/fake_components/generic_system.cpp b/hardware_interface/src/fake_components/generic_system.cpp new file mode 100644 index 0000000000..d377a80498 --- /dev/null +++ b/hardware_interface/src/fake_components/generic_system.cpp @@ -0,0 +1,407 @@ +// Copyright (c) 2021 PickNik, Inc. +// +// 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. +// +// Author: Jafar Abdi, Denis Stogl + +#include "fake_components/generic_system.hpp" + +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rcutils/logging_macros.h" + +namespace fake_components +{ +return_type GenericSystem::configure(const hardware_interface::HardwareInfo & info) +{ + if (configure_default(info) != return_type::OK) + { + return return_type::ERROR; + } + + // check if to create fake command interface for sensor + auto it = info_.hardware_parameters.find("fake_sensor_commands"); + if (it != info_.hardware_parameters.end()) + { + // TODO(anyone): change this to parse_bool() (see ros2_control#339) + fake_sensor_command_interfaces_ = it->second == "true" || it->second == "True"; + } + else + { + fake_sensor_command_interfaces_ = false; + } + + // check if there is parameter that disables commands + // this way we simulate disconnected driver + it = info_.hardware_parameters.find("disable_commands"); + if (it != info.hardware_parameters.end()) + { + command_propagation_disabled_ = it->second == "true" || it->second == "True"; + } + else + { + command_propagation_disabled_ = false; + } + + // process parameters about state following + position_state_following_offset_ = 0.0; + custom_interface_with_following_offset_ = ""; + + it = info_.hardware_parameters.find("position_state_following_offset"); + if (it != info_.hardware_parameters.end()) + { + position_state_following_offset_ = std::stod(it->second); + it = info_.hardware_parameters.find("custom_interface_with_following_offset"); + if (it != info_.hardware_parameters.end()) + { + custom_interface_with_following_offset_ = it->second; + } + } + // its extremlly unprobably that std::distance results int this value - therefore default + index_custom_interface_with_following_offset_ = std::numeric_limits::max(); + + // Initialize storage for standard interfaces + initialize_storage_vectors(joint_commands_, joint_states_, standard_interfaces_); + // set all values without initial values to 0 + for (auto i = 0u; i < info_.joints.size(); i++) + { + for (auto j = 0u; j < standard_interfaces_.size(); j++) + { + if (std::isnan(joint_states_[j][i])) + { + joint_states_[j][i] = 0.0; + } + } + } + + // Search for mimic joints + for (auto i = 0u; i < info_.joints.size(); ++i) + { + const auto & joint = info_.joints.at(i); + if (joint.parameters.find("mimic") != joint.parameters.cend()) + { + const auto mimicked_joint_it = std::find_if( + info_.joints.begin(), info_.joints.end(), + [&mimicked_joint = + joint.parameters.at("mimic")](const hardware_interface::ComponentInfo & joint_info) { + return joint_info.name == mimicked_joint; + }); + if (mimicked_joint_it == info_.joints.cend()) + { + throw std::runtime_error( + std::string("Mimicked joint '") + joint.parameters.at("mimic") + "' not found"); + } + MimicJoint mimic_joint; + mimic_joint.joint_index = i; + mimic_joint.mimicked_joint_index = std::distance(info_.joints.begin(), mimicked_joint_it); + auto param_it = joint.parameters.find("multiplier"); + if (param_it != joint.parameters.end()) + { + mimic_joint.multiplier = std::stod(joint.parameters.at("multiplier")); + } + mimic_joints_.push_back(mimic_joint); + } + } + // search for non-standard joint interfaces + for (const auto & joint : info_.joints) + { + for (const auto & interface : joint.command_interfaces) + { + // add to list if non-standard interface + if ( + std::find(standard_interfaces_.begin(), standard_interfaces_.end(), interface.name) == + standard_interfaces_.end()) + { + if ( + std::find(other_interfaces_.begin(), other_interfaces_.end(), interface.name) == + other_interfaces_.end()) + { + other_interfaces_.emplace_back(interface.name); + } + } + } + for (const auto & interface : joint.state_interfaces) + { + // add to list if non-standard interface + if ( + std::find(standard_interfaces_.begin(), standard_interfaces_.end(), interface.name) == + standard_interfaces_.end()) + { + if ( + std::find(other_interfaces_.begin(), other_interfaces_.end(), interface.name) == + other_interfaces_.end()) + { + other_interfaces_.emplace_back(interface.name); + } + } + } + } + // Initialize storage for non-standard interfaces + initialize_storage_vectors(other_commands_, other_states_, other_interfaces_); + + // when following offset is used on custom interface then find its index + if (!custom_interface_with_following_offset_.empty()) + { + auto if_it = std::find( + other_interfaces_.begin(), other_interfaces_.end(), custom_interface_with_following_offset_); + if (if_it != other_interfaces_.end()) + { + index_custom_interface_with_following_offset_ = + std::distance(other_interfaces_.begin(), if_it); + RCUTILS_LOG_INFO_NAMED( + "fake_generic_system", "Custom interface with following offset '%s' found at index: %zu.", + custom_interface_with_following_offset_.c_str(), + index_custom_interface_with_following_offset_); + } + else + { + RCUTILS_LOG_WARN_NAMED( + "fake_generic_system", + "Custom interface with following offset '%s' does not exist. Offset will not be applied", + custom_interface_with_following_offset_.c_str()); + } + } + + for (const auto & sensor : info_.sensors) + { + for (const auto & interface : sensor.state_interfaces) + { + if ( + std::find(sensor_interfaces_.begin(), sensor_interfaces_.end(), interface.name) == + sensor_interfaces_.end()) + { + sensor_interfaces_.emplace_back(interface.name); + } + } + } + initialize_storage_vectors(sensor_fake_commands_, sensor_states_, sensor_interfaces_); + + status_ = hardware_interface::status::CONFIGURED; + return hardware_interface::return_type::OK; +} + +std::vector GenericSystem::export_state_interfaces() +{ + std::vector state_interfaces; + + // Joints' state interfaces + for (auto i = 0u; i < info_.joints.size(); i++) + { + const auto & joint = info_.joints[i]; + for (const auto & interface : joint.state_interfaces) + { + // Add interface: if not in the standard list than use "other" interface list + if (!get_interface( + joint.name, standard_interfaces_, interface.name, i, joint_states_, state_interfaces)) + { + if (!get_interface( + joint.name, other_interfaces_, interface.name, i, other_states_, state_interfaces)) + { + throw std::runtime_error( + "Interface is not found in the standard nor other list. " + "This should never happen!"); + } + } + } + } + + // Sensor state interfaces + for (auto i = 0u; i < info_.sensors.size(); i++) + { + const auto & sensor = info_.sensors[i]; + for (const auto & interface : sensor.state_interfaces) + { + if (!get_interface( + sensor.name, sensor_interfaces_, interface.name, i, sensor_states_, state_interfaces)) + { + throw std::runtime_error( + "Interface is not found in the standard nor other list. " + "This should never happen!"); + } + } + } + + return state_interfaces; +} + +std::vector GenericSystem::export_command_interfaces() +{ + std::vector command_interfaces; + + // Joints' state interfaces + for (auto i = 0u; i < info_.joints.size(); i++) + { + const auto & joint = info_.joints[i]; + for (const auto & interface : joint.command_interfaces) + { + // Add interface: if not in the standard list than use "other" interface list + if (!get_interface( + joint.name, standard_interfaces_, interface.name, i, joint_commands_, + command_interfaces)) + { + if (!get_interface( + joint.name, other_interfaces_, interface.name, i, other_commands_, + command_interfaces)) + { + throw std::runtime_error( + "Interface is not found in the standard nor other list. " + "This should never happen!"); + } + } + } + } + + // Fake sensor command interfaces + if (fake_sensor_command_interfaces_) + { + for (auto i = 0u; i < info_.sensors.size(); i++) + { + const auto & sensor = info_.sensors[i]; + for (const auto & interface : sensor.state_interfaces) + { + if (!get_interface( + sensor.name, sensor_interfaces_, interface.name, i, sensor_fake_commands_, + command_interfaces)) + { + throw std::runtime_error( + "Interface is not found in the standard nor other list. " + "This should never happen!"); + } + } + } + } + + return command_interfaces; +} + +return_type GenericSystem::read() +{ + // apply offset to positions only + for (size_t j = 0; j < joint_states_[POSITION_INTERFACE_INDEX].size(); ++j) + { + if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j]) && !command_propagation_disabled_) + { + joint_states_[POSITION_INTERFACE_INDEX][j] = + joint_commands_[POSITION_INTERFACE_INDEX][j] + + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ : 0.0); + } + } + // do loopback on all other interfaces - starts from 1 because 0 index is position interface + for (size_t i = 1; i < joint_states_.size(); ++i) + { + for (size_t j = 0; j < joint_states_[i].size(); ++j) + { + if (!std::isnan(joint_commands_[i][j])) + { + joint_states_[i][j] = joint_commands_[i][j]; + } + } + } + for (const auto & mimic_joint : mimic_joints_) + { + for (auto i = 0u; i < joint_states_.size(); ++i) + { + joint_states_[i][mimic_joint.joint_index] = + mimic_joint.multiplier * joint_states_[i][mimic_joint.mimicked_joint_index]; + } + } + + for (size_t i = 0; i < other_states_.size(); ++i) + { + for (size_t j = 0; j < other_states_[i].size(); ++j) + { + if ( + i == index_custom_interface_with_following_offset_ && + !std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) + { + other_states_[i][j] = + joint_commands_[POSITION_INTERFACE_INDEX][j] + position_state_following_offset_; + } + else if (!std::isnan(other_commands_[i][j])) + { + other_states_[i][j] = other_commands_[i][j]; + } + } + } + + if (fake_sensor_command_interfaces_) + { + for (size_t i = 0; i < sensor_states_.size(); ++i) + { + for (size_t j = 0; j < sensor_states_[i].size(); ++j) + { + if (!std::isnan(sensor_fake_commands_[i][j])) + { + sensor_states_[i][j] = sensor_fake_commands_[i][j]; + } + } + } + } + return return_type::OK; +} + +// Private methods +template +bool GenericSystem::get_interface( + const std::string & name, const std::vector & interface_list, + const std::string & interface_name, const size_t vector_index, + std::vector> & values, std::vector & interfaces) +{ + auto it = std::find(interface_list.begin(), interface_list.end(), interface_name); + if (it != interface_list.end()) + { + auto j = std::distance(interface_list.begin(), it); + interfaces.emplace_back(name, *it, &values[j][vector_index]); + return true; + } + return false; +} + +void GenericSystem::initialize_storage_vectors( + std::vector> & commands, std::vector> & states, + const std::vector & interfaces) +{ + // Initialize storage for all joints, regardless of their existence + commands.resize(interfaces.size()); + states.resize(interfaces.size()); + for (auto i = 0u; i < interfaces.size(); i++) + { + commands[i].resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + states[i].resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + } + + // Initialize with values from URDF + for (auto i = 0u; i < info_.joints.size(); i++) + { + const auto & joint = info_.joints[i]; + for (auto j = 0u; j < interfaces.size(); j++) + { + auto it = joint.parameters.find("initial_" + interfaces[j]); + if (it != joint.parameters.end()) + { + states[j][i] = std::stod(it->second); + } + } + } +} + +} // namespace fake_components + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(fake_components::GenericSystem, hardware_interface::SystemInterface) From b4724d8226142dd7054d46fcc27b2040bf5a543d Mon Sep 17 00:00:00 2001 From: root Date: Fri, 10 Sep 2021 16:31:55 +0000 Subject: [PATCH 02/14] Make velocity work. --- .../fake_components/generic_system.hpp | 29 ++- .../src/fake_components/generic_system.cpp | 220 ++++++++++++++++-- 2 files changed, 229 insertions(+), 20 deletions(-) diff --git a/hardware_interface/include/fake_components/generic_system.hpp b/hardware_interface/include/fake_components/generic_system.hpp index eb5a927ed1..547b17017b 100644 --- a/hardware_interface/include/fake_components/generic_system.hpp +++ b/hardware_interface/include/fake_components/generic_system.hpp @@ -17,6 +17,7 @@ #ifndef FAKE_COMPONENTS__GENERIC_SYSTEM_HPP_ #define FAKE_COMPONENTS__GENERIC_SYSTEM_HPP_ +#include #include #include @@ -32,6 +33,13 @@ using hardware_interface::return_type; namespace fake_components { +enum StoppingInterface +{ + NONE, + STOP_POSITION, + STOP_VELOCITY +}; + class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::BaseInterface { @@ -58,6 +66,14 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem return_type write() override { return return_type::OK; } + return_type prepare_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) override; + + return_type perform_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) override; + protected: /// Use standard interfaces for joints because they are relevant for dynamic behaviour /** @@ -71,6 +87,7 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem hardware_interface::HW_IF_ACCELERATION, hardware_interface::HW_IF_EFFORT}; const size_t POSITION_INTERFACE_INDEX = 0; + const size_t VELOCITY_INTERFACE_INDEX = 1; struct MimicJoint { @@ -83,6 +100,7 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem /// The size of this vector is (standard_interfaces_.size() x nr_joints) std::vector> joint_commands_; std::vector> joint_states_; + double joint_vel_commands_[2]; std::vector other_interfaces_; /// The size of this vector is (other_interfaces_.size() x nr_joints) @@ -109,7 +127,16 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem double position_state_following_offset_; std::string custom_interface_with_following_offset_; size_t index_custom_interface_with_following_offset_; - bool command_propagation_disabled_; + bool command_propagation_disabled_; + + // resources switching aux vars + std::vector stop_modes_; + std::vector start_modes_; + bool position_controller_running_; + bool velocity_controller_running_; + std::chrono::system_clock::time_point begin; + // for velocity control, store last position command + std::vector joint_pos_commands_old_; }; typedef GenericSystem GenericRobot; diff --git a/hardware_interface/src/fake_components/generic_system.cpp b/hardware_interface/src/fake_components/generic_system.cpp index d377a80498..54bf882038 100644 --- a/hardware_interface/src/fake_components/generic_system.cpp +++ b/hardware_interface/src/fake_components/generic_system.cpp @@ -17,6 +17,7 @@ #include "fake_components/generic_system.hpp" #include +#include #include #include #include @@ -78,6 +79,7 @@ return_type GenericSystem::configure(const hardware_interface::HardwareInfo & in // Initialize storage for standard interfaces initialize_storage_vectors(joint_commands_, joint_states_, standard_interfaces_); + // set all values without initial values to 0 for (auto i = 0u; i < info_.joints.size(); i++) { @@ -90,6 +92,10 @@ return_type GenericSystem::configure(const hardware_interface::HardwareInfo & in } } + // set memory position vector to initial value + joint_pos_commands_old_.resize(joint_commands_[POSITION_INTERFACE_INDEX].size()); + joint_pos_commands_old_ = joint_commands_[POSITION_INTERFACE_INDEX]; + // Search for mimic joints for (auto i = 0u; i < info_.joints.size(); ++i) { @@ -192,6 +198,15 @@ return_type GenericSystem::configure(const hardware_interface::HardwareInfo & in } initialize_storage_vectors(sensor_fake_commands_, sensor_states_, sensor_interfaces_); + stop_modes_ = {StoppingInterface::NONE, StoppingInterface::NONE, StoppingInterface::NONE, + StoppingInterface::NONE, StoppingInterface::NONE, StoppingInterface::NONE}; + start_modes_ = {hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_POSITION, + hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_POSITION, + hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_POSITION}; + position_controller_running_ = false; + velocity_controller_running_ = false; + begin = std::chrono::system_clock::now(); + status_ = hardware_interface::status::CONFIGURED; return hardware_interface::return_type::OK; } @@ -247,24 +262,35 @@ std::vector GenericSystem::export_command_ // Joints' state interfaces for (auto i = 0u; i < info_.joints.size(); i++) { - const auto & joint = info_.joints[i]; - for (const auto & interface : joint.command_interfaces) - { +// const auto & joint = info_.joints[i]; +// for (const auto & interface : joint.command_interfaces) +// { + + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joint_commands_[POSITION_INTERFACE_INDEX][i])); + + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joint_vel_commands_[i])); + + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &joint_commands_[2][i])); + // Add interface: if not in the standard list than use "other" interface list - if (!get_interface( - joint.name, standard_interfaces_, interface.name, i, joint_commands_, - command_interfaces)) - { - if (!get_interface( - joint.name, other_interfaces_, interface.name, i, other_commands_, - command_interfaces)) - { - throw std::runtime_error( - "Interface is not found in the standard nor other list. " - "This should never happen!"); - } - } - } +// if (!get_interface( +// joint.name, standard_interfaces_, interface.name, i, joint_commands_, +// command_interfaces)) +// { +// +// if (!get_interface( +// joint.name, other_interfaces_, interface.name, i, other_commands_, +// command_interfaces)) +// { +// throw std::runtime_error( +// "Interface is not found in the standard nor other list. " +// "This should never happen!"); +// } +// } +// } } // Fake sensor command interfaces @@ -290,20 +316,176 @@ std::vector GenericSystem::export_command_ return command_interfaces; } +return_type GenericSystem::prepare_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) +{ + hardware_interface::return_type ret_val = hardware_interface::return_type::OK; + + start_modes_.clear(); + stop_modes_.clear(); + + // Starting interfaces + // add start interface per joint in tmp var for later check + for (const auto & key : start_interfaces) + { + for (auto i = 0u; i < info_.joints.size(); i++) + { + if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) + { + start_modes_.push_back(hardware_interface::HW_IF_POSITION); + } + if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) + { + start_modes_.push_back(hardware_interface::HW_IF_VELOCITY); + } + } + } + // set new mode to all interfaces at the same time + if (start_modes_.size() != 0 && start_modes_.size() != info_.joints.size()) + { + ret_val = hardware_interface::return_type::ERROR; + } + + // all start interfaces must be the same - can't mix position and velocity control + if ( + start_modes_.size() != 0 && + !std::equal(start_modes_.begin() + 1, start_modes_.end(), start_modes_.begin())) + { + ret_val = hardware_interface::return_type::ERROR; + } + + // Stopping interfaces + // add stop interface per joint in tmp var for later check + for (const auto & key : stop_interfaces) + { + for (auto i = 0u; i < info_.joints.size(); i++) + { + if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) + { + stop_modes_.push_back(StoppingInterface::STOP_POSITION); + } + if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) + { + stop_modes_.push_back(StoppingInterface::STOP_VELOCITY); + } + } + } + // stop all interfaces at the same time + if ( + stop_modes_.size() != 0 && + (stop_modes_.size() != info_.joints.size() || + !std::equal(stop_modes_.begin() + 1, stop_modes_.end(), stop_modes_.begin()))) + { + ret_val = hardware_interface::return_type::ERROR; + } + + return ret_val; +} + +return_type GenericSystem::perform_command_mode_switch( + const std::vector & /*start_interfaces*/, + const std::vector & /*stop_interfaces*/) +{ + hardware_interface::return_type ret_val = hardware_interface::return_type::OK; + + position_controller_running_ = false; + velocity_controller_running_ = false; + + if ( + start_modes_.size() != 0 && + std::find(start_modes_.begin(), start_modes_.end(), hardware_interface::HW_IF_POSITION) != + start_modes_.end()) + { + joint_commands_[POSITION_INTERFACE_INDEX] = joint_states_[POSITION_INTERFACE_INDEX]; + position_controller_running_ = true; + } + else if ( + start_modes_.size() != 0 && + std::find(start_modes_.begin(), start_modes_.end(), hardware_interface::HW_IF_VELOCITY) != + start_modes_.end()) + { + joint_commands_[VELOCITY_INTERFACE_INDEX] = std::vector(info_.joints.size(), 0.0); + velocity_controller_running_ = true; + } + return ret_val; +} + return_type GenericSystem::read() { + std::chrono::system_clock::time_point begin_last = begin; + begin = std::chrono::system_clock::now(); + double period = + std::chrono::duration_cast(begin - begin_last).count() / 1000.0; + +// printf("#############################\n"); +// +// for (size_t i = 0; i 1) + joint_states_[VELOCITY_INTERFACE_INDEX][j] = + (joint_commands_[POSITION_INTERFACE_INDEX][j] - joint_pos_commands_old_[j]) / period; + } + } + + // velocity +// for (size_t j = 0; j < joint_commands_[VELOCITY_INTERFACE_INDEX].size(); ++j) + for (size_t j = 0; j < 2; ++j) + + { + if ( + !std::isnan(joint_commands_[VELOCITY_INTERFACE_INDEX][j]) && !command_propagation_disabled_ && + velocity_controller_running_) + { +// joint_states_[POSITION_INTERFACE_INDEX][j] += +// joint_commands_[VELOCITY_INTERFACE_INDEX][j] * period; + + joint_states_[POSITION_INTERFACE_INDEX][j] += + joint_vel_commands_[j] * period; + + +// joint_states_[VELOCITY_INTERFACE_INDEX][j] = joint_commands_[VELOCITY_INTERFACE_INDEX][j]; + + joint_states_[VELOCITY_INTERFACE_INDEX][j] = joint_vel_commands_[j]; + + joint_commands_[POSITION_INTERFACE_INDEX][j] = joint_states_[POSITION_INTERFACE_INDEX][j]; } } + + // remember old value of position + joint_pos_commands_old_ = joint_commands_[POSITION_INTERFACE_INDEX]; + // do loopback on all other interfaces - starts from 1 because 0 index is position interface - for (size_t i = 1; i < joint_states_.size(); ++i) + for (size_t i = 2; i < joint_states_.size(); ++i) { for (size_t j = 0; j < joint_states_[i].size(); ++j) { From 04baa00816bd368504ec73886f7c32bac8d1f667 Mon Sep 17 00:00:00 2001 From: root Date: Mon, 13 Sep 2021 09:23:14 +0000 Subject: [PATCH 03/14] Remove debugg stuff. --- .../src/fake_components/generic_system.cpp | 22 ------------------- 1 file changed, 22 deletions(-) diff --git a/hardware_interface/src/fake_components/generic_system.cpp b/hardware_interface/src/fake_components/generic_system.cpp index 54bf882038..59f7762318 100644 --- a/hardware_interface/src/fake_components/generic_system.cpp +++ b/hardware_interface/src/fake_components/generic_system.cpp @@ -418,28 +418,6 @@ return_type GenericSystem::read() double period = std::chrono::duration_cast(begin - begin_last).count() / 1000.0; -// printf("#############################\n"); -// -// for (size_t i = 0; i Date: Mon, 13 Sep 2021 17:51:59 +0000 Subject: [PATCH 04/14] Set initial velocity to zero. --- .../src/fake_components/generic_system.cpp | 77 ++++++++++--------- 1 file changed, 41 insertions(+), 36 deletions(-) diff --git a/hardware_interface/src/fake_components/generic_system.cpp b/hardware_interface/src/fake_components/generic_system.cpp index 59f7762318..4fdab512a3 100644 --- a/hardware_interface/src/fake_components/generic_system.cpp +++ b/hardware_interface/src/fake_components/generic_system.cpp @@ -96,6 +96,12 @@ return_type GenericSystem::configure(const hardware_interface::HardwareInfo & in joint_pos_commands_old_.resize(joint_commands_[POSITION_INTERFACE_INDEX].size()); joint_pos_commands_old_ = joint_commands_[POSITION_INTERFACE_INDEX]; + // joint velocity commands to zero + for (auto i = 0u; i < info_.joints.size(); ++i) + { + joint_vel_commands_[i] = 0.0; + } + // Search for mimic joints for (auto i = 0u; i < info_.joints.size(); ++i) { @@ -262,35 +268,36 @@ std::vector GenericSystem::export_command_ // Joints' state interfaces for (auto i = 0u; i < info_.joints.size(); i++) { -// const auto & joint = info_.joints[i]; -// for (const auto & interface : joint.command_interfaces) -// { - - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joint_commands_[POSITION_INTERFACE_INDEX][i])); - - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joint_vel_commands_[i])); - - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &joint_commands_[2][i])); - - // Add interface: if not in the standard list than use "other" interface list -// if (!get_interface( -// joint.name, standard_interfaces_, interface.name, i, joint_commands_, -// command_interfaces)) -// { -// -// if (!get_interface( -// joint.name, other_interfaces_, interface.name, i, other_commands_, -// command_interfaces)) -// { -// throw std::runtime_error( -// "Interface is not found in the standard nor other list. " -// "This should never happen!"); -// } -// } -// } + // const auto & joint = info_.joints[i]; + // for (const auto & interface : joint.command_interfaces) + // { + + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, + &joint_commands_[POSITION_INTERFACE_INDEX][i])); + + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joint_vel_commands_[i])); + + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &joint_commands_[2][i])); + + // Add interface: if not in the standard list than use "other" interface list + // if (!get_interface( + // joint.name, standard_interfaces_, interface.name, i, joint_commands_, + // command_interfaces)) + // { + // + // if (!get_interface( + // joint.name, other_interfaces_, interface.name, i, other_commands_, + // command_interfaces)) + // { + // throw std::runtime_error( + // "Interface is not found in the standard nor other list. " + // "This should never happen!"); + // } + // } + // } } // Fake sensor command interfaces @@ -436,7 +443,7 @@ return_type GenericSystem::read() } // velocity -// for (size_t j = 0; j < joint_commands_[VELOCITY_INTERFACE_INDEX].size(); ++j) + // for (size_t j = 0; j < joint_commands_[VELOCITY_INTERFACE_INDEX].size(); ++j) for (size_t j = 0; j < 2; ++j) { @@ -444,14 +451,12 @@ return_type GenericSystem::read() !std::isnan(joint_commands_[VELOCITY_INTERFACE_INDEX][j]) && !command_propagation_disabled_ && velocity_controller_running_) { -// joint_states_[POSITION_INTERFACE_INDEX][j] += -// joint_commands_[VELOCITY_INTERFACE_INDEX][j] * period; - - joint_states_[POSITION_INTERFACE_INDEX][j] += - joint_vel_commands_[j] * period; + // joint_states_[POSITION_INTERFACE_INDEX][j] += + // joint_commands_[VELOCITY_INTERFACE_INDEX][j] * period; + joint_states_[POSITION_INTERFACE_INDEX][j] += joint_vel_commands_[j] * period; -// joint_states_[VELOCITY_INTERFACE_INDEX][j] = joint_commands_[VELOCITY_INTERFACE_INDEX][j]; + // joint_states_[VELOCITY_INTERFACE_INDEX][j] = joint_commands_[VELOCITY_INTERFACE_INDEX][j]; joint_states_[VELOCITY_INTERFACE_INDEX][j] = joint_vel_commands_[j]; From 9e5a92fb0de5f4932f9953e83c3b38d84b86ae84 Mon Sep 17 00:00:00 2001 From: root Date: Tue, 21 Sep 2021 17:48:28 +0000 Subject: [PATCH 05/14] Adapt fake component to use velocity control. --- .../fake_components/generic_system.hpp | 2 +- .../src/fake_components/generic_system.cpp | 80 ++++++++----------- 2 files changed, 35 insertions(+), 47 deletions(-) diff --git a/hardware_interface/include/fake_components/generic_system.hpp b/hardware_interface/include/fake_components/generic_system.hpp index 547b17017b..1540ffd6fd 100644 --- a/hardware_interface/include/fake_components/generic_system.hpp +++ b/hardware_interface/include/fake_components/generic_system.hpp @@ -100,7 +100,6 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem /// The size of this vector is (standard_interfaces_.size() x nr_joints) std::vector> joint_commands_; std::vector> joint_states_; - double joint_vel_commands_[2]; std::vector other_interfaces_; /// The size of this vector is (other_interfaces_.size() x nr_joints) @@ -137,6 +136,7 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem std::chrono::system_clock::time_point begin; // for velocity control, store last position command std::vector joint_pos_commands_old_; + double period_; }; typedef GenericSystem GenericRobot; diff --git a/hardware_interface/src/fake_components/generic_system.cpp b/hardware_interface/src/fake_components/generic_system.cpp index 4fdab512a3..c9bebc3b58 100644 --- a/hardware_interface/src/fake_components/generic_system.cpp +++ b/hardware_interface/src/fake_components/generic_system.cpp @@ -97,9 +97,9 @@ return_type GenericSystem::configure(const hardware_interface::HardwareInfo & in joint_pos_commands_old_ = joint_commands_[POSITION_INTERFACE_INDEX]; // joint velocity commands to zero - for (auto i = 0u; i < info_.joints.size(); ++i) + for (auto i = 0u; i < joint_commands_[VELOCITY_INTERFACE_INDEX].size(); ++i) { - joint_vel_commands_[i] = 0.0; + joint_commands_[VELOCITY_INTERFACE_INDEX][i] = 0.0; } // Search for mimic joints @@ -268,36 +268,24 @@ std::vector GenericSystem::export_command_ // Joints' state interfaces for (auto i = 0u; i < info_.joints.size(); i++) { - // const auto & joint = info_.joints[i]; - // for (const auto & interface : joint.command_interfaces) - // { - - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, - &joint_commands_[POSITION_INTERFACE_INDEX][i])); - - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joint_vel_commands_[i])); - - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &joint_commands_[2][i])); - - // Add interface: if not in the standard list than use "other" interface list - // if (!get_interface( - // joint.name, standard_interfaces_, interface.name, i, joint_commands_, - // command_interfaces)) - // { - // - // if (!get_interface( - // joint.name, other_interfaces_, interface.name, i, other_commands_, - // command_interfaces)) - // { - // throw std::runtime_error( - // "Interface is not found in the standard nor other list. " - // "This should never happen!"); - // } - // } - // } + const auto & joint = info_.joints[i]; + for (const auto & interface : joint.command_interfaces) + { + // Add interface: if not in the standard list than use "other" interface list + if (!get_interface( + joint.name, standard_interfaces_, interface.name, i, joint_commands_, + command_interfaces)) + { + if (!get_interface( + joint.name, other_interfaces_, interface.name, i, other_commands_, + command_interfaces)) + { + throw std::runtime_error( + "Interface is not found in the standard nor other list. " + "This should never happen!"); + } + } + } } // Fake sensor command interfaces @@ -404,7 +392,10 @@ return_type GenericSystem::perform_command_mode_switch( std::find(start_modes_.begin(), start_modes_.end(), hardware_interface::HW_IF_POSITION) != start_modes_.end()) { - joint_commands_[POSITION_INTERFACE_INDEX] = joint_states_[POSITION_INTERFACE_INDEX]; + for (size_t i = 0; i < joint_commands_[POSITION_INTERFACE_INDEX].size(); ++i) + { + joint_commands_[POSITION_INTERFACE_INDEX][i] = joint_states_[POSITION_INTERFACE_INDEX][i]; + } position_controller_running_ = true; } else if ( @@ -412,7 +403,10 @@ return_type GenericSystem::perform_command_mode_switch( std::find(start_modes_.begin(), start_modes_.end(), hardware_interface::HW_IF_VELOCITY) != start_modes_.end()) { - joint_commands_[VELOCITY_INTERFACE_INDEX] = std::vector(info_.joints.size(), 0.0); + for (size_t i = 0; i < joint_commands_[VELOCITY_INTERFACE_INDEX].size(); ++i) + { + joint_commands_[VELOCITY_INTERFACE_INDEX][i] = 0.0; + } velocity_controller_running_ = true; } return ret_val; @@ -422,7 +416,7 @@ return_type GenericSystem::read() { std::chrono::system_clock::time_point begin_last = begin; begin = std::chrono::system_clock::now(); - double period = + period_ = std::chrono::duration_cast(begin - begin_last).count() / 1000.0; // apply offset to positions only @@ -438,27 +432,21 @@ return_type GenericSystem::read() if (standard_interfaces_.size() > 1) joint_states_[VELOCITY_INTERFACE_INDEX][j] = - (joint_commands_[POSITION_INTERFACE_INDEX][j] - joint_pos_commands_old_[j]) / period; + (joint_commands_[POSITION_INTERFACE_INDEX][j] - joint_pos_commands_old_[j]) / period_; } } // velocity - // for (size_t j = 0; j < joint_commands_[VELOCITY_INTERFACE_INDEX].size(); ++j) - for (size_t j = 0; j < 2; ++j) - + for (size_t j = 0; j < joint_commands_[VELOCITY_INTERFACE_INDEX].size(); ++j) { if ( !std::isnan(joint_commands_[VELOCITY_INTERFACE_INDEX][j]) && !command_propagation_disabled_ && velocity_controller_running_) { - // joint_states_[POSITION_INTERFACE_INDEX][j] += - // joint_commands_[VELOCITY_INTERFACE_INDEX][j] * period; - - joint_states_[POSITION_INTERFACE_INDEX][j] += joint_vel_commands_[j] * period; - - // joint_states_[VELOCITY_INTERFACE_INDEX][j] = joint_commands_[VELOCITY_INTERFACE_INDEX][j]; + joint_states_[POSITION_INTERFACE_INDEX][j] += + joint_commands_[VELOCITY_INTERFACE_INDEX][j] * period_; - joint_states_[VELOCITY_INTERFACE_INDEX][j] = joint_vel_commands_[j]; + joint_states_[VELOCITY_INTERFACE_INDEX][j] = joint_commands_[VELOCITY_INTERFACE_INDEX][j]; joint_commands_[POSITION_INTERFACE_INDEX][j] = joint_states_[POSITION_INTERFACE_INDEX][j]; } From c3f89d64ae0ae73108a43d49ba1ace26ea40466a Mon Sep 17 00:00:00 2001 From: root Date: Tue, 21 Sep 2021 12:33:06 +0000 Subject: [PATCH 06/14] Additional changes for velocity ctrl. --- .../src/fake_components/generic_system.cpp | 52 +++++++++++++------ 1 file changed, 37 insertions(+), 15 deletions(-) diff --git a/hardware_interface/src/fake_components/generic_system.cpp b/hardware_interface/src/fake_components/generic_system.cpp index c9bebc3b58..6bf5fe4d57 100644 --- a/hardware_interface/src/fake_components/generic_system.cpp +++ b/hardware_interface/src/fake_components/generic_system.cpp @@ -99,6 +99,7 @@ return_type GenericSystem::configure(const hardware_interface::HardwareInfo & in // joint velocity commands to zero for (auto i = 0u; i < joint_commands_[VELOCITY_INTERFACE_INDEX].size(); ++i) { + joint_vel_commands_[i] = 0.0; joint_commands_[VELOCITY_INTERFACE_INDEX][i] = 0.0; } @@ -271,6 +272,16 @@ std::vector GenericSystem::export_command_ const auto & joint = info_.joints[i]; for (const auto & interface : joint.command_interfaces) { +// command_interfaces.emplace_back(hardware_interface::CommandInterface( +// info_.joints[i].name, hardware_interface::HW_IF_POSITION, +// &joint_commands_[POSITION_INTERFACE_INDEX][i])); + +// command_interfaces.emplace_back(hardware_interface::CommandInterface( +// info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joint_vel_commands_[i])); +// +// command_interfaces.emplace_back(hardware_interface::CommandInterface( +// info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &joint_commands_[2][i])); + // Add interface: if not in the standard list than use "other" interface list if (!get_interface( joint.name, standard_interfaces_, interface.name, i, joint_commands_, @@ -288,7 +299,14 @@ std::vector GenericSystem::export_command_ } } - // Fake sensor command interfaces + for (auto i =0; i< command_interfaces.size(); ++i) + { + + printf("%s %f\n", command_interfaces[i].get_full_name().c_str(), command_interfaces[i].get_value()); + + } + + // Fake sensor command interfaces if (fake_sensor_command_interfaces_) { for (auto i = 0u; i < info_.sensors.size(); i++) @@ -392,10 +410,7 @@ return_type GenericSystem::perform_command_mode_switch( std::find(start_modes_.begin(), start_modes_.end(), hardware_interface::HW_IF_POSITION) != start_modes_.end()) { - for (size_t i = 0; i < joint_commands_[POSITION_INTERFACE_INDEX].size(); ++i) - { - joint_commands_[POSITION_INTERFACE_INDEX][i] = joint_states_[POSITION_INTERFACE_INDEX][i]; - } + joint_commands_[POSITION_INTERFACE_INDEX] = joint_states_[POSITION_INTERFACE_INDEX]; position_controller_running_ = true; } else if ( @@ -403,10 +418,7 @@ return_type GenericSystem::perform_command_mode_switch( std::find(start_modes_.begin(), start_modes_.end(), hardware_interface::HW_IF_VELOCITY) != start_modes_.end()) { - for (size_t i = 0; i < joint_commands_[VELOCITY_INTERFACE_INDEX].size(); ++i) - { - joint_commands_[VELOCITY_INTERFACE_INDEX][i] = 0.0; - } + joint_commands_[VELOCITY_INTERFACE_INDEX] = std::vector(info_.joints.size(), 0.0); velocity_controller_running_ = true; } return ret_val; @@ -416,7 +428,7 @@ return_type GenericSystem::read() { std::chrono::system_clock::time_point begin_last = begin; begin = std::chrono::system_clock::now(); - period_ = + double period = std::chrono::duration_cast(begin - begin_last).count() / 1000.0; // apply offset to positions only @@ -432,20 +444,29 @@ return_type GenericSystem::read() if (standard_interfaces_.size() > 1) joint_states_[VELOCITY_INTERFACE_INDEX][j] = - (joint_commands_[POSITION_INTERFACE_INDEX][j] - joint_pos_commands_old_[j]) / period_; + (joint_commands_[POSITION_INTERFACE_INDEX][j] - joint_pos_commands_old_[j]) / period; } } // velocity - for (size_t j = 0; j < joint_commands_[VELOCITY_INTERFACE_INDEX].size(); ++j) + // for (size_t j = 0; j < joint_commands_[VELOCITY_INTERFACE_INDEX].size(); ++j) + for (size_t j = 0; j < 2; ++j) + { if ( !std::isnan(joint_commands_[VELOCITY_INTERFACE_INDEX][j]) && !command_propagation_disabled_ && velocity_controller_running_) { - joint_states_[POSITION_INTERFACE_INDEX][j] += - joint_commands_[VELOCITY_INTERFACE_INDEX][j] * period_; + // joint_states_[POSITION_INTERFACE_INDEX][j] += + // joint_commands_[VELOCITY_INTERFACE_INDEX][j] * period; + + +// joint_states_[POSITION_INTERFACE_INDEX][j] += joint_vel_commands_[j] * period; + joint_states_[POSITION_INTERFACE_INDEX][j] += joint_commands_[VELOCITY_INTERFACE_INDEX][j] * period; + + // joint_states_[VELOCITY_INTERFACE_INDEX][j] = joint_commands_[VELOCITY_INTERFACE_INDEX][j]; +// joint_states_[VELOCITY_INTERFACE_INDEX][j] = joint_vel_commands_[j]; joint_states_[VELOCITY_INTERFACE_INDEX][j] = joint_commands_[VELOCITY_INTERFACE_INDEX][j]; joint_commands_[POSITION_INTERFACE_INDEX][j] = joint_states_[POSITION_INTERFACE_INDEX][j]; @@ -520,7 +541,8 @@ bool GenericSystem::get_interface( if (it != interface_list.end()) { auto j = std::distance(interface_list.begin(), it); - interfaces.emplace_back(name, *it, &values[j][vector_index]); + printf("%s %ld %ld %ld\n",interface_name.c_str(), j, vector_index, &values[j][vector_index] ); + interfaces.emplace_back(HandleType(name, *it, (double*)&(values[j][vector_index]))); return true; } return false; From 53616e8cc9d9946b3df936d04efc64ef7851836b Mon Sep 17 00:00:00 2001 From: root Date: Tue, 21 Sep 2021 13:08:30 +0000 Subject: [PATCH 07/14] Test with parenthesis. --- hardware_interface/src/fake_components/generic_system.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/hardware_interface/src/fake_components/generic_system.cpp b/hardware_interface/src/fake_components/generic_system.cpp index 6bf5fe4d57..a0b340023e 100644 --- a/hardware_interface/src/fake_components/generic_system.cpp +++ b/hardware_interface/src/fake_components/generic_system.cpp @@ -543,6 +543,8 @@ bool GenericSystem::get_interface( auto j = std::distance(interface_list.begin(), it); printf("%s %ld %ld %ld\n",interface_name.c_str(), j, vector_index, &values[j][vector_index] ); interfaces.emplace_back(HandleType(name, *it, (double*)&(values[j][vector_index]))); +// interfaces.emplace_back(name, *it, &values[j][vector_index]); + return true; } return false; From b671c3e486fbf63582a7a097da3f6d8532aa0af4 Mon Sep 17 00:00:00 2001 From: root Date: Tue, 21 Sep 2021 18:32:53 +0000 Subject: [PATCH 08/14] Add option to use velocity and position mode at the same time. --- .../src/fake_components/generic_system.cpp | 82 +++++-------------- 1 file changed, 20 insertions(+), 62 deletions(-) diff --git a/hardware_interface/src/fake_components/generic_system.cpp b/hardware_interface/src/fake_components/generic_system.cpp index a0b340023e..9ac3638569 100644 --- a/hardware_interface/src/fake_components/generic_system.cpp +++ b/hardware_interface/src/fake_components/generic_system.cpp @@ -99,7 +99,6 @@ return_type GenericSystem::configure(const hardware_interface::HardwareInfo & in // joint velocity commands to zero for (auto i = 0u; i < joint_commands_[VELOCITY_INTERFACE_INDEX].size(); ++i) { - joint_vel_commands_[i] = 0.0; joint_commands_[VELOCITY_INTERFACE_INDEX][i] = 0.0; } @@ -272,16 +271,6 @@ std::vector GenericSystem::export_command_ const auto & joint = info_.joints[i]; for (const auto & interface : joint.command_interfaces) { -// command_interfaces.emplace_back(hardware_interface::CommandInterface( -// info_.joints[i].name, hardware_interface::HW_IF_POSITION, -// &joint_commands_[POSITION_INTERFACE_INDEX][i])); - -// command_interfaces.emplace_back(hardware_interface::CommandInterface( -// info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joint_vel_commands_[i])); -// -// command_interfaces.emplace_back(hardware_interface::CommandInterface( -// info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &joint_commands_[2][i])); - // Add interface: if not in the standard list than use "other" interface list if (!get_interface( joint.name, standard_interfaces_, interface.name, i, joint_commands_, @@ -299,14 +288,7 @@ std::vector GenericSystem::export_command_ } } - for (auto i =0; i< command_interfaces.size(); ++i) - { - - printf("%s %f\n", command_interfaces[i].get_full_name().c_str(), command_interfaces[i].get_value()); - - } - - // Fake sensor command interfaces + // Fake sensor command interfaces if (fake_sensor_command_interfaces_) { for (auto i = 0u; i < info_.sensors.size(); i++) @@ -354,19 +336,6 @@ return_type GenericSystem::prepare_command_mode_switch( } } } - // set new mode to all interfaces at the same time - if (start_modes_.size() != 0 && start_modes_.size() != info_.joints.size()) - { - ret_val = hardware_interface::return_type::ERROR; - } - - // all start interfaces must be the same - can't mix position and velocity control - if ( - start_modes_.size() != 0 && - !std::equal(start_modes_.begin() + 1, start_modes_.end(), start_modes_.begin())) - { - ret_val = hardware_interface::return_type::ERROR; - } // Stopping interfaces // add stop interface per joint in tmp var for later check @@ -384,14 +353,6 @@ return_type GenericSystem::prepare_command_mode_switch( } } } - // stop all interfaces at the same time - if ( - stop_modes_.size() != 0 && - (stop_modes_.size() != info_.joints.size() || - !std::equal(stop_modes_.begin() + 1, stop_modes_.end(), stop_modes_.begin()))) - { - ret_val = hardware_interface::return_type::ERROR; - } return ret_val; } @@ -410,15 +371,22 @@ return_type GenericSystem::perform_command_mode_switch( std::find(start_modes_.begin(), start_modes_.end(), hardware_interface::HW_IF_POSITION) != start_modes_.end()) { - joint_commands_[POSITION_INTERFACE_INDEX] = joint_states_[POSITION_INTERFACE_INDEX]; + for (size_t i = 0; i < joint_commands_[POSITION_INTERFACE_INDEX].size(); ++i) + { + joint_commands_[POSITION_INTERFACE_INDEX][i] = joint_states_[POSITION_INTERFACE_INDEX][i]; + } position_controller_running_ = true; } - else if ( + + if ( start_modes_.size() != 0 && std::find(start_modes_.begin(), start_modes_.end(), hardware_interface::HW_IF_VELOCITY) != start_modes_.end()) { - joint_commands_[VELOCITY_INTERFACE_INDEX] = std::vector(info_.joints.size(), 0.0); + for (size_t i = 0; i < joint_commands_[VELOCITY_INTERFACE_INDEX].size(); ++i) + { + joint_commands_[VELOCITY_INTERFACE_INDEX][i] = 0.0; + } velocity_controller_running_ = true; } return ret_val; @@ -442,34 +410,27 @@ return_type GenericSystem::read() joint_commands_[POSITION_INTERFACE_INDEX][j] + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ : 0.0); - if (standard_interfaces_.size() > 1) + if (info_.joints[j].state_interfaces.size() > 1) joint_states_[VELOCITY_INTERFACE_INDEX][j] = (joint_commands_[POSITION_INTERFACE_INDEX][j] - joint_pos_commands_old_[j]) / period; } } // velocity - // for (size_t j = 0; j < joint_commands_[VELOCITY_INTERFACE_INDEX].size(); ++j) - for (size_t j = 0; j < 2; ++j) - + for (size_t j = 0; j < joint_commands_[VELOCITY_INTERFACE_INDEX].size(); ++j) { if ( !std::isnan(joint_commands_[VELOCITY_INTERFACE_INDEX][j]) && !command_propagation_disabled_ && velocity_controller_running_) { - // joint_states_[POSITION_INTERFACE_INDEX][j] += - // joint_commands_[VELOCITY_INTERFACE_INDEX][j] * period; - - -// joint_states_[POSITION_INTERFACE_INDEX][j] += joint_vel_commands_[j] * period; - joint_states_[POSITION_INTERFACE_INDEX][j] += joint_commands_[VELOCITY_INTERFACE_INDEX][j] * period; - - // joint_states_[VELOCITY_INTERFACE_INDEX][j] = joint_commands_[VELOCITY_INTERFACE_INDEX][j]; + if (!position_controller_running_) + { + joint_states_[POSITION_INTERFACE_INDEX][j] += + joint_commands_[VELOCITY_INTERFACE_INDEX][j] * period; + joint_commands_[POSITION_INTERFACE_INDEX][j] = joint_states_[POSITION_INTERFACE_INDEX][j]; + } -// joint_states_[VELOCITY_INTERFACE_INDEX][j] = joint_vel_commands_[j]; joint_states_[VELOCITY_INTERFACE_INDEX][j] = joint_commands_[VELOCITY_INTERFACE_INDEX][j]; - - joint_commands_[POSITION_INTERFACE_INDEX][j] = joint_states_[POSITION_INTERFACE_INDEX][j]; } } @@ -541,10 +502,7 @@ bool GenericSystem::get_interface( if (it != interface_list.end()) { auto j = std::distance(interface_list.begin(), it); - printf("%s %ld %ld %ld\n",interface_name.c_str(), j, vector_index, &values[j][vector_index] ); - interfaces.emplace_back(HandleType(name, *it, (double*)&(values[j][vector_index]))); -// interfaces.emplace_back(name, *it, &values[j][vector_index]); - + interfaces.emplace_back(name, *it, &values[j][vector_index]); return true; } return false; From 15ec02319b5ac459941b963e56e20d8dc849b642 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 17 May 2023 02:31:01 +0200 Subject: [PATCH 09/14] Added dynamic simulation functionality. --- .../mock_components/generic_system.hpp | 23 +- .../src/mock_components/generic_system.cpp | 227 +++++++++++++- .../mock_components/test_generic_system.cpp | 294 ++++++++++++------ 3 files changed, 438 insertions(+), 106 deletions(-) diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index c244ee1254..dacf71c461 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/hardware_interface/include/mock_components/generic_system.hpp @@ -32,6 +32,10 @@ namespace mock_components { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +static constexpr size_t POSITION_INTERFACE_INDEX = 0; +static constexpr size_t VELOCITY_INTERFACE_INDEX = 1; +static constexpr size_t ACCELERATION_INTERFACE_INDEX = 2; + class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::SystemInterface { public: @@ -41,6 +45,14 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste std::vector export_command_interfaces() override; + return_type prepare_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) override; + + return_type perform_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) override; + return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override @@ -60,8 +72,6 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION, hardware_interface::HW_IF_EFFORT}; - const size_t POSITION_INTERFACE_INDEX = 0; - struct MimicJoint { std::size_t joint_index; @@ -81,12 +91,12 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste std::vector sensor_interfaces_; /// The size of this vector is (sensor_interfaces_.size() x nr_joints) - std::vector> sensor_mock_commands_; + std::vector> sensor_fake_commands_; std::vector> sensor_states_; std::vector gpio_interfaces_; /// The size of this vector is (gpio_interfaces_.size() x nr_joints) - std::vector> gpio_mock_commands_; + std::vector> gpio_fake_commands_; std::vector> gpio_commands_; std::vector> gpio_states_; @@ -108,14 +118,15 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste std::vector & interfaces, std::vector> & storage, std::vector & target_interfaces, bool using_state_interfaces); - bool use_mock_gpio_command_interfaces_; + bool use_fake_gpio_command_interfaces_; bool use_mock_sensor_command_interfaces_; double position_state_following_offset_; std::string custom_interface_with_following_offset_; size_t index_custom_interface_with_following_offset_; - bool command_propagation_disabled_; + bool calculate_dynamics_; + std::vector joint_control_mode_; }; typedef GenericSystem GenericRobot; diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 13a5a8b679..0c07784cdc 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -131,6 +131,18 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i command_propagation_disabled_ = false; } + // check if there is parameter that enables dynamic calculation + // this way we simulate disconnected driver + it = info_.hardware_parameters.find("calculate_dynamics"); + if (it != info.hardware_parameters.end()) + { + calculate_dynamics_ = hardware_interface::parse_bool(it->second); + } + else + { + calculate_dynamics_ = false; + } + // process parameters about state following position_state_following_offset_ = 0.0; custom_interface_with_following_offset_ = ""; @@ -312,7 +324,7 @@ std::vector GenericSystem::export_command_ std::vector command_interfaces; // Joints' state interfaces - for (auto i = 0u; i < info_.joints.size(); i++) + for (size_t i = 0; i < info_.joints.size(); ++i) { const auto & joint = info_.joints[i]; for (const auto & interface : joint.command_interfaces) @@ -333,6 +345,8 @@ std::vector GenericSystem::export_command_ } } } + // Set position control mode per default + joint_control_mode_.resize(info_.joints.size(), POSITION_INTERFACE_INDEX); // Mock sensor command interfaces if (use_mock_sensor_command_interfaces_) @@ -369,7 +383,124 @@ std::vector GenericSystem::export_command_ return command_interfaces; } -return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +return_type GenericSystem::prepare_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & /*stop_interfaces*/) +{ + hardware_interface::return_type ret_val = hardware_interface::return_type::OK; + + if (!calculate_dynamics_) + { + return ret_val; + } + + const size_t FOUND_ONCE_FLAG = 1000000; + + std::vector joint_found_in_x_requests_; + joint_found_in_x_requests_.resize(info_.joints.size(), 0); + + for (const auto & key : start_interfaces) + { + // check if interface is joint + auto joint_it_found = std::find_if( + info_.joints.begin(), info_.joints.end(), + [key](const auto & joint) { return (key.find(joint.name) != std::string::npos); }); + + if (joint_it_found != info_.joints.end()) + { + const size_t joint_index = std::distance(info_.joints.begin(), joint_it_found); + if (joint_found_in_x_requests_[joint_index] == 0) + { + joint_found_in_x_requests_[joint_index] = FOUND_ONCE_FLAG; + } + + if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) + { + joint_found_in_x_requests_[joint_index] += 1; + } + if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) + { + joint_found_in_x_requests_[joint_index] += 1; + } + if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) + { + joint_found_in_x_requests_[joint_index] += 1; + } + } + else + { + RCUTILS_LOG_DEBUG_NAMED( + "mock_generic_system", "Got interface '%s' that is not joint - nothing to do!", + key.c_str()); + } + } + + for (size_t i = 0; i < info_.joints.size(); ++i) + { + // There has to always be at least one control mode from the above three set + if (joint_found_in_x_requests_[i] == FOUND_ONCE_FLAG) + { + RCUTILS_LOG_ERROR_NAMED( + "mock_generic_system", "Joint '%s' has to have '%s', '%s', or '%s' interface!", + info_.joints[i].name.c_str(), hardware_interface::HW_IF_POSITION, + hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION); + ret_val = hardware_interface::return_type::ERROR; + } + + // Currently we don't support multiple interface request + if (joint_found_in_x_requests_[i] > (FOUND_ONCE_FLAG + 1)) + { + RCUTILS_LOG_ERROR_NAMED( + "mock_generic_system", + "Got multiple (%zu) starting interfaces for joint '%s' - this is not " + "supported!", + joint_found_in_x_requests_[i] - FOUND_ONCE_FLAG, info_.joints[i].name.c_str()); + ret_val = hardware_interface::return_type::ERROR; + } + } + + return ret_val; +} + +return_type GenericSystem::perform_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & /*stop_interfaces*/) +{ + if (!calculate_dynamics_) + { + return hardware_interface::return_type::OK; + } + + for (const auto & key : start_interfaces) + { + // check if interface is joint + auto joint_it_found = std::find_if( + info_.joints.begin(), info_.joints.end(), + [key](const auto & joint) { return (key.find(joint.name) != std::string::npos); }); + + if (joint_it_found != info_.joints.end()) + { + const size_t joint_index = std::distance(info_.joints.begin(), joint_it_found); + + if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) + { + joint_control_mode_[joint_index] = POSITION_INTERFACE_INDEX; + } + if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) + { + joint_control_mode_[joint_index] = VELOCITY_INTERFACE_INDEX; + } + if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) + { + joint_control_mode_[joint_index] = ACCELERATION_INTERFACE_INDEX; + } + } + } + + return hardware_interface::return_type::OK; +} + +return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { if (command_propagation_disabled_) { @@ -392,19 +523,97 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur } }; - // apply offset to positions only for (size_t j = 0; j < joint_states_[POSITION_INTERFACE_INDEX].size(); ++j) { - if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) + if (calculate_dynamics_) { - joint_states_[POSITION_INTERFACE_INDEX][j] = - joint_commands_[POSITION_INTERFACE_INDEX][j] + - (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ : 0.0); + switch (joint_control_mode_[j]) + { + case ACCELERATION_INTERFACE_INDEX: + { + // currently we do backward integration + joint_states_[POSITION_INTERFACE_INDEX][j] += // apply offset to positions only + joint_states_[VELOCITY_INTERFACE_INDEX][j] * period.seconds() + + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ + : 0.0); + + joint_states_[VELOCITY_INTERFACE_INDEX][j] += + joint_states_[ACCELERATION_INTERFACE_INDEX][j] * period.seconds(); + + if (!std::isnan(joint_commands_[ACCELERATION_INTERFACE_INDEX][j])) + { + joint_states_[ACCELERATION_INTERFACE_INDEX][j] = + joint_commands_[ACCELERATION_INTERFACE_INDEX][j]; + } + break; + } + case VELOCITY_INTERFACE_INDEX: + { + // currently we do backward integration + joint_states_[POSITION_INTERFACE_INDEX][j] += // apply offset to positions only + joint_states_[VELOCITY_INTERFACE_INDEX][j] * period.seconds() + + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ + : 0.0); + + if (!std::isnan(joint_commands_[VELOCITY_INTERFACE_INDEX][j])) + { + const double old_velocity = joint_states_[VELOCITY_INTERFACE_INDEX][j]; + + joint_states_[VELOCITY_INTERFACE_INDEX][j] = + joint_commands_[VELOCITY_INTERFACE_INDEX][j]; + + joint_states_[ACCELERATION_INTERFACE_INDEX][j] = + (joint_states_[VELOCITY_INTERFACE_INDEX][j] - old_velocity) / period.seconds(); + } + break; + } + case POSITION_INTERFACE_INDEX: + { + if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) + { + const double old_position = joint_states_[POSITION_INTERFACE_INDEX][j]; + const double old_velocity = joint_states_[VELOCITY_INTERFACE_INDEX][j]; + + joint_states_[POSITION_INTERFACE_INDEX][j] = // apply offset to positions only + joint_commands_[POSITION_INTERFACE_INDEX][j] + + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ + : 0.0); + + joint_states_[VELOCITY_INTERFACE_INDEX][j] = + (joint_states_[POSITION_INTERFACE_INDEX][j] - old_position) / period.seconds(); + + joint_states_[ACCELERATION_INTERFACE_INDEX][j] = + (joint_states_[VELOCITY_INTERFACE_INDEX][j] - old_velocity) / period.seconds(); + } + break; + } + } + } + else + { + for (size_t j = 0; j < joint_states_[POSITION_INTERFACE_INDEX].size(); ++j) + { + if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) + { + joint_states_[POSITION_INTERFACE_INDEX][j] = // apply offset to positions only + joint_commands_[POSITION_INTERFACE_INDEX][j] + + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ + : 0.0); + } + } } } - // do loopback on all other interfaces - starts from 1 because 0 index is position interface - mirror_command_to_state(joint_states_, joint_commands_, 1); + // do loopback on all other interfaces - starts from 1 or 3 because 0, 1, 3 are position, + // velocity, and acceleration interface + if (calculate_dynamics_) + { + mirror_command_to_state(joint_states_, joint_commands_, 3); + } + else + { + mirror_command_to_state(joint_states_, joint_commands_, 1); + } for (const auto & mimic_joint : mimic_joints_) { diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 8e78ade7fb..52642a130a 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -33,15 +33,15 @@ namespace { const auto TIME = rclcpp::Time(0); -const auto PERIOD = rclcpp::Duration::from_seconds(0.01); +const auto PERIOD = rclcpp::Duration::from_seconds(0.1); // 0.1 seconds for easier math +const auto COMPARE_DELTA = 0.0001; } // namespace class TestGenericSystem : public ::testing::Test { public: - void test_generic_system_with_mimic_joint(std::string & urdf); void test_generic_system_with_mock_sensor_commands(std::string & urdf); - void test_generic_system_with_mock_gpio_commands(std::string & urdf); + void test_generic_system_with_mimic_joint(std::string & urdf); protected: void SetUp() override @@ -186,7 +186,7 @@ class TestGenericSystem : public ::testing::Test )"; - hardware_system_2dof_with_sensor_mock_command_ = + hardware_system_2dof_with_sensor_fake_command_ = R"( @@ -215,7 +215,7 @@ class TestGenericSystem : public ::testing::Test )"; - hardware_system_2dof_with_sensor_mock_command_True_ = + hardware_system_2dof_with_sensor_fake_command_True_ = R"( @@ -382,41 +382,7 @@ class TestGenericSystem : public ::testing::Test )"; - valid_urdf_ros2_control_system_robot_with_gpio_mock_command_ = - R"( - - - mock_components/GenericSystem - true - - - - - - - 3.45 - - - - - - - 2.78 - - - - - - - - - - - - -)"; - - valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_ = + valid_urdf_ros2_control_system_robot_with_gpio_fake_command_ = R"( @@ -484,12 +450,12 @@ class TestGenericSystem : public ::testing::Test )"; - disabled_commands_ = + hardware_system_2dof_standard_interfaces_with_different_controli_modes_ = R"( - fake_components/GenericSystem - True + mock_components/GenericSystem + true @@ -498,7 +464,22 @@ class TestGenericSystem : public ::testing::Test 3.45 + + + + + + + 2.78 + + + + + + + + )"; } @@ -510,18 +491,17 @@ class TestGenericSystem : public ::testing::Test std::string hardware_system_2dof_standard_interfaces_; std::string hardware_system_2dof_with_other_interface_; std::string hardware_system_2dof_with_sensor_; - std::string hardware_system_2dof_with_sensor_mock_command_; - std::string hardware_system_2dof_with_sensor_mock_command_True_; + std::string hardware_system_2dof_with_sensor_fake_command_; + std::string hardware_system_2dof_with_sensor_fake_command_True_; std::string hardware_system_2dof_with_mimic_joint_; std::string hardware_system_2dof_standard_interfaces_with_offset_; std::string hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_; std::string hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_missing_; std::string valid_urdf_ros2_control_system_robot_with_gpio_; - std::string valid_urdf_ros2_control_system_robot_with_gpio_mock_command_; - std::string valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_; + std::string valid_urdf_ros2_control_system_robot_with_gpio_fake_command_; std::string sensor_with_initial_value_; std::string gpio_with_initial_value_; - std::string disabled_commands_; + std::string hardware_system_2dof_standard_interfaces_with_different_controli_modes_; }; // Forward declaration @@ -540,12 +520,12 @@ class TestableResourceManager : public hardware_interface::ResourceManager FRIEND_TEST(TestGenericSystem, generic_system_2dof_asymetric_interfaces); FRIEND_TEST(TestGenericSystem, generic_system_2dof_other_interfaces); FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor); - FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor_mock_command); - FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor_mock_command_True); + FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor_fake_command); + FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor_fake_command_True); FRIEND_TEST(TestGenericSystem, hardware_system_2dof_with_mimic_joint); FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio); - FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command); - FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True); + FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_fake_command); + FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_fake_command); TestableResourceManager() : hardware_interface::ResourceManager() {} @@ -1125,18 +1105,18 @@ void TestGenericSystem::test_generic_system_with_mock_sensor_commands(std::strin ASSERT_EQ(4.44, sty_c.get_value()); } -TEST_F(TestGenericSystem, generic_system_2dof_sensor_mock_command) +TEST_F(TestGenericSystem, generic_system_2dof_sensor_fake_command) { - auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_sensor_mock_command_ + + auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_sensor_fake_command_ + ros2_control_test_assets::urdf_tail; test_generic_system_with_mock_sensor_commands(urdf); } -TEST_F(TestGenericSystem, generic_system_2dof_sensor_mock_command_True) +TEST_F(TestGenericSystem, generic_system_2dof_sensor_fake_command_True) { auto urdf = ros2_control_test_assets::urdf_head + - hardware_system_2dof_with_sensor_mock_command_True_ + + hardware_system_2dof_with_sensor_fake_command_True_ + ros2_control_test_assets::urdf_tail; test_generic_system_with_mock_sensor_commands(urdf); @@ -1451,8 +1431,11 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio) generic_system_functional_test(urdf); } -void TestGenericSystem::test_generic_system_with_mock_gpio_commands(std::string & urdf) +TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_fake_command) { + auto urdf = ros2_control_test_assets::urdf_head + + valid_urdf_ros2_control_system_robot_with_gpio_fake_command_ + + ros2_control_test_assets::urdf_tail; TestableResourceManager rm(urdf); // check is hardware is started @@ -1559,25 +1542,7 @@ void TestGenericSystem::test_generic_system_with_mock_gpio_commands(std::string ASSERT_EQ(2.22, gpio2_vac_c.get_value()); } -TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command) -{ - auto urdf = ros2_control_test_assets::urdf_head + - valid_urdf_ros2_control_system_robot_with_gpio_mock_command_ + - ros2_control_test_assets::urdf_tail; - - test_generic_system_with_mock_gpio_commands(urdf); -} - -TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True) -{ - auto urdf = ros2_control_test_assets::urdf_head + - valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_ + - ros2_control_test_assets::urdf_tail; - - test_generic_system_with_mock_gpio_commands(urdf); -} - -TEST_F(TestGenericSystem, sensor_with_initial_value) +TEST_F(TestGenericSystem, sensor_with_initial_value_) { auto urdf = ros2_control_test_assets::urdf_head + sensor_with_initial_value_ + ros2_control_test_assets::urdf_tail; @@ -1605,7 +1570,7 @@ TEST_F(TestGenericSystem, sensor_with_initial_value) ASSERT_EQ(0.0, force_z_s.get_value()); } -TEST_F(TestGenericSystem, gpio_with_initial_value) +TEST_F(TestGenericSystem, gpio_with_initial_value_) { auto urdf = ros2_control_test_assets::urdf_head + gpio_with_initial_value_ + ros2_control_test_assets::urdf_tail; @@ -1624,50 +1589,197 @@ TEST_F(TestGenericSystem, gpio_with_initial_value) ASSERT_EQ(1, state.get_value()); } -TEST_F(TestGenericSystem, disabled_commands_flag_is_active) +TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces) { - auto urdf = - ros2_control_test_assets::urdf_head + disabled_commands_ + ros2_control_test_assets::urdf_tail; + auto urdf = ros2_control_test_assets::urdf_head + + hardware_system_2dof_standard_interfaces_with_different_controli_modes_ + + ros2_control_test_assets::urdf_tail; TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); - ASSERT_EQ(2u, rm.state_interface_keys().size()); + ASSERT_EQ(7u, rm.state_interface_keys().size()); EXPECT_TRUE(rm.state_interface_exists("joint1/position")); EXPECT_TRUE(rm.state_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm.state_interface_exists("joint1/acceleration")); + EXPECT_TRUE(rm.state_interface_exists("joint2/velocity")); + EXPECT_TRUE(rm.state_interface_exists("joint2/velocity")); + EXPECT_TRUE(rm.state_interface_exists("joint2/acceleration")); + EXPECT_TRUE(rm.state_interface_exists("flange_vacuum/vacuum")); - ASSERT_EQ(2u, rm.command_interface_keys().size()); + ASSERT_EQ(5u, rm.command_interface_keys().size()); EXPECT_TRUE(rm.command_interface_exists("joint1/position")); EXPECT_TRUE(rm.command_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm.command_interface_exists("joint2/velocity")); + EXPECT_TRUE(rm.command_interface_exists("joint2/acceleration")); + EXPECT_TRUE(rm.command_interface_exists("flange_vacuum/vacuum")); // Check initial values hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position"); hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface("joint1/velocity"); + hardware_interface::LoanedStateInterface j1a_s = rm.claim_state_interface("joint1/acceleration"); + hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface("joint2/position"); + hardware_interface::LoanedStateInterface j2v_s = rm.claim_state_interface("joint2/velocity"); + hardware_interface::LoanedStateInterface j2a_s = rm.claim_state_interface("joint2/acceleration"); hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface("joint1/position"); + hardware_interface::LoanedCommandInterface j1v_c = rm.claim_command_interface("joint1/velocity"); + hardware_interface::LoanedCommandInterface j2v_c = rm.claim_command_interface("joint2/velocity"); + hardware_interface::LoanedCommandInterface j2a_c = + rm.claim_command_interface("joint2/acceleration"); - ASSERT_EQ(3.45, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(3.45, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.78, j2p_s.get_value()); + EXPECT_EQ(0.0, j2v_s.get_value()); + EXPECT_EQ(0.0, j2a_s.get_value()); ASSERT_TRUE(std::isnan(j1p_c.get_value())); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_TRUE(std::isnan(j2a_c.get_value())); + + // Test error management in prepare mode switch + ASSERT_EQ( // joint2 has non 'position', 'velocity', or 'acceleration' interface + rm.prepare_command_mode_switch({"joint1/position", "joint2/effort"}, {}), false); + ASSERT_EQ( // joint1 has two interfaces + rm.prepare_command_mode_switch({"joint1/position", "joint1/acceleration"}, {}), false); + + // switch controller mode as controller manager is doing - gpio itf 'vacuum' will be ignored + ASSERT_EQ( + rm.prepare_command_mode_switch( + {"joint1/position", "joint2/acceleration", "flange_vacuum/vacuum"}, {}), + true); + ASSERT_EQ( + rm.perform_command_mode_switch( + {"joint1/position", "joint2/acceleration", "flange_vacuum/vacuum"}, {}), + true); // set some new values in commands j1p_c.set_value(0.11); + j2a_c.set_value(3.5); // State values should not be changed - ASSERT_EQ(3.45, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(3.45, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.78, j2p_s.get_value()); + EXPECT_EQ(0.0, j2v_s.get_value()); + EXPECT_EQ(0.0, j2a_s.get_value()); ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.5, j2a_c.get_value()); // write() does not change values rm.write(TIME, PERIOD); - ASSERT_EQ(3.45, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(3.45, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.78, j2p_s.get_value()); + EXPECT_EQ(0.0, j2v_s.get_value()); + EXPECT_EQ(0.0, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // read() mirrors commands to states and calculate dynamics + rm.read(TIME, PERIOD); + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(-33.4, j1v_s.get_value()); + EXPECT_NEAR(-334.0, j1a_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(2.78, j2p_s.get_value()); + EXPECT_EQ(0.0, j2v_s.get_value()); + EXPECT_EQ(3.5, j2a_s.get_value()); ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.5, j2a_c.get_value()); - // read() also does not change values + // read() mirrors commands to states and calculate dynamics rm.read(TIME, PERIOD); - ASSERT_EQ(3.45, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_NEAR(334.0, j1a_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(2.78, j2p_s.get_value()); + EXPECT_NEAR(0.35, j2v_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // read() mirrors commands to states and calculate dynamics + rm.read(TIME, PERIOD); + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.815, j2p_s.get_value()); + EXPECT_NEAR(0.7, j2v_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // switch controller mode as controller manager is doing + ASSERT_EQ(rm.prepare_command_mode_switch({"joint1/velocity", "joint2/velocity"}, {}), true); + ASSERT_EQ(rm.perform_command_mode_switch({"joint1/velocity", "joint2/velocity"}, {}), true); + + // set some new values in commands + j1v_c.set_value(0.5); + j2v_c.set_value(2.0); + + // State values should not be changed + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.815, j2p_s.get_value()); + EXPECT_NEAR(0.7, j2v_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.5, j1v_c.get_value()); + ASSERT_EQ(2.0, j2v_c.get_value()); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // write() does not change values + rm.write(TIME, PERIOD); + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.815, j2p_s.get_value()); + EXPECT_NEAR(0.7, j2v_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.5, j1v_c.get_value()); + ASSERT_EQ(2.0, j2v_c.get_value()); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // read() mirrors commands to states and calculate dynamics (both velocity mode) + rm.read(TIME, PERIOD); + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(0.5, j1v_s.get_value()); + EXPECT_EQ(5.0, j1a_s.get_value()); + EXPECT_EQ(2.885, j2p_s.get_value()); + EXPECT_EQ(2.0, j2v_s.get_value()); + EXPECT_NEAR(13.0, j2a_s.get_value(), COMPARE_DELTA); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.5, j1v_c.get_value()); + ASSERT_EQ(2.0, j2v_c.get_value()); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // read() mirrors commands to states and calculate dynamics (both velocity mode) + rm.read(TIME, PERIOD); + EXPECT_EQ(0.16, j1p_s.get_value()); + EXPECT_EQ(0.5, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(3.085, j2p_s.get_value()); + EXPECT_EQ(2.0, j2v_s.get_value()); + EXPECT_EQ(0.0, j2a_s.get_value()); ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.5, j1v_c.get_value()); + ASSERT_EQ(2.0, j2v_c.get_value()); + ASSERT_EQ(3.5, j2a_c.get_value()); } From 8da6bb567004da62ea04d059a44a08279cdadcc7 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 5 Jun 2023 16:08:30 +0200 Subject: [PATCH 10/14] Fixup some details --- .../mock_components/test_generic_system.cpp | 166 +++++++++++++++--- 1 file changed, 142 insertions(+), 24 deletions(-) diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 52642a130a..cd344b600e 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -40,8 +40,9 @@ const auto COMPARE_DELTA = 0.0001; class TestGenericSystem : public ::testing::Test { public: - void test_generic_system_with_mock_sensor_commands(std::string & urdf); void test_generic_system_with_mimic_joint(std::string & urdf); + void test_generic_system_with_mock_sensor_commands(std::string & urdf); + void test_generic_system_with_mock_gpio_commands(std::string & urdf); protected: void SetUp() override @@ -186,7 +187,7 @@ class TestGenericSystem : public ::testing::Test )"; - hardware_system_2dof_with_sensor_fake_command_ = + hardware_system_2dof_with_sensor_mock_command_ = R"( @@ -215,7 +216,7 @@ class TestGenericSystem : public ::testing::Test )"; - hardware_system_2dof_with_sensor_fake_command_True_ = + hardware_system_2dof_with_sensor_mock_command_True_ = R"( @@ -382,7 +383,41 @@ class TestGenericSystem : public ::testing::Test )"; - valid_urdf_ros2_control_system_robot_with_gpio_fake_command_ = + valid_urdf_ros2_control_system_robot_with_gpio_mock_command_ = + R"( + + + mock_components/GenericSystem + true + + + + + + + 3.45 + + + + + + + 2.78 + + + + + + + + + + + + +)"; + + valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_ = R"( @@ -450,7 +485,7 @@ class TestGenericSystem : public ::testing::Test )"; - hardware_system_2dof_standard_interfaces_with_different_controli_modes_ = + hardware_system_2dof_standard_interfaces_with_different_control_modes_ = R"( @@ -482,6 +517,24 @@ class TestGenericSystem : public ::testing::Test )"; + + disabled_commands_ = + R"( + + + fake_components/GenericSystem + True + + + + + + 3.45 + + + + +)"; } std::string hardware_robot_2dof_; @@ -491,17 +544,19 @@ class TestGenericSystem : public ::testing::Test std::string hardware_system_2dof_standard_interfaces_; std::string hardware_system_2dof_with_other_interface_; std::string hardware_system_2dof_with_sensor_; - std::string hardware_system_2dof_with_sensor_fake_command_; - std::string hardware_system_2dof_with_sensor_fake_command_True_; + std::string hardware_system_2dof_with_sensor_mock_command_; + std::string hardware_system_2dof_with_sensor_mock_command_True_; std::string hardware_system_2dof_with_mimic_joint_; std::string hardware_system_2dof_standard_interfaces_with_offset_; std::string hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_; std::string hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_missing_; std::string valid_urdf_ros2_control_system_robot_with_gpio_; - std::string valid_urdf_ros2_control_system_robot_with_gpio_fake_command_; + std::string valid_urdf_ros2_control_system_robot_with_gpio_mock_command_; + std::string valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_; std::string sensor_with_initial_value_; std::string gpio_with_initial_value_; - std::string hardware_system_2dof_standard_interfaces_with_different_controli_modes_; + std::string hardware_system_2dof_standard_interfaces_with_different_control_modes_; + std::string disabled_commands_; }; // Forward declaration @@ -520,12 +575,12 @@ class TestableResourceManager : public hardware_interface::ResourceManager FRIEND_TEST(TestGenericSystem, generic_system_2dof_asymetric_interfaces); FRIEND_TEST(TestGenericSystem, generic_system_2dof_other_interfaces); FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor); - FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor_fake_command); - FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor_fake_command_True); + FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor_mock_command); + FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor_mock_command_True); FRIEND_TEST(TestGenericSystem, hardware_system_2dof_with_mimic_joint); FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio); - FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_fake_command); - FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_fake_command); + FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command); + FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True); TestableResourceManager() : hardware_interface::ResourceManager() {} @@ -1105,18 +1160,18 @@ void TestGenericSystem::test_generic_system_with_mock_sensor_commands(std::strin ASSERT_EQ(4.44, sty_c.get_value()); } -TEST_F(TestGenericSystem, generic_system_2dof_sensor_fake_command) +TEST_F(TestGenericSystem, generic_system_2dof_sensor_mock_command) { - auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_sensor_fake_command_ + + auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_sensor_mock_command_ + ros2_control_test_assets::urdf_tail; test_generic_system_with_mock_sensor_commands(urdf); } -TEST_F(TestGenericSystem, generic_system_2dof_sensor_fake_command_True) +TEST_F(TestGenericSystem, generic_system_2dof_sensor_mock_command_True) { auto urdf = ros2_control_test_assets::urdf_head + - hardware_system_2dof_with_sensor_fake_command_True_ + + hardware_system_2dof_with_sensor_mock_command_True_ + ros2_control_test_assets::urdf_tail; test_generic_system_with_mock_sensor_commands(urdf); @@ -1431,11 +1486,8 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio) generic_system_functional_test(urdf); } -TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_fake_command) +void TestGenericSystem::test_generic_system_with_mock_gpio_commands(std::string & urdf) { - auto urdf = ros2_control_test_assets::urdf_head + - valid_urdf_ros2_control_system_robot_with_gpio_fake_command_ + - ros2_control_test_assets::urdf_tail; TestableResourceManager rm(urdf); // check is hardware is started @@ -1542,7 +1594,25 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_fake_co ASSERT_EQ(2.22, gpio2_vac_c.get_value()); } -TEST_F(TestGenericSystem, sensor_with_initial_value_) +TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command) +{ + auto urdf = ros2_control_test_assets::urdf_head + + valid_urdf_ros2_control_system_robot_with_gpio_mock_command_ + + ros2_control_test_assets::urdf_tail; + + test_generic_system_with_mock_gpio_commands(urdf); +} + +TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True) +{ + auto urdf = ros2_control_test_assets::urdf_head + + valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_ + + ros2_control_test_assets::urdf_tail; + + test_generic_system_with_mock_gpio_commands(urdf); +} + +TEST_F(TestGenericSystem, sensor_with_initial_value) { auto urdf = ros2_control_test_assets::urdf_head + sensor_with_initial_value_ + ros2_control_test_assets::urdf_tail; @@ -1570,7 +1640,7 @@ TEST_F(TestGenericSystem, sensor_with_initial_value_) ASSERT_EQ(0.0, force_z_s.get_value()); } -TEST_F(TestGenericSystem, gpio_with_initial_value_) +TEST_F(TestGenericSystem, gpio_with_initial_value) { auto urdf = ros2_control_test_assets::urdf_head + gpio_with_initial_value_ + ros2_control_test_assets::urdf_tail; @@ -1592,8 +1662,9 @@ TEST_F(TestGenericSystem, gpio_with_initial_value_) TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces) { auto urdf = ros2_control_test_assets::urdf_head + - hardware_system_2dof_standard_interfaces_with_different_controli_modes_ + + hardware_system_2dof_standard_interfaces_with_different_control_modes_ + ros2_control_test_assets::urdf_tail; + TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); @@ -1783,3 +1854,50 @@ TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces) ASSERT_EQ(2.0, j2v_c.get_value()); ASSERT_EQ(3.5, j2a_c.get_value()); } + +TEST_F(TestGenericSystem, disabled_commands_flag_is_active) +{ + auto urdf = + ros2_control_test_assets::urdf_head + disabled_commands_ + ros2_control_test_assets::urdf_tail; + TestableResourceManager rm(urdf); + // Activate components to get all interfaces available + activate_components(rm); + + // Check interfaces + EXPECT_EQ(1u, rm.system_components_size()); + ASSERT_EQ(2u, rm.state_interface_keys().size()); + EXPECT_TRUE(rm.state_interface_exists("joint1/position")); + EXPECT_TRUE(rm.state_interface_exists("joint1/velocity")); + + ASSERT_EQ(2u, rm.command_interface_keys().size()); + EXPECT_TRUE(rm.command_interface_exists("joint1/position")); + EXPECT_TRUE(rm.command_interface_exists("joint1/velocity")); + // Check initial values + hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position"); + hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface("joint1/velocity"); + hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface("joint1/position"); + + ASSERT_EQ(3.45, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_TRUE(std::isnan(j1p_c.get_value())); + + // set some new values in commands + j1p_c.set_value(0.11); + + // State values should not be changed + ASSERT_EQ(3.45, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + + // write() does not change values + rm.write(TIME, PERIOD); + ASSERT_EQ(3.45, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + + // read() also does not change values + rm.read(TIME, PERIOD); + ASSERT_EQ(3.45, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); +} From 0528a8aaa71dbcf2d79b0d2242259e702f3f92b0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 15 Jun 2023 09:48:04 +0200 Subject: [PATCH 11/14] Sync with the last functional state. --- .../fake_components/generic_system.hpp | 123 +----------------- .../mock_components/generic_system.hpp | 8 +- 2 files changed, 8 insertions(+), 123 deletions(-) diff --git a/hardware_interface/include/fake_components/generic_system.hpp b/hardware_interface/include/fake_components/generic_system.hpp index 1540ffd6fd..3dfb2b9b70 100644 --- a/hardware_interface/include/fake_components/generic_system.hpp +++ b/hardware_interface/include/fake_components/generic_system.hpp @@ -17,130 +17,13 @@ #ifndef FAKE_COMPONENTS__GENERIC_SYSTEM_HPP_ #define FAKE_COMPONENTS__GENERIC_SYSTEM_HPP_ -#include -#include -#include - -#include "hardware_interface/base_interface.hpp" -#include "hardware_interface/handle.hpp" -#include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/system_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/hardware_interface_status_values.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" - -using hardware_interface::return_type; +#include "mock_components/generic_system.hpp" namespace fake_components { -enum StoppingInterface -{ - NONE, - STOP_POSITION, - STOP_VELOCITY -}; - -class HARDWARE_INTERFACE_PUBLIC GenericSystem -: public hardware_interface::BaseInterface -{ -public: - return_type configure(const hardware_interface::HardwareInfo & info) override; - - std::vector export_state_interfaces() override; - - std::vector export_command_interfaces() override; - - return_type start() override - { - status_ = hardware_interface::status::STARTED; - return return_type::OK; - } - - return_type stop() override - { - status_ = hardware_interface::status::STOPPED; - return return_type::OK; - } - - return_type read() override; - - return_type write() override { return return_type::OK; } - - return_type prepare_command_mode_switch( - const std::vector & start_interfaces, - const std::vector & stop_interfaces) override; - - return_type perform_command_mode_switch( - const std::vector & start_interfaces, - const std::vector & stop_interfaces) override; - -protected: - /// Use standard interfaces for joints because they are relevant for dynamic behaviour - /** - * By splitting the standard interfaces from other type, the users are able to inherit this - * class and simply create small "simulation" with desired dynamic behaviour. - * The advantage over using Gazebo is that enables "quick & dirty" tests of robot's URDF and - * controllers. - */ - const std::vector standard_interfaces_ = { - hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, - hardware_interface::HW_IF_ACCELERATION, hardware_interface::HW_IF_EFFORT}; - - const size_t POSITION_INTERFACE_INDEX = 0; - const size_t VELOCITY_INTERFACE_INDEX = 1; - - struct MimicJoint - { - std::size_t joint_index; - std::size_t mimicked_joint_index; - double multiplier = 1.0; - }; - std::vector mimic_joints_; - - /// The size of this vector is (standard_interfaces_.size() x nr_joints) - std::vector> joint_commands_; - std::vector> joint_states_; - - std::vector other_interfaces_; - /// The size of this vector is (other_interfaces_.size() x nr_joints) - std::vector> other_commands_; - std::vector> other_states_; - - std::vector sensor_interfaces_; - /// The size of this vector is (other_interfaces_.size() x nr_joints) - std::vector> sensor_fake_commands_; - std::vector> sensor_states_; - -private: - template - bool get_interface( - const std::string & name, const std::vector & interface_list, - const std::string & interface_name, const size_t vector_index, - std::vector> & values, std::vector & interfaces); - - void initialize_storage_vectors( - std::vector> & commands, std::vector> & states, - const std::vector & interfaces); - - bool fake_sensor_command_interfaces_; - double position_state_following_offset_; - std::string custom_interface_with_following_offset_; - size_t index_custom_interface_with_following_offset_; - bool command_propagation_disabled_; - - // resources switching aux vars - std::vector stop_modes_; - std::vector start_modes_; - bool position_controller_running_; - bool velocity_controller_running_; - std::chrono::system_clock::time_point begin; - // for velocity control, store last position command - std::vector joint_pos_commands_old_; - double period_; -}; - -typedef GenericSystem GenericRobot; +using GenericSystem [[deprecated]] = mock_components::GenericSystem; +using GenericSystem [[deprecated]] = mock_components::GenericRobot; } // namespace fake_components #endif // FAKE_COMPONENTS__GENERIC_SYSTEM_HPP_ diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index dacf71c461..e9b38de65d 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/hardware_interface/include/mock_components/generic_system.hpp @@ -91,12 +91,12 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste std::vector sensor_interfaces_; /// The size of this vector is (sensor_interfaces_.size() x nr_joints) - std::vector> sensor_fake_commands_; + std::vector> sensor_mock_commands_; std::vector> sensor_states_; std::vector gpio_interfaces_; /// The size of this vector is (gpio_interfaces_.size() x nr_joints) - std::vector> gpio_fake_commands_; + std::vector> gpio_mock_commands_; std::vector> gpio_commands_; std::vector> gpio_states_; @@ -118,7 +118,7 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste std::vector & interfaces, std::vector> & storage, std::vector & target_interfaces, bool using_state_interfaces); - bool use_fake_gpio_command_interfaces_; + bool use_mock_gpio_command_interfaces_; bool use_mock_sensor_command_interfaces_; double position_state_following_offset_; @@ -127,6 +127,8 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste bool calculate_dynamics_; std::vector joint_control_mode_; + + bool command_propagation_disabled_; }; typedef GenericSystem GenericRobot; From e29f664cd3a5f4ba7e015c72cdf99013289cf8e3 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 15 Jun 2023 09:51:28 +0200 Subject: [PATCH 12/14] Remove obsolete comment. --- hardware_interface/src/mock_components/generic_system.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 0c07784cdc..d806d33bd3 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -132,7 +132,6 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } // check if there is parameter that enables dynamic calculation - // this way we simulate disconnected driver it = info_.hardware_parameters.find("calculate_dynamics"); if (it != info.hardware_parameters.end()) { From ea054bc456aaa27769303ee4f2c0688e82de82dd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 15 Jun 2023 09:50:32 +0200 Subject: [PATCH 13/14] Remove obsolete file. --- .../src/fake_components/generic_system.cpp | 542 ------------------ 1 file changed, 542 deletions(-) delete mode 100644 hardware_interface/src/fake_components/generic_system.cpp diff --git a/hardware_interface/src/fake_components/generic_system.cpp b/hardware_interface/src/fake_components/generic_system.cpp deleted file mode 100644 index 9ac3638569..0000000000 --- a/hardware_interface/src/fake_components/generic_system.cpp +++ /dev/null @@ -1,542 +0,0 @@ -// Copyright (c) 2021 PickNik, Inc. -// -// 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. -// -// Author: Jafar Abdi, Denis Stogl - -#include "fake_components/generic_system.hpp" - -#include -#include -#include -#include -#include -#include -#include - -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rcutils/logging_macros.h" - -namespace fake_components -{ -return_type GenericSystem::configure(const hardware_interface::HardwareInfo & info) -{ - if (configure_default(info) != return_type::OK) - { - return return_type::ERROR; - } - - // check if to create fake command interface for sensor - auto it = info_.hardware_parameters.find("fake_sensor_commands"); - if (it != info_.hardware_parameters.end()) - { - // TODO(anyone): change this to parse_bool() (see ros2_control#339) - fake_sensor_command_interfaces_ = it->second == "true" || it->second == "True"; - } - else - { - fake_sensor_command_interfaces_ = false; - } - - // check if there is parameter that disables commands - // this way we simulate disconnected driver - it = info_.hardware_parameters.find("disable_commands"); - if (it != info.hardware_parameters.end()) - { - command_propagation_disabled_ = it->second == "true" || it->second == "True"; - } - else - { - command_propagation_disabled_ = false; - } - - // process parameters about state following - position_state_following_offset_ = 0.0; - custom_interface_with_following_offset_ = ""; - - it = info_.hardware_parameters.find("position_state_following_offset"); - if (it != info_.hardware_parameters.end()) - { - position_state_following_offset_ = std::stod(it->second); - it = info_.hardware_parameters.find("custom_interface_with_following_offset"); - if (it != info_.hardware_parameters.end()) - { - custom_interface_with_following_offset_ = it->second; - } - } - // its extremlly unprobably that std::distance results int this value - therefore default - index_custom_interface_with_following_offset_ = std::numeric_limits::max(); - - // Initialize storage for standard interfaces - initialize_storage_vectors(joint_commands_, joint_states_, standard_interfaces_); - - // set all values without initial values to 0 - for (auto i = 0u; i < info_.joints.size(); i++) - { - for (auto j = 0u; j < standard_interfaces_.size(); j++) - { - if (std::isnan(joint_states_[j][i])) - { - joint_states_[j][i] = 0.0; - } - } - } - - // set memory position vector to initial value - joint_pos_commands_old_.resize(joint_commands_[POSITION_INTERFACE_INDEX].size()); - joint_pos_commands_old_ = joint_commands_[POSITION_INTERFACE_INDEX]; - - // joint velocity commands to zero - for (auto i = 0u; i < joint_commands_[VELOCITY_INTERFACE_INDEX].size(); ++i) - { - joint_commands_[VELOCITY_INTERFACE_INDEX][i] = 0.0; - } - - // Search for mimic joints - for (auto i = 0u; i < info_.joints.size(); ++i) - { - const auto & joint = info_.joints.at(i); - if (joint.parameters.find("mimic") != joint.parameters.cend()) - { - const auto mimicked_joint_it = std::find_if( - info_.joints.begin(), info_.joints.end(), - [&mimicked_joint = - joint.parameters.at("mimic")](const hardware_interface::ComponentInfo & joint_info) { - return joint_info.name == mimicked_joint; - }); - if (mimicked_joint_it == info_.joints.cend()) - { - throw std::runtime_error( - std::string("Mimicked joint '") + joint.parameters.at("mimic") + "' not found"); - } - MimicJoint mimic_joint; - mimic_joint.joint_index = i; - mimic_joint.mimicked_joint_index = std::distance(info_.joints.begin(), mimicked_joint_it); - auto param_it = joint.parameters.find("multiplier"); - if (param_it != joint.parameters.end()) - { - mimic_joint.multiplier = std::stod(joint.parameters.at("multiplier")); - } - mimic_joints_.push_back(mimic_joint); - } - } - // search for non-standard joint interfaces - for (const auto & joint : info_.joints) - { - for (const auto & interface : joint.command_interfaces) - { - // add to list if non-standard interface - if ( - std::find(standard_interfaces_.begin(), standard_interfaces_.end(), interface.name) == - standard_interfaces_.end()) - { - if ( - std::find(other_interfaces_.begin(), other_interfaces_.end(), interface.name) == - other_interfaces_.end()) - { - other_interfaces_.emplace_back(interface.name); - } - } - } - for (const auto & interface : joint.state_interfaces) - { - // add to list if non-standard interface - if ( - std::find(standard_interfaces_.begin(), standard_interfaces_.end(), interface.name) == - standard_interfaces_.end()) - { - if ( - std::find(other_interfaces_.begin(), other_interfaces_.end(), interface.name) == - other_interfaces_.end()) - { - other_interfaces_.emplace_back(interface.name); - } - } - } - } - // Initialize storage for non-standard interfaces - initialize_storage_vectors(other_commands_, other_states_, other_interfaces_); - - // when following offset is used on custom interface then find its index - if (!custom_interface_with_following_offset_.empty()) - { - auto if_it = std::find( - other_interfaces_.begin(), other_interfaces_.end(), custom_interface_with_following_offset_); - if (if_it != other_interfaces_.end()) - { - index_custom_interface_with_following_offset_ = - std::distance(other_interfaces_.begin(), if_it); - RCUTILS_LOG_INFO_NAMED( - "fake_generic_system", "Custom interface with following offset '%s' found at index: %zu.", - custom_interface_with_following_offset_.c_str(), - index_custom_interface_with_following_offset_); - } - else - { - RCUTILS_LOG_WARN_NAMED( - "fake_generic_system", - "Custom interface with following offset '%s' does not exist. Offset will not be applied", - custom_interface_with_following_offset_.c_str()); - } - } - - for (const auto & sensor : info_.sensors) - { - for (const auto & interface : sensor.state_interfaces) - { - if ( - std::find(sensor_interfaces_.begin(), sensor_interfaces_.end(), interface.name) == - sensor_interfaces_.end()) - { - sensor_interfaces_.emplace_back(interface.name); - } - } - } - initialize_storage_vectors(sensor_fake_commands_, sensor_states_, sensor_interfaces_); - - stop_modes_ = {StoppingInterface::NONE, StoppingInterface::NONE, StoppingInterface::NONE, - StoppingInterface::NONE, StoppingInterface::NONE, StoppingInterface::NONE}; - start_modes_ = {hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_POSITION, - hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_POSITION, - hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_POSITION}; - position_controller_running_ = false; - velocity_controller_running_ = false; - begin = std::chrono::system_clock::now(); - - status_ = hardware_interface::status::CONFIGURED; - return hardware_interface::return_type::OK; -} - -std::vector GenericSystem::export_state_interfaces() -{ - std::vector state_interfaces; - - // Joints' state interfaces - for (auto i = 0u; i < info_.joints.size(); i++) - { - const auto & joint = info_.joints[i]; - for (const auto & interface : joint.state_interfaces) - { - // Add interface: if not in the standard list than use "other" interface list - if (!get_interface( - joint.name, standard_interfaces_, interface.name, i, joint_states_, state_interfaces)) - { - if (!get_interface( - joint.name, other_interfaces_, interface.name, i, other_states_, state_interfaces)) - { - throw std::runtime_error( - "Interface is not found in the standard nor other list. " - "This should never happen!"); - } - } - } - } - - // Sensor state interfaces - for (auto i = 0u; i < info_.sensors.size(); i++) - { - const auto & sensor = info_.sensors[i]; - for (const auto & interface : sensor.state_interfaces) - { - if (!get_interface( - sensor.name, sensor_interfaces_, interface.name, i, sensor_states_, state_interfaces)) - { - throw std::runtime_error( - "Interface is not found in the standard nor other list. " - "This should never happen!"); - } - } - } - - return state_interfaces; -} - -std::vector GenericSystem::export_command_interfaces() -{ - std::vector command_interfaces; - - // Joints' state interfaces - for (auto i = 0u; i < info_.joints.size(); i++) - { - const auto & joint = info_.joints[i]; - for (const auto & interface : joint.command_interfaces) - { - // Add interface: if not in the standard list than use "other" interface list - if (!get_interface( - joint.name, standard_interfaces_, interface.name, i, joint_commands_, - command_interfaces)) - { - if (!get_interface( - joint.name, other_interfaces_, interface.name, i, other_commands_, - command_interfaces)) - { - throw std::runtime_error( - "Interface is not found in the standard nor other list. " - "This should never happen!"); - } - } - } - } - - // Fake sensor command interfaces - if (fake_sensor_command_interfaces_) - { - for (auto i = 0u; i < info_.sensors.size(); i++) - { - const auto & sensor = info_.sensors[i]; - for (const auto & interface : sensor.state_interfaces) - { - if (!get_interface( - sensor.name, sensor_interfaces_, interface.name, i, sensor_fake_commands_, - command_interfaces)) - { - throw std::runtime_error( - "Interface is not found in the standard nor other list. " - "This should never happen!"); - } - } - } - } - - return command_interfaces; -} - -return_type GenericSystem::prepare_command_mode_switch( - const std::vector & start_interfaces, - const std::vector & stop_interfaces) -{ - hardware_interface::return_type ret_val = hardware_interface::return_type::OK; - - start_modes_.clear(); - stop_modes_.clear(); - - // Starting interfaces - // add start interface per joint in tmp var for later check - for (const auto & key : start_interfaces) - { - for (auto i = 0u; i < info_.joints.size(); i++) - { - if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) - { - start_modes_.push_back(hardware_interface::HW_IF_POSITION); - } - if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) - { - start_modes_.push_back(hardware_interface::HW_IF_VELOCITY); - } - } - } - - // Stopping interfaces - // add stop interface per joint in tmp var for later check - for (const auto & key : stop_interfaces) - { - for (auto i = 0u; i < info_.joints.size(); i++) - { - if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) - { - stop_modes_.push_back(StoppingInterface::STOP_POSITION); - } - if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) - { - stop_modes_.push_back(StoppingInterface::STOP_VELOCITY); - } - } - } - - return ret_val; -} - -return_type GenericSystem::perform_command_mode_switch( - const std::vector & /*start_interfaces*/, - const std::vector & /*stop_interfaces*/) -{ - hardware_interface::return_type ret_val = hardware_interface::return_type::OK; - - position_controller_running_ = false; - velocity_controller_running_ = false; - - if ( - start_modes_.size() != 0 && - std::find(start_modes_.begin(), start_modes_.end(), hardware_interface::HW_IF_POSITION) != - start_modes_.end()) - { - for (size_t i = 0; i < joint_commands_[POSITION_INTERFACE_INDEX].size(); ++i) - { - joint_commands_[POSITION_INTERFACE_INDEX][i] = joint_states_[POSITION_INTERFACE_INDEX][i]; - } - position_controller_running_ = true; - } - - if ( - start_modes_.size() != 0 && - std::find(start_modes_.begin(), start_modes_.end(), hardware_interface::HW_IF_VELOCITY) != - start_modes_.end()) - { - for (size_t i = 0; i < joint_commands_[VELOCITY_INTERFACE_INDEX].size(); ++i) - { - joint_commands_[VELOCITY_INTERFACE_INDEX][i] = 0.0; - } - velocity_controller_running_ = true; - } - return ret_val; -} - -return_type GenericSystem::read() -{ - std::chrono::system_clock::time_point begin_last = begin; - begin = std::chrono::system_clock::now(); - double period = - std::chrono::duration_cast(begin - begin_last).count() / 1000.0; - - // apply offset to positions only - for (size_t j = 0; j < joint_states_[POSITION_INTERFACE_INDEX].size(); ++j) - { - if ( - !std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j]) && !command_propagation_disabled_ && - position_controller_running_) - { - joint_states_[POSITION_INTERFACE_INDEX][j] = - joint_commands_[POSITION_INTERFACE_INDEX][j] + - (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ : 0.0); - - if (info_.joints[j].state_interfaces.size() > 1) - joint_states_[VELOCITY_INTERFACE_INDEX][j] = - (joint_commands_[POSITION_INTERFACE_INDEX][j] - joint_pos_commands_old_[j]) / period; - } - } - - // velocity - for (size_t j = 0; j < joint_commands_[VELOCITY_INTERFACE_INDEX].size(); ++j) - { - if ( - !std::isnan(joint_commands_[VELOCITY_INTERFACE_INDEX][j]) && !command_propagation_disabled_ && - velocity_controller_running_) - { - if (!position_controller_running_) - { - joint_states_[POSITION_INTERFACE_INDEX][j] += - joint_commands_[VELOCITY_INTERFACE_INDEX][j] * period; - joint_commands_[POSITION_INTERFACE_INDEX][j] = joint_states_[POSITION_INTERFACE_INDEX][j]; - } - - joint_states_[VELOCITY_INTERFACE_INDEX][j] = joint_commands_[VELOCITY_INTERFACE_INDEX][j]; - } - } - - // remember old value of position - joint_pos_commands_old_ = joint_commands_[POSITION_INTERFACE_INDEX]; - - // do loopback on all other interfaces - starts from 1 because 0 index is position interface - for (size_t i = 2; i < joint_states_.size(); ++i) - { - for (size_t j = 0; j < joint_states_[i].size(); ++j) - { - if (!std::isnan(joint_commands_[i][j])) - { - joint_states_[i][j] = joint_commands_[i][j]; - } - } - } - for (const auto & mimic_joint : mimic_joints_) - { - for (auto i = 0u; i < joint_states_.size(); ++i) - { - joint_states_[i][mimic_joint.joint_index] = - mimic_joint.multiplier * joint_states_[i][mimic_joint.mimicked_joint_index]; - } - } - - for (size_t i = 0; i < other_states_.size(); ++i) - { - for (size_t j = 0; j < other_states_[i].size(); ++j) - { - if ( - i == index_custom_interface_with_following_offset_ && - !std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) - { - other_states_[i][j] = - joint_commands_[POSITION_INTERFACE_INDEX][j] + position_state_following_offset_; - } - else if (!std::isnan(other_commands_[i][j])) - { - other_states_[i][j] = other_commands_[i][j]; - } - } - } - - if (fake_sensor_command_interfaces_) - { - for (size_t i = 0; i < sensor_states_.size(); ++i) - { - for (size_t j = 0; j < sensor_states_[i].size(); ++j) - { - if (!std::isnan(sensor_fake_commands_[i][j])) - { - sensor_states_[i][j] = sensor_fake_commands_[i][j]; - } - } - } - } - return return_type::OK; -} - -// Private methods -template -bool GenericSystem::get_interface( - const std::string & name, const std::vector & interface_list, - const std::string & interface_name, const size_t vector_index, - std::vector> & values, std::vector & interfaces) -{ - auto it = std::find(interface_list.begin(), interface_list.end(), interface_name); - if (it != interface_list.end()) - { - auto j = std::distance(interface_list.begin(), it); - interfaces.emplace_back(name, *it, &values[j][vector_index]); - return true; - } - return false; -} - -void GenericSystem::initialize_storage_vectors( - std::vector> & commands, std::vector> & states, - const std::vector & interfaces) -{ - // Initialize storage for all joints, regardless of their existence - commands.resize(interfaces.size()); - states.resize(interfaces.size()); - for (auto i = 0u; i < interfaces.size(); i++) - { - commands[i].resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - states[i].resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - } - - // Initialize with values from URDF - for (auto i = 0u; i < info_.joints.size(); i++) - { - const auto & joint = info_.joints[i]; - for (auto j = 0u; j < interfaces.size(); j++) - { - auto it = joint.parameters.find("initial_" + interfaces[j]); - if (it != joint.parameters.end()) - { - states[j][i] = std::stod(it->second); - } - } - } -} - -} // namespace fake_components - -#include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(fake_components::GenericSystem, hardware_interface::SystemInterface) From 16d971b91aeb104ec469f0830c9381dd72cdb8be Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Tue, 3 Oct 2023 18:41:12 +0200 Subject: [PATCH 14/14] Add warning message when velocity or accleration interface are usied without dynamic-mode ON. --- .../src/mock_components/generic_system.cpp | 21 ++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index d806d33bd3..3ae3568a9e 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -388,11 +388,6 @@ return_type GenericSystem::prepare_command_mode_switch( { hardware_interface::return_type ret_val = hardware_interface::return_type::OK; - if (!calculate_dynamics_) - { - return ret_val; - } - const size_t FOUND_ONCE_FLAG = 1000000; std::vector joint_found_in_x_requests_; @@ -419,10 +414,26 @@ return_type GenericSystem::prepare_command_mode_switch( } if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) { + if (!calculate_dynamics_) + { + RCUTILS_LOG_WARN_NAMED( + "mock_generic_system", + "Requested velocity mode for joint '%s' without dynamics calculation enabled - this " + "might lead to wrong feedback and unexpected behavior.", + info_.joints[joint_index].name.c_str()); + } joint_found_in_x_requests_[joint_index] += 1; } if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) { + if (!calculate_dynamics_) + { + RCUTILS_LOG_WARN_NAMED( + "mock_generic_system", + "Requested acceleration mode for joint '%s' without dynamics calculation enabled - " + "this might lead to wrong feedback and unexpected behavior.", + info_.joints[joint_index].name.c_str()); + } joint_found_in_x_requests_[joint_index] += 1; } }