Add opt-in per wheel acceleration limits to the ER-Force simulator

src/extlibs/er_force_sim/README.md

@@ -3,6 +3,34 @@
 The simulator is adapted from [robotics-erlangen/framework](https://github.com/robotics-erlangen/framework). It contains several bug fixes and general code quality improvements, and it is modified so that time steps can be controlled by a `stepSimulation` function.
 
 
+## Upstream sync
+
+This is a fork, not a vendored copy. It was forked at upstream `73e139db` (2021-12-16)
+and individual upstream changes have been ported onto it by hand since, because the fork
+has diverged too far for cherry-picks to apply: Qt was removed (upstream is on Qt6), the
+API was made synchronous for our deterministic test runner, and the ball model was
+replaced with our own.
+
+Last sync: upstream `38563d11` (2026-07-27), covering every change to
+`src/amun/simulator` up to that commit.
+
+To find what is new upstream since then:
+
+```
+git clone https://github.com/robotics-erlangen/framework
+git -C framework log 38563d11..HEAD -- src/amun/simulator src/protobuf/protobuf/ssl_sim
+```
+
+Deliberately not ported:
+
+- the Qt6 migration, the protobuf folder reorganisation and compiler warning fixes,
+  which conflict with our own versions of the same code
+- `fastsimulator` and `erroraggregator`, which serve upstream's own tooling
+- `Specs.simulation_limits` in the upstream unit of wheel rotations; ours uses linear
+  units, matching our robot constants
+- everything outside of the simulator (Ra, strategy, tracking, logging)
+
+
 ## Copyright
 
 ```

src/extlibs/er_force_sim/src/amun/simulator/BUILD

@@ -12,6 +12,7 @@ cc_library(
         "//proto:ssl_simulation_cc_proto",
         "//shared:constants",
         "@bullet",
+        "@eigen",
     ],
     alwayslink = True,
 )

src/extlibs/er_force_sim/src/amun/simulator/simrobot.cpp

@@ -128,6 +128,8 @@ SimRobot::SimRobot(const robot::Specs& specs,
 
     m_shapes.push_back(std::move(wholeShape));
     m_shapes.push_back(std::move(dribblerShape));
+
+    generateVelocityCoupling();
 }
 
 SimRobot::~SimRobot()
@@ -506,16 +508,11 @@ void SimRobot::begin(SimBall& ball, double time)
     // as a certain part of the acceleration is required to compensate damping,
     // the robot will run into a speed limit! bound acceleration the speed limit
     // is acceleration * accelScale / V
-    float a_f = V * v_f + K * error_v_f + K_I * m_error_sum_v_f;
-    float a_s = V * v_s + K * error_v_s + K_I * m_error_sum_v_s;
+    const float a_f = V * v_f + K * error_v_f + K_I * m_error_sum_v_f;
+    const float a_s = V * v_s + K * error_v_s + K_I * m_error_sum_v_s;
 
     const float accelScale =
         2.f;  // let robot accelerate / brake faster than the accelerator does
-    a_f = bound(a_f, v_f, accelScale * m_specs.strategy().a_speedup_f_max(),
-                accelScale * m_specs.strategy().a_brake_f_max());
-    a_s = bound(a_s, v_s, accelScale * m_specs.strategy().a_speedup_s_max(),
-                accelScale * m_specs.strategy().a_brake_s_max());
-    const btVector3 force(a_s * m_specs.mass(), a_f * m_specs.mass(), 0);
 
     // localInertia.z() / SIMULATOR_SCALE^2 \approx
     // 1/12*mass*(robot_width^2+robot_depth^2)
@@ -532,9 +529,29 @@ void SimRobot::begin(SimBall& ball, double time)
     // the forward acceleration into rotational acceleration
     const float a_phi_with_error = a_phi + m_rotationError * a_f;
 
-    const float a_phi_bound = bound(a_phi_with_error, omega,
-                                    accelScale * m_specs.strategy().a_speedup_phi_max(),
-                                    accelScale * m_specs.strategy().a_brake_phi_max());
+    float a_f_bound, a_s_bound, a_phi_bound;
+    if (m_limitWheelAcceleration)
+    {
+        // Limit the acceleration of every wheel individually, which for example makes
+        // the robot accelerate slower diagonally than straight ahead
+        const Eigen::Vector3f limited =
+            limitAcceleration(a_f, a_s, a_phi_with_error, v_f, v_s, omega);
+        a_s_bound   = limited[0];
+        a_f_bound   = limited[1];
+        a_phi_bound = limited[2];
+    }
+    else
+    {
+        a_f_bound   = bound(a_f, v_f, accelScale * m_specs.strategy().a_speedup_f_max(),
+                            accelScale * m_specs.strategy().a_brake_f_max());
+        a_s_bound   = bound(a_s, v_s, accelScale * m_specs.strategy().a_speedup_s_max(),
+                            accelScale * m_specs.strategy().a_brake_s_max());
+        a_phi_bound = bound(a_phi_with_error, omega,
+                            accelScale * m_specs.strategy().a_speedup_phi_max(),
+                            accelScale * m_specs.strategy().a_brake_phi_max());
+    }
+
+    const btVector3 force(a_s_bound * m_specs.mass(), a_f_bound * m_specs.mass(), 0);
     const btVector3 torque(0, 0, a_phi_bound * 0.007884f);
 
     if (force.length2() > 0 || torque.length2() > 0)
@@ -545,6 +562,49 @@ void SimRobot::begin(SimBall& ball, double time)
     }
 }
 
+void SimRobot::generateVelocityCoupling()
+{
+    const auto& limits = m_specs.simulation_limits();
+    if (limits.wheel_velocity_coupling_size() !=
+        m_velocityCoupling.rows() * m_velocityCoupling.cols())
+    {
+        m_limitWheelAcceleration = false;
+        return;
+    }
+
+    for (int row = 0; row < m_velocityCoupling.rows(); row++)
+    {
+        for (int col = 0; col < m_velocityCoupling.cols(); col++)
+        {
+            m_velocityCoupling(row, col) =
+                limits.wheel_velocity_coupling(row * m_velocityCoupling.cols() + col);
+        }
+    }
+
+    m_inverseCoupling        = m_velocityCoupling.completeOrthogonalDecomposition();
+    m_limitWheelAcceleration = true;
+}
+
+Eigen::Vector3f SimRobot::limitAcceleration(float a_f, float a_s, float a_phi, float v_f,
+                                            float v_s, float omega) const
+{
+    const float wheelAccel = m_specs.simulation_limits().a_speedup_wheel_max();
+    const float wheelDecel = m_specs.simulation_limits().a_brake_wheel_max();
+
+    const Eigen::Vector3f speed{v_s, v_f, omega};
+    const Eigen::Vector4f wheelSpeed = m_velocityCoupling * speed;
+
+    const Eigen::Vector3f acceleration{a_s, a_f, a_phi};
+    Eigen::Vector4f limitedWheelAcceleration = m_velocityCoupling * acceleration;
+    for (int i = 0; i < limitedWheelAcceleration.size(); i++)
+    {
+        limitedWheelAcceleration[i] =
+            bound(limitedWheelAcceleration[i], wheelSpeed[i], wheelAccel, wheelDecel);
+    }
+
+    return m_inverseCoupling.solve(limitedWheelAcceleration);
+}
+
 // copy-paste from accelerator
 float SimRobot::bound(float acceleration, float oldSpeed, float speedupLimit,
                       float brakeLimit) const

src/extlibs/er_force_sim/src/amun/simulator/simrobot.h

@@ -24,6 +24,9 @@
 #include <BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h>
 #include <btBulletDynamicsCommon.h>
 
+#include <Eigen/Dense>
+#include <Eigen/QR>
+
 #include "extlibs/er_force_sim/src/core/rng.h"
 #include "extlibs/er_force_sim/src/protobuf/command.pb.h"
 #include "extlibs/er_force_sim/src/protobuf/robot.pb.h"
@@ -115,6 +118,28 @@ class camun::simulator::SimRobot
                                const btVector3 linVel, float omega);
     void dribble(const SimBall& ball, float speed);
 
+    /**
+     * Builds the matrix that maps the robot's local velocity to the speed of each of
+     * its wheels, and its inverse, from the robot's simulation limits
+     */
+    void generateVelocityCoupling();
+
+    /**
+     * Limits the given acceleration so that no single wheel accelerates faster than
+     * the robot's simulation limits allow
+     *
+     * @param a_f the forward acceleration
+     * @param a_s the sideways acceleration
+     * @param a_phi the rotational acceleration
+     * @param v_f the current forward velocity
+     * @param v_s the current sideways velocity
+     * @param omega the current rotational velocity
+     *
+     * @return the bounded {a_s, a_f, a_phi}
+     */
+    Eigen::Vector3f limitAcceleration(float a_f, float a_s, float a_phi, float v_f,
+                                      float v_s, float omega) const;
+
     RNG m_rng;
     robot::Specs m_specs;
     std::shared_ptr<btDiscreteDynamicsWorld> m_world;
@@ -143,6 +168,12 @@ class camun::simulator::SimRobot
     bool m_perfectDribbler = false;
     float m_rotationError  = 0.0f;
 
+    // Whether this robot limits the acceleration of each of its wheels individually,
+    // rather than just its acceleration as a whole
+    bool m_limitWheelAcceleration = false;
+    Eigen::Matrix<float, 4, 3> m_velocityCoupling;
+    Eigen::CompleteOrthogonalDecomposition<Eigen::Matrix<float, 4, 3>> m_inverseCoupling;
+
     int64_t m_lastSendTime = 0;
 };
 

src/extlibs/er_force_sim/src/protobuf/robot.proto

@@ -13,6 +13,20 @@ message LimitParameters
     optional float a_brake_phi_max   = 6;
 };
 
+message SimulationLimits
+{
+    // Limits the maximum acceleration of every wheel individually in the simulator,
+    // measured at the contact point of the wheel with the field [m/s^2].
+    // NOTE: upstream ER-Force specifies these per wheel rotation [rot/s^2]. We use
+    // linear units instead, because that is what our robot constants are given in.
+    optional float a_speedup_wheel_max = 1;
+    optional float a_brake_wheel_max   = 2;
+    // Row major 4x3 matrix that maps the robot's local velocity (v_s, v_f, omega),
+    // where v_s points to the right of the robot and v_f forwards, to the speed of
+    // each of its four wheels. The limits above are only applied if this is set.
+    repeated float wheel_velocity_coupling = 3;
+};
+
 message Specs
 {
     enum GenerationType
@@ -40,6 +54,9 @@ message Specs
     optional float shoot_radius    = 17;
     optional float dribbler_height = 18;
     reserved 20;
+    // If unset, the robot's acceleration is only limited by the 'acceleration' limits
+    // above (with an additional acceleration factor)
+    optional SimulationLimits simulation_limits = 21;
 };
 
 message Generation

src/software/er_force_simulator_main.cpp

@@ -15,10 +15,11 @@ int main(int argc, char** argv)
 {
     struct CommandLineArgs
     {
-        bool help               = false;
-        std::string runtime_dir = "/tmp/tbots";
-        std::string division    = "div_b";
-        bool enable_realism     = false;  // realism flag
+        bool help                             = false;
+        std::string runtime_dir               = "/tmp/tbots";
+        std::string division                  = "div_b";
+        bool enable_realism                   = false;  // realism flag
+        bool enable_wheel_acceleration_limits = false;
     };
 
     CommandLineArgs args;
@@ -35,6 +36,10 @@ int main(int argc, char** argv)
     desc.add_options()("enable_realism",
                        boost::program_options::bool_switch(&args.enable_realism),
                        "realism simulator");  // install terminal flag
+    desc.add_options()(
+        "enable_wheel_acceleration_limits",
+        boost::program_options::bool_switch(&args.enable_wheel_acceleration_limits),
+        "limit how fast each wheel of a simulated robot may accelerate");
 
     boost::program_options::variables_map vm;
     boost::program_options::store(parse_command_line(argc, argv, desc), vm);
@@ -85,18 +90,12 @@ int main(int argc, char** argv)
             realism_config = ErForceSimulator::createDefaultRealismConfig();
         }
 
-        if (args.division == "div_a")
-        {
-            er_force_sim = std::make_shared<ErForceSimulator>(
-                TbotsProto::FieldType::DIV_A, robot_constants::createRobotConstants(),
-                realism_config);
-        }
-        else
-        {
-            er_force_sim = std::make_shared<ErForceSimulator>(
-                TbotsProto::FieldType::DIV_B, robot_constants::createRobotConstants(),
-                realism_config);
-        }
+        const TbotsProto::FieldType field_type = args.division == "div_a"
+                                                     ? TbotsProto::FieldType::DIV_A
+                                                     : TbotsProto::FieldType::DIV_B;
+        er_force_sim                           = std::make_shared<ErForceSimulator>(
+            field_type, robot_constants::createRobotConstants(), realism_config,
+            /*ramping=*/true, args.enable_wheel_acceleration_limits);
 
         std::mutex simulator_mutex;
 

src/software/simulation/er_force_simulator.cpp

@@ -19,7 +19,8 @@
 ErForceSimulator::ErForceSimulator(const TbotsProto::FieldType& field_type,
                                    const robot_constants::RobotConstants& robot_constants,
                                    std::unique_ptr<RealismConfigErForce>& realism_config,
-                                   const bool ramping)
+                                   const bool ramping,
+                                   const bool wheel_acceleration_limits)
     : yellow_team_world_msg(std::make_unique<TbotsProto::World>()),
       blue_team_world_msg(std::make_unique<TbotsProto::World>()),
       frame_number(0),
@@ -28,7 +29,8 @@ ErForceSimulator::ErForceSimulator(const TbotsProto::FieldType& field_type,
       field(Field::createField(field_type)),
       blue_robot_with_ball(std::nullopt),
       yellow_robot_with_ball(std::nullopt),
-      ramping(ramping)
+      ramping(ramping),
+      wheel_acceleration_limits(wheel_acceleration_limits)
 {
     std::string full_filename = CONFIG_DIRECTORY;
 
@@ -208,6 +210,7 @@ void ErForceSimulator::setRobots(
 
     robot::Specs ERForce;
     robotSetDefault(&ERForce);
+    addSimulationLimits(ERForce);
 
     // Initialize Team Robots at the bottom of the field
     ::robot::Team* team;
@@ -307,6 +310,35 @@ void ErForceSimulator::setRobots(
     }
 }
 
+void ErForceSimulator::addSimulationLimits(robot::Specs& specs) const
+{
+    if (!wheel_acceleration_limits)
+    {
+        return;
+    }
+
+    auto* limits = specs.mutable_simulation_limits();
+    limits->set_a_speedup_wheel_max(robot_constants.motor_max_acceleration_m_per_s_2);
+    limits->set_a_brake_wheel_max(robot_constants.motor_max_acceleration_m_per_s_2);
+
+    // The simulator expects the coupling matrix in its own local frame, whose first
+    // axis points to the right of the robot and whose second axis points forwards,
+    // while ours has the first axis pointing forwards and the second one to the left.
+    const WheelSpace_t forward_column =
+        euclidean_to_four_wheel.getWheelVelocity(EuclideanSpace_t{1, 0, 0});
+    const WheelSpace_t left_column =
+        euclidean_to_four_wheel.getWheelVelocity(EuclideanSpace_t{0, 1, 0});
+    const WheelSpace_t angular_column =
+        euclidean_to_four_wheel.getWheelVelocity(EuclideanSpace_t{0, 0, 1});
+
+    for (Eigen::Index wheel = 0; wheel < forward_column.size(); wheel++)
+    {
+        limits->add_wheel_velocity_coupling(static_cast<float>(-left_column[wheel]));
+        limits->add_wheel_velocity_coupling(static_cast<float>(forward_column[wheel]));
+        limits->add_wheel_velocity_coupling(static_cast<float>(angular_column[wheel]));
+    }
+}
+
 void ErForceSimulator::setYellowRobotPrimitiveSet(
     const TbotsProto::PrimitiveSet& primitive_set_msg,
     std::unique_ptr<TbotsProto::World> world_msg)

src/software/simulation/er_force_simulator.h

@@ -27,11 +27,22 @@ class ErForceSimulator
      * @param field_type The field type
      * @param robot_constants The robot constants
      * @param realism_config realism configuration
+     * @param ramping whether to ramp the commanded wheel velocities the way the motor
+     * service does on the real robot
+     * @param wheel_acceleration_limits whether the simulated robots limit how fast each
+     * of their wheels may accelerate. Note that this limits the robots inside the
+     * physics simulation, whereas ramping limits the commands sent to them.
+     * Off by default: the limit is applied to whatever the simulator's internal velocity
+     * controller asks for, and since that asks for far more acceleration than any robot
+     * can deliver, the limit ends up starving whichever of translation and rotation
+     * demands less of the wheels. A robot that is told to drive and spin at the same
+     * time therefore barely spins.
      */
     explicit ErForceSimulator(const TbotsProto::FieldType& field_type,
                               const robot_constants::RobotConstants& robot_constants,
                               std::unique_ptr<RealismConfigErForce>& realism_config,
-                              const bool ramping = true);
+                              const bool ramping                   = true,
+                              const bool wheel_acceleration_limits = false);
     ErForceSimulator()  = delete;
     ~ErForceSimulator() = default;
 
@@ -227,10 +238,19 @@ class ErForceSimulator
     robot_constants::RobotConstants robot_constants;
     Field field;
 
+    /**
+     * Sets the per wheel acceleration limits of the given robot specs from our robot
+     * constants, so that the simulated robots accelerate like ours do
+     *
+     * @param specs the robot specs to add the limits to
+     */
+    void addSimulationLimits(robot::Specs& specs) const;
+
     std::optional<RobotId> blue_robot_with_ball;
     std::optional<RobotId> yellow_robot_with_ball;
 
     bool ramping;
+    bool wheel_acceleration_limits;
 
     struct LocalVelocity
     {