Inverse Kinematics

IK_PARAMS = dict(
    L1=0.045,        # Upper link length (45mm)
    L2=0.06,         # Lower link length (60mm)
    base_dist=0.021, # Distance between parallel arms (21mm)
    mode=2           # Default working mode (A down, B up)
)

def parallel_scara_ik_3dof(target_3d, L1=None, L2=None, base_dist=None, 
                            mode=None, tilt_angle=None):
    x, y, z = target_3d
    
    # If tilt angle not specified, compute it based on desired y-position
    if tilt_angle is None:
        if abs(z) > 1e-6:
            tilt_angle = math.atan2(y, abs(z))
        else:
            tilt_angle = 0.0
    
    # Transform target to tilted leg frame
    cos_tilt = math.cos(tilt_angle)
    sin_tilt = math.sin(tilt_angle)
    
    # Project target onto the tilted plane
    z_eff = z * cos_tilt + y * sin_tilt  # Effective Z in tilted frame
    y_eff = -z * sin_tilt + y * cos_tilt  # Should be ~0 for planar motion
    
    # Target in the 2D plane for SCARA IK
    target_2d = np.array([x, -z_eff])  # Note: negative z (leg extends down)

def parallel_scara_ik(target, L1=None, L2=None, base_dist=None, mode=None):
    # Define elbow configurations
    configs = {
        1: (True, False),   # A up, B down
        2: (False, True),   # A down, B up (DEFAULT)
        3: (True, True),    # Both up
        4: (False, False)   # Both down
    }
    elbow_A, elbow_B = configs.get(mode, (True, False))
    
    # Base positions
    base_A = np.array([-base_dist/2, 0])  # Left arm base
    base_B = np.array([base_dist/2, 0])   # Right arm base
    
    # Solve for each arm
    sol_A = solve_2link_ik(target, base_A, L1, L2, elbow_A)
    sol_B = solve_2link_ik(target, base_B, L1, L2, elbow_B)
    
    if sol_A is None or sol_B is None:
        return None  # Target unreachable
    
    return (*sol_A, *sol_B)  # (θ1_A, θ2_A, θ1_B, θ2_B)

def solve_2link_ik(target, base, L1, L2, elbow_up):
    """Solve 2-link planar arm IK."""
    dx, dy = target - base
    dist = np.linalg.norm([dx, dy])
    
    # Check reachability
    if not (abs(L1 - L2) <= dist <= L1 + L2):
        return None
    
    # Base to target angle
    alpha = math.atan2(dy, dx)
    
    # Elbow angle (law of cosines)
    cos_t2 = (dist**2 - L1**2 - L2**2) / (2 * L1 * L2)
    cos_t2 = np.clip(cos_t2, -1, 1)
    theta2 = math.acos(cos_t2) * (1 if elbow_up else -1)
    
    # Shoulder angle
    beta = math.atan2(L2 * math.sin(theta2), L1 + L2 * math.cos(theta2))
    theta1 = alpha - beta
    
    return theta1, theta2

=== 3DOF IK Test ===
Up-Down:   Tilt[-14.0°] A[  31.8°] B[ -31.8°]
Down-Up:   Tilt[-14.0°] A[ -31.8°] B[  31.8°]  ← Mode 2 (Default)
Up-Up:     Tilt[-14.0°] A[  48.2°] B[  48.2°]
Down-Down: Tilt[-14.0°] A[ -48.2°] B[ -48.2°]

def solve_leg_ik_3dof(target_position, tilt_angle=None, L1=None, L2=None, 
                      base_dist=None, mode=None):
    """Convenience function for solving 3DOF leg IK.
    
    Args:
        target_position: [x, y, z] target in leg frame
        tilt_angle: Desired tilt angle (None for auto)
        L1, L2, base_dist: Leg geometry (loaded from config if None)
        mode: SCARA configuration mode (loaded from config if None)
    
    Returns:
        (tilt, shoulder_L, shoulder_R) angles or None
    """
    return parallel_scara_ik_3dof(target_position, L1, L2, base_dist, 
                                   mode, tilt_angle)

# Get foot target from gait controller
target = gait_controller.update(dt)["FL"]

# Apply coordinate frame conversion
target_local = target.copy()
target_local[0] *= FORWARD_SIGN  # -1.0 to match IK convention

# Solve IK
result = solve_leg_ik_3dof(target_local, **IK_PARAMS)
if result is not None:
    tilt, shoulder_A, shoulder_B = result
    apply_leg_angles(data, "FL", result)