From d13120e6520c2781f8091c6827434175e4756e0f Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Sat, 12 Sep 2026 01:09:41 -0700 Subject: [PATCH] Add opt-in per wheel acceleration limits to the ER-Force simulator Ports upstream's per wheel acceleration limit (upstream 3ad65ad6, 7388a292), which bounds how fast each wheel of a simulated robot may accelerate instead of bounding the acceleration of the robot as a whole. Upstream hardcodes their own wheel geometry and specifies the limits per wheel rotation. Ours instead takes the coupling matrix from EuclideanToWheel, so the limit reflects our drivetrain, and specifies the limits in m/s^2 at the wheel, matching motor_max_acceleration_m_per_s_2 in our robot constants. This is off by default, both in the ErForceSimulator constructor and behind --enable_wheel_acceleration_limits on er_force_simulator_main, and it composes with our existing wheel ramping rather than replacing it: ramping limits the commands we send, this limits the robot inside the physics simulation. It is off by default for a reason worth knowing before turning it on: the limit applies to whatever the simulator's internal velocity controller asks for, and that asks for far more acceleration than any robot can deliver, so the limit ends up starving whichever of translation and rotation demands less of the wheels. A robot told to drive and spin at the same time therefore barely spins. Also records in the extlib README which upstream commit this fork is synced with, and what was deliberately left unported, so the next sync is a diff away. --- src/extlibs/er_force_sim/README.md | 28 +++++++ .../er_force_sim/src/amun/simulator/BUILD | 1 + .../src/amun/simulator/simrobot.cpp | 80 ++++++++++++++++--- .../src/amun/simulator/simrobot.h | 31 +++++++ .../er_force_sim/src/protobuf/robot.proto | 17 ++++ src/software/er_force_simulator_main.cpp | 31 ++++--- .../simulation/er_force_simulator.cpp | 36 ++++++++- src/software/simulation/er_force_simulator.h | 22 ++++- .../simulation/er_force_simulator_test.cpp | 77 ++++++++++++++++++ 9 files changed, 294 insertions(+), 29 deletions(-) diff --git a/src/extlibs/er_force_sim/README.md b/src/extlibs/er_force_sim/README.md index 257a2e5dbe..cf16349f88 100644 --- a/src/extlibs/er_force_sim/README.md +++ b/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 ``` diff --git a/src/extlibs/er_force_sim/src/amun/simulator/BUILD b/src/extlibs/er_force_sim/src/amun/simulator/BUILD index c017bc35f9..1a7e7cf73d 100644 --- a/src/extlibs/er_force_sim/src/amun/simulator/BUILD +++ b/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, ) diff --git a/src/extlibs/er_force_sim/src/amun/simulator/simrobot.cpp b/src/extlibs/er_force_sim/src/amun/simulator/simrobot.cpp index 698b4f0556..503e84a74c 100644 --- a/src/extlibs/er_force_sim/src/amun/simulator/simrobot.cpp +++ b/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 diff --git a/src/extlibs/er_force_sim/src/amun/simulator/simrobot.h b/src/extlibs/er_force_sim/src/amun/simulator/simrobot.h index 196773fdf2..10ea5c816d 100644 --- a/src/extlibs/er_force_sim/src/amun/simulator/simrobot.h +++ b/src/extlibs/er_force_sim/src/amun/simulator/simrobot.h @@ -24,6 +24,9 @@ #include #include +#include +#include + #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 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 m_velocityCoupling; + Eigen::CompleteOrthogonalDecomposition> m_inverseCoupling; + int64_t m_lastSendTime = 0; }; diff --git a/src/extlibs/er_force_sim/src/protobuf/robot.proto b/src/extlibs/er_force_sim/src/protobuf/robot.proto index a002ee468a..8e2c12f1e9 100644 --- a/src/extlibs/er_force_sim/src/protobuf/robot.proto +++ b/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 diff --git a/src/software/er_force_simulator_main.cpp b/src/software/er_force_simulator_main.cpp index 51c5bb2ea9..e444664e7b 100644 --- a/src/software/er_force_simulator_main.cpp +++ b/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( - TbotsProto::FieldType::DIV_A, robot_constants::createRobotConstants(), - realism_config); - } - else - { - er_force_sim = std::make_shared( - 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( + field_type, robot_constants::createRobotConstants(), realism_config, + /*ramping=*/true, args.enable_wheel_acceleration_limits); std::mutex simulator_mutex; diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index a113dd10dc..bb578cea03 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/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& realism_config, - const bool ramping) + const bool ramping, + const bool wheel_acceleration_limits) : yellow_team_world_msg(std::make_unique()), blue_team_world_msg(std::make_unique()), 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(-left_column[wheel])); + limits->add_wheel_velocity_coupling(static_cast(forward_column[wheel])); + limits->add_wheel_velocity_coupling(static_cast(angular_column[wheel])); + } +} + void ErForceSimulator::setYellowRobotPrimitiveSet( const TbotsProto::PrimitiveSet& primitive_set_msg, std::unique_ptr world_msg) diff --git a/src/software/simulation/er_force_simulator.h b/src/software/simulation/er_force_simulator.h index 6660e2685c..03c0a2d2d5 100644 --- a/src/software/simulation/er_force_simulator.h +++ b/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& 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 blue_robot_with_ball; std::optional yellow_robot_with_ball; bool ramping; + bool wheel_acceleration_limits; struct LocalVelocity { diff --git a/src/software/simulation/er_force_simulator_test.cpp b/src/software/simulation/er_force_simulator_test.cpp index e2c76289a7..a0c460b4c0 100644 --- a/src/software/simulation/er_force_simulator_test.cpp +++ b/src/software/simulation/er_force_simulator_test.cpp @@ -701,3 +701,80 @@ TEST_F(ErForceSimulatorTest, corner_blocks_keep_the_ball_out_of_the_field_corner // keeps its own radius of distance from each of the two walls EXPECT_GT(closest_approach_to_corner, CORNER_BLOCK_CATHETUS_METERS * 0.75); } + +class ErForceSimulatorWheelLimitTest : public ::testing::Test +{ + protected: + // Creates a simulator with a single yellow robot at the center of the field, either + // with or without per wheel acceleration limits + void createSimulator(bool wheel_acceleration_limits) + { + auto realism_config = ErForceSimulator::createDefaultRealismConfig(); + simulator = std::make_shared( + TbotsProto::FieldType::DIV_B, robot_constants, realism_config, + /*ramping=*/false, wheel_acceleration_limits); + simulator->resetCurrentTime(); + simulator->setYellowRobots({RobotStateWithId{ + .id = 0, + .robot_state = RobotState(Point(0, 0), Vector(0, 0), Angle::zero(), + AngularVelocity::zero())}}); + } + + // Drives the robot with the given local velocity command and returns the speed it + // reaches after a short time + double reachedSpeed(const Vector& velocity, const AngularVelocity& angular_velocity) + { + TbotsProto::PrimitiveSet primitive_set; + (*primitive_set.mutable_robot_primitives())[0] = *createDirectControlPrimitive( + velocity, angular_velocity, /*dribbler_rpm=*/0, TbotsProto::AutoChipOrKick()); + for (unsigned int step = 0; step < 10; step++) + { + simulator->setYellowRobotPrimitiveSet(primitive_set, + std::make_unique()); + simulator->stepSimulation(Duration::fromMilliseconds(5)); + } + + const auto& robot = simulator->getSimulatorState().yellow_robots(0); + return Vector(robot.v_x(), robot.v_y()).length(); + } + + std::shared_ptr simulator; + robot_constants::RobotConstants robot_constants = + robot_constants::createRobotConstants(); +}; + +TEST_F(ErForceSimulatorWheelLimitTest, robots_accelerate_as_before_when_limits_are_off) +{ + const Vector target_velocity(2.0, 0); + + createSimulator(/*wheel_acceleration_limits=*/false); + const double speed_without_limits = + reachedSpeed(target_velocity, AngularVelocity::zero()); + + createSimulator(/*wheel_acceleration_limits=*/true); + const double speed_with_limits = + reachedSpeed(target_velocity, AngularVelocity::zero()); + + // Driving straight ahead the wheels of our robots are the limiting factor well + // before the robot as a whole is, so the limits slow the robot down + EXPECT_GT(speed_without_limits, 0.1); + EXPECT_LT(speed_with_limits, speed_without_limits); +} + +TEST_F(ErForceSimulatorWheelLimitTest, acceleration_depends_on_the_driving_direction) +{ + // Driving diagonally puts more of the load on a single wheel than driving straight + // ahead does, so a robot whose wheels limit its acceleration reaches a lower speed + // in the same time, even though both commands ask for the same speed + constexpr double TARGET_SPEED_METERS_PER_SECOND = 2.0; + const Vector straight(TARGET_SPEED_METERS_PER_SECOND, 0); + const Vector diagonal = straight.rotate(Angle::fromDegrees(45)); + + createSimulator(/*wheel_acceleration_limits=*/true); + const double straight_speed = reachedSpeed(straight, AngularVelocity::zero()); + + createSimulator(/*wheel_acceleration_limits=*/true); + const double diagonal_speed = reachedSpeed(diagonal, AngularVelocity::zero()); + + EXPECT_LT(diagonal_speed, straight_speed); +}