fix(sim): apply Replacement teleport fully to the physics body

src/models/robot.cpp

@@ -117,6 +117,28 @@ float Robot::advanceAxis(float &applied, float cmd, std::deque<float> &buf,
     return applied;
 }
 
+void Robot::resetMotion() {
+    kickspeedx = 0.0f;
+    kickspeedz = 0.0f;
+    spinner = 0.0f;
+
+    cmdTangent = 0.0f;
+    cmdNormal = 0.0f;
+    cmdAngular = 0.0f;
+    appliedTangent = 0.0f;
+    appliedNormal = 0.0f;
+    appliedAngular = 0.0f;
+    veltangent = 0.0f;
+    velnormal = 0.0f;
+    velangular = 0.0f;
+
+    // Drop anything sitting in the transport-delay pipeline so a previously
+    // latched command can't re-emerge a few ticks later.
+    delayBufTangent.clear();
+    delayBufNormal.clear();
+    delayBufAngular.clear();
+}
+
 void Robot::advanceActuation(float dtSec) {
     veltangent = advanceAxis(appliedTangent, cmdTangent, delayBufTangent,
                              tauLinearSec, deadTimeLinearSec, dtSec);

src/models/robot.h

@@ -47,6 +47,13 @@ class Robot : public QObject {
     // Advance applied velocity one tick; getVel* then return the applied value.
     void advanceActuation(float dtSec);
 
+    // Stops all latched motion/kick/dribble commands and their actuation-delay
+    // pipeline immediately (no ramp-down). Called when this robot is teleported
+    // (Replacement): without this, the previous velocity command stays latched
+    // and the very next simulation tick re-applies it, driving the robot away
+    // from the spot it was just placed at.
+    void resetMotion();
+
     uint32_t getId() const;
     float getKickspeedx() const;
     float getKickspeedz() const;

src/observer.cpp

@@ -104,20 +104,42 @@ void Observer::visionReceive(const mocSim_Packet& packet) {
 
     // Robot/ball placement (Replacement). turnon=false (robot removal) is not
     // handled: this side always sends turnon=true.
+    bool blueReplaced = false;
+    bool yellowReplaced = false;
     for (const auto& robotReplacement : packet.replacement().robots()) {
         int id = robotReplacement.id();
         if (id < 0 || id >= MaxRobots) continue;
         float sceneX = robotReplacement.x() * 1000.0f;
         float sceneZ = -robotReplacement.y() * 1000.0f;
         float sceneRotYDeg = robotReplacement.dir() * 180.0 / M_PI - 90.0;
+        // Stop any velocity/kick/dribble command that was in flight for this robot
+        // *before* the teleport. Without this the previous command is still latched
+        // and gets re-applied on the very next tick, so the robot immediately drives
+        // off again and the teleport looks like it never took effect.
+        if (robotReplacement.yellowteam()) {
+            yellowRobots[id]->resetMotion();
+            yellowReplaced = true;
+        } else {
+            blueRobots[id]->resetMotion();
+            blueReplaced = true;
+        }
         emit robotReplacementRequested(id, robotReplacement.yellowteam(), sceneX, sceneZ, sceneRotYDeg);
     }
+    // Push the zeroed motion state to QML now (rather than waiting for the next
+    // incoming commands packet for that team), so botMovement() doesn't have a
+    // window where it still reads the stale, pre-reset velocity.
+    if (blueReplaced) emit blueRobotsChanged();
+    if (yellowReplaced) emit yellowRobotsChanged();
+
     if (packet.replacement().has_ball()) {
         const auto& ballReplacement = packet.replacement().ball();
         if (ballReplacement.has_x() && ballReplacement.has_y()) {
             float sceneX = ballReplacement.x() * 1000.0f;
             float sceneZ = -ballReplacement.y() * 1000.0f;
-            emit ballReplacementRequested(sceneX, sceneZ);
+            bool hasVelocity = ballReplacement.has_vx() || ballReplacement.has_vy();
+            float sceneVx = ballReplacement.has_vx() ? static_cast<float>(ballReplacement.vx() * 1000.0) : 0.0f;
+            float sceneVz = ballReplacement.has_vy() ? static_cast<float>(-ballReplacement.vy() * 1000.0) : 0.0f;
+            emit ballReplacementRequested(sceneX, sceneZ, hasVelocity, sceneVx, sceneVz);
         }
     }
 }

src/observer.h

@@ -156,7 +156,9 @@ class Observer : public QObject {
     void updateSenderData(QVector3D ball, QList<QVector3D> blue, QList<QVector3D> yellow);
     void updateSimulationSignal();
     void robotReplacementRequested(int id, bool isYellow, float sceneX, float sceneZ, float sceneRotYDeg);
-    void ballReplacementRequested(float sceneX, float sceneZ);
+    // hasVelocity is false when the Replacement didn't set vx/vy (they are optional
+    // in mocSim_BallReplacement); sceneVx/sceneVz are only meaningful when true.
+    void ballReplacementRequested(float sceneX, float sceneZ, bool hasVelocity, float sceneVx, float sceneVz);
 
 private:
     QSettings config;

src/qml/sim/GameObjects.qml

@@ -78,10 +78,29 @@ Node {
             }
         }
         function onRobotReplacementRequested(id, isYellow, sceneX, sceneZ, sceneRotYDeg) {
-            (isYellow ? yBotsFrame : bBotsFrame).children[id].reset(Qt.vector3d(sceneX, 0, sceneZ), Qt.vector3d(0, sceneRotYDeg, 0));
+            // reset() (not just assigning position/eulerRotation) is required to actually
+            // warp a DynamicRigidBody's physics pose: for a dynamic body, physics owns the
+            // transform, so plain property assignment wouldn't move the PhysX actor. This
+            // also zeroes the body's linear/angular velocity. The observer has already
+            // stopped any latched velocity command for this robot (Robot::resetMotion(),
+            // called before this signal), so it stays put instead of immediately driving
+            // off again on the next tick.
+            let color = isYellow ? yellow : blue;
+            let frame = (isYellow ? yBotsFrame : bBotsFrame).children[id];
+            frame.reset(Qt.vector3d(sceneX, 0, sceneZ), Qt.vector3d(0, sceneRotYDeg, 0));
+            // botMovement() derives the robot's "current velocity" from the pose delta
+            // across one tick (poses[i] vs. prePoses[i]). Left untouched, prePoses[i]
+            // still holds the pre-teleport pose, so the position jump reads as a huge
+            // one-tick velocity and MotionControl's accel-limiter spends a few frames
+            // coasting it back down to the (now zero) commanded speed instead of the
+            // robot being still immediately. Seeding prePoses/preVelocities to the
+            // just-placed, at-rest pose avoids that phantom delta.
+            let headingRad = mu.normalizeRadian((frame.eulerRotation.y + 90) * Math.PI / 180.0);
+            color.prePoses[id] = Qt.vector4d(frame.position.x, frame.position.y, frame.position.z, headingRad);
+            color.preVelocities[id] = Qt.vector4d(0, 0, 0, 0);
         }
-        function onBallReplacementRequested(sceneX, sceneZ) {
-            ball.reset(Qt.vector3d(sceneX, 21, sceneZ), Qt.vector3d(0, 0, 0));
+        function onBallReplacementRequested(sceneX, sceneZ, hasVelocity, sceneVx, sceneVz) {
+            placeBall(Qt.vector3d(sceneX, 21, sceneZ), hasVelocity ? Qt.vector3d(sceneVx, 0, sceneVz) : null);
         }
     }
 
@@ -611,23 +630,37 @@ Node {
         );
     }
 
+    // Places the ball's PHYSICS body at scenePosition (grounded, y=21) and, if velocity
+    // is non-null, sets its linear velocity too; otherwise it is left at rest (reset()
+    // already zeroes it). Clears every piece of ball state that could otherwise fight
+    // the placement on a later tick (a deferred kick landing, stale tracked spin/velocity,
+    // an in-progress mouse-drag teleop throw), and un-parks any robot's ball-holding
+    // marker so dribble state doesn't linger against the newly placed ball. Shared by the
+    // mouse "place ball" shortcut and network Replacement so both behave identically.
+    function placeBall(scenePosition, velocity) {
+        teleopVelocity = Qt.vector4d(0, 0, 0, 0);
+        ballVelocity = Qt.vector4d(0, 0, 0, 0);
+        ballSpin = Qt.vector3d(0, 0, 0);
+        pendingKickVelocity = null;
+        ball.reset(scenePosition, Qt.vector3d(0, 0, 0));
+        if (velocity !== null) {
+            ball.setLinearVelocity(velocity);
+        }
+        ball.setAngularVelocity(Qt.vector3d(0, 0, 0));
+        ballPosition = Qt.vector4d(ball.position.x, ball.position.y, ball.position.z, 0);
+        preBallPosition = ballPosition;
+        skipRollingFrictionFrames = 30;
+        for (let i = 0; i < blue.num; i++) {
+            bBotsFrame.children[i].collisionShapes[5].position = Qt.vector3d(0, 5000, 0);
+        }
+        for (let i = 0; i < yellow.num; i++) {
+            yBotsFrame.children[i].collisionShapes[5].position = Qt.vector3d(0, 5000, 0);
+        }
+    }
+
     function resetPosition(target, result) {
         if (target == "ball") {
-            teleopVelocity = Qt.vector4d(0, 0, 0, 0);
-            ballVelocity = Qt.vector4d(0, 0, 0, 0);
-            ballSpin = Qt.vector3d(0, 0, 0);
-            pendingKickVelocity = null;
-            ball.reset(result.scenePosition, Qt.vector3d(0, 0, 0));
-            ball.setAngularVelocity(Qt.vector3d(0, 0, 0));
-            ballPosition = Qt.vector4d(ball.position.x, ball.position.y, ball.position.z, 0);
-            preBallPosition = ballPosition;
-            skipRollingFrictionFrames = 30;
-            for (let i = 0; i < blue.num; i++) {
-                bBotsFrame.children[i].collisionShapes[5].position = Qt.vector3d(0, 5000, 0);
-            }
-            for (let i = 0; i < yellow.num; i++) {
-                yBotsFrame.children[i].collisionShapes[5].position = Qt.vector3d(0, 5000, 0);
-            }
+            placeBall(result.scenePosition, null);
         } else if (target == "bot") {
             if (selectedRobotColor == "blue") {
                 bBotsFrame.children[botCursorID].reset(result.scenePosition, Qt.vector3d(0, -90, 0));