Simulate the corner blocks and goal line boundary of the SSL field

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

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

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

@@ -20,28 +20,53 @@
 
 #include "simfield.h"
 
+#include <cmath>
+#include <utility>
+
 #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<btDiscreteDynamicsWorld> 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<btStaticPlaneShape>(btVector3(0, 0, 1), 0);
     m_goalSide = std::make_unique<btBoxShape>(
-        btVector3(goalWallHalf, goalDepthHalf, goalHeightHalf) * SIMULATOR_SCALE);
+        btVector3(goalWallHalf, goalSideWallLengthHalf, goalHeightHalf) *
+        SIMULATOR_SCALE);
     m_goalBack = std::make_unique<btBoxShape>(
         btVector3(goalWidthHalf, goalWallHalf, goalHeightHalf) * SIMULATOR_SCALE);
 
@@ -56,15 +81,47 @@ SimField::SimField(std::shared_ptr<btDiscreteDynamicsWorld> 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<btBoxShape>(
+            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<btDiscreteDynamicsWorld> 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<btBoxShape>(
+            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);

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

@@ -54,6 +54,9 @@ class camun::simulator::SimField
     std::unique_ptr<btCollisionShape> m_plane;
     std::unique_ptr<btCollisionShape> m_goalSide;
     std::unique_ptr<btCollisionShape> m_goalBack;
+    // Only used on fields that have corner blocks / no boundary area respectively
+    std::unique_ptr<btCollisionShape> m_cornerBlock;
+    std::unique_ptr<btCollisionShape> m_goalLineBoundary;
     std::vector<std::unique_ptr<btCollisionObject>> m_objects;
 };
 

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

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

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

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