From 5403f406ddb95f6e84124ad165d9fad2bda4b854 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Sat, 12 Sep 2026 01:06:40 -0700 Subject: [PATCH] Fix ER-Force simulator dribbling physics and reported ball model Ports three upstream ER-Force fixes onto our fork (upstream 73e139db..38563d11): - Perfect dribbler hold ball constraint (upstream ee58ccd0): the hinge between robot and ball is replaced by a point to point constraint plus a constraint that keeps the robot upright. Two robots duelling for the same ball used to flip each other over, and a flipped robot is teleported to the side of the field by Simulator::resetFlipped. Since all our simulated tests run with the perfect dribbler, this happened on the default path. - Quaternion component order in SimRobot::update and restoreState (upstream 5ae9dd8c). We read the separate angle field, so this only affects the rotation field of SimulatorState and restoring a saved simulator state. - The ball model reported in the geometry packet now describes the ball model this simulator actually implements, instead of the values ER-Force reports for their own model (ours was replaced in #2653/#2918). Adds a test that drives two dribbling robots into the same ball and checks that they stay upright and on the field, which fails without the first fix, and a test for the rotation reported in SimulatorState. --- .../src/amun/simulator/simfield.cpp | 2 +- .../src/amun/simulator/simfield.h | 5 ++ .../src/amun/simulator/simrobot.cpp | 42 ++++++---- .../src/amun/simulator/simrobot.h | 5 +- .../src/amun/simulator/simulator.cpp | 15 +++- .../simulation/er_force_simulator_test.cpp | 80 +++++++++++++++++++ 6 files changed, 129 insertions(+), 20 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..3dc439f5ae 100644 --- a/src/extlibs/er_force_sim/src/amun/simulator/simfield.cpp +++ b/src/extlibs/er_force_sim/src/amun/simulator/simfield.cpp @@ -50,7 +50,7 @@ SimField::SimField(std::shared_ptr world, addObject(m_plane.get(), btTransform(btQuaternion(btVector3(1, 0, 0), 0), btVector3(0, 0, 0) * SIMULATOR_SCALE), - 0.56, 0.35); + FLOOR_RESTITUTION, FLOOR_FRICTION); // Roof addObject(m_plane.get(), btTransform(btQuaternion(btVector3(1, 0, 0), M_PI), 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..b5f0cf1048 100644 --- a/src/extlibs/er_force_sim/src/amun/simulator/simfield.h +++ b/src/extlibs/er_force_sim/src/amun/simulator/simfield.h @@ -25,6 +25,11 @@ #include "extlibs/er_force_sim/src/protobuf/world.pb.h" +// The friction and restitution a body experiences on the floor is the product of the +// floor's value and the body's own value (see the note in simulator.h) +constexpr float FLOOR_FRICTION = 0.35f; +constexpr float FLOOR_RESTITUTION = 0.56f; + namespace camun { namespace simulator 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 6b20f86c60..7280009326 100644 --- a/src/extlibs/er_force_sim/src/amun/simulator/simrobot.cpp +++ b/src/extlibs/er_force_sim/src/amun/simulator/simrobot.cpp @@ -135,6 +135,7 @@ SimRobot::~SimRobot() if (m_holdBallConstraint) { m_world->removeConstraint(m_holdBallConstraint.get()); + m_world->removeConstraint(m_notTipOverConstraint.get()); } m_world->removeConstraint(m_dribblerConstraint.get()); m_world->removeRigidBody(m_dribblerBody.get()); @@ -164,18 +165,29 @@ void SimRobot::dribble(const SimBall& ball, float speed) { if (canKickBall(ball) && !m_holdBallConstraint) { - btTransform localA, localB; - localA.setIdentity(); - localB.setIdentity(); + const btTransform robotWorldTransform = m_body->getWorldTransform(); + const btTransform worldToRobot = robotWorldTransform.inverse(); - auto worldToRobot = m_body->getWorldTransform().inverse(); - localA.setOrigin(worldToRobot * ball.position()); - localA.setRotation(btQuaternion(worldToRobot * btVector3(0, 1, 0), M_PI_2)); - localB.setRotation(btQuaternion(worldToRobot * btVector3(0, 1, 0), M_PI_2)); + const btVector3 localA = worldToRobot * ball.position(); + const btVector3 localB(0, 0, 0); - m_holdBallConstraint = std::make_unique( + m_holdBallConstraint = std::make_unique( *m_body, *ball.body(), localA, localB); m_world->addConstraint(m_holdBallConstraint.get(), true); + + // Add a constraint that keeps the robot from tipping over. Without it, + // two robots duelling for the ball regularly flipped each other over, and + // a flipped robot is teleported off to the side by resetFlipped. + // This is an ugly hack, but then again so is the hold ball constraint. + // Note that in bullet a lower limit greater than the upper limit means + // that the axis is free, so only the x and y rotations are locked here. + m_notTipOverConstraint = std::make_unique( + *m_body, robotWorldTransform); + m_notTipOverConstraint->setAngularLowerLimit(btVector3(0, 0, 1)); + m_notTipOverConstraint->setAngularUpperLimit(btVector3(0, 0, 0)); + m_notTipOverConstraint->setLinearLowerLimit(btVector3(1, 1, 1)); + m_notTipOverConstraint->setLinearUpperLimit(btVector3(0, 0, 0)); + m_world->addConstraint(m_notTipOverConstraint.get(), true); } } else @@ -197,7 +209,9 @@ void SimRobot::stopDribbling() if (m_holdBallConstraint) { m_world->removeConstraint(m_holdBallConstraint.get()); + m_world->removeConstraint(m_notTipOverConstraint.get()); m_holdBallConstraint.reset(); + m_notTipOverConstraint.reset(); } } @@ -705,10 +719,10 @@ void SimRobot::update(world::SimRobot& robot, const SimBall& ball) const const btQuaternion q = transform.getRotation(); auto* rotation = robot.mutable_rotation(); - rotation->set_real(q.getX()); - rotation->set_i(q.getY()); - rotation->set_j(q.getZ()); - rotation->set_k(q.getW()); + rotation->set_i(q.getX()); + rotation->set_j(q.getY()); + rotation->set_k(q.getZ()); + rotation->set_real(q.getW()); // Get robot orientation relative to the Z axis float x = 0; @@ -750,8 +764,8 @@ void SimRobot::restoreState(const world::SimRobot& robot) { btVector3 position(robot.p_x(), robot.p_y(), robot.p_z()); m_body->getWorldTransform().setOrigin(position * SIMULATOR_SCALE); - btQuaternion rotation(robot.rotation().real(), robot.rotation().i(), - robot.rotation().j(), robot.rotation().k()); + btQuaternion rotation(robot.rotation().i(), robot.rotation().j(), + robot.rotation().k(), robot.rotation().real()); m_body->getWorldTransform().setRotation(rotation); btVector3 velocity(robot.v_x(), robot.v_y(), robot.v_z()); m_body->setLinearVelocity(velocity * SIMULATOR_SCALE); 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 31e7791a9f..bd60d24a94 100644 --- a/src/extlibs/er_force_sim/src/amun/simulator/simrobot.h +++ b/src/extlibs/er_force_sim/src/amun/simulator/simrobot.h @@ -21,6 +21,7 @@ #ifndef SIMROBOT_H #define SIMROBOT_H +#include #include #include "extlibs/er_force_sim/src/core/rng.h" @@ -111,7 +112,9 @@ class camun::simulator::SimRobot std::unique_ptr m_dribblerConstraint; std::vector> m_shapes; std::unique_ptr m_motionState; - std::unique_ptr m_holdBallConstraint; + std::unique_ptr m_holdBallConstraint; + // Keeps a robot that is holding the ball from tipping over, see dribble() + std::unique_ptr m_notTipOverConstraint; btVector3 m_dribblerCenter; sslsim::TeleportRobot m_move; diff --git a/src/extlibs/er_force_sim/src/amun/simulator/simulator.cpp b/src/extlibs/er_force_sim/src/amun/simulator/simulator.cpp index a2c5a6a2a0..07b3362487 100644 --- a/src/extlibs/er_force_sim/src/amun/simulator/simulator.cpp +++ b/src/extlibs/er_force_sim/src/amun/simulator/simulator.cpp @@ -388,10 +388,17 @@ std::vector Simulator::getWrapperPackets() positionErrorVisionScale.z()); } - // add ball model to geometry data - geometry->mutable_models()->mutable_straight_two_phase()->set_acc_roll(-0.35); - geometry->mutable_models()->mutable_straight_two_phase()->set_acc_slide(-4.5); - geometry->mutable_models()->mutable_straight_two_phase()->set_k_switch(0.69); + // Add the ball model to the geometry data. These values describe the ball model + // that this simulator actually implements (see SimBall::begin), so that consumers + // of the geometry packet can predict the ball the same way we simulate it. The + // sliding deceleration is the effective coefficient of friction between ball and + // floor (the product of both bodies' friction values) times gravity. + auto* ball_model = geometry->mutable_models()->mutable_straight_two_phase(); + ball_model->set_acc_roll( + BALL_ROLLING_FRICTION_DECELERATION_METERS_PER_SECOND_SQUARED); + ball_model->set_acc_slide(-BALL_SLIDING_FRICTION_NEWTONS * FLOOR_FRICTION * + ACCELERATION_DUE_TO_GRAVITY_METERS_PER_SECOND_SQUARED); + ball_model->set_k_switch(FRICTION_TRANSITION_FACTOR); geometry->mutable_models()->mutable_chip_fixed_loss()->set_damping_z(0.566); geometry->mutable_models()->mutable_chip_fixed_loss()->set_damping_xy_first_hop( 0.715); diff --git a/src/software/simulation/er_force_simulator_test.cpp b/src/software/simulation/er_force_simulator_test.cpp index ef8c6fb3ea..3220346ba8 100644 --- a/src/software/simulation/er_force_simulator_test.cpp +++ b/src/software/simulation/er_force_simulator_test.cpp @@ -5,6 +5,7 @@ #include "proto/message_translation/er_force_world.h" #include "proto/message_translation/tbots_protobuf.h" #include "proto/primitive/primitive_msg_factory.h" +#include "shared/constants.h" #include "shared/robot_constants.h" #include "software/geom/vector.h" #include "software/physics/euclidean_to_wheel.h" @@ -454,3 +455,82 @@ TEST_F(ErForceSimulatorRampingTest, ramps_in_motor_service_frame_when_clipping) EXPECT_GT(std::abs(rotated_ramped[1] - expected[0]), 1e-3); } + +TEST_F(ErForceSimulatorTest, robots_duelling_over_the_ball_stay_upright) +{ + // Two robots drive into the ball from opposite sides with their dribblers running. + // The perfect dribbler holds the ball with a constraint between the robot and the + // ball, which used to tip the robots over when two of them pulled on the same ball. + // A tipped over robot is considered flipped and gets teleported to the side of the + // field by Simulator::resetFlipped. + constexpr double DISTANCE_FROM_BALL_METERS = 0.11; + constexpr double DRIVE_SPEED_METERS_PER_SECOND = 1.0; + + simulator->setBallState(BallState(Point(0, 0), Vector(0, 0))); + simulator->setYellowRobots({RobotStateWithId{ + .id = 0, + .robot_state = RobotState(Point(DISTANCE_FROM_BALL_METERS, 0), Vector(0, 0), + Angle::half(), AngularVelocity::zero())}}); + simulator->setBlueRobots({RobotStateWithId{ + .id = 0, + .robot_state = RobotState(Point(-DISTANCE_FROM_BALL_METERS, 0), Vector(0, 0), + Angle::zero(), AngularVelocity::zero())}}); + + // Both robots drive forwards, towards each other and the ball, while dribbling + TbotsProto::PrimitiveSet primitive_set; + (*primitive_set.mutable_robot_primitives())[0] = *createDirectControlPrimitive( + Vector(DRIVE_SPEED_METERS_PER_SECOND, 0), AngularVelocity::zero(), + robot_constants.indefinite_dribbler_speed_rpm, TbotsProto::AutoChipOrKick()); + + for (unsigned int step = 0; step < 400; step++) + { + simulator->setYellowRobotPrimitiveSet(primitive_set, + std::make_unique()); + simulator->setBlueRobotPrimitiveSet(primitive_set, + std::make_unique()); + simulator->stepSimulation(Duration::fromMilliseconds(5)); + } + + auto sim_state = simulator->getSimulatorState(); + ASSERT_EQ(1, sim_state.yellow_robots_size()); + ASSERT_EQ(1, sim_state.blue_robots_size()); + + for (const auto& robot : {sim_state.yellow_robots(0), sim_state.blue_robots(0)}) + { + // The z component of the robot's local z axis in world coordinates. It is 1 when + // the robot stands flat on the field and decreases as the robot tips over. + const double i = robot.rotation().i(); + const double j = robot.rotation().j(); + const double upright_component = 1.0 - 2.0 * (i * i + j * j); + + EXPECT_GT(upright_component, std::cos(Angle::fromDegrees(10).toRadians())) + << "Robot tipped over while duelling for the ball"; + + // A robot that stays upright also stays within the width of the field, rather + // than being teleported to the side by resetFlipped + EXPECT_LT(std::abs(robot.p_y()), simulator->getField().yLength() / 2); + } +} + +TEST_F(ErForceSimulatorTest, simulator_state_rotation_matches_robot_orientation) +{ + const Angle orientation = Angle::fromRadians(0.7); + + simulator->setYellowRobots({RobotStateWithId{ + .id = 0, + .robot_state = RobotState(Point(0, 0), Vector(0, 0), orientation, + AngularVelocity::zero())}}); + simulator->stepSimulation(Duration::fromMilliseconds(5)); + + auto sim_state = simulator->getSimulatorState(); + ASSERT_EQ(1, sim_state.yellow_robots_size()); + const auto& rotation = sim_state.yellow_robots(0).rotation(); + + // A robot standing on the field is only rotated about the z axis, so the rotation + // quaternion is (i, j, k, real) = (0, 0, sin(angle / 2), cos(angle / 2)) + EXPECT_NEAR(rotation.i(), 0.0, 1e-3); + EXPECT_NEAR(rotation.j(), 0.0, 1e-3); + EXPECT_TRUE(TestUtil::equalWithinTolerance( + Angle::fromRadians(2 * std::atan2(rotation.k(), rotation.real())), orientation, + Angle::fromDegrees(1))); +}