change MPC prediction horizon to 10

hotfix viz

ws/src/controllers/include/mit_controller/gait_sequence_to_msg.hpp

@@ -5,7 +5,7 @@
 #include "mit_controller/mit_controller_params.hpp"
 
 interfaces::msg::GaitSequence gait_sequence_to_msg(const GaitSequence& sequence) {
-  static const int size = 21;  // size from message definition
+  static const int size = 11;  // size from message definition
   assert(MPC_PREDICTION_HORIZON + 1 <= size);
   assert(GAIT_SEQUENCE_SIZE >= size);
   interfaces::msg::GaitSequence msg;

ws/src/controllers/include/mit_controller/mit_controller_params.hpp

@@ -3,7 +3,7 @@
 static constexpr int N_LEGS = 4;
 static constexpr int N_JOINTS_PER_LEG = 3;
 static constexpr int GAIT_SEQUENCE_SIZE = 100;
-static constexpr int MPC_PREDICTION_HORIZON = 20;
+static constexpr int MPC_PREDICTION_HORIZON = 10;
 static constexpr double MPC_DT = 0.05;
 static constexpr double MPC_CONTROL_DT = 0.01;
 static constexpr double SWING_LEG_DT = 0.002;

ws/src/controllers/src/mit_controller_node.cpp

@@ -920,7 +920,7 @@ void MITController::MPCLoopCallback() {
   }
 
   if (open_loop_publisher_ != nullptr) {
-    assert(MPC_PREDICTION_HORIZON == 20);  // fixed message size
+    assert(MPC_PREDICTION_HORIZON == 10);  // fixed message size
     static interfaces::msg::PositionSequence open_loop_positions;
     for (unsigned int ol_idx = 0; ol_idx < MPC_PREDICTION_HORIZON; ol_idx++) {
       open_loop_positions.valid[ol_idx] = true;

ws/src/controllers/src/tools/plot_gs.cpp

@@ -4,7 +4,7 @@
 #include <eigen3/Eigen/Dense>
 #include <vector>
 
-#define SIZE 21
+#define SIZE 11
 
 GaitSequencePlottingNode::GaitSequencePlottingNode(const std::string& nodeName) : Node(nodeName) {
   gait_sequence_subscription_ = this->create_subscription<interfaces::msg::GaitSequence>(

ws/src/interfaces/msg/GaitSequence.msg

@@ -6,54 +6,54 @@ float32[3] target_twist
 float32 target_height
 float32 current_height
 
-bool[21] contact_sequence_0
-bool[21] contact_sequence_1
-bool[21] contact_sequence_2
-bool[21] contact_sequence_3
-
-float32[21] foot_position_sequence_x_0
-float32[21] foot_position_sequence_x_1
-float32[21] foot_position_sequence_x_2
-float32[21] foot_position_sequence_x_3
-float32[21] foot_position_sequence_y_0
-float32[21] foot_position_sequence_y_1
-float32[21] foot_position_sequence_y_2
-float32[21] foot_position_sequence_y_3
-float32[21] foot_position_sequence_z_0
-float32[21] foot_position_sequence_z_1
-float32[21] foot_position_sequence_z_2
-float32[21] foot_position_sequence_z_3
-
-float32[21] reference_trajectory_position_x
-float32[21] reference_trajectory_position_y
-float32[21] reference_trajectory_position_z
-
-float32[21] reference_trajectory_orientation_w
-float32[21] reference_trajectory_orientation_x
-float32[21] reference_trajectory_orientation_y
-float32[21] reference_trajectory_orientation_z
-
-float32[21] desired_reference_trajectory_position_x
-float32[21] desired_reference_trajectory_position_y
-float32[21] desired_reference_trajectory_position_z
-
-float32[21] desired_reference_trajectory_orientation_w
-float32[21] desired_reference_trajectory_orientation_x
-float32[21] desired_reference_trajectory_orientation_y
-float32[21] desired_reference_trajectory_orientation_z
-
-float32[21] reference_trajectory_velocity_x
-float32[21] reference_trajectory_velocity_y
-float32[21] reference_trajectory_velocity_z
-
-float32[21] reference_trajectory_twist_x
-float32[21] reference_trajectory_twist_y
-float32[21] reference_trajectory_twist_z
-
-float32[21] swing_time_sequence_0
-float32[21] swing_time_sequence_1
-float32[21] swing_time_sequence_2
-float32[21] swing_time_sequence_3
+bool[11] contact_sequence_0
+bool[11] contact_sequence_1
+bool[11] contact_sequence_2
+bool[11] contact_sequence_3
+
+float32[11] foot_position_sequence_x_0
+float32[11] foot_position_sequence_x_1
+float32[11] foot_position_sequence_x_2
+float32[11] foot_position_sequence_x_3
+float32[11] foot_position_sequence_y_0
+float32[11] foot_position_sequence_y_1
+float32[11] foot_position_sequence_y_2
+float32[11] foot_position_sequence_y_3
+float32[11] foot_position_sequence_z_0
+float32[11] foot_position_sequence_z_1
+float32[11] foot_position_sequence_z_2
+float32[11] foot_position_sequence_z_3
+
+float32[11] reference_trajectory_position_x
+float32[11] reference_trajectory_position_y
+float32[11] reference_trajectory_position_z
+
+float32[11] reference_trajectory_orientation_w
+float32[11] reference_trajectory_orientation_x
+float32[11] reference_trajectory_orientation_y
+float32[11] reference_trajectory_orientation_z
+
+float32[11] desired_reference_trajectory_position_x
+float32[11] desired_reference_trajectory_position_y
+float32[11] desired_reference_trajectory_position_z
+
+float32[11] desired_reference_trajectory_orientation_w
+float32[11] desired_reference_trajectory_orientation_x
+float32[11] desired_reference_trajectory_orientation_y
+float32[11] desired_reference_trajectory_orientation_z
+
+float32[11] reference_trajectory_velocity_x
+float32[11] reference_trajectory_velocity_y
+float32[11] reference_trajectory_velocity_z
+
+float32[11] reference_trajectory_twist_x
+float32[11] reference_trajectory_twist_y
+float32[11] reference_trajectory_twist_z
+
+float32[11] swing_time_sequence_0
+float32[11] swing_time_sequence_1
+float32[11] swing_time_sequence_2
+float32[11] swing_time_sequence_3
 
 float32[4] gait_swing_time
 

ws/src/interfaces/msg/PositionSequence.msg

@@ -1,12 +1,12 @@
 # WORLD FRAME
 # It is faster to have arrays of primitive datatypes, thats why we flatten it
 # Assume all arrays have same length of size len
-float64[20] x
-float64[20] y
-float64[20] z
-float64[20] qw
-float64[20] qx
-float64[20] qy
-float64[20] qz
+float64[11] x
+float64[11] y
+float64[11] z
+float64[11] qw
+float64[11] qx
+float64[11] qy
+float64[11] qz
 
-bool[20] valid
+bool[11] valid

ws/src/simulator/src/drake_visualizer.cpp

@@ -27,8 +27,8 @@
 #include "interfaces/msg/wbc_target.hpp"
 #include "rclcpp/rclcpp.hpp"
 
-#define GS_SIZE 21
-#define OPEN_LOOP_SIZE 20
+#define GS_SIZE 11
+#define OPEN_LOOP_SIZE 10
 
 using namespace std::chrono_literals;
 using namespace drake;