idek

Author: Jepson2k

parol6/commands/cartesian_commands.py

@@ -300,17 +300,28 @@ def do_setup(self, state: "ControllerState") -> None:
         logger.log(TRACE, " -> Trajectory prepared with %s steps.", len(self.trajectory_steps))
 
     def execute_step(self, state: "ControllerState") -> ExecutionStatus:
-        if self.command_step >= len(self.trajectory_steps):
+        n = len(self.trajectory_steps)
+        if self.command_step >= n:
             logger.info(f"{type(self).__name__} finished.")
             self.is_finished = True
             self.stop_and_idle(state)
             return ExecutionStatus.completed("MOVEPOSE complete")
         else:
-            self.set_move_position(state, self.trajectory_steps[self.command_step])
-            self.command_step += 1
+            # Advance multiple trajectory steps per tick proportional to measured loop period
+            dt_eff = float(getattr(state, "ema_period_s", 0.0)) if getattr(state, "ema_period_s", 0.0) > 0.0 else float(INTERVAL_S)
+            steps_per_tick = max(1, int(round(dt_eff / float(INTERVAL_S))))
+            next_idx = min(n - 1, self.command_step + steps_per_tick)
+            self.set_move_position(state, self.trajectory_steps[next_idx])
+            self.command_step = next_idx + 1
+            if self.command_step >= n:
+                logger.info("MovePoseCommand finished.")
+                self.is_finished = True
+                self.stop_and_idle(state)
+                return ExecutionStatus.completed("MOVEPOSE complete")
             return ExecutionStatus.executing("MovePose")
 
 
+
 @register_command("MOVECART")
 class MoveCartCommand(MotionCommand):
     """
@@ -444,17 +455,18 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatus:
             return ExecutionStatus.completed("MOVECART complete")
         # Wall-clock progress
         s_wall = float(self.progress01(dur))
-        # Ensure bounded per-tick progress toward s_wall
-        if self._s_step <= 0.0 and dur > 0.0:
-            self._s_step = INTERVAL_S / dur
-        s_next = self._s + self._s_step
+        # Ensure bounded per-tick progress toward s_wall using measured loop period
+        dt_eff = float(getattr(state, "ema_period_s", 0.0)) if getattr(state, "ema_period_s", 0.0) > 0.0 else float(INTERVAL_S)
+        s_step = dt_eff / dur
+        s_next = self._s + s_step
         if s_next > s_wall:
             s_next = s_wall
         if s_next > 1.0:
             s_next = 1.0
         s_scaled = quintic_scaling(float(s_next))
 
 
+
         assert self.initial_pose is not None and self.target_pose is not None
         _ctp = cast(SE3, self.initial_pose).interp(cast(SE3, self.target_pose), s_scaled)
         if not isinstance(_ctp, SE3):