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_trajectory_max_speed_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_trajectory_max_speed_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_trajectory_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'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,
 
-        .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_trajectory_max_speed_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/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 equal to another trajectory.
+     * @param other The other trajectory to compare to
+     * @param threshold The threshold below which the trajectories are considered
+     * equal
+     * @return True if the trajectories are equal, false otherwise
+     */
+    bool equals(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`.

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

@@ -83,6 +83,19 @@ class BangBangTrajectory1DAngular
      */
     double getTotalTime() const override;
 
+    /**
+     * Check if this trajectory is meaningfully equal to another trajectory.
+     * @param other The other trajectory to compare to
+     * @param threshold The threshold below which the trajectories are considered
+     * equal in degrees.
+     * @return True if the trajectories are equal, false otherwise
+     */
+    bool equals(const Trajectory<Angle, AngularVelocity, AngularAcceleration>& other,
+                double threshold) const override
+    {
+        return getDestination().minDiff(other.getDestination()).toDegrees() <= threshold;
+    }
+
    private:
     BangBangTrajectory1D trajectory;
 };

src/software/ai/navigator/trajectory/trajectory.hpp

@@ -59,4 +59,13 @@ class Trajectory
     {
         return getPosition(getTotalTime());
     }
+
+    /**
+     * Check if this trajectory is meaningfully equal to another trajectory.
+     * @param other The other trajectory to compare to
+     * @param threshold The threshold below which the trajectories are considered
+     * equal
+     * @return True if the trajectories are equal, false otherwise
+     */
+    virtual bool equals(const Trajectory<P, V, A>& other, double threshold) const = 0;
 };

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

@@ -1,6 +1,7 @@
 #pragma once
 
 #include "software/ai/navigator/trajectory/trajectory.hpp"
+#include "software/geom/algorithms/distance.h"
 #include "software/geom/point.h"
 #include "software/geom/rectangle.h"
 
@@ -14,4 +15,17 @@ class Trajectory2D : virtual public Trajectory<Point, Vector, Vector>
      * @return bounding boxes which this trajectory passes through
      */
     virtual std::vector<Rectangle> getBoundingBoxes() const = 0;
+
+    /**
+     * Check if this trajectory is meaningfully equal to another trajectory.
+     * @param other The other trajectory to compare to
+     * @param threshold The threshold below which the trajectories are considered
+     * equal
+     * @return True if the trajectories are equal, false otherwise
+     */
+    bool equals(const Trajectory<Point, Vector, Vector>& other,
+                double threshold) const override
+    {
+        return distance(getDestination(), other.getDestination()) <= threshold;
+    }
 };

src/software/constants.h

@@ -53,7 +53,7 @@ const std::string ROBOT_KICK_EXP_COEFF_CONFIG_KEY    = "kick_coeff";
 const std::string ROBOT_CHIP_PULSE_WIDTH_CONFIG_KEY  = "chip_pulse_width";
 
 const std::string SSL_VISION_ADDRESS          = "224.5.23.2";
-static constexpr unsigned int SSL_VISION_PORT = 10020;
+static constexpr unsigned int SSL_VISION_PORT = 10006;
 
 const std::string SSL_REFEREE_ADDRESS          = "224.5.23.1";
 static constexpr unsigned int SSL_REFEREE_PORT = 10003;