Robot Localizer

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,9 @@ 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

@@ -21,21 +21,21 @@ struct RobotConstants
     // The angles are assumed to be symmetric for the left and right sides of the robot.
     //
     //                 FRONT OF ROBOT
-    //         |             ▲ X-Axis
-    //         |   /  -------+---------  \                     eric
-    //         |A / .'       │         '. \  ◄─── Front wheel       
-    //         | /.'         │          .'.\                   grayson      
-    //         |//           │       .     \\                  samuel
-    //         ;             │    . Lever    ;     
-    //        |              │ .    Arm      |      
-    //  ◄─────┼──────────────┼               |      
-    //  Y-Axis|                              |      
-    //         ;                             ;      
-    //         |\\                         //       
-    //         | \'.                     .'/        
-    //         |B \ '.                 .' / ◄─── Back wheel        
-    //         |   \  '-.          _.-'  /          
-    //         |         ''------''
+    //         |             ▲ X-Axis                                       //
+    //         |   /  -------+---------  \                                  //
+    //         |A / .'       │         '. \  ◄─── Front wheel               //
+    //         | /.'         │          .'.\                                //
+    //         |//           │       .     |\                               //
+    //         ;             │    . Lever    ;                              //
+    //        |              │ .    Arm      |                              //
+    //  ◄─────┼──────────────┼               |                              //
+    //  Y-Axis|                              |                              //
+    //         ;                             ;                              //
+    //         |\\                         //                               //
+    //         | \'.                     .'/                                //
+    //         |B \ '.                 .' / ◄─── Back wheel                 //
+    //         |   \  '-.          _.-'  /                                  //
+    //         |         ''------''                                         //
 
     // clang-format on
 
@@ -88,6 +88,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;
 
@@ -97,6 +101,11 @@ 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;
 
@@ -107,6 +116,13 @@ struct RobotConstants
     // Found by sqrt(x^2 + y^2) of a wheel.
     // Front wheel arm = Rear wheel arm. See ASCII image above.
     float expected_lever_arm;
