Improved robot localization and trajectory following

src/proto/message_translation/tbots_protobuf.cpp

@@ -425,7 +425,8 @@ std::unique_ptr<TbotsProto::CostVisualization> createCostVisualization(
 }
 
 std::optional<TrajectoryPath> createTrajectoryPathFromParams(
-    const TbotsProto::TrajectoryPathParams2D& params, const Vector& initial_velocity,
+    const TbotsProto::TrajectoryPathParams2D& params, const Point& start_position,
+    const Vector& initial_velocity,
     const robot_constants::RobotConstants& robot_constants)
 {
     double max_speed = convertMaxAllowedSpeedModeToMaxAllowedSpeed(
@@ -449,8 +450,7 @@ std::optional<TrajectoryPath> createTrajectoryPathFromParams(
     }
 
     auto trajectory = std::make_shared<BangBangTrajectory2D>(
-        createPoint(params.start_position()), initial_destination, initial_velocity,
-        constraints);
+        start_position, initial_destination, initial_velocity, constraints);
 
     TrajectoryPath trajectory_path(trajectory, BangBangTrajectory2D::generator);
 
@@ -475,14 +475,14 @@ std::optional<TrajectoryPath> createTrajectoryPathFromParams(
 }
 
 BangBangTrajectory1DAngular createAngularTrajectoryFromParams(
-    const TbotsProto::TrajectoryParamsAngular1D& params,
+    const TbotsProto::TrajectoryParamsAngular1D& params, const Angle& start_angle,
     const AngularVelocity& initial_velocity,
     const robot_constants::RobotConstants& robot_constants)
 {
     return BangBangTrajectory1DAngular(
-        createAngle(params.start_angle()), createAngle(params.final_angle()),
-        initial_velocity,
-        AngularVelocity::fromRadians(robot_constants.robot_max_ang_speed_rad_per_s),
+        start_angle, createAngle(params.final_angle()), initial_velocity,
+        AngularVelocity::fromRadians(
+            robot_constants.robot_max_ang_speed_trajectory_rad_per_s),
         AngularVelocity::fromRadians(
             robot_constants.robot_max_ang_acceleration_rad_per_s_2),
         AngularVelocity::fromRadians(
@@ -513,7 +513,7 @@ double convertMaxAllowedSpeedModeToMaxAllowedSpeed(
     switch (max_allowed_speed_mode)
     {
         case TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT:
-            return robot_constants.robot_max_speed_m_per_s;
+            return robot_constants.robot_max_speed_trajectory_m_per_s;
         case TbotsProto::MaxAllowedSpeedMode::STOP_COMMAND:
             return STOP_COMMAND_ROBOT_MAX_SPEED_METERS_PER_SECOND -
                    STOP_COMMAND_SPEED_SAFETY_MARGIN_METERS_PER_SECOND;

src/proto/message_translation/tbots_protobuf.h

@@ -238,25 +238,28 @@ std::unique_ptr<TbotsProto::CostVisualization> createCostVisualization(
  * Generate a 2D Trajectory Path given 2D trajectory parameters
  *
  * @param params 2D Trajectory Path
+ * @param start_position Initial position to use for the trajectory
  * @param initial_velocity Initial velocity to use for the trajectory
  * @param robot_constants Constants to use for the trajectory
  * @return TrajectoryPath, or std::nullopt if the trajectory path could not be created
  * from the given parameters
  */
 std::optional<TrajectoryPath> createTrajectoryPathFromParams(
-    const TbotsProto::TrajectoryPathParams2D& params, const Vector& initial_velocity,
+    const TbotsProto::TrajectoryPathParams2D& params, const Point& start_position,
+    const Vector& initial_velocity,
     const robot_constants::RobotConstants& robot_constants);
 
 /**
- * Generate an angular trajectory Path given angular trajectory proto parameters
+ * Generate an angular trajectory path given angular trajectory proto parameters
  *
- * @param params angular Trajectory Path
+ * @param params Angular trajectory path
+ * @param start_angle Initial angle to use for the trajectory
  * @param initial_velocity Initial velocity to use for the trajectory
  * @param robot_constants Constants to use for the trajectory
  * @return Generate angular trajectory
  */
 BangBangTrajectory1DAngular createAngularTrajectoryFromParams(
-    const TbotsProto::TrajectoryParamsAngular1D& params,
+    const TbotsProto::TrajectoryParamsAngular1D& params, const Angle& start_angle,
     const AngularVelocity& initial_velocity,
     const robot_constants::RobotConstants& robot_constants);
 

src/proto/message_translation/tbots_protobuf_test.cpp

@@ -209,8 +209,8 @@ TEST_P(TrajectoryParamConversionTest, trajectory_params_msg_test)
         *(params.add_sub_destinations()) = sub_destination_proto;
     }
 
-    auto converted_trajectory_path_opt =
-        createTrajectoryPathFromParams(params, initial_velocity, robot_constants);
+    auto converted_trajectory_path_opt = createTrajectoryPathFromParams(
+        params, createPoint(params.start_position()), initial_velocity, robot_constants);
     ASSERT_TRUE(converted_trajectory_path_opt.has_value());
 
     TrajectoryPath converted_trajectory_path = converted_trajectory_path_opt.value();

src/shared/constants.h

@@ -224,6 +224,8 @@ static const unsigned THUNDERLOOP_HZ = 300u;
 
 static const unsigned NUM_GENEVA_ANGLES = 5;
 
+static constexpr double RTT_S = 0.03;
+
 // Robot diagnostics constants
 constexpr double AUTO_CHIP_DISTANCE_DEFAULT_M     = 1.5;
 constexpr double AUTO_KICK_SPEED_DEFAULT_M_PER_S  = 1.5;

src/shared/robot_constants.h

@@ -60,6 +60,10 @@ struct RobotConstants
     // The maximum speed achievable by our robots, in metres per second [m/s]
     float robot_max_speed_m_per_s;
 
+    // The maximum speed that the trajectory planner is allowed to command the robot to
+    // move at, while still leaving headroom for the PID to apply correction on lag. [m/s]
+    float robot_max_speed_trajectory_m_per_s;
+
     // The maximum acceleration achievable by our robots [m/s^2]
     float robot_max_acceleration_m_per_s_2;
 
@@ -69,11 +73,23 @@ struct RobotConstants
     // The maximum angular speed achievable by our robots [rad/s]
     float robot_max_ang_speed_rad_per_s;
 
+    // The maximum speed that the trajectory planner is allowed to command the robot to
+    // move at, while still leaving headroom for the PID to apply correction on lag.
+    // [rad/s]
+    float robot_max_ang_speed_trajectory_rad_per_s;
+
     // The maximum angular acceleration achievable by our robots [rad/s^2]
     float robot_max_ang_acceleration_rad_per_s_2;
 
     // The radius of the wheel, in meters
     float wheel_radius_meters;
+
+    // Various variances for the robot localizer
+    float kalman_process_noise_variance_rad_per_s_4;
+
+    float kalman_vision_noise_variance_rad_2;
+
+    float kalman_motor_sensor_noise_variance_rad_per_s_2;
 };
 
 /**
@@ -87,7 +103,7 @@ constexpr RobotConstants createRobotConstants()
     return {
         .robot_radius_m        = static_cast<float>(ROBOT_MAX_RADIUS_METERS),
         .front_wheel_angle_deg = 32.0f,
-        .back_wheel_angle_deg  = 44.0f,
+        .back_wheel_angle_deg  = 46.0f,
 
         .front_of_robot_width_meters = 0.11f,
         .dribbler_width_meters       = 0.07825f,
@@ -97,18 +113,25 @@ constexpr RobotConstants createRobotConstants()
         .max_force_dribbler_speed_rpm  = -12000,
 
         // Motor constant
-        .motor_max_acceleration_m_per_s_2 = 2.0f,
+        .motor_max_acceleration_m_per_s_2 = 4.0f,
 
         // Robot's linear movement constants
-        .robot_max_speed_m_per_s          = 3.0f,
-        .robot_max_acceleration_m_per_s_2 = 3.0f,
-        .robot_max_deceleration_m_per_s_2 = 3.0f,
+        .robot_max_speed_m_per_s            = 3.0f,
+        .robot_max_speed_trajectory_m_per_s = 2.5f,
+        .robot_max_acceleration_m_per_s_2   = 2.5f,
+        .robot_max_deceleration_m_per_s_2   = 2.0f,
 
         // Robot's angular movement constants
-        .robot_max_ang_speed_rad_per_s          = 10.0f,
-        .robot_max_ang_acceleration_rad_per_s_2 = 30.0f,
+        .robot_max_ang_speed_rad_per_s            = 6.0f,
+        .robot_max_ang_speed_trajectory_rad_per_s = 5.0f,
+        .robot_max_ang_acceleration_rad_per_s_2   = 10.0f,
+
+        .wheel_radius_meters = 0.03f,
 
-        .wheel_radius_meters = 0.03f};
+        // Kalman filter variances for robot localizer
+        .kalman_process_noise_variance_rad_per_s_4      = 1.0f,
+        .kalman_vision_noise_variance_rad_2             = 0.01f,
+        .kalman_motor_sensor_noise_variance_rad_per_s_2 = 0.5};
 }
 #elif CHECK_VERSION(2021)
 constexpr RobotConstants createRobotConstants()
@@ -129,15 +152,22 @@ constexpr RobotConstants createRobotConstants()
         .motor_max_acceleration_m_per_s_2 = 4.5f,
 
         // Robot's linear movement constants
-        .robot_max_speed_m_per_s          = 3.000f,
-        .robot_max_acceleration_m_per_s_2 = 3.0f,
-        .robot_max_deceleration_m_per_s_2 = 3.0f,
+        .robot_max_speed_m_per_s            = 3.000f,
+        .robot_max_speed_trajectory_m_per_s = 3.000f,
+        .robot_max_acceleration_m_per_s_2   = 3.0f,
+        .robot_max_deceleration_m_per_s_2   = 3.0f,
 
         // Robot's angular movement constants
-        .robot_max_ang_speed_rad_per_s          = 10.0f,
-        .robot_max_ang_acceleration_rad_per_s_2 = 30.0f,
+        .robot_max_ang_speed_rad_per_s            = 10.0f,
+        .robot_max_ang_speed_trajectory_rad_per_s = 7.0f,
+        .robot_max_ang_acceleration_rad_per_s_2   = 30.0f,
+
+        .wheel_radius_meters = 0.03f,
 
-        .wheel_radius_meters = 0.03f};
+        // Kalman filter variances for robot localizer
+        .kalman_process_noise_variance_rad_per_s_4      = 0.5f,
+        .kalman_vision_noise_variance_rad_2             = 0.01f * 0.01f,
+        .kalman_motor_sensor_noise_variance_rad_per_s_2 = 0.5f * 0.5f};
 }
 #endif
 

src/software/ai/evaluation/intercept.cpp

@@ -97,7 +97,7 @@ Point findOvershootInterceptPosition(const Robot& robot, const Point intercept_p
     Point best_position   = intercept_position;
     double final_speed    = step_speed;
     bool finished         = false;
-    double max_speed      = robot.robotConstants().robot_max_speed_m_per_s;
+    double max_speed      = robot.robotConstants().robot_max_speed_trajectory_m_per_s;
     double max_acc        = robot.robotConstants().robot_max_acceleration_m_per_s_2;
 
     while (!finished)

src/software/ai/hl/stp/tactic/move_primitive.cpp

@@ -40,7 +40,7 @@ MovePrimitive::MovePrimitive(
         angular_trajectory.generate(
             robot.orientation(), final_angle, robot.angularVelocity(),
             AngularVelocity::fromRadians(
-                robot.robotConstants().robot_max_ang_speed_rad_per_s),
+                robot.robotConstants().robot_max_ang_speed_trajectory_rad_per_s),
             AngularVelocity::fromRadians(
                 robot.robotConstants().robot_max_ang_acceleration_rad_per_s_2),
             AngularVelocity::fromRadians(

src/software/embedded/BUILD

@@ -37,6 +37,7 @@ cc_library(
         "//software/math:math_functions",
         "//software/physics:velocity_conversion_util",
         "//software/time:duration",
+        "//software/util/pid",
         "//software/world:robot_state",
         "//software/world:team_colour",
     ],
@@ -58,11 +59,14 @@ cc_library(
     deps = [
         ":primitive_executor",
         "//proto:tbots_cc_proto",
+        "//software/embedded:robot_localizer",
+        "//software/embedded/services:imu",
         "//software/embedded/services:motor",
         "//software/embedded/services:power",
         "//software/embedded/services/network",
         "//software/embedded/toml_config",
         "//software/logger:network_logger",
+        "//software/physics:velocity_conversion_util",
         "//software/tracy:tracy_constants",
         "//software/util/scoped_timespec_timer",
         "@tracy",
@@ -100,3 +104,21 @@ cc_test(
         "//shared/test_util:tbots_gtest_main",
     ],
 )
+
+cc_library(
+    name = "robot_localizer",
+    srcs = ["robot_localizer.cpp"],
+    hdrs = ["robot_localizer.h"],
+    deps = [
+        "//proto:tbots_cc_proto",
+        "//software:constants",
+        "//software/embedded/services:imu",
+        "//software/geom:angle",
+        "//software/geom:angular_velocity",
+        "//software/geom:point",
+        "//software/geom:vector",
+        "//software/sensor_fusion/filter:kalman_filter",
+        "//software/util/scoped_timespec_timer",
+        "@eigen",
+    ],
+)

src/software/embedded/motor_controller/stspin_motor_controller.h

@@ -57,11 +57,11 @@ class StSpinMotorController : public MotorController
 
     static constexpr int RESET_GPIO_PIN = 12;
 
-    static constexpr int SPEED_PID_PROPORTIONAL_GAIN = 700;
-    static constexpr int SPEED_PID_INTEGRAL_GAIN     = 30;
+    static constexpr int SPEED_PID_PROPORTIONAL_GAIN = 1300;
+    static constexpr int SPEED_PID_INTEGRAL_GAIN     = 40;
 
-    static constexpr int MAX_SPEED_FEED_FORWARD_STATIC_GAIN = 750;
-    static constexpr int MIN_SPEED_FEED_FORWARD_STATIC_GAIN = 300;
+    static constexpr int MAX_SPEED_FEED_FORWARD_STATIC_GAIN = 800;
+    static constexpr int MIN_SPEED_FEED_FORWARD_STATIC_GAIN = 250;
 
     robot_constants::RobotConstants robot_constants_;