Lab 07-01: Basic Grasping in Simulation

Objectives

By the end of this lab, you will be able to:

• Implement a simple grasp planner for known object poses
• Execute pre-grasp approach and grasp motions
• Use MuJoCo contact simulation for grip verification
• Evaluate grasp success through object lifting

Prerequisites

• Completed Module 06 (Motion Planning)
• Understanding of rigid body transformations
• Familiarity with MuJoCo actuators and contacts

Materials

Type	Name	Tier	Notes
software	MuJoCo 3.0+	required	Physics simulation with contacts
software	Python 3.10+	required	Programming environment
software	NumPy	required	Linear algebra
simulation	gripper-table.xml	required	Parallel jaw gripper with table

Background

Grasp Planning Pipeline

Basic grasping involves:

1. Object localization: Know where the object is
2. Grasp synthesis: Generate candidate grasp poses
3. Approach planning: Move gripper to pre-grasp position
4. Grasp execution: Close gripper on object
5. Lift verification: Confirm successful grasp

Parallel Jaw Grasping

For parallel jaw grippers, a grasp is defined by:

• Approach direction (z-axis of gripper frame)
• Grasp center point on object
• Gripper opening width

Instructions

Step 1: Set Up the Simulation Environment

Load the gripper and table scene:

import mujoco
import numpy as np
import matplotlib.pyplot as plt

# Load model with gripper and objects
model = mujoco.MjModel.from_xml_path("textbook/assets/robot-models/mujoco/gripper-table.xml")
data = mujoco.MjData(model)

# Explore the model
print("Bodies:", [mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, i)
                  for i in range(model.nbody)])
print("Actuators:", [mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_ACTUATOR, i)
                     for i in range(model.nu)])

# Get gripper and object IDs
gripper_body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "gripper_base")
object_body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "target_object")

# Reset and step to initialize
mujoco.mj_resetData(model, data)
mujoco.mj_forward(model, data)

print(f"Gripper position: {data.xpos[gripper_body_id]}")
print(f"Object position: {data.xpos[object_body_id]}")

Expected: Model loads with gripper, object, and table bodies identified.

Step 2: Define Grasp Poses

Create a simple grasp pose generator:

def generate_top_grasp(object_pos, object_size, gripper_height=0.1):
    """
    Generate a top-down grasp pose for a box object.

    Args:
        object_pos: [x, y, z] object center position
        object_size: [sx, sy, sz] object half-sizes
        gripper_height: Height above object for pre-grasp

    Returns:
        pre_grasp_pose: [x, y, z, qw, qx, qy, qz] pre-grasp position/orientation
        grasp_pose: Final grasp position/orientation
        gripper_width: Required gripper opening
    """
    # Grasp from top, gripper pointing down
    # Quaternion for gripper pointing -Z: rotate 180° around X
    quat_down = [0, 1, 0, 0]  # [qw, qx, qy, qz]

    # Pre-grasp: above the object
    pre_grasp_pos = np.array([
        object_pos[0],
        object_pos[1],
        object_pos[2] + object_size[2] + gripper_height
    ])

    # Grasp position: fingers at object center height
    grasp_pos = np.array([
        object_pos[0],
        object_pos[1],
        object_pos[2] + object_size[2] + 0.02  # Small offset above
    ])

    # Gripper width: slightly wider than object
    gripper_width = 2 * object_size[0] + 0.01  # Grasp along X dimension

    pre_grasp_pose = np.concatenate([pre_grasp_pos, quat_down])
    grasp_pose = np.concatenate([grasp_pos, quat_down])

    return pre_grasp_pose, grasp_pose, gripper_width

# Get object properties
object_pos = data.xpos[object_body_id].copy()
object_geom_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, "target_object_geom")
object_size = model.geom_size[object_geom_id].copy()

print(f"Object position: {object_pos}")
print(f"Object size (half-extents): {object_size}")

# Generate grasp poses
pre_grasp, grasp, width = generate_top_grasp(object_pos, object_size)
print(f"\nPre-grasp pose: {pre_grasp[:3]}")
print(f"Grasp pose: {grasp[:3]}")
print(f"Required gripper width: {width:.4f} m")

Expected: Grasp poses generated above the object with appropriate gripper width.

Step 3: Implement Gripper Control

Create functions to control gripper position and fingers:

