Lab 07-03: Dexterous Multi-Finger Manipulation
Objectives​
By the end of this lab, you will be able to:
- Control a multi-fingered robotic hand in simulation
- Implement grasp synthesis for complex objects
- Execute in-hand manipulation (regrasp, rotation)
- Analyze grasp stability using contact analysis
Prerequisites​
- Completed Labs 07-01 and 07-02
- Strong understanding of kinematics and Jacobians
- Familiarity with constrained optimization
Materials​
| Type | Name | Tier | Notes |
|---|---|---|---|
| software | MuJoCo 3.0+ | required | Physics with multi-contact |
| software | Python 3.10+ | required | Programming environment |
| software | NumPy, SciPy | required | Optimization |
| simulation | shadow-hand.xml | required | Shadow Dexterous Hand model |
Background​
Dexterous Manipulation​
Dexterous manipulation enables:
- In-hand repositioning without regrasping
- Manipulation of complex, non-convex objects
- Human-like object handling capabilities
Multi-Finger Coordination​
Key challenges:
- High-dimensional joint space (20+ DOF for Shadow Hand)
- Contact mode transitions (slip, stick, break)
- Force closure maintenance during motion
- Fingertip rolling and sliding
Grasp Quality Metrics​
- Force Closure: Wrench space spanned by contacts
- Grasp Wrench Space (GWS): Set of achievable wrenches
- ε-metric: Largest inscribed ball in GWS
Instructions​
Step 1: Load the Dexterous Hand Model​
Initialize the Shadow Hand simulation:
import mujoco
import numpy as np
from scipy.optimize import minimize
from scipy.spatial import ConvexHull
import matplotlib.pyplot as plt
# Load Shadow Hand model
model = mujoco.MjModel.from_xml_path(
"textbook/assets/robot-models/mujoco/shadow-hand.xml"
)
data = mujoco.MjData(model)
# Explore the model structure
print(f"Number of joints: {model.njnt}")
print(f"Number of actuators: {model.nu}")
print(f"Number of bodies: {model.nbody}")
# Identify finger components
finger_names = ['thumb', 'index', 'middle', 'ring', 'little']
finger_joints = {}
finger_tips = {}
for finger in finger_names:
# Find joints for this finger
joints = []
for i in range(model.njnt):
name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, i)
if name and finger in name.lower():
joints.append((i, name))
finger_joints[finger] = joints
# Find fingertip body
for i in range(model.nbody):
name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, i)
if name and finger in name.lower() and 'tip' in name.lower():
finger_tips[finger] = i
print("\nFinger structure:")
for finger, joints in finger_joints.items():
print(f" {finger}: {len(joints)} joints, tip_id={finger_tips.get(finger, 'N/A')}")
# Initialize
mujoco.mj_resetData(model, data)
mujoco.mj_forward(model, data)
Expected: Shadow Hand model loaded with 20+ joints, 5 finger kinematic chains identified.
Step 2: Implement Finger Kinematics​
Compute forward kinematics and Jacobians for each finger:
class FingerKinematics:
"""Kinematics for a single finger chain."""
def __init__(self, model, data, finger_name, tip_body_id, joint_ids):
self.model = model
self.data = data
self.name = finger_name
self.tip_id = tip_body_id
self.joint_ids = joint_ids # List of joint indices
# Get joint position indices in qpos
self.qpos_indices = []
for jid in joint_ids:
adr = model.jnt_qposadr[jid]
self.qpos_indices.append(adr)
def get_tip_pose(self):
"""Get fingertip position and orientation."""
pos = self.data.xpos[self.tip_id].copy()
quat = self.data.xquat[self.tip_id].copy()
return pos, quat
def get_jacobian(self):
"""Compute Jacobian from finger base to tip."""
jacp = np.zeros((3, self.model.nv))
jacr = np.zeros((3, self.model.nv))
mujoco.mj_jacBody(self.model, self.data, jacp, jacr, self.tip_id)
# Extract columns for this finger's joints
finger_jacp = jacp[:, self.qpos_indices]
finger_jacr = jacr[:, self.qpos_indices]
return finger_jacp, finger_jacr
def set_joint_positions(self, q):
"""Set finger joint positions."""
for i, idx in enumerate(self.qpos_indices):
self.data.qpos[idx] = q[i]
mujoco.mj_forward(self.model, self.data)
# Create finger kinematics objects
fingers = {}
for finger_name in finger_names:
if finger_name in finger_tips:
# Get joint IDs for this finger
joint_ids = [j[0] for j in finger_joints[finger_name]]
fingers[finger_name] = FingerKinematics(
model, data, finger_name, finger_tips[finger_name], joint_ids
)
# Test kinematics
for name, fk in fingers.items():
pos, quat = fk.get_tip_pose()
jacp, _ = fk.get_jacobian()
print(f"{name} tip: pos={pos}, jacobian shape={jacp.shape}")
Expected: Finger kinematics objects created with working forward kinematics and Jacobians.
Step 3: Implement Contact-Based Grasp Analysis​
Analyze grasp quality using contact information:
class GraspAnalyzer:
"""Analyze grasp quality from contact information."""
def __init__(self, model, data, object_body_id):
self.model = model
self.data = data
self.object_id = object_body_id
def get_contacts_with_object(self):
"""Extract all contacts with the grasped object."""
contacts = []
for i in range(self.data.ncon):
contact = self.data.contact[i]
geom1 = contact.geom1
geom2 = contact.geom2
body1 = self.model.geom_bodyid[geom1]
body2 = self.model.geom_bodyid[geom2]
# Check if one body is the object
```python
```python
if body1 == self.object_id or body2 == self.object_id:
# Get contact information
pos = contact.pos.copy()
normal = contact.frame[:3].copy()
# Get contact force
force = np.zeros(6)
mujoco.mj_contactForce(self.model, self.data, i, force)
contacts.append({
'position': pos,
'normal': normal,
'force': force[:3],
'geom1': geom1,
'geom2': geom2
})
return contacts
def compute_grasp_matrix(self, contacts, object_center):
"""
Compute the grasp matrix G that maps contact forces to object wrench.
W_object = G @ f_contacts
Args:
contacts: List of contact dictionaries
object_center: Object center position
Returns:
G: 6 x 3n grasp matrix
"""
n = len(contacts)
```python
```python
if n == 0:
return np.zeros((6, 0))
G = np.zeros((6, 3 * n))
for i, contact in enumerate(contacts):
r = contact['position'] - object_center
# Force component
G[:3, 3*i:3*i+3] = np.eye(3)
# Torque component (cross product matrix)
G[3:6, 3*i:3*i+3] = np.array([
[0, -r[2], r[1]],
[r[2], 0, -r[0]],
[-r[1], r[0], 0]
])
return G
def compute_force_closure(self, contacts, object_center, mu=0.5):
"""
Check if grasp achieves force closure.
Uses the friction cone linearization approach.
Args:
contacts: Contact list
object_center: Object center
mu: Friction coefficient
Returns:
is_closure: Boolean
epsilon: Quality metric (largest perturbation that can be resisted)
"""
n = len(contacts)
```python
```python
if n < 3:
return False, 0.0
# Build wrench set from friction cones
# Linearize friction cone with 4 edges
n_edges = 4
wrenches = []
for contact in contacts:
r = contact['position'] - object_center
normal = contact['normal']
# Build friction cone basis
```python
```python
if abs(normal[2]) < 0.9:
t1 = np.cross(normal, [0, 0, 1])
else:
t1 = np.cross(normal, [1, 0, 0])
t1 /= np.linalg.norm(t1)
t2 = np.cross(normal, t1)
# Generate cone edges
for j in range(n_edges):
angle = 2 * np.pi * j / n_edges
direction = normal + mu * (np.cos(angle) * t1 + np.sin(angle) * t2)
direction /= np.linalg.norm(direction)
# Compute wrench
torque = np.cross(r, direction)
wrench = np.concatenate([direction, torque])
wrenches.append(wrench)
wrenches = np.array(wrenches)
# Check if origin is inside convex hull of wrenches
try:
hull = ConvexHull(wrenches)
# Compute distance to convex hull
# Simplified: check if we can achieve any wrench direction
# More sophisticated: find largest ball around origin in GWS
# Using linear programming
from scipy.optimize import linprog
# Find minimum over all wrench directions
epsilon = float('inf')
for direction in [np.eye(6)[i] for i in range(6)] + [-np.eye(6)[i] for i in range(6)]:
# Can we achieve unit wrench in this direction?
# min c^T x s.t. G @ x = direction, x >= 0
# Using wrench matrix form
c = np.ones(len(wrenches))
result = linprog(c, A_eq=wrenches.T, b_eq=direction,
bounds=(0, None), method='highs')
if result.success:
```python
```python
epsilon = min(epsilon, 1.0 / result.fun if result.fun > 0 else float('inf'))
else:
epsilon = 0
break
```python
```python
is_closure = epsilon > 0.01
return is_closure, min(epsilon, 1.0)
except Exception:
return False, 0.0
def evaluate_grasp(self):
"""Full grasp evaluation."""
contacts = self.get_contacts_with_object()
object_center = self.data.xpos[self.object_id].copy()
result = {
'n_contacts': len(contacts),
'contacts': contacts
}
```python
```python
if len(contacts) >= 2:
G = self.compute_grasp_matrix(contacts, object_center)
result['grasp_matrix'] = G
result['grasp_rank'] = np.linalg.matrix_rank(G)
# Check force closure
is_closure, epsilon = self.compute_force_closure(contacts, object_center)
result['force_closure'] = is_closure
result['epsilon'] = epsilon
else:
result['force_closure'] = False
result['epsilon'] = 0
return result
# Find object body
object_body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "object")
```python
```python
if object_body_id >= 0:
analyzer = GraspAnalyzer(model, data, object_body_id)
print("GraspAnalyzer ready")
Expected: GraspAnalyzer created with methods for contact extraction and closure checking.
Step 4: Implement Multi-Finger Grasp Controller​
Control all fingers for coordinated grasping:
class MultiFingerGraspController:
"""Controller for coordinated multi-finger grasping."""
def __init__(self, model, data, fingers, object_body_id):
self.model = model
self.data = data
self.fingers = fingers
self.object_id = object_body_id
# Control gains
self.kp = 5.0 # Position gain
self.kd = 0.5 # Damping gain
self.kf = 0.1 # Force gain
# Grasp state
self.target_contacts = {}
self.grasp_force = 2.0 # N per finger
def set_target_contacts(self, contact_points):
"""
Set target contact points for each finger.
Args:
contact_points: Dict mapping finger name to target 3D position
"""
self.target_contacts = contact_points
def compute_approach_torques(self):
"""
Compute torques to approach target contact points.
Returns:
tau: Full joint torque vector
"""
tau = np.zeros(self.model.nv)
for finger_name, target_pos in self.target_contacts.items():
if finger_name not in self.fingers:
continue
fk = self.fingers[finger_name]
current_pos, _ = fk.get_tip_pose()
# Position error
error = target_pos - current_pos
# Get Jacobian
jacp, _ = fk.get_jacobian()
# Damping from velocity
finger_vel = np.zeros(len(fk.qpos_indices))
for i, idx in enumerate(fk.qpos_indices):
finger_vel[i] = self.data.qvel[idx]
# Cartesian velocity
cart_vel = jacp @ finger_vel
# Compute Cartesian force
F = self.kp * error - self.kd * cart_vel
# Map to joint torques (simplified: use transpose)
finger_tau = jacp.T @ F
# Apply to full torque vector
for i, idx in enumerate(fk.qpos_indices):
tau[idx] += finger_tau[i]
return tau
def compute_squeeze_torques(self, squeeze_force):
"""
Compute torques to apply squeezing force toward object center.
Args:
squeeze_force: Force magnitude toward object
Returns:
tau: Joint torques
"""
tau = np.zeros(self.model.nv)
object_pos = self.data.xpos[self.object_id]
for finger_name, fk in self.fingers.items():
current_pos, _ = fk.get_tip_pose()
# Direction toward object center
direction = object_pos - current_pos
dist = np.linalg.norm(direction)
```python
```python
if dist > 0.001:
direction /= dist
# Force toward object
F = squeeze_force * direction
# Get Jacobian and map to joints
jacp, _ = fk.get_jacobian()
finger_tau = jacp.T @ F
for i, idx in enumerate(fk.qpos_indices):
tau[idx] += finger_tau[i]
return tau
def execute_grasp(self, n_steps=1000, approach_steps=500):
"""
Execute full grasp sequence.
Args:
n_steps: Total simulation steps
approach_steps: Steps for approach phase
Returns:
history: Recorded data
"""
history = {
'step': [],
'contacts': [],
'force_closure': [],
'object_pos': []
}
for step in range(n_steps):
# Phase 1: Approach
```python
```python
if step < approach_steps:
tau = self.compute_approach_torques()
# Phase 2: Squeeze
else:
tau = self.compute_squeeze_torques(self.grasp_force)
# Apply torques
tau = np.clip(tau, -10, 10)
self.data.ctrl[:len(tau)] = tau
# Step simulation
mujoco.mj_step(self.model, self.data)
# Record periodically
```python
```python
if step % 50 == 0:
analyzer = GraspAnalyzer(self.model, self.data, self.object_id)
result = analyzer.evaluate_grasp()
history['step'].append(step)
history['contacts'].append(result['n_contacts'])
history['force_closure'].append(result['force_closure'])
history['object_pos'].append(self.data.xpos[self.object_id].copy())
return history
# Create grasp controller
```python
```python
if object_body_id >= 0:
grasp_ctrl = MultiFingerGraspController(model, data, fingers, object_body_id)
# Set target contacts around object
object_pos = data.xpos[object_body_id]
object_size = 0.03 # Approximate object radius
target_contacts = {
'thumb': object_pos + np.array([-object_size, 0, 0]),
'index': object_pos + np.array([object_size, object_size, 0]),
'middle': object_pos + np.array([object_size, 0, 0]),
'ring': object_pos + np.array([object_size, -object_size, 0]),
}
grasp_ctrl.set_target_contacts(target_contacts)
print("MultiFingerGraspController ready with targets set")
Expected: Controller configured with target contact positions for approach and squeeze phases.
Step 5: Implement In-Hand Manipulation​
Execute controlled rotation of grasped object:
class InHandManipulator:
"""Controller for in-hand object manipulation."""
def __init__(self, model, data, fingers, object_body_id):
self.model = model
self.data = data
self.fingers = fingers
self.object_id = object_body_id
# Manipulation parameters
self.rotation_gain = 0.5
self.squeeze_force = 3.0
def compute_rotation_torques(self, axis, angle_rate):
"""
Compute fingertip motions to rotate object about axis.
Args:
axis: Rotation axis (unit vector)
angle_rate: Desired angular velocity (rad/s)
Returns:
tau: Joint torques
"""
tau = np.zeros(self.model.nv)
object_pos = self.data.xpos[self.object_id]
for finger_name, fk in self.fingers.items():
tip_pos, _ = fk.get_tip_pose()
# Vector from object center to fingertip
r = tip_pos - object_pos
# Desired tangential velocity
v_desired = angle_rate * np.cross(axis, r)
# Get current velocity
jacp, _ = fk.get_jacobian()
finger_vel = np.zeros(len(fk.qpos_indices))
for i, idx in enumerate(fk.qpos_indices):
finger_vel[i] = self.data.qvel[idx]
v_current = jacp @ finger_vel
# Velocity error
v_error = v_desired - v_current
# Force from velocity control
F_rotation = 2.0 * v_error
# Add inward squeeze force
squeeze_dir = object_pos - tip_pos
squeeze_dist = np.linalg.norm(squeeze_dir)
```python
```python
if squeeze_dist > 0.001:
squeeze_dir /= squeeze_dist
F_squeeze = self.squeeze_force * squeeze_dir
# Total force
F_total = F_rotation + F_squeeze
# Map to joint torques
finger_tau = jacp.T @ F_total
for i, idx in enumerate(fk.qpos_indices):
tau[idx] += finger_tau[i]
return tau
def rotate_object(self, axis, target_angle, duration=2.0, dt=0.002):
"""
Rotate object by specified angle.
Args:
axis: Rotation axis (normalized)
target_angle: Target rotation in radians
duration: Time for rotation
Returns:
history: Rotation trajectory data
"""
history = {
'time': [],
'angle': [],
'object_quat': []
}
axis = axis / np.linalg.norm(axis)
angle_rate = target_angle / duration
steps = int(duration / dt)
# Record initial orientation
quat_init = self.data.xquat[self.object_id].copy()
for step in range(steps):
# Compute manipulation torques
tau = self.compute_rotation_torques(axis, angle_rate)
tau = np.clip(tau, -10, 10)
self.data.ctrl[:len(tau)] = tau
# Step simulation
mujoco.mj_step(self.model, self.data)
# Compute rotation angle from initial
quat_current = self.data.xquat[self.object_id].copy()
# Quaternion difference to get rotation
# Simplified: compute angle from w component
# For small rotations: angle ≈ 2 * arccos(|w|)
quat_diff = self._quat_multiply(quat_current, self._quat_conjugate(quat_init))
angle = 2 * np.arccos(np.clip(abs(quat_diff[0]), -1, 1))
history['time'].append(step * dt)
history['angle'].append(angle)
history['object_quat'].append(quat_current.copy())
return history
def _quat_multiply(self, q1, q2):
"""Quaternion multiplication."""
w1, x1, y1, z1 = q1
w2, x2, y2, z2 = q2
return np.array([
w1*w2 - x1*x2 - y1*y2 - z1*z2,
w1*x2 + x1*w2 + y1*z2 - z1*y2,
w1*y2 - x1*z2 + y1*w2 + z1*x2,
w1*z2 + x1*y2 - y1*x2 + z1*w2
])
def _quat_conjugate(self, q):
"""Quaternion conjugate."""
return np.array([q[0], -q[1], -q[2], -q[3]])
# Create manipulator
```python
```python
if object_body_id >= 0:
manipulator = InHandManipulator(model, data, fingers, object_body_id)
print("InHandManipulator ready")
Expected: InHandManipulator created with object rotation capability.
Step 6: Full Manipulation Demonstration​
Execute complete pick, rotate, and place sequence:
def full_manipulation_demo(grasp_ctrl, manipulator, analyzer):
"""
Execute complete manipulation demonstration.
Sequence:
1. Approach object
2. Form grasp
3. Verify force closure
4. Rotate object 90°
5. Verify grasp maintained
"""
print("=" * 50)
print("DEXTEROUS MANIPULATION DEMONSTRATION")
print("=" * 50)
# Phase 1: Approach and grasp
print("\n[Phase 1] Approaching and grasping object...")
history = grasp_ctrl.execute_grasp(n_steps=800, approach_steps=400)
result = analyzer.evaluate_grasp()
print(f" Contacts established: {result['n_contacts']}")
print(f" Force closure: {result['force_closure']}")
print(f" Epsilon metric: {result.get('epsilon', 0):.3f}")
if not result['force_closure']:
print(" WARNING: No force closure - grasp may fail")
# Phase 2: Lift test
print("\n[Phase 2] Testing grasp stability...")
initial_z = grasp_ctrl.data.xpos[grasp_ctrl.object_id][2]
# Apply upward motion to hand base (if applicable)
for _ in range(200):
tau = grasp_ctrl.compute_squeeze_torques(5.0)
tau = np.clip(tau, -10, 10)
grasp_ctrl.data.ctrl[:len(tau)] = tau
mujoco.mj_step(grasp_ctrl.model, grasp_ctrl.data)
result = analyzer.evaluate_grasp()
print(f" Contacts maintained: {result['n_contacts']}")
# Phase 3: In-hand rotation
print("\n[Phase 3] Rotating object 90°...")
rotation_history = manipulator.rotate_object(
axis=np.array([0, 0, 1]), # Z-axis rotation
target_angle=np.pi/2,
duration=3.0
)
final_angle = rotation_history['angle'][-1]
print(f" Target rotation: {np.pi/2:.3f} rad")
print(f" Achieved rotation: {final_angle:.3f} rad")
print(f" Rotation error: {abs(np.pi/2 - final_angle):.3f} rad")
# Phase 4: Final grasp check
print("\n[Phase 4] Verifying final grasp...")
result = analyzer.evaluate_grasp()
print(f" Final contacts: {result['n_contacts']}")
print(f" Force closure maintained: {result['force_closure']}")
print("\n" + "=" * 50)
print("DEMONSTRATION COMPLETE")
print("=" * 50)
return {
'grasp_history': history,
'rotation_history': rotation_history,
'final_grasp': result
}
# Run demonstration (if models are properly loaded)
print("\nTo run full demonstration:")
print(" demo_result = full_manipulation_demo(grasp_ctrl, manipulator, analyzer)")
Expected: Framework for complete manipulation sequence defined and ready to execute.
Expected Outcomes​
After completing this lab, you should have:
-
Code artifacts:
finger_kinematics.py: Multi-finger FK and Jacobiansgrasp_analyzer.py: Contact analysis and quality metricsmulti_finger_controller.py: Coordinated grasp controlin_hand_manipulator.py: Rotation and repositioning
-
Understanding:
- High-DOF hand control strategies
- Force closure concept and verification
- Contact mode management during manipulation
- Challenges of dexterous manipulation
-
Experimental results:
- Grasp quality metrics (ε-metric, contact count)
- In-hand rotation accuracy
- Grasp stability during manipulation
Rubric​
| Criterion | Points | Description |
|---|---|---|
| Finger Kinematics | 15 | Correct FK and Jacobian for all fingers |
| Grasp Analysis | 20 | Working contact extraction and closure check |
| Multi-Finger Control | 25 | Coordinated approach and squeeze |
| In-Hand Manipulation | 25 | Successful object rotation |
| Integration | 15 | Complete pick-rotate sequence |
| Total | 100 |
Common Errors​
Error: Object drops during rotation Solution: Increase squeeze force, ensure all fingers maintain contact.
Error: Grasp analysis fails with few contacts Solution: Check collision groups, adjust approach positions, verify friction.
Error: Fingers collide with each other Solution: Add collision avoidance, use different approach angles.
Extensions​
For advanced students:
- Tactile Sensing: Use simulated tactile arrays for slip detection
- Learned Manipulation: Train policy with RL for complex objects
- Contact-Rich Tasks: Tool use (screwdriver, hammer)
- Bimanual: Coordinate two hands for large object manipulation
Related Content​
- Theory: Module 07 theory.md, Section 7.4 (Dexterous Manipulation)
- Previous Lab: Lab 07-02 (Force Control)
- Application: Robotic prosthetics, surgical robots, warehouse automation
- Research: OpenAI Rubik's cube, Stanford dexterity benchmarks