Update QuadrupedPyMPCController for enhanced strafe motion dynamics

- Increased strafe amplitude and added extra knee lift during strafe for improved movement.
- Refined strafe behavior by differentiating between leading and trailing legs, optimizing hip roll adjustments during both swing and stance phases.
- Enhanced comments for clarity on strafe mechanics and adjustments.

robots/spot/controllers/quadruped_pympc_controller.py

@@ -74,8 +74,9 @@ class QuadrupedPyMPCController(BaseController):
         # Motion parameters - SIGNIFICANTLY INCREASED
         self.swing_height = 0.35  # radians - big knee lift during swing
         self.stride_length = 0.5  # radians - big hip pitch for forward/back
-        self.strafe_amplitude = 0.4  # radians - lateral hip roll amplitude
-        self.turn_amplitude = 0.25  # radians - turning hip roll amplitude
+        self.strafe_amplitude = 0.6  # radians - lateral hip roll amplitude (INCREASED)
+        self.strafe_knee_extra = 0.2  # extra knee lift during strafe
+        self.turn_amplitude = 0.3  # radians - turning hip roll amplitude
 
         # Feedback gains for balance
         self.kp_roll = 0.5
@@ -228,13 +229,26 @@ class QuadrupedPyMPCController(BaseController):
                     # Also adjust hip pitch for turning
                     hy_offset += turn_sign * 0.15 * vyaw * (1 - 2 * swing_progress)
 
-                # STRAFE: Big lateral hip roll during swing
+                # STRAFE: Aggressive lateral motion during swing
                 if abs(vy) > 0.01:
-                    sign = 1.0 if is_left else -1.0
-                    # Strong lateral motion through hip roll
-                    hx += sign * self.strafe_amplitude * vy * (1 - 2 * swing_progress)
-                    # Also lift knee more during strafe
-                    kn_offset -= 0.1 * abs(vy) * np.sin(np.pi * swing_progress)
+                    # Determine if this leg should step toward or away from strafe direction
+                    # When strafing right (vy < 0): right legs lead, left legs follow
+                    # When strafing left (vy > 0): left legs lead, right legs follow
+                    strafe_dir = 1.0 if vy > 0 else -1.0
+                    is_leading_leg = (is_left and vy > 0) or (not is_left and vy < 0)
+
+                    # Leading leg: big outward hip roll to reach
+                    # Trailing leg: hip roll inward to push
+                    if is_leading_leg:
+                        hx += self.strafe_amplitude * abs(vy) * np.sin(np.pi * swing_progress)
+                    else:
+                        hx -= self.strafe_amplitude * 0.5 * abs(vy) * np.sin(np.pi * swing_progress)
+
+                    # Extra knee lift for clearance during strafe
+                    kn_offset -= self.strafe_knee_extra * abs(vy) * np.sin(np.pi * swing_progress)
+
+                    # Slight hip pitch adjustment for diagonal reach
+                    hy_offset += 0.1 * abs(vy) * (1 - 2 * swing_progress)
 
                 hy += hy_offset
                 kn += kn_offset
@@ -268,10 +282,23 @@ class QuadrupedPyMPCController(BaseController):
                     # Push in opposite direction during stance
                     hx += sign * turn_sign * self.turn_amplitude * vyaw * np.cos(np.pi * stance_progress)
 
-                # STRAFE in stance: push laterally
+                # STRAFE in stance: push body laterally
                 if abs(vy) > 0.01:
-                    sign = 1.0 if is_left else -1.0
-                    hx += sign * self.strafe_amplitude * vy * np.cos(np.pi * stance_progress)
+                    # In stance, push in direction of strafe
+                    is_leading_leg = (is_left and vy > 0) or (not is_left and vy < 0)
+
+                    # Leading leg (already placed outward): pull body toward it
+                    # Trailing leg: push body away
+                    if is_leading_leg:
+                        # Pull - hip roll decreases to bring body over
+                        hx -= self.strafe_amplitude * 0.7 * abs(vy) * (1 - stance_progress)
+                    else:
+                        # Push - hip roll increases to push body away
+                        hx += self.strafe_amplitude * 0.7 * abs(vy) * stance_progress
+
+                    # Also slight knee bend on trailing side for push
+                    if not is_leading_leg:
+                        kn_offset -= 0.1 * abs(vy) * stance_progress
 
                 hy += hy_offset
                 kn += kn_offset