Implement new prim exec logic using motion controllers

src/proto/message_translation/tbots_protobuf.cpp

@@ -482,7 +482,8 @@ BangBangTrajectory1DAngular createAngularTrajectoryFromParams(
     return BangBangTrajectory1DAngular(
         createAngle(params.start_angle()), createAngle(params.final_angle()),
         initial_velocity,
-        AngularVelocity::fromRadians(robot_constants.robot_max_ang_speed_rad_per_s),
+        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 +514,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

@@ -248,9 +248,9 @@ std::optional<TrajectoryPath> createTrajectoryPathFromParams(
     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 initial_velocity Initial velocity to use for the trajectory
  * @param robot_constants Constants to use for the trajectory
  * @return Generate angular trajectory

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 = 2.5f,
+        .robot_max_acceleration_m_per_s_2   = 3.0f,
+        .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            = 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/hl/stp/tactic/attacker/attacker_tactic_test.py

@@ -160,7 +160,9 @@ def setup(*args):
             RobotEventuallyAtOrientation(robot_id=1, orientation=pass_orientation),
             # Larger threshold since techically ball is at passer point, not the robot
             RobotEventuallyAtPosition(
-                robot_id=1, position=passer_point, threshold=ROBOT_MAX_RADIUS_METERS
+                robot_id=1,
+                position=passer_point,
+                threshold=ROBOT_MAX_RADIUS_METERS * 1.5,
             ),
             BallEventuallyKickedInDirection(kick_direction=pass_orientation),
         ]

src/software/ai/hl/stp/tactic/move/move_tactic_test.py

@@ -298,5 +298,49 @@ def setup(*args):
     )
 
 
+def test_move_across_x_axis(simulated_test_runner):
+    initial_position = tbots_cpp.Point(-4.4, 0)
+    destination = tbots_cpp.Point(3, 0)
+
+    def setup(*args):
+        simulated_test_runner.set_world_state(
+            create_world_state(
+                blue_robot_locations=[
+                    initial_position,
+                ],
+                yellow_robot_locations=[],
+                ball_location=tbots_cpp.Point(-3, -3),
+                ball_velocity=tbots_cpp.Vector(0, 0),
+            ),
+        )
+
+        simulated_test_runner.set_tactics(
+            blue_tactics={
+                0: MoveTactic(
+                    destination=tbots_cpp.createPointProto(destination),
+                    final_orientation=tbots_cpp.createAngleProto(
+                        tbots_cpp.Angle.zero()
+                    ),
+                )
+            }
+        )
+
+    eventually_validation_sequence_set = [
+        # Robot stays at destination for one second
+        [
+            DurationValidation(
+                duration_s=1, validation=RobotEventuallyAtPosition(0, destination)
+            )
+        ]
+    ]
+
+    simulated_test_runner.run_test(
+        setup=setup,
+        inv_eventually_validation_sequence_set=eventually_validation_sequence_set,
+        ag_eventually_validation_sequence_set=eventually_validation_sequence_set,
+        test_timeout_s=6,
+    )
+
+
 if __name__ == "__main__":
     pytest_main(__file__)

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/BUILD

@@ -22,6 +22,7 @@ cc_library(
         ":controller",
         ":pid_controller",
         "//software/ai/navigator/trajectory:trajectory_path",
+        "//software/geom/algorithms",
     ],
 )
 

src/software/embedded/motion_control/controller.h

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

src/software/embedded/motion_control/orientation_controller.cpp

@@ -4,30 +4,27 @@ AngularVelocity OrientationController::step(
     const Angle& orientation, const BangBangTrajectory1DAngular& target_trajectory,
     Duration elapsed_time, Duration delta_time)
 {
-    const Angle difference_from_target =
-        (target_trajectory.getDestination() - orientation).clamp();
+    // feedforward trajectory angular velocity with small pid control effort
+    const Angle error_angular =
+        (target_trajectory.getPosition(elapsed_time.toSeconds()) - orientation).clamp();
+    const AngularVelocity pid_effort_angular = AngularVelocity::fromRadians(
+        w_pid_.step(error_angular.toRadians(), delta_time.toSeconds()));
 
-    if (difference_from_target.abs().toDegrees() < ANGULAR_PURE_PID_THRESHOLD_DEGREES)
-    {
-        // if target orientation is close enough, use pure PID for angular velocity
-        return AngularVelocity::fromRadians(w_pid_close_.step(
-            difference_from_target.toRadians(), delta_time.toSeconds()));
-    }
-    else
+    AngularVelocity angular_velocity =
+        target_trajectory.getVelocity(elapsed_time.toSeconds()) + pid_effort_angular;
+
+    const Angle orientation_to_destination =
+        orientation.minDiff(target_trajectory.getDestination());
+    // Dampen angular velocity as we get closer to the destination to reduce jittering
+    if (orientation_to_destination.toDegrees() < ANGULAR_DESTINATION_THRESHOLD_DEGREES)
     {
-        // feedforward trajectory angular velocity with small pid control effort
-        const Angle error_angular =
-            (target_trajectory.getPosition(elapsed_time.toSeconds()) - orientation)
-                .clamp();
-        const AngularVelocity pid_effort_angular = AngularVelocity::fromRadians(
-            w_pid_.step(error_angular.toRadians(), delta_time.toSeconds()));
-        return target_trajectory.getVelocity(elapsed_time.toSeconds()) +
-               pid_effort_angular;
+        angular_velocity *= orientation_to_destination.toDegrees() /
+                            ANGULAR_DESTINATION_THRESHOLD_DEGREES;
     }
+    return angular_velocity;
 }
 
 void OrientationController::reset()
 {
     w_pid_.reset();
-    w_pid_close_.reset();
 }

