added Robot kinematic use for training

HUGE BUG -> SimManager physics broken (at least with Robot kinematics)

Robot.py

@@ -55,12 +55,16 @@ class PyBulletBackend:
         self.body_id = body_id
 
     def send_angles(self, rad_array: dt.RadArray) -> None:
-        if self.sim:
-            # If body_id is set, target that specific robot body
-            if self.body_id is not None and hasattr(self.sim, 'updatePosForBody'):
-                self.sim.updatePosForBody(self.body_id, rad_array)
-            else:
-                self.sim.updatePos(rad_array)
+        if not self.sim:
+            return
+
+        if self.body_id is not None and hasattr(self.sim, 'updatePosForBody'):
+            self.sim.updatePosForBody(self.body_id, rad_array)
+        elif hasattr(self.sim, 'updatePos'):
+            self.sim.updatePos(rad_array)
+        elif hasattr(self.sim, 'set_robot_joint_angles') and self.body_id is not None:
+            joint_indices = getattr(self.sim, 'joint_indices', list(range(18)))
+            self.sim.set_robot_joint_angles(self.body_id, joint_indices, rad_array.data.flatten())
 
     def step_simulation(self) -> None:
         if self.sim:
@@ -111,6 +115,7 @@ class Robot:
 
         # RL configuration
         self.action_scale = 0.1  # Joint delta step size (radians)
+        self.gait_phase = 0.0
 
         # Gait / motion variables
         self.leg_state = np.array(["step", "drag", "step", "drag", "step", "drag"])
@@ -146,6 +151,7 @@ class Robot:
         pose_deg = ri.init_deg if self.start_pose == "init_deg" else ri.init90_deg
         self.current_rad = pose_deg.to_rad()
         self.current_pos = kin.ikpyForward(self.current_rad)
+        self.gait_phase = 0.0
         self.set_joint_angles(self.current_rad)
         self.step_sim()
 
@@ -169,20 +175,129 @@ class Robot:
             self.transition_to(next_state_key)
         self.step_sim()
 
