Introduce Wheel Ramping in Simulation

src/shared/robot_constants.cpp

@@ -30,7 +30,7 @@ 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.5f,
 
         // Robot's linear movement constants
         .robot_max_speed_m_per_s            = 3.0f,
@@ -57,7 +57,7 @@ RobotConstants createRobotConstants()
         .kalman_motor_sensor_noise_variance_rad_per_s_2 = 0.5f};
 }
 #elif CHECK_VERSION(2021)
-RobotConstants createRobotConstants()
+constexpr RobotConstants createRobotConstants()
 {
     return {
         .robot_radius_m        = static_cast<float>(ROBOT_MAX_RADIUS_METERS),

src/software/ai/hl/stp/tactic/crease_defender/crease_defender_tactic_test.py

@@ -170,7 +170,7 @@ def setup(*args):
         [
             RobotEventuallyEntersRegion(regions=[defender_regions[region_index]]),
             DelayValidation(
-                delay_s=3,
+                delay_s=4,
                 validation=RobotEventuallyEntersRegion(
                     regions=[defender_regions[region_index]]
                 ),
@@ -182,7 +182,7 @@ def setup(*args):
         setup=setup,
         inv_eventually_validation_sequence_set=eventually_validations,
         ag_eventually_validation_sequence_set=eventually_validations,
-        test_timeout_s=5,
+        test_timeout_s=7,
     )
 
 

src/software/ai/hl/stp/tactic/dribble/dribble_tactic_test.py

@@ -336,6 +336,7 @@ def setup(*args):
         inv_always_validation_sequence_set=always_validation_sequence_set,
         ag_eventually_validation_sequence_set=eventually_validation_sequence_set,
         ag_always_validation_sequence_set=always_validation_sequence_set,
+        test_timeout_s=5,
     )
 
 
@@ -422,7 +423,7 @@ def setup(*args):
             RobotEventuallyReceivedBall(1),
             BallEventuallyEntersRegion([tbots_cpp.Circle(dribble_destination, 0.3)]),
             RobotEventuallyAtOrientation(1, dribble_orientation),
-            DelayValidation(delay_s=2, validation=RobotEventuallyReceivedBall(1)),
+            DelayValidation(delay_s=3, validation=RobotEventuallyReceivedBall(1)),
         ]
     ]
 
@@ -433,7 +434,7 @@ def setup(*args):
         setup=setup,
         inv_eventually_validation_sequence_set=eventually_validations,
         ag_eventually_validation_sequence_set=eventually_validations,
-        test_timeout_s=10,
+        test_timeout_s=12,
     )
 
 

src/software/ai/hl/stp/tactic/receiver/receiver_tactic_test.py

@@ -236,6 +236,7 @@ def setup(*args):
         setup=setup,
         inv_eventually_validation_sequence_set=eventually_validation_sequence_set,
         ag_eventually_validation_sequence_set=eventually_validation_sequence_set,
+        test_timeout_s=4,
         run_till_end=False,
     )
 

src/software/simulation/BUILD

@@ -31,6 +31,7 @@ cc_test(
         "//proto/message_translation:er_force_world",
         "//proto/primitive:primitive_msg_factory",
         "//shared/test_util:tbots_gtest_main",
+        "//software/physics:euclidean_to_wheel",
         "//software/test_util",
         "//software/world",
     ],

src/software/simulation/er_force_simulator.cpp

