direct and residual movement inconsistency fixed

Robot.py

@@ -188,11 +188,7 @@ class Robot:
             self.current_state = STATE_REGISTRY[next_state_key]
             self.current_state.enter(self)
 
-    def tick(self, action: Optional[np.ndarray] = None) -> None:
-        """
-        Unified control loop tick.
-        Processes commands through direct RL, residual RL, or State Machine kinematics.
-        """
+    def tick(self, action: Optional[np.ndarray] = None, physics_substeps: int = 4) -> None:
         vx, vy, omega = self.vector_dirmov
 
         if self.mode == "direct":
@@ -204,12 +200,12 @@ class Robot:
             if action is not None:
                 self.apply_rl_action_delta(action)
 
-        else:  # "kinematics" / standard State Machine execution
-            next_state_key = self.current_state.execute(self)
-            if next_state_key:
-                self.transition_to(next_state_key)
+        else:  # kinematics mode
+            self.step_kinematic_gait(vx, vy, omega)
 
-        self.step_sim()
+        # Step PyBullet engine sub-steps to allow physics actuation
+        for _ in range(physics_substeps):
+            self.step_sim()
 
     def step_kinematic_gait(self, vx: float, vy: float, omega: float) -> None:
         """Procedural Tripod Gait solver."""
@@ -265,9 +261,17 @@ class Robot:
         self.set_joint_angles(target_rad)
 
     def apply_rl_action(self, action: np.ndarray) -> None:
-        action = np.asarray(action, dtype=np.float32)
-        new_rad = dt.RadArray(data=action.reshape(self.current_rad.data.shape))
-        self.set_joint_angles(new_rad)
+        """
+        Applies direct RL joint action deltas to the current joint positions.
+        """
+        action_flat = np.asarray(action, dtype=np.float32).flatten()
+        
+        # Keep internal memory updated in (6, 3) format for Kinematic/IK math
+        self.current_rad = dt.RadArray(data=action_flat.reshape(6, 3))
+
+        # Send target positions to the PyBullet backend
+        if self.backend:
+            self.backend.send_angles(self.current_rad)
 
     def apply_rl_action_delta(self, action: np.ndarray) -> None:
         """Applies action deltas on top of joint state for Residual RL."""