class GripperController:
    """Controller for a parallel jaw gripper on a Cartesian arm."""

    def __init__(self, model, data):
        self.model = model
        self.data = data

        # Find actuator indices
        self.arm_actuator_ids = []
        self.finger_actuator_ids = []

        for i in range(model.nu):
            name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_ACTUATOR, i)
            if 'finger' in name.lower() or 'grip' in name.lower():
                self.finger_actuator_ids.append(i)
            else:
                self.arm_actuator_ids.append(i)

        print(f"Arm actuators: {len(self.arm_actuator_ids)}")
        print(f"Finger actuators: {len(self.finger_actuator_ids)}")

        # Control gains
        self.kp_pos = 500
        self.kd_pos = 50
        self.kp_grip = 100

    def set_gripper_width(self, target_width, force_limit=20):
        """
        Set gripper opening width.

        Args:
            target_width: Desired finger separation (meters)
            force_limit: Maximum grip force (N)
        """
        # Assuming finger actuators control position directly
        finger_pos = target_width / 2  # Each finger moves half the distance

        for act_id in self.finger_actuator_ids:
            self.data.ctrl[act_id] = finger_pos

    def close_gripper(self, force=20):
        """Close gripper with specified force."""
        for act_id in self.finger_actuator_ids:
            self.data.ctrl[act_id] = -force  # Negative for closing

    def open_gripper(self, width=0.1):
        """Open gripper to specified width."""
        finger_pos = width / 2
        for act_id in self.finger_actuator_ids:
            self.data.ctrl[act_id] = finger_pos

    def move_to_pose(self, target_pos, n_steps=500):
        """
        Move gripper to target position using simple P control.

        This is simplified - assumes Cartesian position control is available.
        In practice, would use IK + joint control.
        """
        gripper_body_id = mujoco.mj_name2id(
            self.model, mujoco.mjtObj.mjOBJ_BODY, "gripper_base"
        )

        for _ in range(n_steps):
            # Get current position
            current_pos = self.data.xpos[gripper_body_id].copy()

            # Position error
            error = target_pos[:3] - current_pos

            # Simple proportional control on arm actuators
            # This is highly simplified - real systems use IK
            for i, act_id in enumerate(self.arm_actuator_ids[:3]):
                self.data.ctrl[act_id] = self.kp_pos * error[i]

            mujoco.mj_step(self.model, self.data)

            # Check convergence
```python
```python
            if np.linalg.norm(error) < 0.005:

                return True

        return False

# Create controller
controller = GripperController(model, data)

Expected: GripperController created with arm and finger actuators identified.

Step 4: Execute Pick Sequence

Implement the full pick operation:

def execute_pick(controller, pre_grasp, grasp, lift_height=0.15):
    """
    Execute a pick operation.

    Args:
        controller: GripperController instance
        pre_grasp: Pre-grasp pose [x, y, z, qw, qx, qy, qz]
        grasp: Grasp pose
        lift_height: Height to lift after grasping

    Returns:
        success: True if object was lifted
    """
    print("Step 1: Open gripper...")
    controller.open_gripper(width=0.1)
    for _ in range(100):
        mujoco.mj_step(controller.model, controller.data)

    print("Step 2: Move to pre-grasp...")
    success = controller.move_to_pose(pre_grasp, n_steps=500)
    if not success:
        print("  Failed to reach pre-grasp")
        return False

    print("Step 3: Move to grasp pose...")
    success = controller.move_to_pose(grasp, n_steps=300)
    if not success:
        print("  Failed to reach grasp pose")
        return False

    print("Step 4: Close gripper...")
    controller.close_gripper(force=30)
    for _ in range(200):
        mujoco.mj_step(controller.model, controller.data)

    print("Step 5: Lift object...")
    lift_pose = grasp.copy()
    lift_pose[2] += lift_height
    success = controller.move_to_pose(lift_pose, n_steps=500)

    # Check if object is still grasped (object height increased)
    object_body_id = mujoco.mj_name2id(
        controller.model, mujoco.mjtObj.mjOBJ_BODY, "target_object"
    )
    object_z = controller.data.xpos[object_body_id][2]
    initial_z = grasp[2] - 0.05  # Approximate table height

```python
```python
    lifted = object_z > initial_z + 0.05

    if lifted:
        print(f"  Object lifted to z={object_z:.3f}m")
    else:
        print(f"  Object dropped (z={object_z:.3f}m)")

    return lifted

# Reset simulation
mujoco.mj_resetData(model, data)
mujoco.mj_forward(model, data)

# Regenerate grasp for current object pose
object_pos = data.xpos[object_body_id].copy()
pre_grasp, grasp, width = generate_top_grasp(object_pos, object_size)

# Execute pick
success = execute_pick(controller, pre_grasp, grasp)
print(f"\nPick operation {'succeeded' if success else 'failed'}")

Expected: Pick sequence executes, object should be lifted if grasp is successful.

Step 5: Grasp Quality Evaluation

Implement grasp quality metrics:

def evaluate_grasp_quality(model, data, object_body_id):
    """
    Evaluate the quality of current grasp.

    Returns:
        Dictionary of grasp quality metrics
    """
    metrics = {}

    # Count contact points with object
    n_contacts = 0
    total_force = 0
    contact_points = []

    for i in range(data.ncon):
        contact = data.contact[i]
        geom1 = contact.geom1
        geom2 = contact.geom2

        # Check if one of the geoms is the object
        body1 = model.geom_bodyid[geom1]
        body2 = model.geom_bodyid[geom2]

```python
```python
        if body1 == object_body_id or body2 == object_body_id:

            n_contacts += 1
            contact_points.append(contact.pos.copy())

            # Get contact force
            force = np.zeros(6)
            mujoco.mj_contactForce(model, data, i, force)
            total_force += np.linalg.norm(force[:3])

    metrics['n_contacts'] = n_contacts
    metrics['total_force'] = total_force
    metrics['contact_points'] = contact_points

    # Grasp stability: check if contacts span the object
