Lab 02-01: Computing Rigid Body Forces

Objectives

Calculate forces and moments on rigid bodies
Understand the relationship between forces and acceleration
Verify Newton's second law in simulation
Analyze contact forces in MuJoCo

Materials

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

Instructions

Step 1: Create a Simple Rigid Body

import mujoco
import numpy as np

# Create a simple box model
box_xml = """
<mujoco>
  <option gravity="0 0 -9.81" timestep="0.001"/>

  <worldbody>
    <light diffuse="1 1 1" pos="0 0 3"/>
    <geom type="plane" size="5 5 0.1" rgba="0.8 0.8 0.8 1"/>

    <body name="box" pos="0 0 2">
      <joint type="free"/>
      <geom type="box" size="0.2 0.2 0.2" mass="1.0" rgba="0.2 0.6 0.8 1"/>
    </body>
  </worldbody>
</mujoco>
"""

model = mujoco.MjModel.from_xml_string(box_xml)
data = mujoco.MjData(model)

print(f"Box mass: {model.body_mass[1]:.2f} kg")
print(f"Gravity: {model.opt.gravity}")
print(f"Expected weight: {model.body_mass[1] * abs(model.opt.gravity[2]):.2f} N")

Verification: Box mass should be 1.0 kg, weight should be 9.81 N.

Step 2: Analyze Free Fall

# Reset and let the box fall
mujoco.mj_resetData(model, data)

# Record trajectory during free fall
times = []
heights = []
velocities = []
accelerations = []

prev_vel = 0
dt = model.opt.timestep

for i in range(500):  # 0.5 seconds
    mujoco.mj_step(model, data)

    times.append(data.time)
    heights.append(data.qpos[2])  # z position
    vel = data.qvel[2]  # z velocity
    velocities.append(vel)

    # Compute acceleration from velocity change
```python
```python
    if i > 0:

        acc = (vel - prev_vel) / dt
        accelerations.append(acc)
    prev_vel = vel

# Verify Newton's second law: a = g during free fall
print("\nFree Fall Analysis:")
print(f"Expected acceleration: {model.opt.gravity[2]:.2f} m/s²")
print(f"Measured acceleration: {np.mean(accelerations):.2f} m/s² (before contact)")

# Plot results
import matplotlib.pyplot as plt

fig, axes = plt.subplots(3, 1, figsize=(10, 10))

axes[0].plot(times, heights)
axes[0].set_ylabel('Height (m)')
axes[0].set_title('Position vs Time')
axes[0].grid(True)

axes[1].plot(times, velocities)
axes[1].set_ylabel('Velocity (m/s)')
axes[1].set_title('Velocity vs Time')
axes[1].grid(True)

axes[2].plot(times[1:], accelerations)
axes[2].axhline(y=-9.81, color='r', linestyle='--', label='g')
axes[2].set_xlabel('Time (s)')
axes[2].set_ylabel('Acceleration (m/s²)')
axes[2].set_title('Acceleration vs Time')
axes[2].legend()
axes[2].grid(True)

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

Expected: Acceleration should be approximately -9.81 m/s² until contact. Analysis: Note how acceleration changes suddenly at ground contact.

Step 3: Analyze Contact Forces

# MuJoCo computes contact forces automatically
# Let's examine them

mujoco.mj_resetData(model, data)

# Run until steady state (box resting on ground)
for _ in range(2000):
    mujoco.mj_step(model, data)

# Access contact information
print(f"\nNumber of contacts: {data.ncon}")

for i in range(data.ncon):
    contact = data.contact[i]
    print(f"\nContact {i}:")
    print(f"  Position: {contact.pos}")
    print(f"  Normal: {contact.frame[:3]}")
    print(f"  Distance: {contact.dist:.6f}")

    # Get contact force
    force = np.zeros(6)
    mujoco.mj_contactForce(model, data, i, force)
    print(f"  Normal force: {force[0]:.2f} N")
    print(f"  Friction force: [{force[1]:.2f}, {force[2]:.2f}] N")

# Verify: normal force should equal weight
total_normal = 0
for i in range(data.ncon):
    force = np.zeros(6)
    mujoco.mj_contactForce(model, data, i, force)
    total_normal += force[0]

print(f"\nTotal normal force: {total_normal:.2f} N")
print(f"Box weight: {model.body_mass[1] * 9.81:.2f} N")

Verification: Total normal force should approximately equal the box weight (9.81 N). Understanding: This demonstrates Newton's third law (equal and opposite forces).

