From b6f4ff133578efae9979c4f02deca45d7711e0e7 Mon Sep 17 00:00:00 2001 From: mich-pest Date: Tue, 11 Aug 2026 16:42:11 +0200 Subject: [PATCH 1/2] control_layer: reading and publishing unified new ControlSignal msgs --- .../include/dls2/controller/control_modes.hpp | 10 ++ .../include/dls2/controller/controller.hpp | 1 + .../dls2/controller/controller_data.hpp | 1 + .../dls2/core_framework/control_layer.hpp | 8 +- .../core_framework/src/control_layer.cpp.in | 107 +++++++++++++++--- modules/plugin/test/CMakeLists.txt | 1 + .../test/include/hello_world_plugin.hpp | 1 + .../plugin/test/src/hello_world_plugin.cpp | 15 +-- 8 files changed, 118 insertions(+), 26 deletions(-) create mode 100644 modules/controller/include/dls2/controller/control_modes.hpp diff --git a/modules/controller/include/dls2/controller/control_modes.hpp b/modules/controller/include/dls2/controller/control_modes.hpp new file mode 100644 index 00000000..22969816 --- /dev/null +++ b/modules/controller/include/dls2/controller/control_modes.hpp @@ -0,0 +1,10 @@ +#pragma once + +#include + +enum class ControlMode : uint8_t { + TORQUE_MODE = 0, + IMPEDANCE_MODE, + POSITION_MODE, + VELOCITY_MODE +}; diff --git a/modules/controller/include/dls2/controller/controller.hpp b/modules/controller/include/dls2/controller/controller.hpp index 105558ae..185ebcea 100644 --- a/modules/controller/include/dls2/controller/controller.hpp +++ b/modules/controller/include/dls2/controller/controller.hpp @@ -6,6 +6,7 @@ // ============================================================================= #include "dls2/application/periodic_app.hpp" #include "dls2/controller/controller_data.hpp" +#include "dls2/controller/control_modes.hpp" #include "dls2/util/messaging/dds_participant.hpp" diff --git a/modules/controller/include/dls2/controller/controller_data.hpp b/modules/controller/include/dls2/controller/controller_data.hpp index 5b00badb..f13b4b2a 100644 --- a/modules/controller/include/dls2/controller/controller_data.hpp +++ b/modules/controller/include/dls2/controller/controller_data.hpp @@ -5,6 +5,7 @@ #include "dls2/signal/reader.hpp" #include "dls2/math/ramp.hpp" #include "robotlib/robot_base.hpp" +#include "dls2/controller/control_modes.hpp" #include diff --git a/modules/core_framework/include/dls2/core_framework/control_layer.hpp b/modules/core_framework/include/dls2/core_framework/control_layer.hpp index 1fc27ba0..22ed9bd0 100644 --- a/modules/core_framework/include/dls2/core_framework/control_layer.hpp +++ b/modules/core_framework/include/dls2/core_framework/control_layer.hpp @@ -79,7 +79,9 @@ class ControlLayer : public Layer /// @return true if the generator unloads correctly. bool unloadMotionGenerator(const std::string&); - /// Returns the last published desired torques + /// Returns the last published joint reference + std::vector getPublishedDesiredPosition(); + std::vector getPublishedDesiredVelocity(); std::vector getPublishedDesiredTorques(); private: @@ -118,9 +120,9 @@ class ControlLayer : public Layer std::shared_ptr pRobot; /// @brief Output signals - Writer writer_control_signal; + Writer writer_control_signal; - dls2_interface::msg::DesiredTorques torques; + dls2_interface::msg::ControlSignal control_signal_msg; bool unloadController(std::shared_ptr pData); diff --git a/modules/core_framework/src/control_layer.cpp.in b/modules/core_framework/src/control_layer.cpp.in index 7b953381..2effc374 100644 --- a/modules/core_framework/src/control_layer.cpp.in +++ b/modules/core_framework/src/control_layer.cpp.in @@ -57,11 +57,21 @@ ControlLayer::ControlLayer(std::string ID, std::string robot_name) , pRobot(robotlib::RobotFactory::openRobot(robot_name)) , writer_control_signal( ddsSignalLink, - dls::topics::desired_torques) - , torques() + dls::topics::control_signal) + , control_signal_msg() { - writer_control_signal.msg.desired_torques().resize(pRobot->getNJOINTS()); - torques.desired_torques().resize(pRobot->getNJOINTS()); + writer_control_signal.msg.joints_position().resize(pRobot->getNJOINTS()); + writer_control_signal.msg.joints_velocity().resize(pRobot->getNJOINTS()); + writer_control_signal.msg.joints_torques().resize(pRobot->getNJOINTS()); + writer_control_signal.msg.kp().resize(pRobot->getNJOINTS()); + writer_control_signal.msg.kd().resize(pRobot->getNJOINTS()); + + control_signal_msg.joints_position().resize(pRobot->getNJOINTS()); + control_signal_msg.joints_velocity().resize(pRobot->getNJOINTS()); + control_signal_msg.joints_torques().resize(pRobot->getNJOINTS()); + control_signal_msg.kp().resize(pRobot->getNJOINTS()); + control_signal_msg.kd().resize(pRobot->getNJOINTS()); + command_manager.addCommand ( "loadGenerator", @@ -238,18 +248,77 @@ void *ControlLayer::controlSignalGather(void *data) auto epoch = std::chrono::time_point_cast(now).time_since_epoch(); // Read the control signals - std::fill(layer->torques.desired_torques().begin(), layer->torques.desired_torques().end(), 0); - - for(auto pair : layer->controllers) + std::fill(layer->control_signal_msg.joints_torques().begin(), layer->control_signal_msg.joints_torques().end(), 0); + layer->control_signal_msg.control_mode() = static_cast(ControlMode::TORQUE_MODE); + bool active_position_controller = false; + bool active_velocity_controller = false; + bool active_impedence_controller = false; + bool control_mode_set_once = false; + + for(auto [_, controller_data] : layer->controllers) { - pair.second->reader_control_signal.read(); - for(long unsigned int i=0; ireader_control_signal.msg.torques().size();i++){ - layer->torques.desired_torques()[i] += pair.second->premultiplier * pair.second->reader_control_signal.msg.torques()[i]; + controller_data->reader_control_signal.read(); + const auto& controller_msg = controller_data->reader_control_signal.msg; + const auto controller_mode = static_cast(controller_msg.control_mode()); + + if(!control_mode_set_once){ + layer->control_signal_msg.control_mode() = controller_msg.control_mode(); + control_mode_set_once = true; + } + + const auto aggregate_mode = static_cast(layer->control_signal_msg.control_mode()); + bool torque_controller_in_torque_mode = controller_mode == ControlMode::TORQUE_MODE && aggregate_mode == ControlMode::TORQUE_MODE; + bool torque_controller_in_impedence_mode = aggregate_mode == ControlMode::IMPEDANCE_MODE && controller_mode == ControlMode::TORQUE_MODE; + bool first_impedence_controller = controller_mode == ControlMode::IMPEDANCE_MODE && !active_impedence_controller; + bool first_position_controller = controller_mode == ControlMode::POSITION_MODE && !active_position_controller; + bool first_velocity_controller = controller_mode == ControlMode::VELOCITY_MODE && !active_velocity_controller; + + + if(first_position_controller){ + layer->control_signal_msg.control_mode() = static_cast(ControlMode::POSITION_MODE); + layer->control_signal_msg.joints_position() = controller_msg.joints_position(); + + // Reset all other fields to zero + std::fill(layer->control_signal_msg.joints_velocity().begin(), layer->control_signal_msg.joints_velocity().end(), 0); + std::fill(layer->control_signal_msg.joints_torques().begin(), layer->control_signal_msg.joints_torques().end(), 0); + std::fill(layer->control_signal_msg.kp().begin(), layer->control_signal_msg.kp().end(), 0.0); + std::fill(layer->control_signal_msg.kd().begin(), layer->control_signal_msg.kd().end(), 0.0); + break; + } + + if(first_velocity_controller){ + layer->control_signal_msg.control_mode() = static_cast(ControlMode::VELOCITY_MODE); + layer->control_signal_msg.joints_velocity() = controller_msg.joints_velocity(); + + // Reset all other fields to zero + std::fill(layer->control_signal_msg.joints_position().begin(), layer->control_signal_msg.joints_position().end(), 0); + std::fill(layer->control_signal_msg.joints_torques().begin(), layer->control_signal_msg.joints_torques().end(), 0); + std::fill(layer->control_signal_msg.kp().begin(), layer->control_signal_msg.kp().end(), 0.0); + std::fill(layer->control_signal_msg.kd().begin(), layer->control_signal_msg.kd().end(), 0.0); + break; + } + + + if(torque_controller_in_torque_mode || torque_controller_in_impedence_mode || first_impedence_controller){ + if(first_impedence_controller){ + active_impedence_controller = true; + layer->control_signal_msg.control_mode() = static_cast(ControlMode::IMPEDANCE_MODE); + layer->control_signal_msg.joints_position() = controller_msg.joints_position(); + layer->control_signal_msg.joints_velocity() = controller_msg.joints_velocity(); + layer->control_signal_msg.kp() = controller_msg.kp(); + layer->control_signal_msg.kd() = controller_msg.kd(); + } + + for(size_t i = 0; i < controller_msg.joints_torques().size();i++){ + // Torque contributions summed up + layer->control_signal_msg.joints_torques()[i] += controller_data->premultiplier * controller_msg.joints_torques()[i]; + } } } - // Send the desired torques to HAL - layer->writer_control_signal.msg.desired_torques() = layer->saturateTorques(layer->torques.desired_torques()); + // Fill in the control signal to be sent to HAL + layer->control_signal_msg.joints_torques() = layer->saturateTorques(layer->control_signal_msg.joints_torques()); + layer->writer_control_signal.msg = layer->control_signal_msg; layer->writer_control_signal.msg.timestamp() = std::chrono::duration_cast(epoch).count(); layer->writer_control_signal.publish(); @@ -263,7 +332,6 @@ void *ControlLayer::controlSignalGather(void *data) { std::stringstream ss; ss << "Control Layer is probably breaking real-time. Has period of 1 mseconds but spent: " << mean << " useconds "; - // ss << "Control layer published torques " << desired_torques.transpose(); // << std::endl; layer->app_logger.warning(ss.str(), EventID::WRONG_PROCESS_FREQUENCY); } @@ -591,7 +659,14 @@ std::vector ControlLayer::saturateTorques(const std::vector& req return req; } +std::vector ControlLayer::getPublishedDesiredPosition(){ + return this->writer_control_signal.msg.joints_position(); +} + +std::vector ControlLayer::getPublishedDesiredVelocity(){ + return this->writer_control_signal.msg.joints_velocity(); +} + std::vector ControlLayer::getPublishedDesiredTorques(){ - // std::lock_guard lock(this->last_published_desired_torquesmutex); - return this->writer_control_signal.msg.desired_torques(); -} \ No newline at end of file + return this->writer_control_signal.msg.joints_torques(); +} diff --git a/modules/plugin/test/CMakeLists.txt b/modules/plugin/test/CMakeLists.txt index 7ca08a61..91824ac6 100644 --- a/modules/plugin/test/CMakeLists.txt +++ b/modules/plugin/test/CMakeLists.txt @@ -12,6 +12,7 @@ target_link_libraries(${PLUGIN_NAME} dls_signal dls_messages dls_topics + dls_controller periodic_app_plugin ) target_include_directories(${PLUGIN_NAME} diff --git a/modules/plugin/test/include/hello_world_plugin.hpp b/modules/plugin/test/include/hello_world_plugin.hpp index 17c8d534..c3173652 100644 --- a/modules/plugin/test/include/hello_world_plugin.hpp +++ b/modules/plugin/test/include/hello_world_plugin.hpp @@ -7,6 +7,7 @@ #include "dls2/signal/reader.hpp" #include "dls2/signal/writer.hpp" // ****************************************** +#include "dls2/controller/control_modes.hpp" // plugin loaded at run-time class HelloWorldPlugin : public dls::PeriodicAppPlugin diff --git a/modules/plugin/test/src/hello_world_plugin.cpp b/modules/plugin/test/src/hello_world_plugin.cpp index e201c6db..7a6213a5 100644 --- a/modules/plugin/test/src/hello_world_plugin.cpp +++ b/modules/plugin/test/src/hello_world_plugin.cpp @@ -5,7 +5,8 @@ HelloWorldPlugin::HelloWorldPlugin(const std::string& ID) : dls::PeriodicAppPlugin(ID){ reader_bs = buildInput(dls::topics::low_level_estimation::blind_state, [](){}, false); // false: not required on activation writer_cs = buildOutput(dls::topics::control_signal); - writer_cs->msg.torques().resize(12); + writer_cs->msg.joints_torques().resize(12); + writer_cs->msg.control_mode() = static_cast(ControlMode::TORQUE_MODE); //define console functions here if needed command_manager.addCommand("set_joint_torque", @@ -18,12 +19,12 @@ HelloWorldPlugin::~HelloWorldPlugin(){} void HelloWorldPlugin::run(const std::chrono::system_clock::time_point &time){ read(); - for (size_t i=0; imsg.torques().size(); i++){ - writer_cs->msg.torques()[i] += i; + for (size_t i=0; imsg.joints_torques().size(); i++){ + writer_cs->msg.joints_torques()[i] += i; } std::cout << "[" << std::chrono::duration_cast(time.time_since_epoch()).count() - << "]: Received joint size " << reader_bs->msg.joints_position().size() << "; publishing dummy torques...\n"; + << "]: Received joint size " << reader_bs->msg.joints_position().size() << "; publishing dummy joints_torques...\n"; write(); } @@ -33,9 +34,9 @@ bool HelloWorldPlugin::setJointTorque(){ int idx{0}; dls::CommandHelper::readValue("Joint_ID", idx); double tau{0.0}; - if(dls::CommandHelper::readValue("torque", tau, writer_cs->msg.torques()[idx])) + if(dls::CommandHelper::readValue("torque", tau, writer_cs->msg.joints_torques()[idx])) { - writer_cs->msg.torques()[idx] = tau; + writer_cs->msg.joints_torques()[idx] = tau; } return true; } @@ -48,4 +49,4 @@ extern "C" PeriodicAppPlugin *create(const std::string& ID, const std::string& n } extern "C" void destroy(PeriodicAppPlugin *p){ delete p; -} \ No newline at end of file +} From 0bdd1126250e503f832b105b4b22a6585f2383ab Mon Sep 17 00:00:00 2001 From: mich-pest Date: Thu, 13 Aug 2026 16:09:03 +0200 Subject: [PATCH 2/2] utils: waypointer added --- modules/utils/CMakeLists.txt | 7 +- .../utils/include/dls2/util/waypointer.hpp | 47 +++++++ modules/utils/src/waypointer.cpp | 87 +++++++++++++ modules/utils/test/CMakeLists.txt | 18 +++ modules/utils/test/test_waypointer.cpp | 122 ++++++++++++++++++ 5 files changed, 280 insertions(+), 1 deletion(-) create mode 100644 modules/utils/include/dls2/util/waypointer.hpp create mode 100644 modules/utils/src/waypointer.cpp create mode 100644 modules/utils/test/CMakeLists.txt create mode 100644 modules/utils/test/test_waypointer.cpp diff --git a/modules/utils/CMakeLists.txt b/modules/utils/CMakeLists.txt index 2d11856a..647af2f2 100644 --- a/modules/utils/CMakeLists.txt +++ b/modules/utils/CMakeLists.txt @@ -6,6 +6,7 @@ add_library(dls_utils ${CMAKE_CURRENT_BINARY_DIR}/src/utils.cpp src/pose.cpp src/screw.cpp + src/waypointer.cpp src/process_resource_monitor.cpp src/system_resource_monitor.cpp ) @@ -26,4 +27,8 @@ target_include_directories(dls_utils include ) -dls_install(dls_utils) \ No newline at end of file +if(BUILD_TESTING) + add_subdirectory(test) +endif() + +dls_install(dls_utils) diff --git a/modules/utils/include/dls2/util/waypointer.hpp b/modules/utils/include/dls2/util/waypointer.hpp new file mode 100644 index 00000000..dcdba9b0 --- /dev/null +++ b/modules/utils/include/dls2/util/waypointer.hpp @@ -0,0 +1,47 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace dls +{ + namespace utils + { + using Waypoint = std::pair; + + class Waypointer + { + public: + Waypointer() = default; + + bool init(const std::vector& path); + virtual bool run(const Waypoint& robot_pose, Waypoint& waypoint) = 0; + + protected: + std::vector path_; + }; + + class EuclideanWaypointer : public Waypointer + { + public: + EuclideanWaypointer(std::string config_path); + EuclideanWaypointer(const YAML::Node& config); + + bool init(const std::vector& path); + bool run(const Waypoint& robot_pose, Waypoint& waypoint) override; + std::pair closestWaypoint(const Waypoint& robot_pose); + + private: + bool panic_ { false }; + double panic_threshold_ { 10.0 }; + + bool monotonic_{ true }; + size_t last_min_dist_idx_ { 0 }; + size_t lookahead_ { 0 }; + }; + } // namespace utils +} // namespace dls diff --git a/modules/utils/src/waypointer.cpp b/modules/utils/src/waypointer.cpp new file mode 100644 index 00000000..379bfbc7 --- /dev/null +++ b/modules/utils/src/waypointer.cpp @@ -0,0 +1,87 @@ +#include "dls2/util/waypointer.hpp" + +namespace dls +{ + namespace utils + { + bool Waypointer::init(const std::vector& path){ + if(path.empty()){ + return false; + } + path_ = path; + + return true; + } + + EuclideanWaypointer::EuclideanWaypointer(std::string config_path) + : Waypointer() + { + auto config = YAML::LoadFile(config_path); + monotonic_ = config["monotonic"].as(); + lookahead_ = config["lookahead"].as(); + panic_threshold_ = config["panic_threshold"].as(); + } + + EuclideanWaypointer::EuclideanWaypointer(const YAML::Node& config) + : Waypointer() + { + monotonic_ = config["monotonic"].as(); + lookahead_ = config["lookahead"].as(); + panic_threshold_ = config["panic_threshold"].as(); + } + + bool EuclideanWaypointer::init(const std::vector& path){ + panic_ = false; + last_min_dist_idx_ = 0; + return Waypointer::init(path); + } + + std::pair EuclideanWaypointer::closestWaypoint(const Waypoint& robot_pose){ + + double min_dist = std::numeric_limits::infinity(); + size_t min_dist_idx = path_.size() - 1; + size_t start_idx = (monotonic_ && !panic_) ? last_min_dist_idx_ : 0; + + for(size_t i = start_idx; i < path_.size(); i++){ + const auto [x, y] = path_.at(i); + const auto delta_x = x - robot_pose.first; + const auto delta_y = y - robot_pose.second; + const auto euclidean_distance = sqrt(delta_x * delta_x + delta_y * delta_y); + + if(euclidean_distance < min_dist){ + min_dist = euclidean_distance; + min_dist_idx = i; + } + } + + + std::pair best {min_dist, min_dist_idx}; + return best; + } + + + bool EuclideanWaypointer::run(const Waypoint& robot_pose, Waypoint& waypoint){ + if(path_.empty()){ + return false; + } + + auto [min_dist, min_dist_idx] = closestWaypoint(robot_pose); + + if(min_dist > panic_threshold_){ + panic_ = true; + std::tie(min_dist, min_dist_idx) = closestWaypoint(robot_pose); + }else{ + panic_ = false; + } + + if(lookahead_ > 0){ + min_dist_idx = std::min(min_dist_idx + lookahead_, path_.size() - 1); + } + + last_min_dist_idx_ = min_dist_idx; + waypoint = path_.at(min_dist_idx); + return true; + + } + } // namespace utils +} // namespace dls diff --git a/modules/utils/test/CMakeLists.txt b/modules/utils/test/CMakeLists.txt new file mode 100644 index 00000000..0d072159 --- /dev/null +++ b/modules/utils/test/CMakeLists.txt @@ -0,0 +1,18 @@ +add_executable(dls_utils_waypointer_test + test_waypointer.cpp +) + +target_link_libraries(dls_utils_waypointer_test + PRIVATE + dls_utils +) + +target_include_directories(dls_utils_waypointer_test + PRIVATE + ../include +) + +add_test( + NAME dls_utils_waypointer_test + COMMAND dls_utils_waypointer_test +) diff --git a/modules/utils/test/test_waypointer.cpp b/modules/utils/test/test_waypointer.cpp new file mode 100644 index 00000000..caa7f886 --- /dev/null +++ b/modules/utils/test/test_waypointer.cpp @@ -0,0 +1,122 @@ +#include +#include +#include +#include +#include +#include + +#include "dls2/util/waypointer.hpp" + +namespace +{ +bool check(bool condition, const std::string& message) +{ + if (!condition) { + std::cerr << "FAIL: " << message << std::endl; + return false; + } + return true; +} + +std::string writeConfigFile( + const std::string& file_name, + bool monotonic, + int lookahead, + double panic_threshold) +{ + const auto path = std::filesystem::temp_directory_path() / file_name; + std::ofstream config_file(path); + config_file << "monotonic: " << (monotonic ? "true" : "false") << "\n"; + config_file << "lookahead: " << lookahead << "\n"; + config_file << "panic_threshold: " << panic_threshold << "\n"; + config_file.close(); + return path.string(); +} + +bool testInitRejectsEmptyPath() +{ + const std::string config_path = + writeConfigFile("euclidean_waypointer_empty.yaml", true, 0, 5.0); + + dls::utils::EuclideanWaypointer waypointer(config_path); + const std::vector empty_path; + + return check(!waypointer.init(empty_path), "empty path should be rejected"); +} + +bool testMonotonicSearchUsesLookahead() +{ + const std::string config_path = + writeConfigFile("euclidean_waypointer_lookahead.yaml", true, 1, 100.0); + + dls::utils::EuclideanWaypointer waypointer(config_path); + const std::vector path{ + {0.0, 0.0}, + {1.0, 0.0}, + {2.0, 0.0}, + {3.0, 0.0}, + }; + + if (!check(waypointer.init(path), "path init should succeed")) { + return false; + } + + dls::utils::Waypoint waypoint{}; + if (!check(waypointer.run({0.2, 0.0}, waypoint), "first run should succeed")) { + return false; + } + if (!check(waypoint.first == 1.0 && waypoint.second == 0.0, + "first run should pick lookahead waypoint (1.0, 0.0)")) { + return false; + } + + if (!check(waypointer.run({1.9, 0.0}, waypoint), "second run should succeed")) { + return false; + } + return check(waypoint.first == 3.0 && waypoint.second == 0.0, + "second run should progress monotonically to (3.0, 0.0)"); +} + +bool testPanicFallbackRescansWholePath() +{ + const std::string config_path = + writeConfigFile("euclidean_waypointer_panic.yaml", true, 0, 5.0); + + dls::utils::EuclideanWaypointer waypointer(config_path); + const std::vector path{ + {0.0, 0.0}, + {10.0, 0.0}, + {20.0, 0.0}, + }; + + if (!check(waypointer.init(path), "panic test path init should succeed")) { + return false; + } + + dls::utils::Waypoint waypoint{}; + if (!check(waypointer.run({19.5, 0.0}, waypoint), + "pre-panic run should succeed")) { + return false; + } + if (!check(waypoint.first == 20.0 && waypoint.second == 0.0, + "pre-panic run should pick final waypoint")) { + return false; + } + + if (!check(waypointer.run({0.1, 0.0}, waypoint), + "panic fallback run should succeed")) { + return false; + } + return check(waypoint.first == 0.0 && waypoint.second == 0.0, + "panic fallback should rescan from start and recover waypoint 0"); +} +} // namespace + +int main() +{ + const bool ok = + testInitRejectsEmptyPath() && + testMonotonicSearchUsesLookahead() && + testPanicFallbackRescansWholePath(); + return ok ? EXIT_SUCCESS : EXIT_FAILURE; +}