Fix ER-Force simulator dribbling physics and reported ball model

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

@@ -50,7 +50,7 @@ SimField::SimField(std::shared_ptr<btDiscreteDynamicsWorld> 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),

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

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<btHingeConstraint>(
+            m_holdBallConstraint = std::make_unique<btPoint2PointConstraint>(
                 *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<btGeneric6DofSpring2Constraint>(
+                *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);

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

@@ -21,6 +21,7 @@
 #ifndef SIMROBOT_H
 #define SIMROBOT_H
 
+#include <BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h>
 #include <btBulletDynamicsCommon.h>
 
 #include "extlibs/er_force_sim/src/core/rng.h"
@@ -111,7 +112,9 @@ class camun::simulator::SimRobot
     std::unique_ptr<btHingeConstraint> m_dribblerConstraint;
     std::vector<std::unique_ptr<btCollisionShape>> m_shapes;
     std::unique_ptr<btMotionState> m_motionState;
-    std::unique_ptr<btHingeConstraint> m_holdBallConstraint;
+    std::unique_ptr<btPoint2PointConstraint> m_holdBallConstraint;
+    // Keeps a robot that is holding the ball from tipping over, see dribble()
+    std::unique_ptr<btGeneric6DofSpring2Constraint> m_notTipOverConstraint;
     btVector3 m_dribblerCenter;
 
     sslsim::TeleportRobot m_move;

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

@@ -388,10 +388,17 @@ std::vector<SSLProto::SSL_WrapperPacket> 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);

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<TbotsProto::World>());
+        simulator->setBlueRobotPrimitiveSet(primitive_set,
+                                            std::make_unique<TbotsProto::World>());
+        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)));
+}