fix inertia indexing (#37)

Author: tpoignonec

fd_controllers/fd_inertia_broadcaster/src/fd_inertia_broadcaster.cpp

@@ -43,19 +43,21 @@ class State;
 namespace fd_inertia_broadcaster
 {
 const auto kUninitializedValue = std::numeric_limits<double>::quiet_NaN();
-const size_t sizeFlattenedInertia = 15;
+const size_t sizeFlattenedInertia = 21;
 
-unsigned int flattened_index_from_triangular_index(unsigned int idx_row, unsigned int idx_col)
+unsigned int flattened_index_from_triangular_index(
+  unsigned int idx_row,
+  unsigned int idx_col,
+  unsigned int dim = 6)
 {
   unsigned int i = idx_row;
   unsigned int j = idx_col;
   if (idx_col < idx_row) {
     i = idx_col;
     j = idx_row;
   }
-  return i * (i - 1) / 2 + j;
+  return i * (2 * dim - i - 1) / 2 + j;
 }
-
 template<class Derived>
 void matrixEigenToMsg(const Eigen::MatrixBase<Derived> & e, std_msgs::msg::Float64MultiArray & m)
 {

fd_hardware/src/fd_effort_hi.cpp

@@ -40,9 +40,18 @@ namespace fd_hardware
 
 rclcpp::Logger LOGGER = rclcpp::get_logger("FDEffortHardwareInterface");
 
-unsigned int flattened_index_from_triangular_index(unsigned int i, unsigned int j)
+unsigned int flattened_index_from_triangular_index(
+  unsigned int idx_row,
+  unsigned int idx_col,
+  unsigned int dim = 6)
 {
-  return i * (i - 1) / 2 + j;
+  unsigned int i = idx_row;
+  unsigned int j = idx_col;
+  if (idx_col < idx_row) {
+    i = idx_col;
+    j = idx_row;
+  }
+  return i * (2 * dim - i - 1) / 2 + j;
 }