Merge humble onto humble-devel

.pre-commit-config.yaml

@@ -2,7 +2,7 @@ ci:
     autoupdate_branch: 'devel'
 repos:
 -   repo: https://github.com/pre-commit/mirrors-clang-format
-    rev: v16.0.0
+    rev: v19.1.7
     hooks:
     -   id: clang-format
         args: [--style=Google]
@@ -25,7 +25,7 @@ repos:
     -   id: mixed-line-ending
     -   id: trailing-whitespace
 -   repo: https://github.com/psf/black
-    rev: 23.1.0
+    rev: 24.10.0
     hooks:
     -   id: black
 -   repo: https://github.com/cheshirekow/cmake-format-precommit

CMakeLists.txt

@@ -31,7 +31,7 @@ sec_generate_parameter_library(
 add_dependencies(${PROJECT_NAME}_parameters ${PROJECT_NAME}_parameters_doc)
 # Joint estimator params
 sec_generate_parameter_library_markdown(joint_state_estimator_parameters_doc
-                                        src/linear_feedback_controller.yaml)
+                                        src/joint_state_estimator.yaml)
 sec_generate_parameter_library(
   joint_state_estimator_generated_parameters # Lib name
   joint_state_estimator_parameters # CMake target name for the parameter
@@ -97,6 +97,16 @@ if(BUILD_TESTING)
     test_robot_model_builder
     PROPERTIES ENVIRONMENT_MODIFICATION
                LD_LIBRARY_PATH=path_list_prepend:${CMAKE_CURRENT_BINARY_DIR})
+
+  ament_auto_add_gtest(test_pd_controller tests/test_pd_controller.cpp)
+  target_link_libraries(test_pd_controller ${PROJECT_NAME})
+
+  ament_auto_add_gtest(test_lf_controller tests/test_lf_controller.cpp)
+  target_link_libraries(test_lf_controller ${PROJECT_NAME})
+
+  ament_auto_add_gtest(test_linear_feedback_controller
+                       tests/test_linear_feedback_controller.cpp)
+  target_link_libraries(test_linear_feedback_controller ${PROJECT_NAME})
 endif()
 
 #