+    def step_kinematic_gait(self, vx: float, vy: float, omega: float) -> None:
+        """Procedural Tripod Gait IK solver for kinematic execution & evaluation."""
+        cmd_mag = math.hypot(vx, vy) + abs(omega)
+        if cmd_mag < 0.03:
+            target_rad = self.compute_ik(self.center_points)
+            self.set_joint_angles(target_rad)
+            return
+
+        # Advance gait step cycle phase
+        self.gait_phase = (self.gait_phase + 0.22) % (2.0 * math.pi)
+
+        stride_len = 0.045   # 4.5 cm maximum stride
+        step_height = 0.035  # 3.5 cm foot clearance height
+
+        center_data = (
+            self.center_points.data 
+            if hasattr(self.center_points, 'data') 
+            else np.array(self.center_points)
+        )
+
+        target_positions = []
+        for leg_id in range(6):
+            base_pos = np.array(center_data[leg_id], dtype=np.float32)
+            
+            # Tripod leg grouping phase offset (even vs odd leg IDs)
+            phase_offset = 0.0 if (leg_id % 2 == 0) else math.pi
+            leg_phase = (self.gait_phase + phase_offset) % (2.0 * math.pi)
+
+            # Directional motion unit vector calculation
+            lx, ly = base_pos[0], base_pos[1]
+            rot_dx = -omega * ly
+            rot_dy = omega * lx
+
+            dx_dir = vx + rot_dx
+            dy_dir = vy + rot_dy
+            dir_norm = math.hypot(dx_dir, dy_dir) + 1e-6
+
+            dx_unit = dx_dir / dir_norm
+            dy_unit = dy_dir / dir_norm
+
+            if leg_phase < math.pi:
+                # Swing Phase (Leg lifted & stepping forward)
+                progress = math.cos(leg_phase)
+                lift = math.sin(leg_phase) * step_height
+                dx = -progress * stride_len * dx_unit
+                dy = -progress * stride_len * dy_unit
+                dz = lift
+            else:
+                # Stance Phase (Leg grounded & propelling torso)
+                progress = math.cos(leg_phase - math.pi)
+                dx = progress * stride_len * dx_unit
+                dy = progress * stride_len * dy_unit
+                dz = 0.0
+
+            target_leg_pos = base_pos + np.array([dx, dy, dz], dtype=np.float32)
+            target_positions.append(target_leg_pos)
+
+        target_pos_array = dt.PosArray(np.array(target_positions))
+        target_rad = self.compute_ik(target_pos_array)
+        self.set_joint_angles(target_rad)
+
+    def step_with_command(
+        self, 
+        command: np.ndarray, 
+        action: Optional[np.ndarray] = None, 
+        mode: str = "direct"
+    ) -> None:
+        """
+        Unified motion execution method supporting Direct RL, Residual RL, and Pure Kinematics.
+
+        Args:
+            command: np.ndarray [vx, vy, vz, omega] from RL environment
+            action: np.ndarray [18,] RL action deltas from neural network ([-1, 1])
+            mode: "kinematics_only" | "residual" | "direct"
+        """
+        # 1. Map RL command [vx, vy, vz, omega] to Robot motion vector [vx, vy, omega]
+        cmd_vx, cmd_vy, _, cmd_omega = command
+        self.vector_dirmov = [float(cmd_vx), float(cmd_vy), float(cmd_omega)]
+
+        # 2. Automatically trigger state transitions based on command magnitude
+        cmd_magnitude = math.hypot(cmd_vx, cmd_vy) + abs(cmd_omega)
+        if cmd_magnitude > 0.05 and self.current_state_key == "idle":
+            # Transition to walking state if registered in state machine
+            target_state = "walk" if "walk" in STATE_REGISTRY else "move"
+            if target_state in STATE_REGISTRY:
+                self.transition_to(target_state)
+        elif cmd_magnitude <= 0.05 and self.current_state_key != "idle":
+            self.transition_to("idle")
+
+        # 3. Execute according to chosen mode
+        if mode == "kinematics_only":
+            self.step_kinematic_gait(cmd_vx, cmd_vy, cmd_omega)
+
+        elif mode == "residual":
+            if self.current_state_key != "idle" and self.current_state and self.current_state_key in STATE_REGISTRY:
+                self.current_state.execute(self)
+            else:
+                self.step_kinematic_gait(cmd_vx, cmd_vy, cmd_omega)
+
+            if action is not None:
+                action_flat = np.clip(np.asarray(action, dtype=np.float32), -1.0, 1.0) * self.action_scale
+                kin_flat = self.current_rad.data.flatten()
+                final_flat = np.clip(kin_flat + action_flat, -np.pi / 2, np.pi / 2)
+                self.set_joint_angles(dt.RadArray(data=final_flat.reshape(self.current_rad.data.shape)))
+
+        elif mode == "direct":
+            if action is not None:
+                self.apply_rl_action(action)
+
     # --- RL METHODS ---
 
     def apply_rl_action(self, action: np.ndarray) -> None:
-        """Applies continuous RL action deltas [-1, 1] to current joint angles."""
         action = np.asarray(action, dtype=np.float32)
         scaled_action = np.clip(action, -1.0, 1.0) * self.action_scale
 
-        current_flat = self.current_rad.data.flatten()
-        updated_flat = np.clip(
-            current_flat + scaled_action,
-            -np.pi / 2,
-            np.pi / 2
-        )
+        # Sync with actual PyBullet state if backend supports it
+        if isinstance(self.backend, PyBulletBackend) and self.backend.sim:
+            actual_angles = self.backend.sim.get_robot_joint_angles(self.backend.body_id)
+            current_flat = actual_angles.flatten()
+        else:
+            current_flat = self.current_rad.data.flatten()
 
+        updated_flat = np.clip(current_flat + scaled_action, -np.pi / 2, np.pi / 2)
         new_rad = dt.RadArray(data=updated_flat.reshape(self.current_rad.data.shape))
         self.set_joint_angles(new_rad)