Weeks 11-12: Humanoid Robot Development
Master the unique challenges of humanoid robotics: bipedal locomotion, dynamic balance, dexterous manipulation, and natural human-robot interaction.
Introduction to Humanoid Robotics
Humanoid robots are designed to operate in human-centric environments, using human-like morphology to navigate stairs, doorways, and tools designed for people.
Why Humanoid Form Factor?
Environmental Compatibility:
The world is built for human bodies. Humanoids can use standard infrastructure (stairs, ladders, vehicles) without modification.
Tool Usage:
Operate tools and machinery designed for human hands without specialized interfaces.
Social Acceptance:
Human-like appearance facilitates natural interaction and reduces psychological barriers in caregiving, service, and collaborative scenarios.
Versatility:
Bipedal locomotion combined with dexterous manipulation enables a wide range of tasks in unstructured environments.
Challenges of Humanoid Design
Complexity:
30+ degrees of freedom (DOF) requiring sophisticated control algorithms.
Balance and Stability:
Small support polygon (feet) makes maintaining balance difficult, especially during dynamic motion.
Energy Efficiency:
Bipedal walking is less efficient than wheeled locomotion, requiring advanced actuation and control.
Mechanical Wear:
High forces on joints and actuators during impact and weight-bearing operations.
Humanoid Robot Kinematics
Kinematics describes the motion of robots without considering forces. For humanoids, we need forward kinematics (joint angles → end-effector position) and inverse kinematics (desired position → joint angles).
Forward Kinematics with DH Parameters
Denavit-Hartenberg (DH) parameters provide a systematic way to describe robot link transformations.
import numpy as np
class DHParameter:
def __init__(self, a, alpha, d, theta):
"""
DH Parameters:
a: link length
alpha: link twist
d: link offset
theta: joint angle
"""
self.a = a
self.alpha = alpha
self.d = d
self.theta = theta
def transform_matrix(self):
"""Compute transformation matrix from DH parameters"""
ct = np.cos(self.theta)
st = np.sin(self.theta)
ca = np.cos(self.alpha)
sa = np.sin(self.alpha)
T = np.array([
[ct, -st*ca, st*sa, self.a*ct],
[st, ct*ca, -ct*sa, self.a*st],
[0, sa, ca, self.d ],
[0, 0, 0, 1 ]
])
return T
class HumanoidArm:
def __init__(self):
# 7-DOF arm: shoulder (3), elbow (1), wrist (3)
self.dh_params = [
DHParameter(0, np.pi/2, 0.3, 0), # Shoulder yaw
DHParameter(0, np.pi/2, 0, 0), # Shoulder pitch
DHParameter(0.3, 0, 0, 0), # Shoulder roll
DHParameter(0, np.pi/2, 0, 0), # Elbow pitch
DHParameter(0.25, 0, 0, 0), # Forearm
DHParameter(0, np.pi/2, 0, 0), # Wrist pitch
DHParameter(0, 0, 0.1, 0), # Wrist roll
]
def forward_kinematics(self, joint_angles):
"""
Compute end-effector pose from joint angles
Args:
joint_angles: Array of 7 joint angles (radians)
Returns:
4x4 transformation matrix (position + orientation)
"""
T = np.eye(4) # Start with identity
for i, theta in enumerate(joint_angles):
self.dh_params[i].theta = theta
T = T @ self.dh_params[i].transform_matrix()
return T
def get_end_effector_position(self, joint_angles):
"""Extract position from forward kinematics"""
T = self.forward_kinematics(joint_angles)
return T[:3, 3] # Translation component
# Example usage
arm = HumanoidArm()
joint_config = np.array([0.1, 0.2, 0.0, -1.0, 0.5, 0.3, 0.0])
ee_pose = arm.forward_kinematics(joint_config)
print("End-effector transformation:")
print(ee_pose)
print(f"\nPosition: {ee_pose[:3, 3]}")
Inverse Kinematics with Damped Least Squares
Inverse kinematics is essential for controlling the end-effector to reach target positions.
class InverseKinematics:
def __init__(self, robot_arm, damping=0.01):
self.arm = robot_arm
self.damping = damping # Damping factor for numerical stability
def jacobian(self, joint_angles, delta=1e-5):
"""
Compute Jacobian matrix numerically
J[i,j] = ∂(position_i) / ∂(joint_j)
"""
n_joints = len(joint_angles)
J = np.zeros((3, n_joints)) # 3D position
# Current position
p0 = self.arm.get_end_effector_position(joint_angles)
# Numerical derivative for each joint
for j in range(n_joints):
q_perturbed = joint_angles.copy()
q_perturbed[j] += delta
p_new = self.arm.get_end_effector_position(q_perturbed)
J[:, j] = (p_new - p0) / delta
return J
def solve(self, target_position, initial_angles, max_iters=100, tolerance=1e-3):
"""
Damped Least Squares IK solver
Args:
target_position: Desired 3D position [x, y, z]
initial_angles: Starting joint configuration
max_iters: Maximum iterations
tolerance: Convergence threshold (meters)
Returns:
Joint angles that achieve target position
"""
q = initial_angles.copy()
for i in range(max_iters):
# Current position
current_pos = self.arm.get_end_effector_position(q)
# Error
error = target_position - current_pos
error_magnitude = np.linalg.norm(error)
# Check convergence
if error_magnitude < tolerance:
print(f"Converged in {i} iterations")
return q
# Compute Jacobian
J = self.jacobian(q)
# Damped least squares solution
# Δq = J^T (J J^T + λ²I)^-1 e
lambda_sq = self.damping ** 2
I = np.eye(3)
delta_q = J.T @ np.linalg.inv(J @ J.T + lambda_sq * I) @ error
# Update with step size
alpha = 0.5 # Step size
q = q + alpha * delta_q
# Joint limits (example: ±π)
q = np.clip(q, -np.pi, np.pi)
print(f"Max iterations reached. Error: {error_magnitude:.4f}m")
return q
# Example: Reach target position
ik_solver = InverseKinematics(arm)
target = np.array([0.4, 0.2, 0.5]) # Target position
initial_guess = np.zeros(7)
joint_solution = ik_solver.solve(target, initial_guess)
print(f"\nJoint solution: {joint_solution}")
# Verify
achieved_pos = arm.get_end_effector_position(joint_solution)
print(f"Achieved position: {achieved_pos}")
print(f"Target position: {target}")
print(f"Error: {np.linalg.norm(achieved_pos - target):.6f}m")
Humanoid Robot Dynamics
Dynamics considers forces and torques required to produce motion. This is critical for computing motor torques and ensuring stability.
Joint Torque Computation
import numpy as np
from scipy.spatial.transform import Rotation
class HumanoidDynamics:
def __init__(self):
# Simplified humanoid model
self.g = 9.81 # Gravity
# Link properties (mass, length, inertia)
self.links = {
'thigh': {'mass': 5.0, 'length': 0.4, 'inertia': 0.05},
'shin': {'mass': 3.0, 'length': 0.4, 'inertia': 0.03},
'foot': {'mass': 1.0, 'length': 0.1, 'inertia': 0.01}
}
def gravity_torque(self, joint_angle, link_params):
"""
Compute torque due to gravity at a joint
τ_g = m * g * l * cos(θ)
where l is center of mass distance from joint
"""
m = link_params['mass']
l = link_params['length'] / 2 # CoM at midpoint
tau = m * self.g * l * np.cos(joint_angle)
return tau
def compute_required_torques(self, joint_angles, joint_velocities, joint_accelerations):
"""
Compute torques using inverse dynamics
Simplified for demonstration
"""
# For full humanoid, use algorithms like:
# - Recursive Newton-Euler Algorithm (RNEA)
# - Articulated Body Algorithm (ABA)
n = len(joint_angles)
torques = np.zeros(n)
for i in range(n):
# Gravity compensation
link_key = list(self.links.keys())[min(i, len(self.links)-1)]
tau_g = self.gravity_torque(joint_angles[i], self.links[link_key])
# Inertial torque (simplified)
I = self.links[link_key]['inertia']
tau_inertial = I * joint_accelerations[i]
# Damping (viscous friction)
b = 0.1
tau_damping = b * joint_velocities[i]
torques[i] = tau_g + tau_inertial + tau_damping
return torques
# Example
dynamics = HumanoidDynamics()
angles = np.array([0.2, -0.5, 0.1])
velocities = np.array([0.1, 0.2, 0.0])
accelerations = np.array([0.0, 0.0, 0.0])
torques = dynamics.compute_required_torques(angles, velocities, accelerations)
print(f"Required torques: {torques} N⋅m")
Bipedal Locomotion and Balance Control
Walking is the defining capability of humanoid robots. It requires dynamic balance, precise foot placement, and coordination of all joints.
Zero Moment Point (ZMP) Stability
The Zero Moment Point is a fundamental concept in bipedal stability. A robot is stable if the ZMP is inside the support polygon (convex hull of contact points).
class ZMPStability:
def __init__(self, robot_mass=50.0):
self.mass = robot_mass
self.g = 9.81
def compute_zmp(self, com_position, com_acceleration, height):
"""
Compute ZMP position on the ground
ZMP_x = x_com - (z_com / g) * x_ddot_com
ZMP_y = y_com - (z_com / g) * y_ddot_com
Args:
com_position: [x, y, z] center of mass position
com_acceleration: [x_ddot, y_ddot, z_ddot]
height: CoM height above ground
"""
zmp_x = com_position[0] - (height / self.g) * com_acceleration[0]
zmp_y = com_position[1] - (height / self.g) * com_acceleration[1]
return np.array([zmp_x, zmp_y])
def is_stable(self, zmp, support_polygon):
"""
Check if ZMP is inside support polygon
Args:
zmp: [x, y] position
support_polygon: List of [x, y] vertices (e.g., foot corners)
"""
from matplotlib.path import Path
polygon = Path(support_polygon)
return polygon.contains_point(zmp)
# Example: Single support (one foot)
zmp_checker = ZMPStability(mass=60.0)
# Robot state
com_pos = np.array([0.0, 0.0, 0.9]) # Standing
com_accel = np.array([0.5, 0.0, 0.0]) # Accelerating forward
zmp = zmp_checker.compute_zmp(com_pos, com_accel, height=0.9)
print(f"ZMP position: {zmp}")
# Foot support polygon (rectangular foot)
foot_length = 0.2
foot_width = 0.1
support_polygon = [
[-foot_length/2, -foot_width/2],
[foot_length/2, -foot_width/2],
[foot_length/2, foot_width/2],
[-foot_length/2, foot_width/2]
]
stable = zmp_checker.is_stable(zmp, support_polygon)
print(f"Robot is {'stable' if stable else 'UNSTABLE'}")
Walking Pattern Generation
class WalkingController:
def __init__(self, step_length=0.3, step_height=0.05, step_time=0.8):
self.step_length = step_length
self.step_height = step_height
self.step_time = step_time
def generate_foot_trajectory(self, t, phase='swing'):
"""
Generate smooth foot trajectory during swing phase
Args:
t: Time within step (0 to step_time)
phase: 'swing' or 'stance'
Returns:
[x, y, z] foot position
"""
if phase == 'stance':
# Foot on ground, stationary
return np.array([0, 0, 0])
# Swing phase: foot moves forward and up
progress = t / self.step_time # 0 to 1
# Smooth trajectory using cycloid or spline
# Here: simple sinusoidal profile
x = self.step_length * progress
z = self.step_height * np.sin(np.pi * progress)
y = 0 # No lateral motion
return np.array([x, y, z])
def generate_com_trajectory(self, t, total_time):
"""
Generate CoM trajectory for stable walking
Uses simplified linear inverted pendulum model
"""
# CoM shifts laterally during single support
# and moves forward continuously
freq = 2 * np.pi / self.step_time
# Lateral shift (left-right)
lateral_amplitude = 0.05
y_com = lateral_amplitude * np.sin(freq * t)
# Forward progression
forward_speed = self.step_length / self.step_time
x_com = forward_speed * t
# Height (constant for simplified model)
z_com = 0.9
return np.array([x_com, y_com, z_com])
# Simulate one step
walker = WalkingController(step_length=0.3, step_time=1.0)
timesteps = np.linspace(0, 1.0, 50)
for t in timesteps:
foot_pos = walker.generate_foot_trajectory(t, phase='swing')
com_pos = walker.generate_com_trajectory(t, total_time=2.0)
if t % 0.2 < 0.05: # Print every 0.2s
print(f"t={t:.2f}s: Foot={foot_pos}, CoM={com_pos}")
Balance Control with PID
class BalanceController:
def __init__(self):
# PID gains for ankle torque control
self.Kp = 100.0 # Proportional
self.Ki = 10.0 # Integral
self.Kd = 20.0 # Derivative
self.integral_error = 0.0
self.previous_error = 0.0
self.dt = 0.01 # 100Hz control
def compute_ankle_torque(self, desired_com, actual_com):
"""
Compute ankle torque to maintain balance
Args:
desired_com: Target CoM position [x, y, z]
actual_com: Current CoM position [x, y, z]
Returns:
Ankle torque [pitch, roll]
"""
# Error in CoM position
error = desired_com - actual_com
# PID for pitch (forward-backward)
pitch_error = error[0]
self.integral_error += pitch_error * self.dt
derivative = (pitch_error - self.previous_error) / self.dt
ankle_pitch_torque = (
self.Kp * pitch_error +
self.Ki * self.integral_error +
self.Kd * derivative
)
# Roll (left-right) - simplified
ankle_roll_torque = self.Kp * error[1]
self.previous_error = pitch_error
return np.array([ankle_pitch_torque, ankle_roll_torque])
# Example
balance = BalanceController()
desired = np.array([0.0, 0.0, 0.9])
actual = np.array([0.05, -0.01, 0.9]) # Leaning forward slightly
torque = balance.compute_ankle_torque(desired, actual)
print(f"Corrective ankle torque: {torque} N⋅m")
Manipulation and Grasping with Humanoid Hands
Humanoid hands enable dexterous manipulation similar to human capabilities. Modern designs often have 12-20 DOF.
Grasp Taxonomy
from enum import Enum
class GraspType(Enum):
"""Common grasp types for humanoid hands"""
POWER_GRASP = "power" # Whole hand, high force (hammer, bottle)
PRECISION_PINCH = "pinch" # Fingertip, precision (small objects)
TRIPOD = "tripod" # Three fingers (pen, tool)
LATERAL = "lateral" # Thumb-side of index (key, card)
HOOK = "hook" # Fingers only (bag handle)
class HumanoidHand:
def __init__(self, n_fingers=5, segments_per_finger=3):
self.n_fingers = n_fingers
self.segments = segments_per_finger
self.joint_angles = np.zeros(n_fingers * segments_per_finger)
def power_grasp(self, object_diameter):
"""
Configure hand for power grasp
All fingers curl around object
"""
# Compute joint angles based on object size
base_curl = np.pi / 4 # 45 degrees
for finger in range(self.n_fingers):
for segment in range(self.segments):
idx = finger * self.segments + segment
# Progressively more curl for distal segments
self.joint_angles[idx] = base_curl * (segment + 1) / self.segments
return self.joint_angles
def precision_pinch(self, object_size):
"""
Configure thumb and index for precision grip
"""
angles = np.zeros_like(self.joint_angles)
# Thumb opposition
angles[0:3] = [np.pi/6, np.pi/4, np.pi/6]
# Index finger
angles[3:6] = [0, np.pi/8, np.pi/6]
# Other fingers relaxed
return angles
def apply_grasp(self, grasp_type, object_params):
"""Execute specific grasp type"""
if grasp_type == GraspType.POWER_GRASP:
return self.power_grasp(object_params['diameter'])
elif grasp_type == GraspType.PRECISION_PINCH:
return self.precision_pinch(object_params['size'])
else:
raise NotImplementedError(f"Grasp type {grasp_type} not implemented")
# Example
hand = HumanoidHand(n_fingers=5, segments_per_finger=3)
# Power grasp for bottle
bottle_grasp = hand.apply_grasp(
GraspType.POWER_GRASP,
{'diameter': 0.06}
)
print(f"Power grasp configuration: {bottle_grasp}")
# Precision pinch for pen
pen_grasp = hand.apply_grasp(
GraspType.PRECISION_PINCH,
{'size': 0.01}
)
print(f"Precision pinch configuration: {pen_grasp}")
Contact Force Control
class ForceControlledGrasp:
def __init__(self, target_force=5.0):
self.target_force = target_force # Newtons
self.Kp = 0.5 # Proportional gain
def control_grasp_force(self, measured_forces):
"""
Adjust finger position to maintain desired force
Args:
measured_forces: Array of force sensor readings
Returns:
Position adjustments for each finger
"""
n_fingers = len(measured_forces)
adjustments = np.zeros(n_fingers)
for i, force in enumerate(measured_forces):
error = self.target_force - force
# Proportional control
# Positive error → increase closure
# Negative error → decrease closure
adjustments[i] = self.Kp * error
return adjustments
# Example: Adaptive grasping
force_controller = ForceControlledGrasp(target_force=8.0)
# Simulated sensor readings (some fingers not making contact)
sensor_readings = np.array([7.5, 8.2, 6.0, 0.0, 7.8])
adjustments = force_controller.control_grasp_force(sensor_readings)
print(f"Force readings: {sensor_readings}")
print(f"Position adjustments: {adjustments}")
Natural Human-Robot Interaction Design
Effective HRI combines verbal communication, gesture recognition, gaze tracking, and social awareness.
Speech Recognition and Synthesis
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
class VoiceInteraction(Node):
def __init__(self):
super().__init__('voice_interaction')
# Publishers and subscribers
self.speech_sub = self.create_subscription(
String, '/speech_recognition', self.speech_callback, 10)
self.tts_pub = self.create_publisher(String, '/text_to_speech', 10)
# Intent recognition
self.intents = {
'greeting': ['hello', 'hi', 'hey'],
'follow': ['follow me', 'come with me'],
'stop': ['stop', 'halt', 'wait'],
'fetch': ['get', 'bring', 'fetch']
}
def speech_callback(self, msg):
"""Process recognized speech"""
text = msg.data.lower()
self.get_logger().info(f'Heard: "{text}"')
# Intent recognition
intent = self.recognize_intent(text)
if intent == 'greeting':
self.respond("Hello! How can I help you?")
elif intent == 'follow':
self.respond("I'll follow you.")
self.execute_follow_behavior()
elif intent == 'stop':
self.respond("Stopping.")
self.execute_stop()
elif intent == 'fetch':
obj = self.extract_object(text)
self.respond(f"I'll get the {obj}.")
self.execute_fetch(obj)
def recognize_intent(self, text):
"""Match text to intent"""
for intent, keywords in self.intents.items():
if any(kw in text for kw in keywords):
return intent
return 'unknown'
def extract_object(self, text):
"""Extract object name from fetch command"""
# Simplified: look for noun after 'get' or 'bring'
words = text.split()
try:
idx = max(
[words.index(w) for w in ['get', 'bring', 'fetch']
if w in words]
)
return words[idx + 1] if idx + 1 < len(words) else 'object'
except:
return 'object'
def respond(self, text):
"""Send text to speech synthesizer"""
msg = String()
msg.data = text
self.tts_pub.publish(msg)
self.get_logger().info(f'Responding: "{text}"')
def execute_follow_behavior(self):
"""Trigger follow behavior"""
pass # Implement with person tracking
def execute_stop(self):
"""Stop all motion"""
pass
def execute_fetch(self, object_name):
"""Fetch requested object"""
pass # Implement with navigation + manipulation
Gesture Recognition
import cv2
import numpy as np
class GestureRecognizer:
def __init__(self):
# Using MediaPipe or similar for hand tracking
self.gestures = {}
def recognize_pointing(self, hand_landmarks):
"""
Detect pointing gesture and direction
Args:
hand_landmarks: 21 3D hand keypoints from tracking
Returns:
pointing_direction: 3D vector, or None
"""
# Index finger tip and base
index_tip = hand_landmarks[8]
index_base = hand_landmarks[5]
wrist = hand_landmarks[0]
# Check if index is extended
index_length = np.linalg.norm(index_tip - wrist)
# Other fingers should be curled
middle_tip = hand_landmarks[12]
middle_length = np.linalg.norm(middle_tip - wrist)
if index_length > middle_length * 1.3: # Index extended
# Compute pointing direction
direction = index_tip - index_base
direction = direction / np.linalg.norm(direction)
return direction
return None
def recognize_wave(self, hand_trajectory):
"""
Detect waving motion from hand position history
Args:
hand_trajectory: List of [x, y, z] positions over time
Returns:
bool: True if waving detected
"""
if len(hand_trajectory) < 20:
return False
# Check for oscillating motion in x or y
positions = np.array(hand_trajectory[-20:])
x_positions = positions[:, 0]
# Detect oscillation using zero crossings
x_centered = x_positions - np.mean(x_positions)
zero_crossings = np.sum(np.diff(np.sign(x_centered)) != 0)
# Wave has multiple direction changes
return zero_crossings > 4
# Example usage with mock data
recognizer = GestureRecognizer()
# Simulated hand landmarks
hand_points = np.random.rand(21, 3)
hand_points[8] = np.array([0.5, 0.8, 0.3]) # Index tip extended
pointing = recognizer.recognize_pointing(hand_points)
if pointing is not None:
print(f"Pointing direction: {pointing}")
Social Navigation
class SocialNavigator:
def __init__(self):
self.personal_space = 1.2 # meters
self.approach_angle = np.pi / 3 # 60 degrees front
def compute_socially_aware_path(self, robot_pos, goal, people_positions):
"""
Plan path that respects personal space
Args:
robot_pos: [x, y] robot position
goal: [x, y] goal position
people_positions: List of [x, y] person locations
Returns:
waypoints: List of [x, y] path waypoints
"""
# For simple demo: use potential field
waypoints = []
current = np.array(robot_pos)
goal = np.array(goal)
dt = 0.1
max_steps = 100
for _ in range(max_steps):
# Attractive force to goal
to_goal = goal - current
dist_to_goal = np.linalg.norm(to_goal)
if dist_to_goal < 0.1:
break
attractive = to_goal / dist_to_goal
# Repulsive forces from people
repulsive = np.zeros(2)
for person in people_positions:
person = np.array(person)
to_person = current - person
dist = np.linalg.norm(to_person)
if dist < self.personal_space:
# Strong repulsion when close
magnitude = (self.personal_space - dist) / dist
repulsive += magnitude * to_person / dist
# Combine forces
velocity = attractive + 2.0 * repulsive
velocity = velocity / np.linalg.norm(velocity) * 0.5 # Max speed
current = current + velocity * dt
waypoints.append(current.copy())
return waypoints
def compute_approach_pose(self, person_pos, person_orientation):
"""
Compute socially appropriate approach position
Stand in front, at comfortable distance
"""
# Stand in front at personal space distance
forward_dir = np.array([
np.cos(person_orientation),
np.sin(person_orientation)
])
approach_pos = person_pos + forward_dir * self.personal_space
# Face the person
approach_orientation = person_orientation + np.pi
return approach_pos, approach_orientation
# Example
social_nav = SocialNavigator()
robot = [0, 0]
goal = [5, 5]
people = [[2, 2], [3, 4], [4, 1]]
path = social_nav.compute_socially_aware_path(robot, goal, people)
print(f"Generated {len(path)} waypoints avoiding {len(people)} people")
Practical Exercises
Week 11 Assignment: Kinematics and Walking
Objective: Implement forward/inverse kinematics and basic walking controller
Tasks:
- Implement forward kinematics for a 7-DOF humanoid arm using DH parameters
- Develop inverse kinematics solver using Jacobian transpose or damped least squares
- Create ZMP calculator and stability checker
- Generate walking trajectories for both feet
- Visualize foot and CoM paths in RViz
Deliverable:
Python package with kinematics solvers, walking pattern generator, and simulation demonstrating stable walking in Gazebo or Isaac Sim.
Week 12 Assignment: Manipulation and HRI
Objective: Develop grasping and natural interaction capabilities
Tasks:
- Implement multiple grasp types (power, precision, tripod)
- Create force-controlled grasp with simulated force sensors
- Develop voice command interface with intent recognition
- Implement gesture recognition (pointing, waving)
- Design socially-aware navigation that respects personal space
Deliverable:
Complete HRI system demonstrating: voice-controlled fetch task, gesture-based pointing to objects, and socially-aware approach to person. Include video demonstration and analysis report.
Next: In the final weeks, you'll integrate all components into a complete humanoid robot system for real-world tasks.