@@ -108,8 +118,10 @@ pluginlib_export_plugin_description_file(controller_interface
 #
 # Installation
 #
-install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/doc
-        DESTINATION share/${PROJECT_NAME})
+install(FILES ${CMAKE_CURRENT_BINARY_DIR}/linear_feedback_controller.md
+              ${CMAKE_CURRENT_BINARY_DIR}/joint_state_estimator.md
+        DESTINATION share/${PROJECT_NAME}/doc)
+
 install(
   PROGRAMS tests/pd_plus_controller.py
   DESTINATION lib/${PROJECT_NAME}

cmake/sec_generate_parameter_library.cmake

@@ -111,7 +111,7 @@ function(sec_generate_parameter_library_markdown TARGET_NAME YAML_FILE)
 
   # Set the output parameter header file name
   get_filename_component(MARKDOWN_FILE ${YAML_FILE} NAME_WE)
-  set(MARKDOWN_FILE ${CMAKE_CURRENT_BINARY_DIR}/doc/${MARKDOWN_FILE}.md)
+  set(MARKDOWN_FILE ${CMAKE_CURRENT_BINARY_DIR}/${MARKDOWN_FILE}.md)
 
   # Generate the header for the library
   add_custom_target(

include/linear_feedback_controller/averaging_filter.hpp

@@ -16,7 +16,7 @@ class AveragingFilter {
   /**
    * @brief Destroy the Averaging Filter object
    */
-  ~AveragingFilter(){};
+  ~AveragingFilter() {};
 
   /**
    * @brief Add another data in the buffer in a FIFO manner;

include/linear_feedback_controller/time.hpp

@@ -1,3 +1,6 @@
+#ifndef LINEAR_FEEDBACK_CONTROLLER__TIME_HPP_
+#define LINEAR_FEEDBACK_CONTROLLER__TIME_HPP_
+
 #include <chrono>
 
 namespace linear_feedback_controller {
@@ -6,3 +9,5 @@ using TimePoint = std::chrono::time_point<std::chrono::high_resolution_clock,
 
 using Duration = std::chrono::duration<double>;
 }  // namespace linear_feedback_controller
+
+#endif

src/linear_feedback_controller.cpp

@@ -81,9 +81,13 @@ const Eigen::VectorXd& LinearFeedbackController::compute_control(
   if (remove_gravity_compensation_effort) {
     robot_model_builder_->construct_robot_state(sensor, robot_configuration_,
                                                 robot_velocity_);
-    control_ -= pinocchio::rnea(
-        robot_model_builder_->get_model(), robot_model_builder_->get_data(),
-        robot_configuration_, robot_velocity_null_, robot_velocity_null_);
+
+    // NOTE: .tail() is used to remove the freeflyer components
+    control_ -=
+        pinocchio::rnea(robot_model_builder_->get_model(),
+                        robot_model_builder_->get_data(), robot_configuration_,
+                        robot_velocity_null_, robot_velocity_null_)
+            .tail(control_.size());
   }
 
   return control_;

tests/test_lf_controller.cpp

@@ -0,0 +1,244 @@
+#include <sstream>
+#include <string_view>
+#include <tuple>
+
+#include "utils/mutation.hpp"
+using tests::utils::TemporaryMutate;
+
+#include "utils/lf_controller.hpp"
+using tests::utils::ExpectedLFControlFrom;
+using tests::utils::MakeValidRandomControlFor;
+using tests::utils::MakeValidRandomSensorFor;
+
+#include "utils/robot_model.hpp"
+using tests::utils::JointType;
+using tests::utils::MakeAllModelDescriptionsFor;
+using tests::utils::ModelDescription;
+
+#include "utils/eigen_conversions.hpp"
+using tests::utils::PushNewJointStateTo;
+
+#include "linear_feedback_controller/lf_controller.hpp"
+using linear_feedback_controller::LFController;
+
+#include "gtest/gtest.h"
+
+using namespace std::literals::string_view_literals;
+
+namespace {
+
+struct LFControllerTest : public ::testing::TestWithParam<ModelDescription> {};
+
+TEST(LFControllerTest, Ctor) {
+  EXPECT_NO_THROW({ auto ctrl = LFController(); });
+}
+
+TEST(LFControllerTest, DISABLED_InitializeNullptr) {
+  auto ctrl = LFController();
+  EXPECT_ANY_THROW({ ctrl.initialize(nullptr); });
+}
+
+TEST(LFControllerTest, DISABLED_InitializeEmptyModel) {
+  auto ctrl = LFController();
+  EXPECT_ANY_THROW({
+    ctrl.initialize(
+        std::make_shared<linear_feedback_controller::RobotModelBuilder>());
+  });
+}
+
+TEST_P(LFControllerTest, Initialize) {
+  const auto model_ptr = std::shared_ptr{MakeRobotModelBuilderFrom(GetParam())};
+  ASSERT_NE(model_ptr, nullptr);
+
+  auto ctrl = LFController();
+  EXPECT_NO_THROW({ ctrl.initialize(model_ptr); });
+}
+
+TEST(LFControllerTest, DISABLED_ComputeControlNotInitialized) {
+  auto ctrl = LFController();
+  EXPECT_ANY_THROW({ auto _ = ctrl.compute_control({}, {}); });
+}
+
+TEST_P(LFControllerTest, DISABLED_ComputeControlNoInput) {
+  const auto model_ptr = std::shared_ptr{MakeRobotModelBuilderFrom(GetParam())};
+  ASSERT_NE(model_ptr, nullptr);
+
+  auto ctrl = LFController();
+  ctrl.initialize(model_ptr);
+
+  EXPECT_ANY_THROW({ auto _ = ctrl.compute_control({}, {}); });
+}
+
+TEST_P(LFControllerTest, DISABLED_ComputeControlUnknownJoints) {
+  const auto model_ptr = std::shared_ptr{MakeRobotModelBuilderFrom(GetParam())};
+  ASSERT_NE(model_ptr, nullptr);
+
+  auto ctrl = LFController();
+  ctrl.initialize(model_ptr);
+
+  const auto sensor = MakeValidRandomSensorFor(*model_ptr);
+  const auto control = MakeValidRandomControlFor(*model_ptr);
+
+  EXPECT_ANY_THROW({
+    auto wrong_sensor = sensor;
+    wrong_sensor.joint_state.name[0] = "this joint doesn't exist";
+    auto _ = ctrl.compute_control(wrong_sensor, control);
+  });
+}
+
+TEST_P(LFControllerTest, DISABLED_ComputeControlSizeMismatch) {
+  const auto model_ptr = std::shared_ptr{MakeRobotModelBuilderFrom(GetParam())};
+  ASSERT_NE(model_ptr, nullptr);
+
+  auto ctrl = LFController();
+  ctrl.initialize(model_ptr);
+
+  const auto sensor = MakeValidRandomSensorFor(*model_ptr);
+  const auto control = MakeValidRandomControlFor(*model_ptr);
+
+  EXPECT_ANY_THROW({
+    auto wrong_sensor = sensor;
+
+    // One more unknown joint inside sensor
+    PushNewJointStateTo(
+        wrong_sensor.joint_state,
+        {.name = "foo", .position = 0.0, .velocity = 0.0, .effort = 0.0});
+
+    auto _ = ctrl.compute_control(wrong_sensor, control);
+  });
+
+  EXPECT_ANY_THROW({
+    auto wrong_control = control;
+
+    // One more unknown joint inside control.initial_state
+    PushNewJointStateTo(
+        wrong_control.initial_state.joint_state,
+        {.name = "foo", .position = 0.0, .velocity = 0.0, .effort = 0.0});
+
+    auto _ = ctrl.compute_control(sensor, wrong_control);
+  });
+
+  EXPECT_ANY_THROW({
+    auto wrong_control = control;
+
+    // One more feed forward term
+    tests::utils::Grow(wrong_control.feedforward, 1);
+    wrong_control.feedforward.tail<1>()[0] = 0.0;
+
+    auto _ = ctrl.compute_control(sensor, wrong_control);
+  });
+
+  EXPECT_ANY_THROW({
+    auto wrong_control = control;
+
+    // One more row/col to feedback gain
+    tests::utils::Grow(wrong_control.feedback_gain, 1);
+
+    wrong_control.feedback_gain.bottomRows<1>() =
+        ::Eigen::VectorXd::Random(wrong_control.feedback_gain.cols());
+
+    wrong_control.feedback_gain.rightCols<1>() =
+        ::Eigen::VectorXd::Random(wrong_control.feedback_gain.rows());
+
+    auto _ = ctrl.compute_control(sensor, wrong_control);
+  });
+}
+
+/// Create a std::tuple<T&, string_view> with the ref expression as string
+#define MakeRefOf(val) std::make_tuple(std::ref((val)), std::string_view{#val})
+
+TEST_P(LFControllerTest, DISABLED_ComputeControlSpecialDouble) {
+  const auto model_ptr = std::shared_ptr{MakeRobotModelBuilderFrom(GetParam())};
+  ASSERT_NE(model_ptr, nullptr);
+
+  auto ctrl = LFController();
+  ctrl.initialize(model_ptr);
+
+  auto sensor = MakeValidRandomSensorFor(*model_ptr);
+  auto control = MakeValidRandomControlFor(*model_ptr);
+
+  // Test for special double values acceptance or not ?
+  for (const auto& [ref, str] : {
+           MakeRefOf(sensor.base_pose(0)),
+           MakeRefOf(sensor.base_pose(3)),
+           MakeRefOf(sensor.base_twist(4)),
+           MakeRefOf(sensor.joint_state.position(0)),
+           MakeRefOf(sensor.joint_state.velocity(0)),
+           MakeRefOf(control.feedforward(0)),
+           MakeRefOf(control.feedback_gain(0, 0)),
+       }) {
+    for (auto tmp_value : {
+             std::numeric_limits<double>::infinity(),
+             std::numeric_limits<double>::quiet_NaN(),
+             std::numeric_limits<double>::signaling_NaN(),
+         }) {
+      const auto mutation = TemporaryMutate(ref, tmp_value);
+      EXPECT_ANY_THROW({ auto _ = ctrl.compute_control(sensor, control); })
+          << str << " = " << tmp_value << " (was " << mutation.OldValue()
+          << ")";
+    }
+  }
+}
+
+TEST_P(LFControllerTest, ComputeControl) {
+  const auto model_ptr = std::shared_ptr{MakeRobotModelBuilderFrom(GetParam())};
+  ASSERT_NE(model_ptr, nullptr);
+
+  auto ctrl = LFController();
+  ctrl.initialize(model_ptr);
+
+  const auto sensor = MakeValidRandomSensorFor(*model_ptr);
+  const auto control = MakeValidRandomControlFor(*model_ptr);
+  EXPECT_EQ(ExpectedLFControlFrom(*model_ptr, sensor, control),
+            ctrl.compute_control(sensor, control));
+}
+
+constexpr auto dummy_urdf =
+    "<?xml version=\"1.0\" encoding=\"utf-8\"?>"
+    "<robot name=\"dummy\">"
+    "  <link name=\"l0\"/>"
+    "  "
+    "  <joint name=\"l01\" type=\"revolute\">"
+    "    <parent link=\"l0\"/>"
+    "    <child link=\"l1\"/>"
+    "    <origin xyz=\"0 0 1\" rpy=\"0 0 1\"/>"
+    "    <axis xyz=\"0 0 1\"/>"
+    "    <limit lower=\"0\" upper=\"3.14\" velocity=\"100\" effort=\"100\"/>"
+    "  </joint>"
+    "  "
+    "  <link name=\"l1\"/>"
+    "  "
+    "  <joint name=\"l12\" type=\"revolute\">"
+    "    <parent link=\"l1\"/>"
+    "    <child link=\"l2\"/>"
+    "    <origin xyz=\"0 1 0\" rpy=\"1 0 0\"/>"
+    "    <axis xyz=\"0 1 0\"/>"
+    "    <limit lower=\"-3.14\" upper=\"3.14\" velocity=\"100\" effort=\"10\"/>"
+    "  </joint>"
+    "  "
+    "  <link name=\"l2\"/>"
+    "</robot>"sv;
+
+INSTANTIATE_TEST_SUITE_P(
+    DummyUrdf, LFControllerTest,
+    ::testing::ValuesIn(MakeAllModelDescriptionsFor(
+        dummy_urdf,
+        {
+            {
+                {.name = "l01"},
+            },
+            {
+                {.name = "l02", .type = JointType::Controlled},
+            },
+            {
+                {.name = "l01"},
+                {.name = "l12"},
+            },
+        })),
+    [](const auto& info) {
+      std::stringstream stream;
+      PrintTo(info.param, &stream, {.as_param_name = true});
+      return stream.str();
+    });
+
+}  // namespace