Feature/feedback control

README.md

@@ -26,5 +26,5 @@ For humans, configuring robot control systems via names is convenient, butString
 
 ### Hardware Abstraction Layer (HAL)
 The modular HAL allows the same controller code to run on simulated and real hardware without modification.
-- **`RobotHardware`**: The central interface exposing `RobotState` and `RobotJointAction` to the control loop.
+- **`RobotHardware`**: The central interface exposing `RobotState` and `RobotJointFeedbackAction` to the control loop.
 - **`DriverBase`**: Abstract base class for implementing specific backend drivers (e.g., MuJoCo simulation, EtherCAT masters, CAN bus interfaces).

motorium/motorium_control/BUILD.bazel

@@ -2,7 +2,7 @@ load("@rules_cc//cc:defs.bzl", "cc_library", "cc_test")
 
 cc_library(
     name = "motorium_control",
-    srcs = ["src/ImplicitPDController.cpp"],
+    srcs = ["src/JointPDController.cpp"],
     hdrs = glob(["include/**/*.h"]),
     includes = ["include"],
     visibility = ["//visibility:public"],
@@ -14,7 +14,17 @@ cc_library(
 
 cc_test(
     name = "test_implicit_pd_controller",
-    srcs = ["test/testImplicitPDController.cpp"],
+    srcs = ["test/testImplicitJointPDController.cpp"],
+    deps = [
+        ":motorium_control",
+        "//motorium/motorium_model",
+        "@googletest//:gtest_main",
+    ],
+)
+
+cc_test(
+    name = "test_explicit_pd_controller",
+    srcs = ["test/testExplicitJointPDController.cpp"],
     deps = [
         ":motorium_control",
         "//motorium/motorium_model",

motorium/motorium_control/CMakeLists.txt

@@ -37,7 +37,7 @@ set(dependencies
 #############
 
 add_library(${PROJECT_NAME}
-        src/ImplicitPDController.cpp
+        src/JointPDController.cpp
 )
 ament_target_dependencies(${PROJECT_NAME}
         ${dependencies}
@@ -60,11 +60,16 @@ target_include_directories(${PROJECT_NAME}
 if (BUILD_TESTING)
         find_package(ament_cmake_gtest REQUIRED)
 
-        ament_add_gtest(test_implicit_pd_controller test/testImplicitPDController.cpp)
+        ament_add_gtest(test_implicit_pd_controller test/testImplicitJointPDController.cpp)
         target_link_libraries(test_implicit_pd_controller
                 ${PROJECT_NAME}
         )
 
+        ament_add_gtest(test_explicit_pd_controller test/testExplicitJointPDController.cpp)
+        target_link_libraries(test_explicit_pd_controller
+                ${PROJECT_NAME}
+        )
+
 endif ()
 
 #############

motorium/motorium_control/include/motorium_control/ControllerBase.h

@@ -31,7 +31,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <motorium_core/Types.h>
 #include <motorium_model/RobotDescription.h>
-#include <motorium_model/RobotJointAction.h>
+#include <motorium_model/RobotJointFeedbackAction.h>
 #include <motorium_model/RobotState.h>
 
 namespace motorium::control {
@@ -43,8 +43,9 @@ class ControllerBase {
 
   // Computes [commands] given [joints] states and configs.
   virtual void computeJointControlAction(scalar_t time,
-                                         const model::RobotState& robot_state,
-                                         model::RobotJointAction& robot_joint_action) = 0;
+                                         const model::RobotState& current_state,
+                                         const model::RobotState& desired_state,
+                                         model::RobotJointFeedbackAction& joint_action) = 0;
 };
 
 }  // namespace motorium::control

motorium/motorium_control/include/motorium_control/ImplicitPDController.h → motorium/motorium_control/include/motorium_control/JointPDController.h

@@ -35,24 +35,45 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace motorium::control {
 
-struct ImplicitPDControllerConfig {
+struct JointPDControllerConfig {
   std::vector<std::string> joint_names;
   vector_t kp;
   vector_t kd;
+
+  void validate() const;
 };
 
-class ImplicitPDController : public ControllerBase {
- public:
-  ImplicitPDController(const model::RobotDescription& robot_description, const ImplicitPDControllerConfig& config);
-  ~ImplicitPDController() override = default;
+/*
+@brief
+Implicit PD Controller does not compute the feedback terms itself but forwards gains and setpoints to be evaluated at the driver. This way
+the feedback can happen at potentially higher loop rates. Explicit PD Controller computes the feedback terms itself and forwards the
+feedback torques to the driver.
+*/
 
-  void validateConfig() const;
+template <bool IsImplicit = true>
+class JointPDController : public ControllerBase {
+ public:
+  JointPDController(const model::RobotDescription& robot_description, const JointPDControllerConfig& config);
+  ~JointPDController() override = default;
 
-  void computeJointControlAction(scalar_t time, const model::RobotState& robot_state, model::RobotJointAction& robot_joint_action) override;
+  void computeJointControlAction(scalar_t time,
+                                 const model::RobotState& current_state,
+                                 const model::RobotState& desired_state,
+                                 model::RobotJointFeedbackAction& joint_action) override;
 
  private:
-  ImplicitPDControllerConfig config_;
+  JointPDControllerConfig config_;
   std::vector<joint_index_t> joint_indices_;
+
+  void computeImplicitPD(const model::RobotState& desired_state, model::RobotJointFeedbackAction& joint_action);
+
+  void computeExplicitPD(const model::RobotState& current_state,
+                         const model::RobotState& desired_state,
+                         model::RobotJointFeedbackAction& joint_action);
 };
 
+// Type aliases
+using ImplicitJointPDController = JointPDController<true>;
+using ExplicitJointPDController = JointPDController<false>;
+
 }  // namespace motorium::control

motorium/motorium_control/src/JointPDController.cpp

@@ -0,0 +1,108 @@
+/******************************************************************************
+Copyright (c) 2025, Manuel Yves Galliker. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+******************************************************************************/
+
+#include <motorium_control/JointPDController.h>
+
+#include <motorium_core/Check.h>
+
+namespace motorium::control {
+
+void JointPDControllerConfig::validate() const {
+  MT_CHECK(!joint_names.empty()) << "[JointPDController] Configuration error: Joint names list is empty.";
+  MT_CHECK(static_cast<long>(joint_names.size()) == kp.size())
+      << "[JointPDController] Configuration error: Size mismatch between joint_names and kp.";
+  MT_CHECK(static_cast<long>(joint_names.size()) == kd.size())
+      << "[JointPDController] Configuration error: Size mismatch between joint_names and kd.";
+}
+
+template <bool IsImplicit>
+JointPDController<IsImplicit>::JointPDController(const model::RobotDescription& robot_description, const JointPDControllerConfig& config)
+    : ControllerBase(robot_description), config_(config), joint_indices_(robot_description.getJointIndices(config_.joint_names)) {
+  config_.validate();
+}
+
+template <bool IsImplicit>
+void JointPDController<IsImplicit>::computeJointControlAction([[maybe_unused]] scalar_t time,
+                                                              const model::RobotState& current_state,
+                                                              const model::RobotState& desired_state,
+                                                              model::RobotJointFeedbackAction& joint_action) {
+  if constexpr (IsImplicit) {
+    computeImplicitPD(desired_state, joint_action);
+  } else {
+    computeExplicitPD(current_state, desired_state, joint_action);
+  }
+}
+
+template <bool IsImplicit>
+void JointPDController<IsImplicit>::computeImplicitPD(const model::RobotState& desired_state,
+                                                      model::RobotJointFeedbackAction& joint_action) {
+  for (size_t i = 0; i < joint_indices_.size(); ++i) {
+    const auto joint_index = joint_indices_[i];
+    if (joint_action.contains(joint_index)) {
+      model::JointFeedbackAction& action = joint_action[joint_index];
+      const model::JointState& desired = desired_state.getJointState(joint_index);
+
+      action.q_des = desired.q;
+      action.v_des = desired.v;
+      action.feed_forward_effort = desired.effort;
+      action.kp = config_.kp(i);
+      action.kd = config_.kd(i);
+    }
+  }
+}
+
+template <bool IsImplicit>
+void JointPDController<IsImplicit>::computeExplicitPD(const model::RobotState& current_state,
+                                                      const model::RobotState& desired_state,
+                                                      model::RobotJointFeedbackAction& joint_action) {
+  for (size_t i = 0; i < joint_indices_.size(); ++i) {
+    const auto joint_index = joint_indices_[i];
+    if (joint_action.contains(joint_index)) {
+      model::JointFeedbackAction& action = joint_action[joint_index];
+      const model::JointState& desired = desired_state.getJointState(joint_index);
+      const model::JointState& current = current_state.getJointState(joint_index);
+
+      scalar_t q_error = desired.q - current.q;
+      scalar_t v_error = desired.v - current.v;
+      scalar_t feedback = config_.kp(i) * q_error + config_.kd(i) * v_error;
+
+      action.q_des = desired.q;
+      action.v_des = desired.v;
+      action.feed_forward_effort = desired.effort + feedback;
+      action.kp = 0.0;
+      action.kd = 0.0;
+    }
+  }
+}
+
+// Explicit template instantiations
+template class JointPDController<true>;
+template class JointPDController<false>;
+
+}  // namespace motorium::control