Lab 03-03: Full Humanoid FK/IK

Objectives

Implement FK for multi-DOF humanoid limbs
Apply IK to position humanoid hands and feet
Handle kinematic chains with multiple joints
Create coordinated multi-limb poses

Materials

Type	Name	Version	Tier
software	Python	3.10+	required
software	MuJoCo	3.0+	required
software	NumPy	1.24+	required
software	SciPy	1.10+	required

Background

Humanoid robots have complex kinematic structures with multiple branching chains. This lab applies FK/IK concepts to realistic humanoid limbs.

Instructions

Step 1: Load and Analyze the Humanoid

import mujoco
import numpy as np
from scipy.optimize import minimize

# Load humanoid model
model = mujoco.MjModel.from_xml_path(
    mujoco.util.get_resource_path("humanoid/humanoid.xml")
)
data = mujoco.MjData(model)

# Analyze kinematic structure
print("Humanoid Kinematic Structure")
print("=" * 50)
print(f"Bodies: {model.nbody}")
print(f"Joints: {model.njnt}")
print(f"DOF: {model.nv}")

# List all bodies
print("\nBody hierarchy:")
for i in range(model.nbody):
    name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, i)
    parent_id = model.body_parentid[i]
```python
```python
    parent_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, parent_id) if i > 0 else "world"

    print(f"  {i}: {name} (parent: {parent_name})")

# Identify end-effectors
end_effectors = ['right_hand', 'left_hand', 'right_foot', 'left_foot']
for ee in end_effectors:
    body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, ee)
```python
```python
    if body_id >= 0:

        print(f"\n{ee} (body {body_id}):")
        # Find joints in chain from root to this body
        chain = []
        current = body_id
```python
```python
        while current > 0:

            for j in range(model.njnt):
```python
```python
                if model.jnt_bodyid[j] == current:

                    jnt_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, j)
                    chain.append(jnt_name)
            current = model.body_parentid[current]
        print(f"  Joint chain: {list(reversed(chain))}")

Understanding: Note the kinematic chains for each limb. Right arm chain typically: right_shoulder1 → right_shoulder2 → right_elbow

Step 2: Implement FK for a Limb

def get_body_position(model, data, body_name):
    """Get world position of a body."""
    body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, body_name)
    return data.xpos[body_id].copy()

def get_body_orientation(model, data, body_name):
    """Get world orientation (rotation matrix) of a body."""
    body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, body_name)
    return data.xmat[body_id].reshape(3, 3).copy()

def compute_limb_fk(model, data, joint_angles, joint_names, end_effector):
    """
    Compute FK for a limb by setting joint angles.

    Args:
        model, data: MuJoCo model and data
        joint_angles: Dict mapping joint names to angles
        joint_names: List of joint names in the limb
        end_effector: Name of end-effector body

    Returns:
        End-effector position and orientation
    """
    # Reset
    mujoco.mj_resetData(model, data)

    # Set joint angles
    for jname, angle in joint_angles.items():
        jnt_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, jname)
```python
```python
        if jnt_id >= 0:

            qpos_addr = model.jnt_qposadr[jnt_id]
            data.qpos[qpos_addr] = angle

    # Forward kinematics
    mujoco.mj_forward(model, data)

    pos = get_body_position(model, data, end_effector)
    ori = get_body_orientation(model, data, end_effector)

    return pos, ori

# Test FK for right arm
right_arm_joints = ['right_shoulder1', 'right_shoulder2', 'right_elbow']

# Set some joint angles
test_angles = {
    'right_shoulder1': np.radians(45),
    'right_shoulder2': np.radians(-30),
    'right_elbow': np.radians(60),
}

pos, ori = compute_limb_fk(model, data, test_angles, right_arm_joints, 'right_hand')

print(f"Right hand FK result:")
print(f"  Position: {pos}")
print(f"  Orientation:\n{ori}")

Verification: Change joint angles and observe how hand position changes.

Step 3: Implement Numerical IK for Humanoid

def ik_cost_function(joint_values, model, data, joint_names, target_pos,
                     target_body, weight_pos=1.0, weight_ori=0.0, target_ori=None):
    """
    Cost function for IK optimization.

    Args:
        joint_values: Array of joint angles to optimize
        target_pos: Desired end-effector position
        target_body: Name of end-effector body
        weight_pos, weight_ori: Weights for position and orientation error
        target_ori: Optional target orientation (3x3 matrix)
    """
    # Set joint values
    mujoco.mj_resetData(model, data)

    for i, jname in enumerate(joint_names):
        jnt_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, jname)
