[lf_controller] Fix the error computation (using pinocchio)

loco-3d · Jan 31, 2025 · 2049ffe · 2049ffe
1 parent 2b3def1
commit 2049ffe
Show file tree

Hide file tree

Showing 3 changed files with 51 additions and 41 deletions.
diff --git a/tests/test_lf_controller.cpp b/tests/test_lf_controller.cpp
@@ -15,7 +15,6 @@ using tests::utils::MakeAllModelDescriptionsFor;
 using tests::utils::ModelDescription;
 
 #include "utils/eigen_conversions.hpp"
-using tests::utils::GetCompleteStateFrom;
 using tests::utils::PushNewJointStateTo;
 
 #include "linear_feedback_controller/lf_controller.hpp"
@@ -191,8 +190,7 @@ TEST_P(LFControllerTest, ComputeControl) {
 
   const auto sensor = MakeValidRandomSensorFor(*model_ptr);
   const auto control = MakeValidRandomControlFor(*model_ptr);
-  EXPECT_EQ(ExpectedLFControlFrom(sensor, control,
-                                  model_ptr->get_robot_has_free_flyer()),
+  EXPECT_EQ(ExpectedLFControlFrom(sensor, control, *model_ptr),
             ctrl.compute_control(sensor, control));
 }
 
@@ -233,10 +231,6 @@ INSTANTIATE_TEST_SUITE_P(
             {
                 {.name = "l02", .type = JointType::Controlled},
             },
-            // FIXME: Do not work
-            {
-                {.name = "l01", .type = JointType::Moving},
-            },
             {
                 {.name = "l01"},
                 {.name = "l12"},

diff --git a/tests/utils/eigen_conversions.hpp b/tests/utils/eigen_conversions.hpp
@@ -125,32 +125,40 @@ inline auto PushNewJointStateTo(
   joint_state.effort << joint_state.effort, 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 {
-  Eigen::VectorXd out;
-
-  if (with_free_flyer) {
-    out.resize(sensor.base_pose.size() + sensor.joint_state.position.size() +
-               sensor.base_twist.size() + sensor.joint_state.velocity.size());
-    out << sensor.base_pose, sensor.joint_state.position, sensor.base_twist,
-        sensor.joint_state.velocity;
-  } else {
-    out.resize(sensor.joint_state.position.size() +
-               sensor.joint_state.velocity.size());
-    out << sensor.joint_state.position, sensor.joint_state.velocity;
+struct RobotState {
+  Eigen::VectorXd position;
+  Eigen::VectorXd velocity;
+
+  /**
+   *  @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 RobotState Containing the complete state X
+   */
+  static inline auto From(
+      const linear_feedback_controller_msgs::Eigen::Sensor& sensor,
+      bool with_free_flyer) noexcept -> RobotState {
+    RobotState out;
+    if (with_free_flyer) {
+      out.position.resize(sensor.base_pose.size() +
+                          sensor.joint_state.position.size());
+      out.velocity.resize(sensor.base_twist.size() +
+                          sensor.joint_state.velocity.size());
+
+      out.position << sensor.base_pose, sensor.joint_state.position;
+      out.velocity << sensor.base_twist, sensor.joint_state.velocity;
+    } else {
+      out.position.resize(sensor.joint_state.position.size());
+      out.velocity.resize(sensor.joint_state.velocity.size());
+
+      out.position = sensor.joint_state.position;
+      out.velocity = sensor.joint_state.velocity;
+    }
+    return out;
   }
-
-  return out;
-}
+};
 
 }  // namespace tests::utils
 

diff --git a/tests/utils/lf_controller.hpp b/tests/utils/lf_controller.hpp
@@ -3,6 +3,7 @@
 
 #include "eigen_conversions.hpp"
 #include "linear_feedback_controller/lf_controller.hpp"
+#include "pinocchio/algorithm/joint-configuration.hpp"  // pinocchio::difference
 #include "robot_model.hpp"
 
 namespace tests::utils {
@@ -76,10 +77,10 @@ inline auto MakeValidRandomControlFor(
   linear_feedback_controller_msgs::Eigen::Control control;
 
   // get_n* function take into account the free flyer stuff
-  control.feedforward = Eigen::VectorXd::Random(model.get_nv());
+  control.feedforward = Eigen::VectorXd::Random(model.get_joint_nv());
   control.feedback_gain = Eigen::MatrixXd::Random(
-      /* rows = */ model.get_nv(),
-      /* cols = */ model.get_nv() + model.get_nq());
+      /* rows = */ model.get_joint_nv(),
+      /* cols = */ model.get_nv() * 2);
 
   control.initial_state = MakeValidRandomSensorFor(model);
 
@@ -89,14 +90,21 @@ inline auto MakeValidRandomControlFor(
 inline auto ExpectedLFControlFrom(
     const linear_feedback_controller_msgs::Eigen::Sensor& sensor,
     const linear_feedback_controller_msgs::Eigen::Control& control,
-    bool with_free_flyer) -> Eigen::VectorXd {
-  const auto x = GetCompleteStateFrom(sensor, with_free_flyer);
-  const auto x_0 = GetCompleteStateFrom(control.initial_state, with_free_flyer);
+    const linear_feedback_controller::RobotModelBuilder& model)
+    -> Eigen::VectorXd {
+  Eigen::VectorXd out;
+
+  const auto x = RobotState::From(sensor, model.get_robot_has_free_flyer());
+  const auto x_0 =
+      RobotState::From(control.initial_state, model.get_robot_has_free_flyer());
 
-  // FIXME: free flyer ?
-  const auto error = x_0 - x;
+  auto error = Eigen::VectorXd{control.feedback_gain.cols()};
+  pinocchio::difference(model.get_model(), x.position, x_0.position,
+                        error.head(model.get_model().nv));
+  error.tail(model.get_model().nv) = x_0.velocity - x.velocity;
 
-  return (control.feedforward + (control.feedback_gain * error));
+  out = control.feedforward + (control.feedback_gain * error);
+  return out;
 }
 
 }  // namespace tests::utils