From 2daba48a0994017aea08b0d2c475433c183c2904 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Sat, 12 Sep 2026 01:09:10 -0700 Subject: [PATCH] Simulate the corner blocks and goal line boundary of the SSL field Ports the field collider changes upstream made for the newer SSL field layouts (upstream 74afb1f4, d496e5ff, 48245210, c002db2f, db1fddb0): - triangular blocks that smooth out the corners of the field, which real fields have had since 2023. Both of our field configs now declare them, so a ball rolled into a corner is deflected instead of coming to rest in the corner - boundary_width_goal_line, for fields whose boundary behind the goal lines is wider than the one along the touch lines. It falls back to boundary_width, so our fields keep the walls exactly where they are, and the side walls of the goals now extend back to the wall - a mode for playing without any boundary area at all, where the goals stand outside the field lines and the wall behind the goal line is split around them The three new geometry fields are added to the simulator's world.proto and to our SSL_GeometryFieldSize, using the field numbers of the official SSL Vision protocol so that we stay compatible with real vision. Field keeps its single boundary_buffer_size for now; a non uniform boundary there belongs with adopting the 2026 field dimensions. Note that this also shifts the goals by half a line width, which is what upstream's code does (their comment says the opposite). --- .../er_force_sim/config/simulator/2020.txt | 1 + .../er_force_sim/config/simulator/2020B.txt | 1 + .../src/amun/simulator/simfield.cpp | 170 ++++++++++++++++-- .../src/amun/simulator/simfield.h | 3 + .../er_force_sim/src/protobuf/geometry.cpp | 19 ++ .../er_force_sim/src/protobuf/world.proto | 10 ++ src/proto/ssl_vision_geometry.proto | 5 + .../simulation/er_force_simulator_test.cpp | 38 ++++ 8 files changed, 228 insertions(+), 19 deletions(-) diff --git a/src/extlibs/er_force_sim/config/simulator/2020.txt b/src/extlibs/er_force_sim/config/simulator/2020.txt index 2ca34b0113..b8af1424bc 100644 --- a/src/extlibs/er_force_sim/config/simulator/2020.txt +++ b/src/extlibs/er_force_sim/config/simulator/2020.txt @@ -16,6 +16,7 @@ geometry { defense_width: 3.6 defense_height: 1.8 type: TYPE_2018 + corner_block_cathetus_length: 0.09 } camera_setup { camera_id: 0 diff --git a/src/extlibs/er_force_sim/config/simulator/2020B.txt b/src/extlibs/er_force_sim/config/simulator/2020B.txt index a23cf16364..7b6b9d226a 100644 --- a/src/extlibs/er_force_sim/config/simulator/2020B.txt +++ b/src/extlibs/er_force_sim/config/simulator/2020B.txt @@ -16,6 +16,7 @@ geometry { defense_width: 2.0 defense_height: 1.0 type: TYPE_2018 + corner_block_cathetus_length: 0.09 } camera_setup { camera_id: 0 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 3dc439f5ae..a0744eddbb 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,53 @@ #include "simfield.h" +#include +#include + #include "simulator.h" using namespace camun::simulator; +/** + * Whether the field is played on without any boundary area around it, in which case the + * walls stand right at the field lines and the goals stand outside of them + * + * @param geometry the field geometry + * + * @return true if the field has no boundary area + */ +static bool hasNoBoundaryArea(const world::Geometry& geometry) +{ + return geometry.boundary_width() == 0.0f && + geometry.boundary_width_goal_line() == 0.0f; +} + 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(); + // The boundary behind the goal lines may be wider than the one along the touch + // lines. Fields that do not report it use the same width all around. + const float boundaryWidthGoalLine = geometry.has_boundary_width_goal_line() + ? geometry.boundary_width_goal_line() + : geometry.boundary_width(); + + const float totalWidth = geometry.field_width() / 2.0f + 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; + // The side walls of the goal extend all the way back to the field wall + const float goalSideWallLengthHalf = boundaryWidthGoalLine / 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 +81,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. Without a boundary area the goals themselves stand + // where the wall would be, so the wall is split into one block on each side of them. + if (hasNoBoundaryArea(geometry)) + { + const float goalLineBoundaryWidthHalf = 0.5f * (totalWidth - goalWidthHalf); + m_goalLineBoundary = std::make_unique( + btVector3(goalLineBoundaryWidthHalf, 0.5f, roomHeight * 0.5f) * + SIMULATOR_SCALE); + const float shapeOffsetX = goalWidthHalf + goalLineBoundaryWidthHalf; + + for (const float side : {-1.0f, 1.0f}) + { + // offset the blocks so that they start at the goal line and extend away + // from the field + const btVector3 shapeOffsetIntoVoid = + btVector3(0, side * 0.5f, roomHeight * 0.5f) * SIMULATOR_SCALE; + for (const float xSide : {-1.0f, 1.0f}) + { + addObject( + m_goalLineBoundary.get(), + btTransform(btQuaternion::getIdentity(), + shapeOffsetIntoVoid + btVector3(xSide * 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 +131,99 @@ SimField::SimField(std::shared_ptr world, btVector3(totalWidth, 0, 0) * SIMULATOR_SCALE), 0.3, 0.35); - // Create goals + // Blocks that smooth out the corners of the field. On the real field these are + // triangular blocks put into the corners; here only the one side of them that + // matters for collision checking is simulated. + if (geometry.has_corner_block_cathetus_length()) + { + const float cathetus = geometry.corner_block_cathetus_length(); + const float hypotenuse = std::sqrt(2 * cathetus * cathetus); + // the height of the triangle when looking top down onto the field + const float blockOffset = (cathetus * cathetus) / hypotenuse; + m_cornerBlock = std::make_unique( + btVector3(hypotenuse * 0.5f, goalWallHalf, roomHeight * 0.5f) * + SIMULATOR_SCALE); + + // The blocks are placed in order of angle. Half of them are just the other half + // rotated by 180 degrees, so only the angles of the positive y half are listed + // and negativeYHalf rotates those by 180 degrees. + for (const bool negativeYHalf : {false, true}) + { + for (const auto& [multipleOfPi, mirrorX] : + {std::pair{0.25f, false}, std::pair{0.75f, true}}) + { + // the signs of the two offsets are flipped relative to each other, + // because for the same rotation the triangle is for example at the + // lower left corner of the field but at the upper left corner of the + // goal + 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 auto mirrorIntoYHalf = [negativeYHalf](btTransform transform) + { + if (negativeYHalf) + { + return btTransform(btQuaternion(btVector3(0, 0, 1), M_PI)) * + transform; + } + return transform; + }; + + btTransform cornerTransform = baseTransform; + cornerTransform.getOrigin() += + btVector3(totalWidthOffset, totalHeight, 0) * SIMULATOR_SCALE; + addObject(m_cornerBlock.get(), mirrorIntoYHalf(cornerTransform), 0.3, + 0.35); + + // a field with a boundary area also has blocks around the goals + if (!hasNoBoundaryArea(geometry)) + { + btTransform goalTransform = baseTransform; + goalTransform.getOrigin() += + btVector3(goalWidthOffset, totalHeight, 0) * SIMULATOR_SCALE; + addObject(m_cornerBlock.get(), mirrorIntoYHalf(goalTransform), 0.3, + 0.35); + } + } + } + } + + // Create goals. Without a boundary area the goal stands on the outside of the goal + // line rather than on its middle, so it is offset by half a line width. + const float lineWidthOffset = + hasNoBoundaryArea(geometry) ? 0.0f : geometry.line_width() * 0.5f; + for (const float side : {-1.0f, 1.0f}) { addObject(m_goalSide.get(), btTransform(btQuaternion::getIdentity(), btVector3((goalWidthHalf - goalWallHalf), - side * (height + goalDepthHalf), goalHeightHalf) * + side * (height + goalSideWallLengthHalf + + lineWidthOffset), + goalHeightHalf) * SIMULATOR_SCALE), 0.3, 0.5); addObject(m_goalSide.get(), btTransform(btQuaternion::getIdentity(), btVector3(-(goalWidthHalf - goalWallHalf), - side * (height + goalDepthHalf), goalHeightHalf) * + side * (height + goalSideWallLengthHalf + + lineWidthOffset), + 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 b5f0cf1048..8ba421315f 100644 --- a/src/extlibs/er_force_sim/src/amun/simulator/simfield.h +++ b/src/extlibs/er_force_sim/src/amun/simulator/simfield.h @@ -54,6 +54,9 @@ class camun::simulator::SimField std::unique_ptr m_plane; std::unique_ptr m_goalSide; std::unique_ptr m_goalBack; + // Only used on fields that have corner blocks / no boundary area respectively + std::unique_ptr m_cornerBlock; + std::unique_ptr m_goalLineBoundary; std::vector> m_objects; }; diff --git a/src/extlibs/er_force_sim/src/protobuf/geometry.cpp b/src/extlibs/er_force_sim/src/protobuf/geometry.cpp index cb3a913816..bd245b2e72 100644 --- a/src/extlibs/er_force_sim/src/protobuf/geometry.cpp +++ b/src/extlibs/er_force_sim/src/protobuf/geometry.cpp @@ -28,6 +28,7 @@ void geometrySetDefault(world::Geometry* geometry, bool useQuadField) geometry->set_field_width((useQuadField) ? 9.00f : 6.00f); geometry->set_field_height((useQuadField) ? 12.00f : 9.00f); geometry->set_boundary_width((useQuadField) ? 0.30f : 0.25f); + geometry->set_boundary_width_goal_line(geometry->boundary_width()); geometry->set_goal_width((useQuadField) ? 1.20f : 1.00f); geometry->set_goal_depth(0.18f); geometry->set_goal_wall_width(0.02f); @@ -56,6 +57,15 @@ void convertFromSSlGeometry(const SSLProto::SSL_GeometryFieldSize& g, outGeometry.set_goal_width(g.goal_width() / 1000.0f); outGeometry.set_goal_depth(g.goal_depth() / 1000.0f); outGeometry.set_boundary_width(g.boundary_width() / 1000.0f); + const float boundaryWidthGoalLine = g.has_boundary_width_goal_line() + ? g.boundary_width_goal_line() + : g.boundary_width(); + outGeometry.set_boundary_width_goal_line(boundaryWidthGoalLine / 1000.0f); + if (g.has_goal_substitution_area_width()) + { + outGeometry.set_goal_substitution_area_width(g.goal_substitution_area_width() / + 1000.0f); + } outGeometry.set_goal_height(0.155f); outGeometry.set_goal_wall_width(0.02f); outGeometry.set_free_kick_from_defense_dist(0.20f); @@ -157,6 +167,15 @@ void convertToSSlGeometry(const world::Geometry& geometry, field->set_field_width(geometry.field_width() * 1000.0f); field->set_field_length(geometry.field_height() * 1000.0f); field->set_boundary_width(geometry.boundary_width() * 1000.0f); + const float boundaryWidthGoalLine = geometry.has_boundary_width_goal_line() + ? geometry.boundary_width_goal_line() + : geometry.boundary_width(); + field->set_boundary_width_goal_line(boundaryWidthGoalLine * 1000.0f); + if (geometry.has_goal_substitution_area_width()) + { + field->set_goal_substitution_area_width(geometry.goal_substitution_area_width() * + 1000.0f); + } field->set_goal_width(geometry.goal_width() * 1000.0f); field->set_goal_depth(geometry.goal_depth() * 1000.0f); diff --git a/src/extlibs/er_force_sim/src/protobuf/world.proto b/src/extlibs/er_force_sim/src/protobuf/world.proto index 1f0402f60d..2226a1e35f 100644 --- a/src/extlibs/er_force_sim/src/protobuf/world.proto +++ b/src/extlibs/er_force_sim/src/protobuf/world.proto @@ -37,6 +37,16 @@ message Geometry optional float defense_height = 17 [default = 1.0]; optional GeometryType type = 18 [default = TYPE_2014]; optional Division division = 19 [default = A]; + // deprecated = 20 + // Length of the cathetus of the triangular blocks that smooth out the corners of + // the field [m]. Fields without such blocks leave this unset. + optional float corner_block_cathetus_length = 21; + // Width of the boundary area behind the goal lines [m]. Defaults to + // boundary_width when unset. + optional float boundary_width_goal_line = 22; + // Width of the goal substitution area (distance from goal line center plus + // boundary width to boundary walls) [m] + optional float goal_substitution_area_width = 23; } message BallPosition diff --git a/src/proto/ssl_vision_geometry.proto b/src/proto/ssl_vision_geometry.proto index baa3079202..48dbab161b 100644 --- a/src/proto/ssl_vision_geometry.proto +++ b/src/proto/ssl_vision_geometry.proto @@ -66,6 +66,11 @@ message SSL_GeometryFieldSize optional int32 goal_height = 13; optional float ball_radius = 14; optional float max_robot_radius = 15; + // Width of the boundary area behind the goal lines [mm]. Defaults to + // boundary_width when unset. + optional int32 boundary_width_goal_line = 16; + // Width of the goal substitution area [mm] + optional int32 goal_substitution_area_width = 17; } message SSL_GeometryCameraCalibration diff --git a/src/software/simulation/er_force_simulator_test.cpp b/src/software/simulation/er_force_simulator_test.cpp index ebdfe6853c..e2c76289a7 100644 --- a/src/software/simulation/er_force_simulator_test.cpp +++ b/src/software/simulation/er_force_simulator_test.cpp @@ -663,3 +663,41 @@ TEST_F(ErForceSimulatorRealismTest, commands_are_only_applied_after_the_command_ // Once the delay has passed, the robot drives just like it does without a delay EXPECT_GT(driveFor(Duration::fromSeconds(1.0)), 0.1); } + +TEST_F(ErForceSimulatorTest, corner_blocks_keep_the_ball_out_of_the_field_corners) +{ + // The corners of the field are blocked off by triangular blocks, so a ball rolling + // into a corner is deflected by them instead of coming to rest in the corner itself + constexpr double CORNER_BLOCK_CATHETUS_METERS = 0.09; + + const double corner_x = + simulator->getField().xLength() / 2 + simulator->getField().boundaryMargin(); + const double corner_y = + simulator->getField().yLength() / 2 + simulator->getField().boundaryMargin(); + + simulator->setBallState(BallState(Point(3.9, 2.4), Vector(2.5, 2.5))); + + // How far the ball gets into the corner, measured as the distance from the corner + // along both axes summed up. The block face runs diagonally across the corner, so + // this value cannot get below the length of its cathetus while the block is there. + double closest_approach_to_corner = std::numeric_limits::max(); + for (unsigned int step = 0; step < 400; step++) + { + simulator->stepSimulation(Duration::fromMilliseconds(5)); + + for (const auto& packet : simulator->getSSLWrapperPackets()) + { + for (const auto& ball : packet.detection().balls()) + { + const double x = std::abs(ball.x() * METERS_PER_MILLIMETER); + const double y = std::abs(ball.y() * METERS_PER_MILLIMETER); + closest_approach_to_corner = + std::min(closest_approach_to_corner, (corner_x - x) + (corner_y - y)); + } + } + } + + // Without the corner block the ball rolls right up into the corner, where it only + // keeps its own radius of distance from each of the two walls + EXPECT_GT(closest_approach_to_corner, CORNER_BLOCK_CATHETUS_METERS * 0.75); +}