```python
```python
        if jnt_id >= 0:

            qpos_addr = model.jnt_qposadr[jnt_id]
            data.qpos[qpos_addr] = joint_values[i]

    mujoco.mj_forward(model, data)

    # Position error
    current_pos = get_body_position(model, data, target_body)
    pos_error = np.sum((current_pos - target_pos)**2)

    # Orientation error (optional)
    ori_error = 0
```python
```python
    if target_ori is not None and weight_ori > 0:

        current_ori = get_body_orientation(model, data, target_body)
        ori_error = np.sum((current_ori - target_ori)**2)

    return weight_pos * pos_error + weight_ori * ori_error

def solve_limb_ik(model, data, target_pos, joint_names, end_effector,
                  initial_guess=None, bounds=None):
    """
    Solve IK for a limb using numerical optimization.

    Returns:
        Dictionary of joint names to angles, or None if failed
    """
    n_joints = len(joint_names)

    if initial_guess is None:
        initial_guess = np.zeros(n_joints)

    if bounds is None:
        # Default bounds: ±180 degrees
        bounds = [(-np.pi, np.pi)] * n_joints

    # Optimize
    result = minimize(
        ik_cost_function,
        initial_guess,
        args=(model, data, joint_names, target_pos, end_effector),
        method='L-BFGS-B',
        bounds=bounds,
        options={'maxiter': 200, 'ftol': 1e-8}
    )

```python
```python
    if result.fun < 0.001:  # Converged close enough

        return {jname: angle for jname, angle in zip(joint_names, result.x)}
    else:
        print(f"IK failed to converge. Final error: {result.fun:.6f}")
        return None

# Test IK: Move right hand to specific position
target_position = np.array([0.3, -0.3, 1.2])

solution = solve_limb_ik(
    model, data,
    target_pos=target_position,
    joint_names=right_arm_joints,
    end_effector='right_hand'
)

if solution:
    print(f"\nIK Solution for target {target_position}:")
    for jname, angle in solution.items():
        print(f"  {jname}: {np.degrees(angle):.1f}°")

    # Verify
    pos, _ = compute_limb_fk(model, data, solution, right_arm_joints, 'right_hand')
    error = np.linalg.norm(pos - target_position)
    print(f"\nVerification:")
    print(f"  Achieved position: {pos}")
    print(f"  Position error: {error:.6f} m")

Verification: IK solution should place the hand close to target position.

Step 4: Multi-Limb Coordination

def create_coordinated_pose(model, data, pose_spec):
    """
    Create a coordinated pose with multiple limb targets.

    Args:
        pose_spec: Dictionary with structure:
            {
                'right_hand': {'pos': [x,y,z], 'joints': [...]},
                'left_hand': {'pos': [x,y,z], 'joints': [...]},
                ...
            }

    Returns:
        Dictionary of all joint angles
    """
    all_joints = {}

    for limb_name, spec in pose_spec.items():
        if 'pos' in spec and 'joints' in spec:
            solution = solve_limb_ik(
                model, data,
                target_pos=np.array(spec['pos']),
                joint_names=spec['joints'],
                end_effector=limb_name
            )
            if solution:
                all_joints.update(solution)
            else:
                print(f"Warning: Could not solve IK for {limb_name}")

    return all_joints

# Define a reaching pose
reaching_pose = {
    'right_hand': {
        'pos': [0.4, -0.2, 1.3],
        'joints': ['right_shoulder1', 'right_shoulder2', 'right_elbow']
    },
    'left_hand': {
        'pos': [0.2, 0.3, 1.0],
        'joints': ['left_shoulder1', 'left_shoulder2', 'left_elbow']
    }
}

print("Solving coordinated reaching pose...")
pose_joints = create_coordinated_pose(model, data, reaching_pose)

if pose_joints:
    print("\nCoordinated pose joint angles:")
    for jname, angle in sorted(pose_joints.items()):
        print(f"  {jname}: {np.degrees(angle):.1f}°")

Challenge: Try creating different coordinated poses (arms up, reaching forward, etc.)

Step 5: Visualize the Pose

def apply_pose(model, data, joint_angles):
    """Apply joint angles to the model."""
    mujoco.mj_resetData(model, data)

    for jname, angle in joint_angles.items():
        jnt_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, jname)