src/software/embedded/motion_control/orientation_controller.h

@@ -37,8 +37,7 @@ class OrientationController
 
    private:
     // TODO(#3737): tune constants
-    PidController w_pid_{0.7, 0.0, 2.0, 0.0};
-    PidController w_pid_close_{2.0, 0.0, 4.0, 0.0};
+    PidController w_pid_{0.4, 0.0, 0.0, 0.0};
 
-    static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 25.0;
+    static constexpr double ANGULAR_DESTINATION_THRESHOLD_DEGREES = 5;
 };

src/software/embedded/motion_control/position_controller.cpp

@@ -1,34 +1,32 @@
 #include "software/embedded/motion_control/position_controller.h"
 
+#include "software/geom/algorithms/distance.h"
+
 Vector PositionController::step(const Point& position,
                                 const TrajectoryPath& target_trajectory,
                                 Duration elapsed_time, Duration delta_time)
 {
-    const Vector distance_from_destination =
-        target_trajectory.getDestination() - position;
+    // feedforward trajectory velocity with small pid control effort
+    const Vector error =
+        target_trajectory.getPosition(elapsed_time.toSeconds()) - position;
+    const Vector control_effort{x_pid_.step(error.x(), delta_time.toSeconds()),
+                                y_pid_.step(error.y(), delta_time.toSeconds())};
+    double distance_to_destination =
+        distance(position, target_trajectory.getDestination());
 
-    if (distance_from_destination.length() < LINEAR_PURE_PID_THRESHOLD_METERS)
-    {
-        // if target destination is close enough, use pure PID for velocity
-        return Vector{
-            x_pid_close_.step(distance_from_destination.x(), delta_time.toSeconds()),
-            y_pid_close_.step(distance_from_destination.y(), delta_time.toSeconds())};
-    }
-    else
+    Vector local_velocity =
+        target_trajectory.getVelocity(elapsed_time.toSeconds()) + control_effort;
+
+    // Dampen velocity as we get closer to the destination to reduce jittering
+    if (distance_to_destination < MAX_DAMPENING_VELOCITY_DISTANCE_M)
     {
-        // feedforward trajectory velocity with small pid control effort
-        const Vector error =
-            target_trajectory.getPosition(elapsed_time.toSeconds()) - position;
-        const Vector control_effort{x_pid_.step(error.x(), delta_time.toSeconds()),
-                                    y_pid_.step(error.y(), delta_time.toSeconds())};
-        return target_trajectory.getVelocity(elapsed_time.toSeconds()) + control_effort;
+        local_velocity *= distance_to_destination / MAX_DAMPENING_VELOCITY_DISTANCE_M;
     }
+    return local_velocity;
 }
 
 void PositionController::reset()
 {
     x_pid_.reset();
     y_pid_.reset();
-    x_pid_close_.reset();
-    y_pid_close_.reset();
 }

src/software/embedded/motion_control/position_controller.h

@@ -35,11 +35,8 @@ class PositionController : public MotionController<Point, TrajectoryPath, Vector
 
    private:
     // TODO(#3737): tune constants
-    PidController x_pid_{0.8, 0.0, 0.0, 0.0};
-    PidController y_pid_{0.8, 0.0, 0.0, 0.0};
+    PidController x_pid_{0.2, 0.0, 0.0, 0.0};
+    PidController y_pid_{0.2, 0.0, 0.0, 0.0};
 
-    PidController x_pid_close_{2.0, 0.0, 0.0, 0.0};
-    PidController y_pid_close_{2.0, 0.0, 0.0, 0.0};
-
-    static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.5;
+    static constexpr double MAX_DAMPENING_VELOCITY_DISTANCE_M = 0.05;
 };