```python
```python
    if len(contact_points) >= 2:

        points = np.array(contact_points)
        span = np.max(points, axis=0) - np.min(points, axis=0)
        metrics['contact_span'] = np.linalg.norm(span)
    else:
        metrics['contact_span'] = 0

    # Simple quality score
```python
```python
    if n_contacts >= 2 and total_force > 5:

        metrics['quality_score'] = min(1.0, total_force / 50) * min(1.0, n_contacts / 4)
    else:
        metrics['quality_score'] = 0

    return metrics

# Evaluate current grasp
metrics = evaluate_grasp_quality(model, data, object_body_id)
print("\nGrasp Quality Metrics:")
print(f"  Contact points: {metrics['n_contacts']}")
print(f"  Total force: {metrics['total_force']:.2f} N")
print(f"  Contact span: {metrics['contact_span']:.4f} m")
print(f"  Quality score: {metrics['quality_score']:.2f}")

Expected: Grasp metrics computed showing contact information and quality score.

Step 6: Multiple Grasp Attempts

Test grasp robustness with different approaches:

def generate_side_grasp(object_pos, object_size, approach_axis='x'):
    """Generate a side grasp approaching from specified axis."""
```python
```python
    if approach_axis == 'x':

        # Approach from +X direction
        quat = [0.707, 0, 0.707, 0]  # Rotate 90° around Y
        offset = np.array([object_size[0] + 0.1, 0, 0])
        grasp_dim = object_size[1]  # Grasp along Y
    elif approach_axis == 'y':
        # Approach from +Y direction
        quat = [0.707, 0.707, 0, 0]  # Rotate 90° around X
        offset = np.array([0, object_size[1] + 0.1, 0])
        grasp_dim = object_size[0]  # Grasp along X
    else:
        return generate_top_grasp(object_pos, object_size)

    pre_grasp_pos = object_pos + offset
    grasp_pos = object_pos + offset * 0.3  # Move closer

    gripper_width = 2 * grasp_dim + 0.01

    pre_grasp_pose = np.concatenate([pre_grasp_pos, quat])
    grasp_pose = np.concatenate([grasp_pos, quat])

    return pre_grasp_pose, grasp_pose, gripper_width

def test_multiple_grasps(model, data, object_body_id, n_trials=3):
    """Test multiple grasp approaches."""
    results = []

    approaches = ['top', 'x', 'y']

    for approach in approaches:
        # Reset
        mujoco.mj_resetData(model, data)
        mujoco.mj_forward(model, data)

        controller = GripperController(model, data)
        object_pos = data.xpos[object_body_id].copy()

        # Get object size
        object_geom_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, "target_object_geom")
        object_size = model.geom_size[object_geom_id].copy()

        # Generate grasp
```python
```python
        if approach == 'top':

            pre_grasp, grasp, width = generate_top_grasp(object_pos, object_size)
        else:
            pre_grasp, grasp, width = generate_side_grasp(object_pos, object_size, approach)

        print(f"\nTesting {approach} grasp...")
        success = execute_pick(controller, pre_grasp, grasp)
        metrics = evaluate_grasp_quality(model, data, object_body_id)

        results.append({
            'approach': approach,
            'success': success,
            'quality': metrics['quality_score']
        })

    return results

# This would run multiple tests - simplified for demonstration
print("\nGrasp approach testing complete")

Expected: Framework for testing multiple grasp approaches defined.

Expected Outcomes

After completing this lab, you should have:

1. Code artifacts:

   • grasp_planner.py: Grasp pose generation functions
   • gripper_controller.py: Gripper motion and grip control
   • grasp_evaluation.py: Quality metrics computation

2. Visual outputs:

   • Console output showing pick sequence progress
   • Grasp quality metrics

3. Understanding:

   • Basic grasp planning pipeline
   • Contact-based grasp verification
   • Factors affecting grasp success

Rubric

Criterion	Points	Description
Grasp Pose Generation	20	Correct top-down grasp poses
Gripper Control	25	Working position and grip control
Pick Execution	25	Complete pick sequence that lifts object
Quality Evaluation	20	Contact-based metrics computation
Code Quality	10	Clean, documented, modular code
Total	100

Common Errors

Error: Object slips during lift Solution: Increase grip force, check object friction properties in XML.

Error: Gripper collides with table Solution: Adjust pre-grasp height, check IK solution validity.

Error: No contacts detected Solution: Verify geom collision groups, check contact margin settings.

Extensions

For advanced students:

1. Grasp Synthesis: Generate multiple candidate grasps and rank them
2. Unknown Objects: Add perception to estimate object pose
3. Grasp Stability: Implement form/force closure analysis
4. Reactive Grasping: Adjust grasp based on contact feedback

Related Content

• Theory: Module 07 theory.md, Section 7.1 (Grasp Mechanics)
• Next Lab: Lab 07-02 (Force Control)
• Application: See Amazon Digit case study for warehouse picking