[lf_controller] Update the Valid ComputeControl test

tests/test_lf_controller.cpp

@@ -15,6 +15,7 @@ using tests::utils::ModelDescription;
 using linear_feedback_controller_msgs::Eigen::Control;
 using linear_feedback_controller_msgs::Eigen::JointState;
 using linear_feedback_controller_msgs::Eigen::Sensor;
+using tests::utils::GetCompleteStateFrom;
 using tests::utils::PushNewJointStateTo;
 
 #include "linear_feedback_controller/lf_controller.hpp"
@@ -94,7 +95,7 @@ auto MakeValidRandomControlFor(const RobotModelBuilder& model) -> Control {
   control.feedforward = Eigen::VectorXd::Random(model.get_nv());
   control.feedback_gain = Eigen::MatrixXd::Random(
       /* rows = */ model.get_nv(),
-      /* cols = */ model.get_nv() * 2);
+      /* cols = */ model.get_nv() + model.get_nq());
 
   control.initial_state = MakeValidRandomSensorFor(model);
 
@@ -256,13 +257,16 @@ TEST_P(LFControllerTest, ComputeControl) {
   const auto sensor = MakeValidRandomSensorFor(*model_ptr);
   const auto control = MakeValidRandomControlFor(*model_ptr);
 
-  // FIXME: Replace Random with the expected stuff...
-  const auto error = Eigen::VectorXd::Random(control.feedback_gain.cols());
+  const auto x =
+      GetCompleteStateFrom(sensor, model_ptr->get_robot_has_free_flyer());
 
-  const Eigen::VectorXd expected_control =
-      control.feedforward + (control.feedback_gain * error);
+  const auto x_0 = GetCompleteStateFrom(control.initial_state,
+                                        model_ptr->get_robot_has_free_flyer());
 
-  EXPECT_EQ(expected_control, ctrl.compute_control(sensor, control));
+  // FIXME: Compute the error differently based on free flyer ?
+  const auto error = x_0 - x;
+  EXPECT_EQ((control.feedforward + (control.feedback_gain * error)),
+            ctrl.compute_control(sensor, control));
 }
 
 constexpr auto dummy_urdf =
@@ -298,9 +302,9 @@ INSTANTIATE_TEST_SUITE_P(
         JointListType{
             {.name = "l01", .type = JointType::Controlled},
         },
-        JointListType{
-            {.name = "l01", .type = JointType::Moving},
-        },
+        // JointListType{
+        //     {.name = "l01", .type = JointType::Moving},
+        // },
         JointListType{
             {.name = "l01"},
             {.name = "l12"},

tests/utils/eigen.hpp

@@ -68,8 +68,49 @@ constexpr auto Grow(Eigen::PlainObjectBase<MatrixType> &matrix,
                             matrix.cols() + col_inc.value_or(row_inc));
 }
 
-/// Inside a vector, represents a strip using the expected number of head/tail
-/// elements needed
+/**
+ *  @brief Creates an OutVector from the concatenation of all given \a vectors
+ *
+ *  @note SFINAE: Only available for Vectors (NumDimension equals to 1)
+ *
+ *  @tparam OutVector The output vector type returned
+ *  @tparam ...VectorTypes All input vectors
+ *
+ *  @param[in] ...vectors All vectors we wish to concatenates
+ *
+ *  @return OutVector With coefs set as the concatenation of all input vectors
+ */
+template <typename OutVector, typename... VectorTypes>
+constexpr auto ConcatenateAs(const Eigen::DenseBase<VectorTypes> &...vectors)
+    -> std::enable_if_t<details::IsEigenVector_v<OutVector> and
+                            (details::IsEigenVector_v<VectorTypes> and ...),
+                        OutVector> {
+  // NOTE: Couldn't find a way to use the fold expression with the Eigen
+  // stream + comma operators initialization shenanigans...
+  // 'out << (vectors, ...);' doesn't work ??
+  // Therefore we use the segment(i, n) with i begin updated through the comma
+  // operator, which should be the same as 'out << A, B, C, ...;'
+
+  OutVector out((0 + ... + vectors.size()));
+  std::size_t i = 0;
+
+  // NOTE: the void(...) here is just because of comma operator of Eigen...
+  ((void(out.segment(i, vectors.size()) = vectors), i += vectors.size()), ...);
+
+  // This evaluates to:
+  // out.segment(i, vectors[0].size());
+  // i += vectors[0].size();
+  // out.segment(i, vectors[1].size());
+  // i += vectors[1].size();
+  //  ...
+  // out.segment(i, vectors[N].size());
+  // i += vectors[N].size();
+
+  return out;
+}
+
+/// Inside a vector, represents a strip using the expected number of
+/// head/tail elements needed
 struct Strip {
   std::size_t head; /*!< Number of elements at the head of the vector */
   std::size_t tail; /*!< Number of elements at the tail of the vector */

tests/utils/eigen_conversions.hpp

@@ -132,4 +132,25 @@ inline auto PushNewJointStateTo(
   joint_state.effort.tail<1>()[0] = new_joint_state.effort;
 }
 
+/**
+ *  @brief Reconstruct the expected state X (= [q, v]) from the given sensor
+ *
+ *  @param[in] sensor The complete sensor data
+ *  @param[in] with_free_flyer Indicates if we have a free flyer or not
+ *
+ *  @return Eigen::VectorXd Containing the complete state X
+ */
+inline auto GetCompleteStateFrom(
+    const linear_feedback_controller_msgs::Eigen::Sensor& sensor,
+    bool with_free_flyer) -> Eigen::VectorXd {
+  if (with_free_flyer) {
+    return ConcatenateAs<Eigen::VectorXd>(
+        sensor.base_pose, sensor.joint_state.position, sensor.base_twist,
+        sensor.joint_state.velocity);
+  } else {
+    return ConcatenateAs<Eigen::VectorXd>(sensor.joint_state.position,
+                                          sensor.joint_state.velocity);
+  }
+}
+
 }  // namespace tests::utils