MuJoCo Robotics Lab — Part 7: Dynamics & Force Control

Lab 3 moves the series out of pure kinematics and into physics. Once the robot has to push against something — a wall, a table, a fixture — position tracking alone breaks down. This post builds three controllers on top of Pinocchio's rigid-body dynamics on the Menagerie UR5e with a mounted Robotiq 2F-85: gravity compensation, Cartesian impedance, and hybrid force/position control that holds a 5 N contact force on a table while moving in the plane above it.

Problem

Every controller in the series so far treated the robot as a geometric object: given a target pose, solve IK, track joint angles with a PD loop. That works beautifully in free space. It fails the moment the end-effector touches anything.

Three things change on contact:

• Position error is no longer the right objective. A stiff position controller commanded 1 mm below a rigid surface will push with whatever force the actuators can deliver. You do not want that on a real fixture.
• Gravity and inertia stop being negligible. The UR5e with a mounted gripper swings 10–20 Nm of gravity torque around the shoulder depending on configuration. Without compensation, any "hold this pose" command sags.
• Interaction dynamics couple the robot to the environment. The right abstraction is not a setpoint — it is an impedance. You specify how the robot should respond to external force, not where it should be.

Solving this needs the full rigid-body model, not just FK and J. That means computing M(q), C(q, q̇), and g(q) every control cycle and letting the controller reason about forces.

Theory

Manipulator equation

The standard rigid-body dynamics equation for a serial manipulator:

M(q)·q̈ + C(q, q̇)·q̇ + g(q) = τ + J^T·F_ext

• M(q) — 6×6 symmetric positive-definite mass matrix
• C(q, q̇) — 6×6 Coriolis/centrifugal matrix; Ṁ − 2C is skew-symmetric (passivity)
• g(q) — 6×1 gravity torque vector
• J^T·F_ext — environment wrenches mapped to joint torques

Pinocchio computes each term analytically from the URDF:

M(q) = pin.crba(model, data, q)              # Composite Rigid Body Algorithm
C(q, q̇) = pin.computeCoriolisMatrix(...)     # recursive Newton-Euler
g(q) = pin.computeGeneralizedGravity(...)    # RNEA with q̇ = q̈ = 0

RNEA (Recursive Newton-Euler) and ABA (Articulated Body Algorithm) are the inverse and forward dynamics primitives; CRBA is a specialized variant that extracts M(q) directly without per-column RNEA passes. All three are O(n) or O(n²) and run comfortably at 1 kHz on 6 DOF.

Cross-validation against MuJoCo

Pinocchio is only useful if its numbers agree with the simulator. Validation on the canonical Menagerie UR5e + Robotiq stack:

Getting to these numbers required matching MuJoCo's armature inertia via model.armature[:] (not rotorInertia) and adding the mounted Robotiq payload as a fixed inertial load on the URDF ee_link. Without that payload, gravity parity breaks by several Nm at the shoulder.

Impedance law

Task-space impedance turns the end-effector into a virtual spring-damper:

F = K_p·(x_d − x) + K_d·(ẋ_d − ẋ)
τ = J^T·F + g(q)

J is taken in the LOCAL_WORLD_ALIGNED frame. For 6-DOF pose control, orientation error uses the Lie-log map instead of the small-angle skew formulation:

e_R = log3(R_d · R^T)

That change matters: the skew form drifts badly once errors exceed ~30°, and the log-map stays accurate over the full rotation group.

Hybrid force/position decomposition

In constrained tasks (polishing, wiping, assembly), the task space splits cleanly: some directions are position-controlled, the rest force-controlled. Selection matrices partition the Cartesian wrench:

S_p = diag(1, 1, 0)   # position control in XY
S_f = diag(0, 0, 1)   # force control in Z

F_p = K_p·S_p·(x_d − x) + K_d·S_p·(ẋ_d − ẋ)
e_f = F_desired − F_measured
F_f = −(K_fp·e_f + K_fi·∫e_f dt) − K_dz·ẋ_z
τ   = J^T·(F_p + F_f) + g(q)

