fix: accumulate EEF teleop commands

Author: TomCC7

dimos/control/tasks/eef_twist_task/eef_twist_task.py

@@ -78,6 +78,8 @@ def __init__(self, name: str, config: EEFTwistTaskConfig) -> None:
         self._lock = threading.Lock()
         self._latest_twist: TwistStamped | None = None
         self._last_update_time = 0.0
+        self._last_commanded_positions: NDArray[np.float64] | None = None
+        self._last_commanded_pose: pinocchio.SE3 | None = None
 
     def claim(self) -> ResourceClaim:
         return ResourceClaim(self._joint_names, self._config.priority, ControlMode.SERVO_POSITION)
@@ -92,10 +94,6 @@ def on_ee_twist_command(self, twist: TwistStamped, t_now: float) -> bool:
             logger.warning("EEFTwistTask rejecting non-finite twist", task=self._name)
             return False
         with self._lock:
-            if np.allclose(values, 0.0):
-                self._clear_locked()
-                self._last_update_time = t_now
-                return True
             self._latest_twist = twist
             self._last_update_time = t_now
         return True
@@ -109,13 +107,25 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None:
                 self._config.timeout > 0
                 and state.t_now - self._last_update_time > self._config.timeout
             ):
-                self._clear_locked()
+                self._clear_locked(clear_target=True)
                 return None
 
         q_current = self._get_current_joints(state)
         if q_current is None or not np.all(np.isfinite(q_current)):
             return None
-        target_pose = self._ik.forward_kinematics(q_current)
+        q_current = np.asarray(q_current, dtype=np.float64)
+        if np.allclose(twist_to_numpy(twist), 0.0):
+            if self._last_commanded_positions is None:
+                return None
+            return JointCommandOutput(
+                joint_names=self._joint_names_list,
+                positions=self._last_commanded_positions.tolist(),
+                mode=ControlMode.SERVO_POSITION,
+            )
+
+        target_pose = self._last_commanded_pose
+        if target_pose is None:
+            target_pose = self._ik.forward_kinematics(q_current)
         dt = min(max(state.dt, 0.0), _MAX_DT)
         candidate = self._integrate_twist(target_pose, twist, dt)
 
@@ -139,9 +149,13 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None:
             )
             return None
 
+        commanded_positions = np.asarray(q_solution, dtype=np.float64).reshape(-1)
+        self._last_commanded_positions = commanded_positions.copy()
+        self._last_commanded_pose = self._ik.forward_kinematics(commanded_positions)
+
         return JointCommandOutput(
             joint_names=self._joint_names_list,
-            positions=q_solution.flatten().tolist(),
+            positions=commanded_positions.tolist(),
             mode=ControlMode.SERVO_POSITION,
         )
 
@@ -160,8 +174,11 @@ def _get_current_joints(self, state: CoordinatorState) -> NDArray[np.floating[An
             positions.append(pos)
         return np.array(positions, dtype=np.float64)
 
-    def _clear_locked(self) -> None:
+    def _clear_locked(self, *, clear_target: bool = False) -> None:
         self._latest_twist = None
+        if clear_target:
+            self._last_commanded_positions = None
+            self._last_commanded_pose = None
 
     def _integrate_twist(
         self, pose: pinocchio.SE3, twist: TwistStamped, dt: float

dimos/control/tasks/eef_twist_task/test_eef_twist_task.py

@@ -42,6 +42,7 @@ def __init__(self) -> None:
         self.solution = np.array([0.01, 0.02, 0.03], dtype=np.float64)
         self.converged = True
         self.final_error = 0.0
+        self.solve_from_pose = False
 
     def forward_kinematics(self, q_current: NDArray[np.float64]) -> FakePose:
         self.fk_calls.append(q_current.copy())
@@ -51,6 +52,8 @@ def solve(
         self, pose: FakePose, q_current: NDArray[np.float64]
     ) -> tuple[NDArray[np.float64], bool, float]:
         self.solve_calls.append(pose.copy())
+        if self.solve_from_pose:
+            return pose.translation.copy(), self.converged, self.final_error
         return self.solution.copy(), self.converged, self.final_error
 
 
@@ -124,6 +127,23 @@ def test_integration_uses_current_fk_and_coordinator_dt(
     assert fake_ik.solve_calls[1].translation[0] > fake_ik.solve_calls[0].translation[0]
 
 
+def test_commands_accumulate_from_last_command_with_stale_feedback(
+    task: EEFTwistTask, fake_ik: FakeIK
+) -> None:
+    fake_ik.solve_from_pose = True
+    assert task.on_ee_twist_command(_twist(1.0), t_now=1.0)
+
+    first = task.compute(_state(1.01, positions=[0.0, 0.0, 0.0], dt=0.01))
+    second = task.compute(_state(1.02, positions=[0.0, 0.0, 0.0], dt=0.01))
+
+    assert first is not None
+    assert second is not None
+    assert first.positions is not None
+    assert second.positions is not None
+    assert second.positions[0] > first.positions[0]
+    assert second.positions[0] == pytest.approx(0.02)
+
+
 def test_non_converged_ik_solution_is_accepted_when_joint_delta_is_safe(
     task: EEFTwistTask, fake_ik: FakeIK
 ) -> None:
@@ -190,4 +210,7 @@ def test_timeout_and_zero_command_clear_then_next_nonzero_reseeds(
     assert fake_ik.solve_calls[-1].translation[0] > 1.0
 
     assert task.on_ee_twist_command(_twist(0.0), t_now=2.02)
-    assert not task.is_active()
+    assert task.is_active()
+    held = task.compute(_state(2.03, positions=[0.0, 0.0, 0.0]))
+    assert held is not None
+    assert held.positions == [1.01, 0.0, 0.0]