Joint Angles → FK → End-Effector Position

     θ2
      ╱╲
     ╱  ╲ L2
    ╱    ╲
   ●──────● End-Effector (x, y)
  θ1  L1
   │
   ● Base

x = L1 * cos(θ1) + L2 * cos(θ1 + θ2)
y = L1 * sin(θ1) + L2 * sin(θ1 + θ2)

import numpy as np

def forward_kinematics_2link(theta1, theta2, L1, L2):
    """
    Calculate end-effector position for 2-link planar arm.

    Args:
        theta1: Joint 1 angle (radians)
        theta2: Joint 2 angle (radians)
        L1: Link 1 length
        L2: Link 2 length

    Returns:
        (x, y) position of end-effector
    """
    x = L1 * np.cos(theta1) + L2 * np.cos(theta1 + theta2)
    y = L1 * np.sin(theta1) + L2 * np.sin(theta1 + theta2)
    return x, y

# Example: OP3 arm-like configuration
L1, L2 = 0.1, 0.1  # 10cm each
theta1, theta2 = np.radians(45), np.radians(30)
x, y = forward_kinematics_2link(theta1, theta2, L1, L2)
print(f"End-effector at: ({x:.3f}, {y:.3f})")

Target Position → IK → Joint Angles

def inverse_kinematics_2link(x, y, L1, L2):
    """
    Calculate joint angles for target position.

    Returns:
        (theta1, theta2) in radians, or None if unreachable
    """
    # Check if reachable
    dist = np.sqrt(x**2 + y**2)
    if dist > L1 + L2 or dist < abs(L1 - L2):
        return None  # Unreachable

    # Calculate theta2 using law of cosines
    cos_theta2 = (x**2 + y**2 - L1**2 - L2**2) / (2 * L1 * L2)
    theta2 = np.arccos(np.clip(cos_theta2, -1, 1))

    # Calculate theta1
    k1 = L1 + L2 * np.cos(theta2)
    k2 = L2 * np.sin(theta2)
    theta1 = np.arctan2(y, x) - np.arctan2(k2, k1)

    return theta1, theta2

T_i = Rot_z(θ_i) × Trans_z(d_i) × Trans_x(a_i) × Rot_x(α_i)

def dh_transform(theta, d, a, alpha):
    """Create DH transformation matrix."""
    ct, st = np.cos(theta), np.sin(theta)
    ca, sa = np.cos(alpha), np.sin(alpha)

    return np.array([
        [ct, -st*ca,  st*sa, a*ct],
        [st,  ct*ca, -ct*sa, a*st],
        [0,   sa,     ca,    d   ],
        [0,   0,      0,     1   ]
    ])

Hip Yaw → Hip Roll → Hip Pitch → Knee → Ankle Pitch → Ankle Roll

def leg_ik(foot_position, foot_orientation, hip_position):
    """
    Calculate 6 joint angles for leg.

    Args:
        foot_position: [x, y, z] target foot position
        foot_orientation: [roll, pitch, yaw] target orientation
        hip_position: [x, y, z] hip joint position

    Returns:
        [hip_yaw, hip_roll, hip_pitch, knee, ankle_pitch, ankle_roll]
    """
    # Implementation depends on specific robot geometry
    # Usually uses geometric approach or numerical methods
    pass

import matplotlib.pyplot as plt

def plot_workspace(L1, L2, L3):
    """Visualize 2D projection of leg workspace."""
    theta1_range = np.linspace(-np.pi/2, np.pi/2, 50)
    theta2_range = np.linspace(0, np.pi, 50)

    points = []
    for t1 in theta1_range:
        for t2 in theta2_range:
            x = L1*np.cos(t1) + L2*np.cos(t1+t2)
            y = L1*np.sin(t1) + L2*np.sin(t1+t2)
            points.append([x, y])

    points = np.array(points)
    plt.scatter(points[:,0], points[:,1], s=1)
    plt.title("Leg Workspace (2D Projection)")
    plt.xlabel("X (m)")
    plt.ylabel("Y (m)")
    plt.axis('equal')
    plt.show()

ẋ = J(q) × q̇

ẋ = end-effector velocity [6×1]
J = Jacobian matrix [6×n]
q̇ = joint velocities [n×1]