+
+    // 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;
 };
 
 /**
@@ -120,7 +136,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,
 
         .fr_x_pos_meters = 0.03485f,
         .fr_y_pos_meters = -0.06632f,
@@ -142,17 +158,24 @@ constexpr RobotConstants createRobotConstants()
         .motor_max_acceleration_m_per_s_2 = 2.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.0f,
+        .robot_max_deceleration_m_per_s_2   = 1.5f,
 
         // 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,
 
-        .expected_lever_arm = 0.0749f};
+        .expected_lever_arm = 0.0749f,
+
+        // Kalman filter variances for robot localizer
+        .kalman_process_noise_variance_rad_per_s_4      = 1.0f,
+        .kalman_vision_noise_variance_rad_2             = 0.01f * 0.01f,
+        .kalman_motor_sensor_noise_variance_rad_per_s_2 = 0.5};
 }
 #elif CHECK_VERSION(2021)
 constexpr RobotConstants createRobotConstants()
@@ -173,15 +196,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/play/ball_placement/ball_placement_play_test.py

@@ -200,7 +200,7 @@ def run_ball_placement_scenario(
         inv_eventually_validation_sequence_set=placement_eventually_validation_sequence_set,
         ag_always_validation_sequence_set=[[]],
         ag_eventually_validation_sequence_set=placement_eventually_validation_sequence_set,
-        test_timeout_s=[15],
+        test_timeout_s=15,
     )
 
     simulated_test_runner.run_test(
@@ -209,7 +209,7 @@ def run_ball_placement_scenario(
         inv_eventually_validation_sequence_set=drop_ball_eventually_validation_sequence_set,
         ag_always_validation_sequence_set=drop_ball_always_validation_sequence_set,
         ag_eventually_validation_sequence_set=drop_ball_eventually_validation_sequence_set,
-        test_timeout_s=[5],
+        test_timeout_s=5,
     )
 
 

src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.py

@@ -1,5 +1,3 @@
-import threading
-
 import software.python_bindings as tbots_cpp
 from proto.play_pb2 import PlayName
 
@@ -57,14 +55,6 @@ def setup(*args):
             gc_command=Command.Type.KICKOFF, team=Team.YELLOW
         )
 
-        # Let robots get ready before starting kickoff
-        threading.Timer(
-            4.0,
-            lambda: simulated_test_runner.send_gamecontroller_command(
-                gc_command=Command.Type.NORMAL_START, team=Team.YELLOW
-            ),
-        ).start()
-
         simulated_test_runner.set_plays(
             blue_play=PlayName.KickoffEnemyPlay,
             yellow_play=PlayName.KickoffFriendlyPlay,
@@ -145,6 +135,9 @@ def setup(*args):
         ag_eventually_validation_sequence_set=eventually_validation_sequence_set,
         ag_always_validation_sequence_set=always_validation_sequence_set,
         test_timeout_s=10,
+        ci_cmd_with_delay=[
+            (4.0, Command.Type.NORMAL_START, Team.YELLOW),
+        ],
     )
 
 

src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_fsm.cpp

@@ -42,7 +42,7 @@ void KickoffFriendlyPlayFSM::createKickoffSetupPositions(const WorldPtr& world_p
         kickoff_setup_positions = {
             // Robot 1
             Point(world_ptr->field().centerPoint() +
-                  Vector(-world_ptr->field().centerCircleRadius(), 0)),
+                  Vector(-0.95 * world_ptr->field().centerCircleRadius(), 0)),
             // Robot 2
             // Goalie positions will be handled by the goalie tactic
             // Robot 3

src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.py

@@ -1,5 +1,3 @@
-import threading
-
 import software.python_bindings as tbots_cpp
 from proto.play_pb2 import PlayName
 
@@ -62,14 +60,6 @@ def setup(*args):
             gc_command=Command.Type.KICKOFF, team=Team.BLUE
         )
 
-        # Let robots get ready before starting kickoff
-        threading.Timer(
-            4.0,
-            lambda: simulated_test_runner.send_gamecontroller_command(
-                gc_command=Command.Type.NORMAL_START, team=Team.BLUE
-            ),
-        ).start()
-
         simulated_test_runner.set_plays(
             blue_play=PlayName.KickoffFriendlyPlay,
             yellow_play=PlayName.KickoffEnemyPlay,
@@ -145,6 +135,9 @@ def setup(*args):
         ag_eventually_validation_sequence_set=eventually_validation_sequence_set,
         ag_always_validation_sequence_set=always_validation_sequence_set,
         test_timeout_s=10,
+        ci_cmd_with_delay=[
+            (4.0, Command.Type.NORMAL_START, Team.BLUE),
+        ],
     )
 
 

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/ai/navigator/trajectory/BUILD

@@ -21,6 +21,7 @@ cc_library(
     deps = [
         ":trajectory",
         "//software/geom:point",
+        "//software/geom/algorithms",
     ],
 )
 

src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <array>
+#include <cmath>
 #include <cstddef>
 
 #include "software/ai/navigator/trajectory/trajectory.hpp"
@@ -103,6 +104,19 @@ class BangBangTrajectory1D : public Trajectory<double, double, double>
      */
     size_t getNumTrajectoryParts() const;
 
+    /**
+     * Check if this trajectory is meaningfully different from another trajectory.
+     * @param other The other trajectory to compare to
+     * @param threshold The threshold above which the trajectories are considered
+     * different
+     * @return True if the trajectories are different, false otherwise
+     */
+    bool isNew(const Trajectory<double, double, double>& other,
+               double threshold) const override
+    {
+        return std::abs(getDestination() - other.getDestination()) > threshold;
+    }
+
    private:
     /**
      * Generate a trapezoidal trajectory and fill in `trajectory_parts`.