@@ -383,10 +383,37 @@ SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots(
         {
             auto direct_control_no_ramp =
                 primitive_executor->stepPrimitive(status, primitive_executor_time_step);
-            const auto& robot_state = robot_map.at(robot_id);
-            direct_control          = getRampedVelocityPrimitive(
-                         robot_state.localVelocity(), robot_state.angularVelocity(),
-                         *direct_control_no_ramp, primitive_executor_time_step);
+
+            auto* prev_ramp_velocities = &yellow_prev_ramp_velocities;
+            if (side == gameController::Team::BLUE)
+            {
+                prev_ramp_velocities = &blue_prev_ramp_velocities;
+            }
+            auto prev_it = prev_ramp_velocities->find(robot_id);
+            if (prev_it == prev_ramp_velocities->end())
+            {
+                LocalVelocity seed{Vector(0, 0), AngularVelocity::zero()};
+                auto robot_state_it = robot_map.find(robot_id);
+                if (robot_state_it != robot_map.end())
+                {
+                    seed = LocalVelocity{robot_state_it->second.localVelocity(),
+                                         robot_state_it->second.angularVelocity()};
+                }
+                prev_it = prev_ramp_velocities->insert({robot_id, seed}).first;
+            }
+
+            direct_control = getRampedVelocityPrimitive(
+                prev_it->second.linear, prev_it->second.angular, *direct_control_no_ramp,
+                primitive_executor_time_step);
+
+            // Persist the ramped command as the setpoint to ramp from next tick.
+            const auto& ramped =
+                direct_control->motor_control().direct_velocity_control();
+            prev_it->second =
+                LocalVelocity{Vector(ramped.velocity().x_component_meters(),
+                                     ramped.velocity().y_component_meters()),
+                              AngularVelocity::fromRadians(
+                                  ramped.angular_velocity().radians_per_second())};
         }
         else
         {
@@ -412,16 +439,16 @@ ErForceSimulator::getRampedVelocityPrimitive(
 
     // getting the target wheel velocity
     EuclideanSpace_t target_euclidean_velocity = {
-        -direct_velocity.velocity().y_component_meters(),
         direct_velocity.velocity().x_component_meters(),
+        direct_velocity.velocity().y_component_meters(),
         direct_velocity.angular_velocity().radians_per_second()};
 
     WheelSpace_t target_wheel_velocity =
         euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity);
 
     // getting the current wheel velocity
     EuclideanSpace_t current_euclidean_velocity = {
-        -current_local_velocity.y(), current_local_velocity.x(),
+        current_local_velocity.x(), current_local_velocity.y(),
         current_local_angular_velocity.toRadians()};
 
     WheelSpace_t current_wheel_velocity =
@@ -436,7 +463,7 @@ ErForceSimulator::getRampedVelocityPrimitive(
     auto mutable_direct_velocity = target_velocity_primitive.mutable_motor_control()
                                        ->mutable_direct_velocity_control();
     *(mutable_direct_velocity->mutable_velocity()) =
-        *createVectorProto({ramped_euclidean[1], -ramped_euclidean[0]});
+        *createVectorProto({ramped_euclidean[0], ramped_euclidean[1]});
     *(mutable_direct_velocity->mutable_angular_velocity()) =
         *createAngularVelocityProto(AngularVelocity::fromRadians(ramped_euclidean[2]));
 

src/software/simulation/er_force_simulator.h

@@ -31,7 +31,7 @@ class ErForceSimulator
     explicit ErForceSimulator(
         const TbotsProto::FieldType& field_type,
         const robot_constants::RobotConstants& robot_constants,
-        std::unique_ptr<RealismConfigErForce>& realism_config, const bool ramping = false,
+        std::unique_ptr<RealismConfigErForce>& realism_config, const bool ramping = true,
         Duration primitive_executor_time_step_s =
             Duration::fromSeconds(DEFAULT_SIMULATOR_TICK_RATE_SECONDS_PER_TICK));
     ErForceSimulator()  = delete;
@@ -144,6 +144,9 @@ class ErForceSimulator
     static std::unique_ptr<RealismConfigErForce> createRealisticRealismConfig();
 
    private:
+    // Grants the ramping unit test access to the private getRampedVelocityPrimitive()
+    friend class ErForceSimulatorRampingTest;
+
     /**
      * Sets the primitive being simulated by the robot in simulation
      *
@@ -232,6 +235,18 @@ class ErForceSimulator
 
     bool ramping;
 
+    struct LocalVelocity
+    {
+        Vector linear;
+        AngularVelocity angular;
+    };
+
+    // The previously commanded velocity for each robot, kept per team. When ramping is
+    // enabled the wheel velocities are ramped open-loop from these setpoints, mirroring
+    // the real motor service
+    std::unordered_map<RobotId, LocalVelocity> blue_prev_ramp_velocities;
+    std::unordered_map<RobotId, LocalVelocity> yellow_prev_ramp_velocities;
+
     const std::string CONFIG_FILE      = "simulator/2020";
     const std::string CONFIG_DIRECTORY = "extlibs/er_force_sim/config/";
 };

src/software/simulation/er_force_simulator_test.cpp

@@ -6,6 +6,8 @@
 #include "proto/message_translation/tbots_protobuf.h"
 #include "proto/primitive/primitive_msg_factory.h"
 #include "shared/robot_constants.h"
+#include "software/geom/vector.h"
+#include "software/physics/euclidean_to_wheel.h"
 #include "software/test_util/test_util.h"
 
 class ErForceSimulatorTest : public ::testing::Test
@@ -329,3 +331,126 @@ TEST(ErForceSimulatorFieldTest, check_field_B_configuration)
 
     EXPECT_EQ(simulator->getField(), Field::createSSLDivisionBField());
 }
+
+class ErForceSimulatorRampingTest : public ::testing::Test
+{
+   protected:
+    void SetUp() override
+    {
+        auto realism_config = ErForceSimulator::createDefaultRealismConfig();
+        simulator = std::make_shared<ErForceSimulator>(TbotsProto::FieldType::DIV_B,
+                                                       robot_constants, realism_config,
+                                                       /*ramping=*/true);
+    }
+
+    // Forwarding wrapper so TEST_F bodies (which derive from this fixture) can reach the
+    // private method through this friend class.
+    std::unique_ptr<TbotsProto::DirectControlPrimitive> rampVelocityPrimitive(
+        const Vector& current_local_velocity,
+        const AngularVelocity& current_local_angular_velocity,
+        TbotsProto::DirectControlPrimitive& target_velocity_primitive,
+        Duration time_to_ramp)
+    {
+        return simulator->getRampedVelocityPrimitive(
+            current_local_velocity, current_local_angular_velocity,
+            target_velocity_primitive, time_to_ramp);
+    }
+
+    // Builds a direct-velocity-control primitive with the given local target velocity.
+    static TbotsProto::DirectControlPrimitive makeTargetPrimitive(
+        const Vector& velocity, const AngularVelocity& angular_velocity)
+    {
+        return createDirectControlPrimitive(velocity, angular_velocity,
+                                            /*dribbler_rpm=*/0,
+                                            TbotsProto::AutoChipOrKick())
+            ->direct_control();
+    }
+
+    std::shared_ptr<ErForceSimulator> simulator;
+    robot_constants::RobotConstants robot_constants =
+        robot_constants::createRobotConstants();
+};
+
+TEST_F(ErForceSimulatorRampingTest, passes_target_through_when_within_acceleration_limit)
+{
+    const Vector target_velocity(0.5, -0.3);
+    const AngularVelocity target_angular = AngularVelocity::fromRadians(0.2);
+
+    auto target_primitive = makeTargetPrimitive(target_velocity, target_angular);
+
+    // Start from rest, but allow a large ramp window so nothing clips.
+    auto ramped = rampVelocityPrimitive(Vector(0, 0), AngularVelocity::zero(),
+                                        target_primitive, Duration::fromSeconds(10.0));
+
+    const auto& velocity = ramped->motor_control().direct_velocity_control().velocity();
+    EXPECT_NEAR(velocity.x_component_meters(), target_velocity.x(), 1e-9);
+    EXPECT_NEAR(velocity.y_component_meters(), target_velocity.y(), 1e-9);
+    EXPECT_NEAR(ramped->motor_control()
+                    .direct_velocity_control()
+                    .angular_velocity()
+                    .radians_per_second(),
+                target_angular.toRadians(), 1e-9);
+}
+
+TEST_F(ErForceSimulatorRampingTest, holds_velocity_when_already_at_target)
+{
+    const Vector velocity(1.0, 0.5);
+    const AngularVelocity angular = AngularVelocity::fromRadians(0.4);
+
+    auto target_primitive = makeTargetPrimitive(velocity, angular);
+
+    auto ramped = rampVelocityPrimitive(velocity, angular, target_primitive,
+                                        Duration::fromSeconds(0.001));
+
+    const auto& out = ramped->motor_control().direct_velocity_control().velocity();
+    EXPECT_NEAR(out.x_component_meters(), velocity.x(), 1e-9);
+    EXPECT_NEAR(out.y_component_meters(), velocity.y(), 1e-9);
+}
+
+TEST_F(ErForceSimulatorRampingTest, ramps_in_motor_service_frame_when_clipping)
+{
+    const Vector current_velocity(0.0, 0.0);
+    const AngularVelocity current_angular = AngularVelocity::zero();
+    const Vector target_velocity(3.0, 0.5);
+    const AngularVelocity target_angular = AngularVelocity::fromRadians(1.0);
+    // Tiny timestep forces the acceleration limit to clip hard.
+    const Duration time_to_ramp = Duration::fromSeconds(0.01);
+
+    EuclideanToWheel euclidean_to_wheel(robot_constants);
+
+    EuclideanSpace_t current_euclidean{current_velocity.x(), current_velocity.y(),
+                                       current_angular.toRadians()};
+    EuclideanSpace_t target_euclidean{target_velocity.x(), target_velocity.y(),
+                                      target_angular.toRadians()};
+    WheelSpace_t ramped_wheel = euclidean_to_wheel.rampWheelVelocity(
+        euclidean_to_wheel.getWheelVelocity(current_euclidean),
+        euclidean_to_wheel.getWheelVelocity(target_euclidean), time_to_ramp.toSeconds());
+    EuclideanSpace_t expected = euclidean_to_wheel.getEuclideanVelocity(ramped_wheel);
+
+    ASSERT_LT(expected[0], target_velocity.x());
+
+    auto target_primitive = makeTargetPrimitive(target_velocity, target_angular);
+    auto ramped           = rampVelocityPrimitive(current_velocity, current_angular,
+                                                  target_primitive, time_to_ramp);
+
+    const auto& velocity = ramped->motor_control().direct_velocity_control().velocity();
+    EXPECT_NEAR(velocity.x_component_meters(), expected[0], 1e-9);
+    EXPECT_NEAR(velocity.y_component_meters(), expected[1], 1e-9);
+    EXPECT_NEAR(ramped->motor_control()
+                    .direct_velocity_control()
+                    .angular_velocity()
+                    .radians_per_second(),
+                expected[2], 1e-9);
+
+    EuclideanSpace_t rotated_current{-current_velocity.y(), current_velocity.x(),
+                                     current_angular.toRadians()};
+    EuclideanSpace_t rotated_target{-target_velocity.y(), target_velocity.x(),
+                                    target_angular.toRadians()};
+    EuclideanSpace_t rotated_ramped =
+        euclidean_to_wheel.getEuclideanVelocity(euclidean_to_wheel.rampWheelVelocity(
+            euclidean_to_wheel.getWheelVelocity(rotated_current),
+            euclidean_to_wheel.getWheelVelocity(rotated_target),
+            time_to_ramp.toSeconds()));
+
+    EXPECT_GT(std::abs(rotated_ramped[1] - expected[0]), 1e-3);
+}