Implement new prim exec logic using motion controllers

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_trajectory_max_ang_speed_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/robot_constants.h

@@ -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_trajectory_max_ang_speed_rad_per_s;
+
     // The maximum angular acceleration achievable by our robots [rad/s^2]
     float robot_max_ang_acceleration_rad_per_s_2;
 
@@ -142,13 +151,15 @@ 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_trajectory_max_ang_speed_rad_per_s = 7.0f,
+        .robot_max_ang_acceleration_rad_per_s_2   = 30.0f,
 
         .wheel_radius_meters = 0.03f,
 
@@ -173,13 +184,15 @@ 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_trajectory_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'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_trajectory_max_ang_speed_rad_per_s = 7.0f,
+        .robot_max_ang_acceleration_rad_per_s_2   = 30.0f,
 
         .wheel_radius_meters = 0.03f};
 }

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,21 @@ class BangBangTrajectory1D : public Trajectory<double, double, double>
      */
     size_t getNumTrajectoryParts() const;
 
+    /**
+     * Checks if this 1D trajectory terminates at approximately the same destination
+     * position as another trajectory.
+     *
+     * @param other The other trajectory to compare against.
+     * @param threshold The maximum allowable distance between the two destinations.
+     * @return True if the distance between destinations is less than or equal to the
+     * threshold. False otherwise.
+     */
+    bool hasSameDestination(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,23 @@ class BangBangTrajectory1DAngular
      */
     double getTotalTime() const override;
 
+    /**
+     * Checks if this angular trajectory terminates at approximately the same destination
+     * orientation as another trajectory.
+     *
+     * @param other The other trajectory to compare against.
+     * @param threshold The maximum allowable distance in radians between the two
+     * destinations.
+     * @return True if the distance between destinations is less than or equal to the
+     * threshold. False otherwise.
+     */
+    bool hasSameDestination(
+        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,16 @@ class Trajectory
     {
         return getPosition(getTotalTime());
     }
+
+    /**
+     * Checks if this trajectory terminates at approximately the same destination as
+     * another trajectory.
+     *
+     * @param other The other trajectory to compare against.
+     * @param threshold The maximum allowable distance between the two destinations.
+     * @return True if the distance between destinations is less than or equal to the
+     * threshold. False otherwise.
+     */
+    virtual bool hasSameDestination(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,20 @@ class Trajectory2D : virtual public Trajectory<Point, Vector, Vector>
      * @return bounding boxes which this trajectory passes through
      */
     virtual std::vector<Rectangle> getBoundingBoxes() const = 0;
+
+    /**
+     * Checks if this 2D trajectory terminates at approximately the same destination
+     * position as another trajectory.
+     *
+     * @param other The other trajectory to compare against.
+     * @param threshold The maximum allowable distance in metres between the two
+     * destinations.
+     * @return True if the distance between destinations is less than or equal to the
+     * threshold. False otherwise.
+     */
+    bool hasSameDestination(const Trajectory<Point, Vector, Vector>& other,
+                            double threshold) const override
+    {
+        return distance(getDestination(), other.getDestination()) <= threshold;
+    }
 };

src/software/embedded/BUILD

@@ -34,6 +34,8 @@ cc_library(
         "//proto/primitive:primitive_msg_factory",
         "//software/ai/navigator/trajectory:bang_bang_trajectory_1d_angular",
         "//software/ai/navigator/trajectory:trajectory_path",
+        "//software/embedded/motion_control:orientation_controller",
+        "//software/embedded/motion_control:position_controller",
         "//software/math:math_functions",
         "//software/physics:velocity_conversion_util",
         "//software/time:duration",

src/software/embedded/motion_control/controller.h

@@ -1,3 +1,5 @@
+#pragma once
+
 #include "software/time/duration.h"
 
 /**