Step 4: Apply External Forces

# Apply an external force to the box
mujoco.mj_resetData(model, data)

# Lift box slightly above ground
data.qpos[2] = 0.5

# Apply a horizontal force
external_force = np.array([5.0, 0, 0, 0, 0, 0])  # 5N in x direction

# Record trajectory
times = []
x_positions = []
x_velocities = []

for i in range(1000):
    # Apply force to body
    data.xfrc_applied[1] = external_force

    mujoco.mj_step(model, data)

    times.append(data.time)
    x_positions.append(data.qpos[0])  # x position
    x_velocities.append(data.qvel[0])  # x velocity

# Analyze motion
# F = ma => a = F/m = 5/1 = 5 m/s²
expected_acc = 5.0 / model.body_mass[1]

# Fit velocity to get acceleration
from numpy.polynomial import polynomial as P
coeffs = np.polyfit(times, x_velocities, 1)
measured_acc = coeffs[0]

print(f"\nForce Application Analysis:")
print(f"Applied force: {external_force[0]:.1f} N")
print(f"Expected acceleration: {expected_acc:.2f} m/s²")
print(f"Measured acceleration: {measured_acc:.2f} m/s²")

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

axes[0].plot(times, x_positions)
axes[0].set_ylabel('X Position (m)')
axes[0].set_title('Position Under Constant Force')
axes[0].grid(True)

axes[1].plot(times, x_velocities, label='Measured')
axes[1].plot(times, [expected_acc * t for t in times], '--', label='Expected (F=ma)')
axes[1].set_xlabel('Time (s)')
axes[1].set_ylabel('X Velocity (m/s)')
axes[1].legend()
axes[1].grid(True)

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

Expected: Measured acceleration should match F/m = 5 m/s². Note: Slight deviations occur due to numerical integration and any friction.

Step 5: Rotational Dynamics

# Create a box and apply torque
mujoco.mj_resetData(model, data)
data.qpos[2] = 1.0  # Raise box

# Apply torque about z-axis
torque = np.array([0, 0, 0, 0, 0, 2.0])  # 2 Nm about z

# Record rotation
times = []
angles = []
angular_velocities = []

for i in range(500):
    data.xfrc_applied[1] = torque
    mujoco.mj_step(model, data)

    times.append(data.time)

    # Get rotation from quaternion
    quat = data.qpos[3:7]
    # Convert to axis-angle (simplified: just track z rotation)
    # For small rotations: angle ≈ 2 * quat[3] for z-axis
    angles.append(2 * np.arcsin(quat[3]))
    angular_velocities.append(data.qvel[5])  # z angular velocity

# Compute inertia (for a uniform box)
# I = (1/12) * m * (a² + b²) for rotation about z
# where a, b are the x, y dimensions
box_size = 0.2  # half-size
mass = model.body_mass[1]
I_zz = (1/12) * mass * (2 * (2*box_size)**2)
print(f"\nRotational Dynamics:")
print(f"Computed moment of inertia (z): {I_zz:.4f} kg⋅m²")

# Expected angular acceleration: alpha = tau / I
expected_alpha = torque[5] / I_zz
print(f"Applied torque: {torque[5]:.1f} Nm")
print(f"Expected angular acceleration: {expected_alpha:.2f} rad/s²")

# Measure from simulation
alpha_measured = np.gradient(angular_velocities, times).mean()
print(f"Measured angular acceleration: {alpha_measured:.2f} rad/s²")

Verification: Angular acceleration should match τ/I. Analysis: Any discrepancy may be due to the actual inertia tensor vs. simplified calculation.

Expected Outcomes

Verified Newton's second law in free fall
Contact force analysis matching body weight
External force response matching F=ma
Rotational dynamics matching τ=Iα
Saved plots: free_fall_analysis.png, force_application.png

Rubric

Criterion	Points	Description
Free Fall Analysis	25	Correctly analyzes motion and verifies g
Contact Forces	25	Properly extracts and interprets contact data
Linear Dynamics	25	Verifies F=ma with external forces
Rotational Dynamics	15	Analyzes torque and angular acceleration
Documentation	10	Clear explanations and analysis

Total: 100 points

Objectives​

Materials​

Instructions​

Step 1: Create a Simple Rigid Body​

Step 2: Analyze Free Fall​

Step 3: Analyze Contact Forces​

Step 4: Apply External Forces​

Step 5: Rotational Dynamics​

Expected Outcomes​

Rubric​