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); +}