Lab 01-03: Basic Actuation and Control

Objectives

Implement position-based joint control
Understand the difference between torque and position control
Create a simple pose controller for the humanoid
Analyze control performance with different gains

Materials

Type	Name	Version	Tier
software	Python	3.10+	required
software	MuJoCo	3.0+	required
software	NumPy	1.24+	required
software	Matplotlib	3.5+	required

Prerequisites

Completion of Lab 01-01 and Lab 01-02
Understanding of joint/actuator mapping

Instructions

Step 1: Understand Control Modes

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

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

# The humanoid model uses torque control by default
# Control signal in data.ctrl directly sets joint torques
print("Control dimensions:", model.nu)
print("Control range:")
for i in range(min(5, model.nu)):
    name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_ACTUATOR, i)
    print(f"  {name}: [{model.actuator_ctrlrange[i, 0]:.1f}, "
          f"{model.actuator_ctrlrange[i, 1]:.1f}]")

Verification: Control range should show torque limits (typically ±100 or similar).

Step 2: Implement Proportional-Derivative (PD) Control

class PDController:
    """
    PD controller for position control of joints.

    Args:
        kp: Proportional gain (stiffness)
        kd: Derivative gain (damping)
    """

    def __init__(self, kp: float = 100.0, kd: float = 10.0):
        self.kp = kp
        self.kd = kd
        self.target_qpos = None

    def set_target(self, target_qpos: np.ndarray):
        """Set target joint positions."""
        self.target_qpos = target_qpos.copy()

```python
```python
    def compute(self, model, data) -> np.ndarray:

        """
        Compute control torques using PD control.

        Returns:
            Control signal (torques) for all actuators
        """
        if self.target_qpos is None:
            return np.zeros(model.nu)

        # Get current joint positions and velocities
        # Skip the free joint (first 7 qpos elements, 6 qvel elements)
        current_qpos = data.qpos[7:7 + model.nu]
        current_qvel = data.qvel[6:6 + model.nu]

        # Compute error
        pos_error = self.target_qpos - current_qpos
        vel_error = -current_qvel  # Target velocity is zero

        # PD control law: tau = Kp * (q_target - q) + Kd * (0 - q_dot)
        torques = self.kp * pos_error + self.kd * vel_error

        # Clip to actuator limits
        for i in range(model.nu):
            torques[i] = np.clip(
                torques[i],
                model.actuator_ctrlrange[i, 0],
                model.actuator_ctrlrange[i, 1]
            )

        return torques

# Create controller
controller = PDController(kp=100.0, kd=10.0)

# Set target to initial pose (standing)
initial_pose = np.zeros(model.nu)
controller.set_target(initial_pose)

Understanding Check: Can you explain why we need both P and D terms?

P term: Drives the joint toward the target position
D term: Prevents overshoot and oscillation

Step 3: Test the Controller

# Reset simulation
mujoco.mj_resetData(model, data)

# Perturb the initial state (drop from height)
data.qpos[2] = 1.5  # Start higher

# Simulation parameters
duration = 5.0
dt = model.opt.timestep

# Data recording
times = []
heights = []
joint_errors = []

# Run simulation
```python
```python
while data.time < duration:

    # Compute control
    data.ctrl[:] = controller.compute(model, data)

    # Step simulation
    mujoco.mj_step(model, data)

    # Record data
    times.append(data.time)
    heights.append(data.qpos[2])  # Height of root body

    # Compute joint position error
    current_qpos = data.qpos[7:7 + model.nu]
    error = np.linalg.norm(controller.target_qpos - current_qpos)
    joint_errors.append(error)

# Plot results
fig, axes = plt.subplots(2, 1, figsize=(10, 8))

axes[0].plot(times, heights)
axes[0].set_ylabel('Height (m)')
axes[0].set_title('Humanoid Height Over Time')
axes[0].grid(True, alpha=0.3)
axes[0].axhline(y=1.4, color='r', linestyle='--', label='Target height')
axes[0].legend()

axes[1].plot(times, joint_errors)
axes[1].set_xlabel('Time (s)')
axes[1].set_ylabel('Joint Error (rad)')
axes[1].set_title('Total Joint Position Error')
axes[1].grid(True, alpha=0.3)

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

Expected: The humanoid should attempt to maintain its pose. Analysis: Note that PD control alone cannot keep the humanoid standing (no balance).

Step 4: Tune Controller Gains

def evaluate_gains(kp, kd, duration=3.0):
    """Evaluate controller with given gains."""
    mujoco.mj_resetData(model, data)
    data.qpos[2] = 1.5

    ctrl = PDController(kp=kp, kd=kd)
    ctrl.set_target(np.zeros(model.nu))

    errors = []
