diff --git a/src/models/robot.cpp b/src/models/robot.cpp index 626c7ab..d7512fb 100644 --- a/src/models/robot.cpp +++ b/src/models/robot.cpp @@ -117,6 +117,28 @@ float Robot::advanceAxis(float &applied, float cmd, std::deque &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); diff --git a/src/models/robot.h b/src/models/robot.h index 33bf7cf..232ef36 100644 --- a/src/models/robot.h +++ b/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; diff --git a/src/observer.cpp b/src/observer.cpp index 0bb2f14..3cb2a77 100644 --- a/src/observer.cpp +++ b/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(ballReplacement.vx() * 1000.0) : 0.0f; + float sceneVz = ballReplacement.has_vy() ? static_cast(-ballReplacement.vy() * 1000.0) : 0.0f; + emit ballReplacementRequested(sceneX, sceneZ, hasVelocity, sceneVx, sceneVz); } } } diff --git a/src/observer.h b/src/observer.h index b9ac2a8..70b236a 100644 --- a/src/observer.h +++ b/src/observer.h @@ -156,7 +156,9 @@ class Observer : public QObject { void updateSenderData(QVector3D ball, QList blue, QList 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; diff --git a/src/qml/sim/GameObjects.qml b/src/qml/sim/GameObjects.qml index 07d72ba..8cee449 100644 --- a/src/qml/sim/GameObjects.qml +++ b/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));