utils: Waypointer added

modules/controller/include/dls2/controller/control_modes.hpp

@@ -0,0 +1,10 @@
+#pragma once
+
+#include <stdint.h>
+
+enum class ControlMode : uint8_t {
+        TORQUE_MODE = 0,
+		IMPEDANCE_MODE,
+		POSITION_MODE,
+		VELOCITY_MODE
+};

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"
 

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 <memory>
 

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<double> getPublishedDesiredPosition();
+	std::vector<double> getPublishedDesiredVelocity();
 	std::vector<double> getPublishedDesiredTorques();
 
 private:
@@ -118,9 +120,9 @@ class ControlLayer : public Layer
 	std::shared_ptr<robotlib::RobotBase> pRobot;
 
 	/// @brief Output signals
-	Writer<dls2_interface::msg::DesiredTorques> writer_control_signal;
+	Writer<dls2_interface::msg::ControlSignal> writer_control_signal;
 
-	dls2_interface::msg::DesiredTorques torques;
+	dls2_interface::msg::ControlSignal control_signal_msg;
 
 	bool unloadController(std::shared_ptr<ControllerData> pData);
 

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<std::string>
 	(
 		"loadGenerator",
@@ -238,18 +248,77 @@ void *ControlLayer::controlSignalGather(void *data)
         auto epoch = std::chrono::time_point_cast<std::chrono::milliseconds>(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<uint8_t>(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; i<pair.second->reader_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<ControlMode>(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<ControlMode>(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<uint8_t>(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<uint8_t>(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<uint8_t>(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<std::chrono::microseconds>(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<double> ControlLayer::saturateTorques(const std::vector<double>& req
 	return req;
 }
 
+std::vector<double> ControlLayer::getPublishedDesiredPosition(){
+	return this->writer_control_signal.msg.joints_position();
+}
+
+std::vector<double> ControlLayer::getPublishedDesiredVelocity(){
+	return this->writer_control_signal.msg.joints_velocity();
+}
+
 std::vector<double> ControlLayer::getPublishedDesiredTorques(){
-	// std::lock_guard<std::mutex>	lock(this->last_published_desired_torquesmutex);
-	return this->writer_control_signal.msg.desired_torques();
-}
+	return this->writer_control_signal.msg.joints_torques();
+}

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}

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

modules/plugin/test/src/hello_world_plugin.cpp

@@ -5,7 +5,8 @@ HelloWorldPlugin::HelloWorldPlugin(const std::string& ID)
 	: dls::PeriodicAppPlugin(ID){
 	reader_bs = buildInput<dls2_interface::msg::BlindState>(dls::topics::low_level_estimation::blind_state, [](){}, false); // false: not required on activation
 	writer_cs = buildOutput<dls2_interface::msg::ControlSignal>(dls::topics::control_signal);
-	writer_cs->msg.torques().resize(12);
+	writer_cs->msg.joints_torques().resize(12);
+	writer_cs->msg.control_mode() = static_cast<uint8_t>(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; i<writer_cs->msg.torques().size(); i++){
-		writer_cs->msg.torques()[i] += i;
+	for (size_t i=0; i<writer_cs->msg.joints_torques().size(); i++){
+		writer_cs->msg.joints_torques()[i] += i;
 	}
 
 	std::cout << "[" << std::chrono::duration_cast<std::chrono::milliseconds>(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<int>("Joint_ID", idx);
 	double tau{0.0};
-	if(dls::CommandHelper::readValue<double>("torque", tau, writer_cs->msg.torques()[idx]))
+	if(dls::CommandHelper::readValue<double>("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;
-}
+}

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)
+if(BUILD_TESTING)
+	add_subdirectory(test)
+endif()
+
+dls_install(dls_utils)

modules/utils/include/dls2/util/waypointer.hpp

@@ -0,0 +1,47 @@
+#pragma once
+
+#include <vector>
+#include <utility>
+#include <string>
+#include <limits>
+#include <tuple>
+#include <yaml-cpp/yaml.h>
+
+namespace dls
+{
+    namespace utils
+    {
+        using Waypoint = std::pair<double, double>;
+
+        class Waypointer
+        {
+            public:
+                Waypointer() = default;
+
+                bool init(const std::vector<Waypoint>& path);
+                virtual bool run(const Waypoint& robot_pose, Waypoint& waypoint) = 0;
+
+            protected:
+                std::vector<Waypoint> path_;
+        };
+
+        class EuclideanWaypointer : public Waypointer
+        {
+            public:
+                EuclideanWaypointer(std::string config_path);
+                EuclideanWaypointer(const YAML::Node& config);
+
+                bool init(const std::vector<Waypoint>& path);
+                bool run(const Waypoint& robot_pose, Waypoint& waypoint) override;
+                std::pair<double, size_t> 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