```python
```python
        if jnt_id >= 0:

            qpos_addr = model.jnt_qposadr[jnt_id]
            data.qpos[qpos_addr] = angle

    # Lift humanoid off ground
    data.qpos[2] = 1.4  # Z position

    mujoco.mj_forward(model, data)

# Apply the coordinated pose and visualize
if pose_joints:
    apply_pose(model, data, pose_joints)

    # Record end-effector positions
    print("\nEnd-effector positions after applying pose:")
    for ee in ['right_hand', 'left_hand']:
        pos = get_body_position(model, data, ee)
        print(f"  {ee}: {pos}")

    # Launch viewer (if available)
    try:
        print("\nLaunching viewer - close window when done...")
        with mujoco.viewer.launch_passive(model, data) as viewer:
            while viewer.is_running():
                mujoco.mj_step(model, data)
                viewer.sync()
    except Exception as e:
        print(f"Viewer not available: {e}")
        print("Pose applied successfully (run in environment with display for visualization)")

Step 6: Animate a Trajectory

def interpolate_poses(pose1, pose2, num_steps=50):
    """Linearly interpolate between two poses."""
    poses = []
    for t in np.linspace(0, 1, num_steps):
        interp_pose = {}
        all_joints = set(pose1.keys()) | set(pose2.keys())
        for jname in all_joints:
            angle1 = pose1.get(jname, 0)
            angle2 = pose2.get(jname, 0)
            interp_pose[jname] = angle1 + t * (angle2 - angle1)
        poses.append(interp_pose)
    return poses

# Create start and end poses
start_pose = create_coordinated_pose(model, data, {
    'right_hand': {
        'pos': [0.3, -0.4, 0.8],
        'joints': right_arm_joints
    }
})

end_pose = create_coordinated_pose(model, data, {
    'right_hand': {
        'pos': [0.5, -0.2, 1.4],
        'joints': right_arm_joints
    }
})

if start_pose and end_pose:
    # Generate trajectory
    trajectory = interpolate_poses(start_pose, end_pose, num_steps=100)

    print(f"Generated trajectory with {len(trajectory)} waypoints")

    # Record hand positions along trajectory
    hand_positions = []
    for pose in trajectory:
        apply_pose(model, data, pose)
        pos = get_body_position(model, data, 'right_hand')
        hand_positions.append(pos.copy())

    hand_positions = np.array(hand_positions)

    # Plot trajectory
    import matplotlib.pyplot as plt
    from mpl_toolkits.mplot3d import Axes3D

    fig = plt.figure(figsize=(10, 8))
    ax = fig.add_subplot(111, projection='3d')

    ax.plot(hand_positions[:, 0], hand_positions[:, 1], hand_positions[:, 2],
            'b-', linewidth=2, label='Hand trajectory')
    ax.scatter(*hand_positions[0], c='g', s=100, marker='o', label='Start')
    ax.scatter(*hand_positions[-1], c='r', s=100, marker='^', label='End')

    ax.set_xlabel('X')
    ax.set_ylabel('Y')
    ax.set_zlabel('Z')
    ax.set_title('Right Hand Trajectory (Joint Space Interpolation)')
    ax.legend()

    plt.savefig('hand_trajectory.png', dpi=150)
    plt.show()

Observation: Joint-space interpolation produces smooth joint motion but may not produce straight-line Cartesian motion.

Expected Outcomes

Understanding of humanoid kinematic structure
Working FK for limb chains
Numerical IK for positioning end-effectors
Multi-limb coordinated poses
Animated trajectories with visualization

Rubric

Criterion	Points	Description
Kinematic Analysis	15	Correct identification of joint chains
FK Implementation	20	Accurate forward kinematics
IK Solver	25	Working numerical IK with good convergence
Multi-Limb Coordination	20	Successful coordinated poses
Trajectory Generation	20	Smooth animated movement

Total: 100 points

Extensions

Cartesian-Space Interpolation: Implement straight-line motion in Cartesian space
Full-Body IK: Include torso and legs for whole-body reaching
Obstacle Avoidance: Add constraints to avoid self-collision
Real-Time IK: Optimize for speed to enable interactive control

Objectives​

Materials​

Background​

Instructions​

Step 1: Load and Analyze the Humanoid​

Step 2: Implement FK for a Limb​

Step 3: Implement Numerical IK for Humanoid​

Step 4: Multi-Limb Coordination​

Step 5: Visualize the Pose​

Step 6: Animate a Trajectory​

Expected Outcomes​

Rubric​

Extensions​