fix: legacy robot.q/qd mutation never wrote handle's integration back

src/swift/Handle.py

@@ -118,3 +118,29 @@ def _sync_legacy(self):
         self._model_q = self.q.copy()
         self._model_qd = self.qd.copy()
         self._model_control_mode = self._control_mode
+
+    def _push_legacy(self):
+        """
+        Mirror image of :meth:`_sync_legacy`. Once a handle has been
+        confirmed to be in the deprecated direct-mutation style
+        (``self._warned``), Swift's own internal updates to
+        ``handle.q``/``handle.qd`` -- e.g. velocity-mode integration in
+        ``Swift._step_assembly`` -- need to reach back into
+        ``robot.q``/``robot.qd`` too, or a control loop that reads
+        ``robot.q`` back after ``env.step()`` (the common pattern, e.g.
+        RTB's own README p_servo example) sees a permanently stale value
+        and never converges.
+
+        Only ever touches the robot once ``_warned`` is set, so a handle
+        never driven the legacy way -- the intended case, and the one
+        where several handles may share one plain, stateless robot model
+        -- never has its q/qd silently overwritten by this.
+        """
+        if self.robot is None or not self._warned:
+            return
+
+        self.robot._q = self.q.copy()
+        self.robot._qd = self.qd.copy()
+        self._model_q = self.q.copy()
+        self._model_qd = self.qd.copy()
+        self._model_control_mode = self._control_mode

src/swift/Swift.py

@@ -1270,6 +1270,7 @@ def _step_assembly(self, handle, dt):
 
             robot = handle.robot
             step_v(robot._n, robot._valid_qlim, dt, handle.q, handle.qd, robot._qlim)
+            handle._push_legacy()
 
         elif handle.control_mode == "a":
             pass

tests/test_assembly_handle.py

@@ -76,6 +76,38 @@ def test_legacy_direct_mutation_still_works_but_warns_once():
     assert np.array_equal(handle.q, panda.q)
 
 
+@pytest.mark.rtb
+def test_legacy_qd_mutation_updates_robot_q_after_step():
+    """
+    A control loop driving the deprecated robot.qd style (e.g. RTB's own
+    README p_servo example) needs robot.q to reflect Swift's own per-step
+    integration afterwards, or it's stuck reading a permanently stale
+    configuration and never converges. See jhavl/swift#125.
+    """
+    env = make_env()
+    panda = rtb.models.Panda()
+    panda.q = panda.qr
+    env.add_robot(panda)
+
+    q_before = panda.q.copy()
+
+    with pytest.warns(DeprecationWarning):
+        panda.qd = np.full(panda.n, 0.1)
+        env.step(0.05)
+
+    assert np.allclose(panda.q, q_before + 0.1 * 0.05)
+
+    # Second step: no further warning, q keeps advancing from where it
+    # left off (not re-integrated from the stale pre-loop value).
+    q_before = panda.q.copy()
+    with warnings.catch_warnings(record=True) as record:
+        warnings.simplefilter("always")
+        panda.qd = np.full(panda.n, 0.1)
+        env.step(0.05)
+    assert len(record) == 0
+    assert np.allclose(panda.q, q_before + 0.1 * 0.05)
+
+
 @pytest.mark.rtb
 def test_new_style_usage_never_warns():
     env = make_env()