From b2346d9233ec1878a2dc053ab141a37f19782c67 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Thu, 5 Nov 2026 20:18:02 -0700 Subject: [PATCH 1/3] add corner bloc --- .../src/amun/simulator/simfield.cpp | 159 +++++++++++++++--- .../src/amun/simulator/simfield.h | 2 + .../er_force_sim/src/protobuf/world.proto | 6 + 3 files changed, 146 insertions(+), 21 deletions(-) diff --git a/src/extlibs/er_force_sim/src/amun/simulator/simfield.cpp b/src/extlibs/er_force_sim/src/amun/simulator/simfield.cpp index c69a02f0af..15417bfc00 100644 --- a/src/extlibs/er_force_sim/src/amun/simulator/simfield.cpp +++ b/src/extlibs/er_force_sim/src/amun/simulator/simfield.cpp @@ -20,28 +20,45 @@ #include "simfield.h" +#include +#include + #include "simulator.h" using namespace camun::simulator; +// the framework treats a field with both boundary widths set to zero as one that is +// played without a boundary area +static bool hasNoBoundary(const world::Geometry& geometry) +{ + return geometry.boundary_width() == 0.0 && geometry.boundary_width_goal_line() == 0.0; +} + SimField::SimField(std::shared_ptr world, const world::Geometry& geometry) : m_world(world) { - const float totalWidth = geometry.field_width() / 2.0f + geometry.boundary_width(); - const float totalHeight = geometry.field_height() / 2.0f + geometry.boundary_width(); - const float roomHeight = 8.0f; // Upper boundary of "room" that the field lives in - const float height = geometry.field_height() / 2.0f - geometry.line_width(); + const bool noBoundary = hasNoBoundary(geometry); + + const float totalWidth = geometry.field_width() / 2.0f + geometry.boundary_width(); + // the boundary behind the goal lines can differ from the one along the touch lines + const float boundaryWidthGoalLine = geometry.has_boundary_width_goal_line() + ? geometry.boundary_width_goal_line() + : geometry.boundary_width(); + const float totalHeight = geometry.field_height() / 2.0f + boundaryWidthGoalLine; + const float roomHeight = 8.0f; // Upper boundary of "room" that the field lives in + const float height = geometry.field_height() / 2.0f - geometry.line_width(); const float goalWidthHalf = geometry.goal_width() / 2.0f + geometry.goal_wall_width(); const float goalHeightHalf = geometry.goal_height() / 2.0f; const float goalDepth = geometry.goal_depth() + geometry.goal_wall_width(); - const float goalDepthHalf = goalDepth / 2.0f; - const float goalWallHalf = geometry.goal_wall_width() / 2.0f; + // goal side walls need to extend to the wall + const float goalSideWallLengthHalf = boundaryWidthGoalLine / 2.0f; + const float goalWallHalf = geometry.goal_wall_width() / 2.0f; // Collision shapes m_plane = std::make_unique(btVector3(0, 0, 1), 0); m_goalSide = std::make_unique( - btVector3(goalWallHalf, goalDepthHalf, goalHeightHalf) * SIMULATOR_SCALE); + btVector3(goalWallHalf, goalSideWallLengthHalf, goalHeightHalf) * SIMULATOR_SCALE); m_goalBack = std::make_unique( btVector3(goalWidthHalf, goalWallHalf, goalHeightHalf) * SIMULATOR_SCALE); @@ -56,15 +73,47 @@ SimField::SimField(std::shared_ptr world, btTransform(btQuaternion(btVector3(1, 0, 0), M_PI), btVector3(0, 0, roomHeight) * SIMULATOR_SCALE), 0.3, 0.35); - // Walls - addObject(m_plane.get(), - btTransform(btQuaternion(btVector3(1, 0, 0), M_PI_2), - btVector3(0, totalHeight, 0) * SIMULATOR_SCALE), - 0.3, 0.35); - addObject(m_plane.get(), - btTransform(btQuaternion(btVector3(1, 0, 0), -M_PI_2), - btVector3(0, -totalHeight, 0) * SIMULATOR_SCALE), - 0.3, 0.35); + + // Walls behind the goal lines + if (noBoundary) + { + // the walls on the goal lines must not block the goals, so they are + // built from boxes on either side of the goal instead of a single plane + const float goalLineBoundaryWidthHalf = 0.5f * (totalWidth - goalWidthHalf); + m_goalLineBoundaryShape = std::make_unique( + btVector3(goalLineBoundaryWidthHalf, 0.5f, roomHeight * 0.5f) * + SIMULATOR_SCALE); + const float shapeOffsetX = goalWidthHalf + goalLineBoundaryWidthHalf; + const btQuaternion identity = btQuaternion::getIdentity(); + for (const float side : {-1.0f, 1.0f}) + { + // the boxes extend away from the field + const btVector3 intoVoid = btVector3(0, side * 0.5f, roomHeight * 0.5f) * + SIMULATOR_SCALE; + for (const float sideX : {-1.0f, 1.0f}) + { + addObject(m_goalLineBoundaryShape.get(), + btTransform(identity, + intoVoid + btVector3(sideX * shapeOffsetX, + side * totalHeight, 0) * + SIMULATOR_SCALE), + 0.3, 0.35); + } + } + } + else + { + addObject(m_plane.get(), + btTransform(btQuaternion(btVector3(1, 0, 0), M_PI_2), + btVector3(0, totalHeight, 0) * SIMULATOR_SCALE), + 0.3, 0.35); + addObject(m_plane.get(), + btTransform(btQuaternion(btVector3(1, 0, 0), -M_PI_2), + btVector3(0, -totalHeight, 0) * SIMULATOR_SCALE), + 0.3, 0.35); + } + + // Walls along the touch lines addObject(m_plane.get(), btTransform(btQuaternion(btVector3(0, 1, 0), M_PI_2), btVector3(-totalWidth, 0, 0) * SIMULATOR_SCALE), @@ -74,24 +123,92 @@ SimField::SimField(std::shared_ptr world, btVector3(totalWidth, 0, 0) * SIMULATOR_SCALE), 0.3, 0.35); + // Corner blocks to smooth out the edges. On the actual field they are triangular + // blocks put in the corners of the field, but here they are approximated by the + // one side that is relevant for collision checking + if (geometry.has_corner_block_cathetus_length()) + { + const float cathetus = geometry.corner_block_cathetus_length(); + const float hypothenuse = std::sqrt(2 * cathetus * cathetus); + // basically the height of the triangle when looking top down onto the field + const float blockOffset = (cathetus * cathetus) / hypothenuse; + m_cornerBlockShape = std::make_unique( + btVector3(hypothenuse * 0.5f, goalWallHalf, roomHeight * 0.5f) * + SIMULATOR_SCALE); + + // The blocks are placed in order of angle. Half of them can be mirrored with a + // 180 degree rotation, so only M_PI / 4 and 3 * M_PI / 4 are listed and, if + // negativeYHalf is set, the same positions are additionally rotated + for (const bool negativeYHalf : {false, true}) + { + for (const auto& [multipleOfPi, mirrorX] : + {std::pair{0.25, false}, std::pair{0.75, true}}) + { + // for the same rotation the triangle is e.g. at the lower left corner + // of the field and at the upper left corner of the goal, so the signs + // of totalWidthOffset and goalWidthOffset are flipped + const float totalWidthOffset = mirrorX ? totalWidth : -totalWidth; + const float goalWidthOffset = mirrorX ? -goalWidthHalf : goalWidthHalf; + + const btVector3 shapeOffsetFromCorner = + btVector3(blockOffset, 0, roomHeight * 0.5f) + .rotate(btVector3(0, 0, 1), M_PI * -multipleOfPi) * + SIMULATOR_SCALE; + const btTransform baseTransform( + btQuaternion(btVector3(0, 0, 1), M_PI * multipleOfPi), + shapeOffsetFromCorner); + const btTransform flip(btQuaternion(btVector3(0, 0, 1), M_PI)); + + btTransform cornerTransform = baseTransform; + cornerTransform.getOrigin() += + btVector3(totalWidthOffset, totalHeight, 0) * SIMULATOR_SCALE; + if (negativeYHalf) + { + cornerTransform = flip * cornerTransform; + } + addObject(m_cornerBlockShape.get(), cornerTransform, 0.3, 0.35); + + // blocks around the goals are only needed when the field has a boundary + if (!noBoundary) + { + btTransform goalTransform = baseTransform; + goalTransform.getOrigin() += + btVector3(goalWidthOffset, totalHeight, 0) * SIMULATOR_SCALE; + if (negativeYHalf) + { + goalTransform = flip * goalTransform; + } + addObject(m_cornerBlockShape.get(), goalTransform, 0.3, 0.35); + } + } + } + } + // Create goals for (const float side : {-1.0f, 1.0f}) { + // with a boundary the goal stands on the outside of the line, so it is + // offset by half the line width + const float lineWidthOffset = noBoundary ? 0.0f : geometry.line_width() * 0.5f; + const float sideWallY = side * (height + goalSideWallLengthHalf + lineWidthOffset); + addObject(m_goalSide.get(), btTransform(btQuaternion::getIdentity(), - btVector3((goalWidthHalf - goalWallHalf), - side * (height + goalDepthHalf), goalHeightHalf) * + btVector3(goalWidthHalf - goalWallHalf, sideWallY, + goalHeightHalf) * SIMULATOR_SCALE), 0.3, 0.5); addObject(m_goalSide.get(), btTransform(btQuaternion::getIdentity(), - btVector3(-(goalWidthHalf - goalWallHalf), - side * (height + goalDepthHalf), goalHeightHalf) * + btVector3(-(goalWidthHalf - goalWallHalf), sideWallY, + goalHeightHalf) * SIMULATOR_SCALE), 0.3, 0.5); addObject(m_goalBack.get(), btTransform(btQuaternion::getIdentity(), - btVector3(0.0f, side * (height + goalDepth - goalWallHalf), + btVector3(0.0f, + side * (height + goalDepth - goalWallHalf + + lineWidthOffset), goalHeightHalf) * SIMULATOR_SCALE), 0.1, 0.5); diff --git a/src/extlibs/er_force_sim/src/amun/simulator/simfield.h b/src/extlibs/er_force_sim/src/amun/simulator/simfield.h index c55394bfc8..4fba914546 100644 --- a/src/extlibs/er_force_sim/src/amun/simulator/simfield.h +++ b/src/extlibs/er_force_sim/src/amun/simulator/simfield.h @@ -49,6 +49,8 @@ class camun::simulator::SimField std::unique_ptr m_plane; std::unique_ptr m_goalSide; std::unique_ptr m_goalBack; + std::unique_ptr m_cornerBlockShape; + std::unique_ptr m_goalLineBoundaryShape; std::vector> m_objects; }; diff --git a/src/extlibs/er_force_sim/src/protobuf/world.proto b/src/extlibs/er_force_sim/src/protobuf/world.proto index 1f0402f60d..8290b99f92 100644 --- a/src/extlibs/er_force_sim/src/protobuf/world.proto +++ b/src/extlibs/er_force_sim/src/protobuf/world.proto @@ -37,6 +37,12 @@ message Geometry optional float defense_height = 17 [default = 1.0]; optional GeometryType type = 18 [default = TYPE_2014]; optional Division division = 19 [default = A]; + // Field numbers match the upstream ER-Force framework + optional float corner_block_cathetus_length = 21; + // Boundary width behind the goal lines, if different from boundary_width [m] + optional float boundary_width_goal_line = 22; + // Width of the goal substitution area [m], currently unused by the simulator + optional float goal_substitution_area_width = 23; } message BallPosition From 9f534f913aba481ae95a8c7851b4a7eb3313074b Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Thu, 5 Nov 2026 20:18:14 -0700 Subject: [PATCH 2/3] add new field configs --- .../er_force_sim/config/simulator/2023.txt | 54 ++++++++++++++++++ .../er_force_sim/config/simulator/2023B.txt | 37 ++++++++++++ .../er_force_sim/config/simulator/2026.txt | 56 +++++++++++++++++++ .../er_force_sim/config/simulator/BUILD | 3 + 4 files changed, 150 insertions(+) create mode 100644 src/extlibs/er_force_sim/config/simulator/2023.txt create mode 100644 src/extlibs/er_force_sim/config/simulator/2023B.txt create mode 100644 src/extlibs/er_force_sim/config/simulator/2026.txt diff --git a/src/extlibs/er_force_sim/config/simulator/2023.txt b/src/extlibs/er_force_sim/config/simulator/2023.txt new file mode 100644 index 0000000000..b8af1424bc --- /dev/null +++ b/src/extlibs/er_force_sim/config/simulator/2023.txt @@ -0,0 +1,54 @@ +geometry { + line_width: 0.01 + field_width: 9.02 + field_height: 12.04 + boundary_width: 0.25 + goal_width: 1.8 + goal_depth: 0.18 + goal_wall_width: 0.02 + center_circle_radius: 0.5 + defense_radius: 1.8 + defense_stretch: 3.6 + free_kick_from_defense_dist: 0.2 + penalty_spot_from_field_line_dist: 1.8 + penalty_line_from_spot_dist: 0.4 + goal_height: 0.155 + defense_width: 3.6 + defense_height: 1.8 + type: TYPE_2018 + corner_block_cathetus_length: 0.09 +} +camera_setup { + camera_id: 0 + focal_length: 390 + principal_point_x: 300 + principal_point_y: 300 + distortion: 0.2 + q0: 0.7 + q1: 0.7 + q2: 0.7 + q3: 0.7 + tx: 0 + ty: 0 + tz: 3500 + derived_camera_world_tx: -3010 + derived_camera_world_ty: -0 + derived_camera_world_tz: 6000 +} +camera_setup { + camera_id: 1 + focal_length: 390 + principal_point_x: 300 + principal_point_y: 300 + distortion: 0.2 + q0: 0.7 + q1: 0.7 + q2: 0.7 + q3: 0.7 + tx: 0 + ty: 0 + tz: 3500 + derived_camera_world_tx: 3010 + derived_camera_world_ty: -0 + derived_camera_world_tz: 6000 +} diff --git a/src/extlibs/er_force_sim/config/simulator/2023B.txt b/src/extlibs/er_force_sim/config/simulator/2023B.txt new file mode 100644 index 0000000000..7b6b9d226a --- /dev/null +++ b/src/extlibs/er_force_sim/config/simulator/2023B.txt @@ -0,0 +1,37 @@ +geometry { + line_width: 0.01 + field_width: 6.0 + field_height: 9.0 + boundary_width: 0.3 + goal_width: 1.0 + goal_depth: 0.18 + goal_wall_width: 0.02 + center_circle_radius: 0.5 + defense_radius: 1.0 + defense_stretch: 2.0 + free_kick_from_defense_dist: 0.2 + penalty_spot_from_field_line_dist: 1.8 + penalty_line_from_spot_dist: 0.4 + goal_height: 0.155 + defense_width: 2.0 + defense_height: 1.0 + type: TYPE_2018 + corner_block_cathetus_length: 0.09 +} +camera_setup { + camera_id: 0 + focal_length: 390 + principal_point_x: 300 + principal_point_y: 300 + distortion: 0.2 + q0: 0.7 + q1: 0.7 + q2: 0.7 + q3: 0.7 + tx: 0 + ty: 0 + tz: 3500 + derived_camera_world_tx: 0 + derived_camera_world_ty: 0 + derived_camera_world_tz: 6000 +} diff --git a/src/extlibs/er_force_sim/config/simulator/2026.txt b/src/extlibs/er_force_sim/config/simulator/2026.txt new file mode 100644 index 0000000000..af892c65df --- /dev/null +++ b/src/extlibs/er_force_sim/config/simulator/2026.txt @@ -0,0 +1,56 @@ +geometry { + line_width: 0.01 + field_width: 9.02 + field_height: 12.04 + boundary_width: 0.25 + boundary_width_goal_line: 0.60 + goal_width: 1.8 + goal_depth: 0.18 + goal_wall_width: 0.02 + center_circle_radius: 0.5 + defense_radius: 1.8 + defense_stretch: 3.6 + free_kick_from_defense_dist: 0.2 + penalty_spot_from_field_line_dist: 1.8 + penalty_line_from_spot_dist: 0.4 + goal_height: 0.155 + defense_width: 3.6 + defense_height: 1.8 + type: TYPE_2018 + corner_block_cathetus_length: 0.09 + goal_substitution_area_width: 0.3 +} +camera_setup { + camera_id: 0 + focal_length: 390 + principal_point_x: 300 + principal_point_y: 300 + distortion: 0.2 + q0: 0.7 + q1: 0.7 + q2: 0.7 + q3: 0.7 + tx: 0 + ty: 0 + tz: 3500 + derived_camera_world_tx: -3010 + derived_camera_world_ty: -0 + derived_camera_world_tz: 6000 +} +camera_setup { + camera_id: 1 + focal_length: 390 + principal_point_x: 300 + principal_point_y: 300 + distortion: 0.2 + q0: 0.7 + q1: 0.7 + q2: 0.7 + q3: 0.7 + tx: 0 + ty: 0 + tz: 3500 + derived_camera_world_tx: 3010 + derived_camera_world_ty: -0 + derived_camera_world_tz: 6000 +} diff --git a/src/extlibs/er_force_sim/config/simulator/BUILD b/src/extlibs/er_force_sim/config/simulator/BUILD index 5c93988331..241cd38a37 100644 --- a/src/extlibs/er_force_sim/config/simulator/BUILD +++ b/src/extlibs/er_force_sim/config/simulator/BUILD @@ -1,6 +1,9 @@ exports_files([ "2020.txt", "2020B.txt", + "2023.txt", + "2023B.txt", + "2026.txt", "default.txt", "realistic.txt", ]) From cae4e31989b5a6d697655de9f678365fa172d384 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Thu, 5 Nov 2026 20:20:37 -0700 Subject: [PATCH 3/3] change to use latest field dimensions --- src/software/simulation/BUILD | 4 ++-- src/software/simulation/er_force_simulator.cpp | 4 ++-- src/software/simulation/er_force_simulator.h | 4 +++- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/software/simulation/BUILD b/src/software/simulation/BUILD index 6be434dbe8..2dfbaca17e 100644 --- a/src/software/simulation/BUILD +++ b/src/software/simulation/BUILD @@ -10,8 +10,8 @@ cc_library( srcs = ["er_force_simulator.cpp"], hdrs = ["er_force_simulator.h"], data = [ - "//extlibs/er_force_sim/config/simulator:2020.txt", - "//extlibs/er_force_sim/config/simulator:2020B.txt", + "//extlibs/er_force_sim/config/simulator:2023B.txt", + "//extlibs/er_force_sim/config/simulator:2026.txt", "//extlibs/er_force_sim/config/simulator:default.txt", "//extlibs/er_force_sim/config/simulator:realistic.txt", ], diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index 3a453df93c..25be008610 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -94,12 +94,12 @@ ErForceSimulator::ErForceSimulator(const TbotsProto::FieldType& field_type, if (field_type == TbotsProto::FieldType::DIV_A) { - full_filename = full_filename + CONFIG_FILE + ".txt"; + full_filename = full_filename + CONFIG_FILE_DIV_A; } else { // loading division B configuration - full_filename = full_filename + CONFIG_FILE + "B.txt"; + full_filename = full_filename + CONFIG_FILE_DIV_B; } std::ifstream config_file(full_filename); diff --git a/src/software/simulation/er_force_simulator.h b/src/software/simulation/er_force_simulator.h index 08ff9e9aaf..ac6a267df4 100644 --- a/src/software/simulation/er_force_simulator.h +++ b/src/software/simulation/er_force_simulator.h @@ -244,6 +244,8 @@ class ErForceSimulator const bool enable_robot_localizer; const SimulatedRobotSensorNoise robot_sensor_noise; - const std::string CONFIG_FILE = "simulator/2020"; + // there is no 2026 division B config upstream, so division B uses the 2023 one + const std::string CONFIG_FILE_DIV_A = "simulator/2026.txt"; + const std::string CONFIG_FILE_DIV_B = "simulator/2023B.txt"; const std::string CONFIG_DIRECTORY = "extlibs/er_force_sim/config/"; };