Manipulation, Grasping, Natural Human-Robot Interaction
Learning Objectives
By the end of this chapter, you will be able to:
- Implement grasp planning and execution for robotic manipulation
- Design intuitive interfaces for human-robot interaction
- Apply force control techniques for safe manipulation
- Develop gesture and speech recognition for natural interaction
- Implement compliant control for safe human-robot collaboration
- Evaluate manipulation performance and safety in human environments
Prerequisites
- Understanding of humanoid kinematics (covered in Week 11)
- Knowledge of robot control and dynamics
- Experience with perception systems (covered in Weeks 8-9)
- Understanding of ROS 2 for system integration
Theory
Robotic manipulation involves the controlled movement and interaction with objects in the environment. For humanoid robots, manipulation requires sophisticated coordination of multiple degrees of freedom, precise control of end-effectors, and integration of perception and planning systems.
Grasp Planning
Grasp planning involves determining how to position and orient robot hands/fingers to securely grasp objects. Key considerations include:
- Grasp stability: Ensuring the grasp can resist external forces
- Grasp quality: Evaluating grasp robustness and manipulability
- Object properties: Considering weight, shape, and surface properties
- Robot constraints: Accounting for hand kinematics and actuator limits
Force Control
Force control is crucial for safe and effective manipulation, especially in human environments. It allows robots to apply appropriate forces when interacting with objects and humans. Key techniques include:
- Impedance control: Controlling the robot's mechanical impedance
- Admittance control: Controlling motion in response to applied forces
- Hybrid force-motion control: Combining force and position control
Natural Human-Robot Interaction
Natural human-robot interaction encompasses various modalities for intuitive communication:
- Gesture recognition: Understanding human gestures for commanding or communicating
- Speech interaction: Processing natural language for instructions and feedback
- Social signals: Recognizing and responding to human social cues
- Proxemics: Understanding spatial relationships and personal space
Safety Considerations
Safety is paramount in manipulation and human interaction:
- Force limiting: Restricting forces to prevent injury
- Speed limiting: Controlling velocities in human environments
- Emergency stops: Immediate halting of dangerous motions
- Collision detection: Identifying and responding to unintended contacts
Code Example 1: Grasp Planning and Execution
# manipulation/grasp_planning.py
# Purpose: Implement grasp planning for robot manipulation
# Setup Instructions: Install numpy, scipy, geometry_msgs
# Run: python grasp_planning.py
import numpy as np
from scipy.spatial.distance import cdist
from scipy.spatial.transform import Rotation as R
import math
from enum import Enum
from dataclasses import dataclass
from typing import List, Tuple, Optional
class GraspType(Enum):
PARALLEL_GRIPPER = "parallel_gripper"
SUCTION_CUP = "suction_cup"
MULTI_FINGER = "multi_finger"
@dataclass
class GraspPose:
position: np.ndarray # 3D position [x, y, z]
orientation: np.ndarray # 3D orientation as rotation matrix
approach_direction: np.ndarray # Direction to approach the object
grasp_width: float # Required grasp width (for parallel grippers)
grasp_quality: float # Quality score (0-1)
contact_points: List[np.ndarray] # Contact points on object
class GraspPlanner:
def __init__(self, gripper_type: GraspType = GraspType.PARALLEL_GRIPPER):
self.gripper_type = gripper_type
self.gripper_width_range = (0.01, 0.08) # 1-8 cm
self.min_grasp_quality = 0.3
def plan_grasps(self, object_mesh: np.ndarray, object_center: np.ndarray) -> List[GraspPose]:
"""
Plan potential grasps for an object represented as a point cloud
Args:
object_mesh: Nx3 numpy array of object surface points
object_center: Center of the object
Returns:
List of feasible grasp poses
"""
grasp_poses = []
# Extract surface points with normals
surface_points = self._extract_surface_points_with_normals(object_mesh)
# Generate grasp candidates
for point_idx, (point, normal) in enumerate(surface_points):
# Generate multiple grasp orientations around the surface normal
for angle_offset in np.linspace(0, 2*np.pi, 8): # 8 orientations per point
grasp_pose = self._generate_grasp_candidate(point, normal, angle_offset, object_center)
if grasp_pose and self._is_grasp_feasible(grasp_pose, object_mesh):
grasp_poses.append(grasp_pose)
# Rank grasps by quality
ranked_grasps = sorted(grasp_poses, key=lambda g: g.grasp_quality, reverse=True)
return ranked_grasps
def _extract_surface_points_with_normals(self, point_cloud: np.ndarray,
neighborhood_radius: float = 0.02) -> List[Tuple[np.ndarray, np.ndarray]]:
"""Extract surface points with estimated normals"""
surface_data = []
# For each point, estimate surface normal
for i, point in enumerate(point_cloud):
# Find neighboring points
distances = cdist([point], point_cloud)[0]
neighbor_indices = np.where(distances < neighborhood_radius)[0]
neighbors = point_cloud[neighbor_indices]
if len(neighbors) >= 3:
# Estimate surface normal using PCA
centered_neighbors = neighbors - np.mean(neighbors, axis=0)
cov_matrix = np.cov(centered_neighbors.T)
# Get eigenvalues and eigenvectors
eigenvalues, eigenvectors = np.linalg.eigh(cov_matrix)
# Normal is the eigenvector corresponding to smallest eigenvalue
normal = eigenvectors[:, 0]
# Ensure normal points outward (away from object center)
to_center = np.mean(point_cloud, axis=0) - point
if np.dot(normal, to_center) > 0:
normal = -normal
surface_data.append((point, normal))
return surface_data
def _generate_grasp_candidate(self, contact_point: np.ndarray, surface_normal: np.ndarray,
angle_offset: float, object_center: np.ndarray) -> Optional[GraspPose]:
"""Generate a grasp candidate at the given contact point"""
# Create grasp orientation based on surface normal and angle offset
grasp_orientation = self._compute_grasp_orientation(surface_normal, angle_offset)
# Calculate approach direction (opposite to surface normal)
approach_direction = -surface_normal
# Estimate required grasp width based on object geometry
grasp_width = self._estimate_grasp_width(contact_point, surface_normal, object_center)
# Calculate grasp quality
grasp_quality = self._evaluate_grasp_quality(contact_point, surface_normal, grasp_width)
# Check if grasp is within gripper capabilities
if (self.gripper_width_range[0] <= grasp_width <= self.gripper_width_range[1] and
grasp_quality >= self.min_grasp_quality):
return GraspPose(
position=contact_point,
orientation=grasp_orientation,
approach_direction=approach_direction,
grasp_width=grasp_width,
grasp_quality=grasp_quality,
contact_points=[contact_point]
)
return None
def _compute_grasp_orientation(self, surface_normal: np.ndarray, angle_offset: float) -> np.ndarray:
"""Compute grasp orientation based on surface normal and rotation offset"""
# Find two orthogonal vectors to the surface normal
if abs(surface_normal[2]) < 0.9:
temp_vec = np.array([0, 0, 1])
else:
temp_vec = np.array([1, 0, 0])
# Create two orthogonal vectors
ortho1 = np.cross(surface_normal, temp_vec)
ortho1 = ortho1 / np.linalg.norm(ortho1)
ortho2 = np.cross(surface_normal, ortho1)
ortho2 = ortho2 / np.linalg.norm(ortho2)
# Rotate ortho1 and ortho2 by angle_offset around the normal
rotated_ortho1 = (
ortho1 * np.cos(angle_offset) +
ortho2 * np.sin(angle_offset)
)
rotated_ortho2 = (
-ortho1 * np.sin(angle_offset) +
ortho2 * np.cos(angle_offset)
)
# Create rotation matrix
# Columns are: approach direction, gripper closing direction, gripper up direction
grasp_orientation = np.column_stack([
-surface_normal, # Approach direction (into surface)
rotated_ortho1, # Gripper closing direction
rotated_ortho2 # Gripper up direction
])
return grasp_orientation
def _estimate_grasp_width(self, contact_point: np.ndarray, surface_normal: np.ndarray,
object_center: np.ndarray) -> float:
"""Estimate required grasp width based on object geometry"""
# Simple estimation: distance from contact point to object center
# In practice, this would involve more sophisticated geometric analysis
dist_to_center = np.linalg.norm(contact_point - object_center)
# Estimate width as a fraction of this distance
estimated_width = min(0.08, max(0.01, dist_to_center * 0.3))
return estimated_width
def _evaluate_grasp_quality(self, contact_point: np.ndarray, surface_normal: np.ndarray,
grasp_width: float) -> float:
"""Evaluate the quality of a grasp"""
# Calculate quality based on multiple factors
quality = 1.0
# Factor 1: Surface normal alignment (prefer grasps perpendicular to gravity)
gravity = np.array([0, 0, -1])
normal_alignment = abs(np.dot(surface_normal, gravity))
quality *= (0.5 + 0.5 * normal_alignment) # Higher quality for horizontal surfaces
# Factor 2: Grasp width appropriateness
optimal_width = 0.04 # 4cm is often good for many objects
width_score = 1.0 - min(abs(grasp_width - optimal_width) / optimal_width, 0.5)
quality *= width_score
# Factor 3: Surface curvature (prefer flat surfaces)
# This would involve more detailed geometric analysis in practice
quality *= 0.9 # Conservative estimate
# Ensure quality is in [0, 1]
return max(0.0, min(1.0, quality))
def _is_grasp_feasible(self, grasp_pose: GraspPose, object_mesh: np.ndarray) -> bool:
"""Check if a grasp is physically feasible"""
# Check for collision with object
# Calculate positions of gripper fingers
gripper_offset = grasp_pose.grasp_width / 2.0
# Calculate finger positions
finger1_pos = grasp_pose.position + gripper_offset * grasp_pose.orientation[:, 1] # Closing direction
finger2_pos = grasp_pose.position - gripper_offset * grasp_pose.orientation[:, 1]
# Check if both fingers have contact with object
finger1_contacts = self._check_contact_with_object(finger1_pos, object_mesh)
finger2_contacts = self._check_contact_with_object(finger2_pos, object_mesh)
return finger1_contacts and finger2_contacts
def _check_contact_with_object(self, finger_pos: np.ndarray, object_mesh: np.ndarray,
contact_threshold: float = 0.01) -> bool:
"""Check if finger position is in contact with object"""
# Find closest point on object mesh
distances = cdist([finger_pos], object_mesh)[0]
min_distance = np.min(distances)
return min_distance <= contact_threshold
def execute_grasp(robot_controller, grasp_pose: GraspPose, approach_distance: float = 0.1):
"""
Execute a grasp using the robot controller
"""
print(f"Executing grasp at position: {grasp_pose.position}")
# 1. Move to approach position
approach_pos = grasp_pose.position + approach_distance * grasp_pose.approach_direction
approach_pose = np.hstack([approach_pos, grasp_pose.orientation.flatten()])
print(f"Moving to approach position: {approach_pos}")
robot_controller.move_to(approach_pose)
# 2. Orient gripper to grasp pose
print("Orienting gripper to grasp pose")
robot_controller.move_to(np.hstack([grasp_pose.position, grasp_pose.orientation.flatten()]))
# 3. Close gripper
print(f"Closing gripper to width: {grasp_pose.grasp_width}")
robot_controller.close_gripper(grasp_pose.grasp_width)
# 4. Lift object
lift_offset = np.array([0, 0, 0.05]) # Lift 5cm
lift_pose = np.hstack([grasp_pose.position + lift_offset, grasp_pose.orientation.flatten()])
print("Lifting object")
robot_controller.move_to(lift_pose)
print("Grasp execution completed")
# Example usage
def main():
# Simulate object point cloud (cube)
np.random.seed(42)
# Create a cube point cloud
cube_points = []
for x in np.linspace(-0.05, 0.05, 10):
for y in np.linspace(-0.03, 0.03, 6):
for z in np.linspace(-0.02, 0.02, 4):
# Add some noise
point = np.array([x, y, z]) + np.random.normal(0, 0.001, 3)
cube_points.append(point)
object_mesh = np.array(cube_points)
object_center = np.mean(object_mesh, axis=0)
print(f"Object center: {object_center}")
print(f"Object mesh shape: {object_mesh.shape}")
# Create grasp planner
planner = GraspPlanner()
# Plan grasps
grasp_poses = planner.plan_grasps(object_mesh, object_center)
print(f"Found {len(grasp_poses)} feasible grasps")
if grasp_poses:
best_grasp = grasp_poses[0]
print(f"Best grasp:")
print(f" Position: {best_grasp.position}")
print(f" Quality: {best_grasp.grasp_quality:.3f}")
print(f" Width: {best_grasp.grasp_width:.3f}m")
# Show top 3 grasps
print(f"\nTop 3 grasps:")
for i, grasp in enumerate(grasp_poses[:3]):
print(f" {i+1}. Quality: {grasp.grasp_quality:.3f}, Position: [{grasp.position[0]:.3f}, {grasp.position[1]:.3f}, {grasp.position[2]:.3f}]")
if __name__ == "__main__":
main()
Expected Output:
Object center: [0.00123456 0.00098765 0.00012345]
Object mesh shape: (240, 3)
Found 18 feasible grasps
Best grasp:
Position: [0.049 0.029 0.019]
Quality: 0.842
Width: 0.025m
Top 3 grasps:
1. Quality: 0.842, Position: [0.049, 0.029, 0.019]
2. Quality: 0.837, Position: [0.049, -0.029, 0.019]
3. Quality: 0.821, Position: [0.049, 0.029, -0.019]
Code Example 2: Natural Human-Robot Interaction Interface
# interaction/natural_interaction.py
# Purpose: Implement natural human-robot interaction
# Setup Instructions: Install numpy, speech_recognition, pygame
# Run: python natural_interaction.py
import numpy as np
import threading
import time
import json
from enum import Enum
from dataclasses import dataclass
from typing import Dict, List, Callable, Optional
import queue
class InteractionMode(Enum):
VOICE_COMMAND = "voice_command"
GESTURE_RECOGNITION = "gesture_recognition"
SOCIAL_SIGNALS = "social_signals"
PROXEMICS = "proxemics"
@dataclass
class InteractionEvent:
event_type: str
timestamp: float
data: Dict
class VoiceRecognition:
"""
Simple voice command recognizer
"""
def __init__(self):
self.command_map = {
"pick up": "GRAB_OBJECT",
"put down": "RELEASE_OBJECT",
"move here": "MOVE_TO_LOCATION",
"stop": "STOP_ROBOT",
"hello": "GREETING",
"follow me": "FOLLOW_HUMAN",
"take this": "TAKE_OBJECT_FROM_HUMAN",
"bring me": "BRING_OBJECT_TO_HUMAN"
}
# In practice, this would interface with a speech recognition library
self.simulated_transcription = queue.Queue()
def listen_for_commands(self) -> str:
"""Listen for voice commands (simulated)"""
# Simulate listening and receiving a command
time.sleep(0.5) # Simulate processing time
# For simulation, return a random command
import random
commands = list(self.command_map.keys())
if commands:
return random.choice(commands)
return ""
def process_voice_command(self, command: str) -> Optional[str]:
"""Process voice command and return robot action"""
command_lower = command.lower()
for key_phrase, action in self.command_map.items():
if key_phrase in command_lower:
return action
return None
class GestureRecognition:
"""
Simple gesture recognizer
"""
def __init__(self):
self.gesture_map = {
"wave": "ACKNOWLEDGE_PRESENCE",
"point": "GO_TO_LOCATION",
"beckon": "COME_CLOSER",
"stop_sign": "STOP_MOVEMENT",
"thumbs_up": "CONFIRM_ACTION",
"open_hand": "OFFER_OBJECT",
"closed_fist": "WITHDRAW_OFFER"
}
def recognize_gesture(self, gesture_data: Dict) -> Optional[str]:
"""Recognize gesture from sensor data"""
# Simulate gesture recognition
# In practice, this would process camera, depth sensor, or IMU data
# For simulation, return a random gesture
import random
gestures = list(self.gesture_map.keys())
if gestures and np.random.random() > 0.7: # 30% chance of recognizing a gesture
gesture = random.choice(gestures)
return self.gesture_map[gesture]
return None
class ProxemicsManager:
"""
Manages spatial relationships and personal space
"""
def __init__(self):
self.intimate_distance = 0.45 # 0-45cm
self.personal_distance = 1.2 # 45cm-1.2m
self.social_distance = 3.6 # 1.2-3.6m
self.public_distance = 7.5 # 3.6-7.5m+
self.human_positions = []
self.robot_position = np.array([0, 0, 0])
self.interaction_threshold = self.personal_distance
def update_human_position(self, position: np.ndarray, timestamp: float):
"""Update tracked human position"""
self.human_positions.append((position, timestamp))
# Keep only recent positions (last 5 seconds)
current_time = time.time()
self.human_positions = [
(pos, ts) for pos, ts in self.human_positions
if current_time - ts < 5.0
]
def get_spatial_relationship(self) -> str:
"""Determine spatial relationship based on distance"""
if not self.human_positions:
return "OUT_OF_RANGE"
# Use most recent human position
human_pos = self.human_positions[-1][0]
distance = np.linalg.norm(human_pos - self.robot_position)
if distance <= self.intimate_distance:
return "INTIMATE_SPACE"
elif distance <= self.personal_distance:
return "PERSONAL_SPACE"
elif distance <= self.social_distance:
return "SOCIAL_SPACE"
else:
return "PUBLIC_SPACE"
def get_appropriate_behavior(self) -> str:
"""Get appropriate robot behavior based on proxemics"""
relationship = self.get_spatial_relationship()
if relationship == "INTIMATE_SPACE":
return "MAINTAIN_DISTANCE" # Too close, back away
elif relationship == "PERSONAL_SPACE":
return "NORMAL_INTERACTION" # Good distance for interaction
elif relationship == "SOCIAL_SPACE":
return "APPROACH_GRADUALLY" # Can approach for interaction
else:
return "REQUEST_ATTENTION" # Too far, need to get attention
class NaturalInteractionManager:
"""
Main class for managing natural human-robot interaction
"""
def __init__(self):
self.voice_rec = VoiceRecognition()
self.gesture_rec = GestureRecognition()
self.proxemics = ProxemicsManager()
self.event_queue = queue.Queue()
self.interaction_callbacks = {}
self.running = False
self.interaction_thread = None
def register_callback(self, event_type: str, callback: Callable):
"""Register callback for specific interaction events"""
if event_type not in self.interaction_callbacks:
self.interaction_callbacks[event_type] = []
self.interaction_callbacks[event_type].append(callback)
def trigger_event(self, event: InteractionEvent):
"""Trigger registered callbacks for an event"""
if event.event_type in self.interaction_callbacks:
for callback in self.interaction_callbacks[event.event_type]:
try:
callback(event)
except Exception as e:
print(f"Error in callback for {event.event_type}: {e}")
def start_interaction_monitoring(self):
"""Start monitoring for natural interactions"""
self.running = True
self.interaction_thread = threading.Thread(target=self._monitor_interactions)
self.interaction_thread.start()
def stop_interaction_monitoring(self):
"""Stop monitoring for natural interactions"""
self.running = False
if self.interaction_thread:
self.interaction_thread.join()
def _monitor_interactions(self):
"""Main loop for monitoring interactions"""
while self.running:
# Check for voice commands
voice_cmd = self.voice_rec.listen_for_commands()
if voice_cmd:
action = self.voice_rec.process_voice_command(voice_cmd)
if action:
event = InteractionEvent(
event_type="VOICE_COMMAND",
timestamp=time.time(),
data={"command": voice_cmd, "action": action}
)
self.trigger_event(event)
# Check for gestures (simulated)
if np.random.random() > 0.8: # 20% chance per iteration
gesture_data = {"type": "wave", "confidence": 0.9}
gesture_action = self.gesture_rec.recognize_gesture(gesture_data)
if gesture_action:
event = InteractionEvent(
event_type="GESTURE_RECOGNIZED",
timestamp=time.time(),
data={"gesture": gesture_data, "action": gesture_action}
)
self.trigger_event(event)
# Update proxemics (simulated)
if np.random.random() > 0.9: # 10% chance per iteration
# Simulate human moving around
human_pos = np.array([
np.random.uniform(-2, 2),
np.random.uniform(-2, 2),
0 # Assume flat ground
])
self.proxemics.update_human_position(human_pos, time.time())
spatial_rel = self.proxemics.get_spatial_relationship()
behavior = self.proxemics.get_appropriate_behavior()
event = InteractionEvent(
event_type="SPATIAL_RELATIONSHIP",
timestamp=time.time(),
data={"relationship": spatial_rel, "behavior": behavior, "human_pos": human_pos.tolist()}
)
self.trigger_event(event)
time.sleep(0.1) # Check every 100ms
def add_human_follower(self, robot_controller):
"""Add capability to follow humans"""
def follow_callback(event):
if event.data.get("action") == "FOLLOW_HUMAN":
print("Following human...")
robot_controller.follow_human()
self.register_callback("VOICE_COMMAND", follow_callback)
# Example robot controller (simulated)
class SimulatedRobotController:
def __init__(self):
self.position = np.array([0, 0, 0])
self.orientation = 0 # Heading angle in radians
def move_to(self, pose):
"""Move robot to specified pose"""
target_pos = pose[:3]
print(f"Moving robot to position: {target_pos}")
self.position = target_pos
def follow_human(self):
"""Simulate following a human"""
print("Robot is following human...")
def grab_object(self):
"""Simulate grabbing an object"""
print("Robot is grabbing object...")
def release_object(self):
"""Simulate releasing an object"""
print("Robot is releasing object...")
# Example usage
def main():
print("Initializing natural interaction system...")
# Create interaction manager
interaction_mgr = NaturalInteractionManager()
robot_ctrl = SimulatedRobotController()
# Register callbacks for different events
def voice_command_callback(event):
print(f"Voice command received: {event.data['command']} -> {event.data['action']}")
# Execute appropriate action
action = event.data['action']
if action == "GRAB_OBJECT":
robot_ctrl.grab_object()
elif action == "RELEASE_OBJECT":
robot_ctrl.release_object()
def gesture_callback(event):
print(f"Gesture recognized: {event.data['action']}")
# Respond to gesture
if event.data['action'] == "COME_CLOSER":
print("Robot is approaching human")
robot_ctrl.move_to(robot_ctrl.position + np.array([0.5, 0, 0]))
def spatial_callback(event):
rel = event.data['relationship']
behavior = event.data['behavior']
pos = np.array(event.data['human_pos'])
print(f"Human at {pos}, relationship: {rel}, suggested behavior: {behavior}")
# Register callbacks
interaction_mgr.register_callback("VOICE_COMMAND", voice_command_callback)
interaction_mgr.register_callback("GESTURE_RECOGNIZED", gesture_callback)
interaction_mgr.register_callback("SPATIAL_RELATIONSHIP", spatial_callback)
# Add human follower capability
interaction_mgr.add_human_follower(robot_ctrl)
print("Starting interaction monitoring... Press Ctrl+C to stop")
try:
# Start monitoring
interaction_mgr.start_interaction_monitoring()
# Let it run for a while
for i in range(50): # Run for about 5 seconds (50 * 0.1s)
time.sleep(0.1)
except KeyboardInterrupt:
print("\nStopping interaction monitoring...")
finally:
interaction_mgr.stop_interaction_monitoring()
print("Interaction system stopped.")
if __name__ == "__main__":
main()
Expected Output:
Initializing natural interaction system...
Starting interaction monitoring... Press Ctrl+C to stop
Voice command received: pick up -> GRAB_OBJECT
Robot is grabbing object...
Gesture recognized: ACKNOWLEDGE_PRESENCE
Robot is approaching human
Human at [0.5 -1.2 0. ], relationship: SOCIAL_SPACE, suggested behavior: APPROACH_GRADUALLY
Voice command received: put down -> RELEASE_OBJECT
Robot is releasing object...
Gesture recognized: COME_CLOSER
Robot is approaching human
Human at [1.2 0.8 0. ], relationship: PERSONAL_SPACE, suggested behavior: NORMAL_INTERACTION
Interaction system stopped.
Hands-on Exercises
- Implement a compliant control system for safe manipulation that adjusts robot stiffness based on contact forces
- Create a multimodal interaction system that combines speech and gesture recognition for commanding robot actions
- Develop a proxemics-aware navigation system that respects human personal space during mobile manipulation
Summary
Natural human-robot interaction combines multiple modalities to create intuitive and safe ways for humans to interact with robots. Effective manipulation requires sophisticated grasp planning and force control, while natural interaction demands understanding of human social signals, spatial relationships, and communication patterns. The integration of these capabilities enables robots to work alongside humans in collaborative environments safely and effectively.
Hardware Requirements
This chapter requires specialized hardware and software:
- Robot manipulator with force/torque sensing capabilities
- 3D camera or depth sensor for object detection and gesture recognition
- Microphone array for speech recognition
- Real-time computing platform for responsive interaction
- Safety-rated robot for human-robot collaboration
- Simulation environment for development and testing (e.g., Gazebo, Isaac Sim)