```python
```python
    while data.time < duration:

        data.ctrl[:] = ctrl.compute(model, data)
        mujoco.mj_step(model, data)
        current_qpos = data.qpos[7:7 + model.nu]
        errors.append(np.linalg.norm(ctrl.target_qpos - current_qpos))

    return np.mean(errors), np.max(errors)

# Test different gain combinations
kp_values = [50, 100, 200, 400]
kd_values = [5, 10, 20, 40]

print("Gain Tuning Results:")
print("-" * 50)
print(f"{'Kp':>8} {'Kd':>8} {'Mean Error':>12} {'Max Error':>12}")
print("-" * 50)

results = []
for kp in kp_values:
    for kd in kd_values:
        mean_err, max_err = evaluate_gains(kp, kd)
        results.append((kp, kd, mean_err, max_err))
        print(f"{kp:>8} {kd:>8} {mean_err:>12.4f} {max_err:>12.4f}")

# Find best combination
best = min(results, key=lambda x: x[2])
print(f"\nBest gains: Kp={best[0]}, Kd={best[1]} (Mean error: {best[2]:.4f})")

Analysis Questions:

How does increasing Kp affect response speed and overshoot?
How does Kd help stabilize the system?
What happens if Kd is too high?

Step 5: Create a Pose Sequence

# Define a simple arm wave motion
def create_wave_sequence(model, num_frames=100):
    """Create a sequence of poses for arm waving."""
    poses = []

    for i in range(num_frames):
        pose = np.zeros(model.nu)

        # Find right shoulder actuator
        shoulder_id = mujoco.mj_name2id(
            model, mujoco.mjtObj.mjOBJ_ACTUATOR, "right_shoulder1"
        )

```python
```python
        if shoulder_id >= 0:

            # Sinusoidal motion
            angle = np.sin(2 * np.pi * i / num_frames) * 0.5
            # Note: We're setting target, not directly controlling
            pose[shoulder_id] = angle

        poses.append(pose)

    return poses

# Generate poses
wave_poses = create_wave_sequence(model)

# Run the wave animation
mujoco.mj_resetData(model, data)
controller = PDController(kp=200.0, kd=20.0)

# Optional: Launch viewer for visualization
try:
    with mujoco.viewer.launch_passive(model, data) as viewer:
        pose_idx = 0
```python
```python
        while viewer.is_running() and data.time < 5.0:

            # Update target pose
            pose_idx = int(data.time * 20) % len(wave_poses)
            controller.set_target(wave_poses[pose_idx])

            # Apply control
            data.ctrl[:] = controller.compute(model, data)

            # Step simulation
            mujoco.mj_step(model, data)
            viewer.sync()

except Exception as e:
    print(f"Viewer not available: {e}")
    print("Running headless simulation...")

    for _ in range(2500):
        pose_idx = int(data.time * 20) % len(wave_poses)
        controller.set_target(wave_poses[pose_idx])
        data.ctrl[:] = controller.compute(model, data)
        mujoco.mj_step(model, data)

    print("Simulation complete")

Final Verification: The humanoid should perform an arm waving motion. Note: Due to lack of balance control, it will still fall, but the arm should wave.

Expected Outcomes

Working PD controller implementation
Understanding of gain tuning effects
Gain tuning results table
Control performance visualization (pd_control_results.png)
Simple pose sequence implementation

Rubric

Criterion	Points	Description
PD Implementation	25	Correctly implements PD control law
Gain Tuning	25	Systematically tests and analyzes gains
Error Analysis	20	Meaningful analysis of control performance
Pose Sequence	20	Successfully creates and runs pose sequence
Documentation	10	Clear explanations and comments

Total: 100 points

Discussion Questions

Why doesn't PD control alone keep the humanoid standing?
What additional control strategies would be needed for balance?
How would you modify the controller for trajectory tracking vs. pose holding?

Extensions

Gravity Compensation: Add feedforward term to compensate for gravity
Trajectory Tracking: Implement smooth interpolation between poses
Multiple Joints: Create a coordinated multi-joint motion (e.g., walking step)

Objectives​

Materials​

Prerequisites​

Instructions​

Step 1: Understand Control Modes​

Step 2: Implement Proportional-Derivative (PD) Control​

Step 3: Test the Controller​

Step 4: Tune Controller Gains​

Step 5: Create a Pose Sequence​

Expected Outcomes​

Rubric​

Discussion Questions​

Extensions​