┌────────────────────────────────────────┐
│              HEAD (2 DOF)              │
│         Pan (yaw) + Tilt (pitch)       │
├────────────────────────────────────────┤
│     LEFT ARM (3)    │    RIGHT ARM (3) │
│  Shoulder P/R + Elbow│ Shoulder P/R + Elbow│
├────────────────────────────────────────┤
│     LEFT LEG (6)    │    RIGHT LEG (6)  │
│  Hip Y/R/P + Knee   │  Hip Y/R/P + Knee │
│  + Ankle P/R        │  + Ankle P/R      │
└────────────────────────────────────────┘
              Total: 20 DOF

Joint Angles (θ₁, θ₂, ..., θₙ) → FK → End-Effector Position (x, y, z, roll, pitch, yaw)

T = Rz(θ) × Tz(d) × Tx(a) × Rx(α)

T_total = T₁ × T₂ × T₃ × ... × Tₙ

Target Position (x, y, z, roll, pitch, yaw) → IK → Joint Angles (θ₁, θ₂, ..., θₙ)

def leg_ik(target_position, target_orientation):
    """
    Calculate joint angles for leg to reach target.

    Args:
        target_position: (x, y, z) in hip frame
        target_orientation: (roll, pitch, yaw)

    Returns:
        joint_angles: [hip_yaw, hip_roll, hip_pitch,
                      knee_pitch, ankle_pitch, ankle_roll]
    """
    # 1. Calculate hip yaw based on foot position
    hip_yaw = atan2(y, x)

    # 2. Transform to sagittal plane
    # 3. Solve for hip pitch and knee using geometry
    # 4. Calculate ankle angles for foot orientation

    return joint_angles

Base → Hip_Yaw → Hip_Roll → Hip_Pitch → Knee → Ankle_Pitch → Ankle_Roll → Foot

Body → Head_Pan → Head_Tilt → Camera

┌─────────────┬─────────────┐
│  Stance     │  Swing      │
│  Phase      │  Phase      │
│  (60%)      │  (40%)      │
└─────────────┴─────────────┘

       CoM Path
         ___
        /   \
    ___/     \___

   Left    Mid    Right
   Foot         Foot

Support Polygon:
┌─────────────────┐
│                 │
│  ┌───┐   ┌───┐  │
│  │ L │   │ R │  │  ← Feet
│  └───┘   └───┘  │
│                 │
└─────────────────┘
    ZMP harus di dalam area ini

world
  └── odom
        └── base_link
              ├── head_link
              │     └── camera_optical_frame
              ├── l_hip_yaw_link
              │     └── ... → l_foot_link
              └── r_hip_yaw_link
                    └── ... → r_foot_link

// Forward Kinematics
Eigen::MatrixXd getJointPose(std::string joint_name);

// Inverse Kinematics
bool calcInverseKinematics(
    int to,                    // target link ID
    Eigen::MatrixXd tar_pos,   // target position
    Eigen::MatrixXd tar_rot,   // target rotation
    std::map<std::string, double> &joint_angle_map
);

# Get current joint positions
ros2 topic echo /joint_states

# View TF tree
ros2 run tf2_tools view_frames