diff --git a/hardware_interface/include/fake_components/generic_system.hpp b/hardware_interface/include/fake_components/generic_system.hpp index 2d09b21cdd..82b894ecea 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 { @@ -109,6 +126,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_; + + // 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 6b37186d03..9ac3638569 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 @@ -47,6 +48,18 @@ return_type GenericSystem::configure(const hardware_interface::HardwareInfo & in 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_ = ""; @@ -66,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++) { @@ -78,6 +92,16 @@ 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]; + + // 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) { @@ -180,6 +204,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; } @@ -278,20 +311,134 @@ 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); + } + } + } + + // 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])) + 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 = 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) {