The Z-axis PI loop drives steady-state force error to zero; the explicit −K_dz·ẋ_z term damps contact chattering that integral action alone cannot kill.

Implementation

The three controllers stack. Gravity compensation is the baseline; impedance adds a Cartesian spring; hybrid control replaces one axis of that spring with a force loop.

1. Gravity compensation

The simplest dynamics-based controller: τ = g(q). With MuJoCo's 1.0 Nm·s/rad joint damping, any residual velocity decays exponentially and the arm settles wherever you released it. This is the correctness test for the Pinocchio model.

2. Cartesian impedance

Compute J and g(q), form the 6D wrench from position and orientation error, and project back to joint torques. The same skeleton handles all three stiffness presets used in the compliance demo:

3. Hybrid force/position

Runs at 1 kHz. Contact force comes from mj_contactForce() filtered over the real EE contact set (wrist_3_link plus the mounted Robotiq bodies), then smoothed with an EMA low-pass. The MuJoCo <force> sensor is deliberately not used — it reports all body-level constraint forces including articulation, not just contact.

def hybrid_control(q, qd, x_d, F_desired, F_measured, integral_f, dt):
    # Pinocchio dynamics
    pin.forwardKinematics(model, data, q, qd)
    pin.updateFramePlacements(model, data)
    J = pin.computeFrameJacobian(model, data, q, ee_frame_id,
                                 pin.LOCAL_WORLD_ALIGNED)[:3, :]
    g = pin.computeGeneralizedGravity(model, data, q)

    x  = data.oMf[ee_frame_id].translation
    xd = J @ qd

    # Position loop on XY only
    e_p  = S_p @ (x_d - x)
    e_pd = S_p @ (0.0 - xd)
    F_p  = K_p_xy @ e_p + K_d_xy @ e_pd

    # Force loop on Z with velocity damping
    e_f          = F_desired - F_measured
    integral_f  += e_f * dt
    F_f_z        = -(K_fp * e_f + K_fi * integral_f) - K_dz * xd[2]
    F_f          = np.array([0.0, 0.0, F_f_z])

    tau = J.T @ (F_p + F_f) + g
    return tau, integral_f

Gains used for the canonical validation:

A four-phase state machine drives the demo: impedance descent (APPROACH), hybrid hold at first contact (SETTLE), hybrid trace along a line (TRACE), hybrid hold at the end (HOLD).

Results

Validated end-to-end on the canonical Menagerie UR5e + Robotiq stack. Full Lab 3 suite: 34 tests passing.

Gravity compensation

Hybrid force hold (static, 5 N target)

Constant-force line trace (50 mm)

The 99.96% in-band rate is the headline number: once contact is established, the PI loop plus EMA filtering plus Z-damping keeps the force inside the ±1 N window for essentially the entire hold. Line tracing drops to 94% because the Jacobian X-row is small (~0.01) in the vertical tool configuration, so XY bandwidth is limited during contact — longer lines need slower trajectories.

Key Takeaways

• Dynamics parity is a precondition, not an optimization. If Pinocchio and MuJoCo disagree on g(q), no downstream controller can be trusted. Match armature through model.armature[:] and include the mounted payload inertial in the URDF.
• Gravity compensation is the baseline everything else adds to. Impedance and hybrid control both reduce to τ = J^T·F + g(q) — always compensate gravity first, then layer the task behavior on top.
• Use the Lie-log orientation error. The skew-symmetric small-angle form is only reliable near the setpoint. log3(R_d · R^T) works across the whole rotation group and costs nothing extra.
• mj_contactForce() is the only honest contact signal in MuJoCo. The <force> sensor measures all body-level constraint forces, not just contacts, and will lie to your force loop.
• Stiff contact needs both filtering and damping. EMA smoothing on the measurement plus explicit −K_dz·ẋ_z on the normal axis — either alone produces chatter under PI force control.
• Hybrid control is a decomposition, not a new law. A selection matrix is enough to run position on XY and force on Z inside the same impedance skeleton; the rest of the pipeline does not need to know the difference.

Resources

• Explore this lab on NotebookLM
• Source code on GitHub
• Full YouTube playlist