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 +}