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