MuJoCo Robotics Lab — Part 9: Grasping & Manipulation

Lab 5 gives the UR5e hands. A custom parallel-jaw gripper is built in MJCF, contact physics is tuned against MuJoCo's soft-constraint solver, and a full pick-and-place state machine picks a 150 g aluminium cube from point A and places it at point B — end to end, in closed loop, with no teleop.

Problem

Everything in the previous labs — forward kinematics, Jacobians, IK, RRT*, TOPP-RA, impedance control — can move the end-effector anywhere inside the UR5e's workspace. None of it picks anything up. The moment the robot has to interact with an external object, three new problems appear at once:

Contact becomes a first-class citizen. Rigid-body dynamics alone no longer describe the system; the gripper–object interface introduces frictional constraints that must be solved by MuJoCo's contact solver every step.
The task is no longer a single trajectory. Picking requires a sequence of controlled phases — approach, descend, close, lift, transport, place, release, retract — each with its own controller and termination condition.
Failure is quiet. A slipped grasp or a 2 mm IK offset does not raise an exception; the box simply stays on the table or falls mid-transport. Every phase needs an explicit success check.

This post walks through how each of those problems was solved.

Theory

Coulomb Friction Cone

At a single contact point with surface normal n, the contact force f must lie inside the Coulomb friction cone:

f = f_n · n + f_t           (normal + tangential)
f_n ≥ 0                     (unilateral: no pulling)
‖f_t‖ ≤ μ · f_n             (no sliding)

Geometrically this is a cone of half-angle arctan(μ) around the inward normal. Every valid contact force — no matter how the gripper is squeezing — has to live inside that cone, or the contact slips.

Force Closure vs Form Closure

A grasp on a rigid body has to resist arbitrary external wrenches (gravity, inertia during transport, small disturbances). Two distinct conditions can achieve this:

Form closure — the object is geometrically caged by the fingers. No friction required. Needs ≥ 7 frictionless contacts on a 3D rigid body. Think of a bolt in a hex socket.
Force closure — friction at each contact is strong enough that the set of realizable contact wrenches positively spans the full 6D wrench space. The parallel-jaw grasp on a cube is force closure: two antipodal contacts, each with a friction cone, together cover all 6 DOF.

The force-closure test for k contacts is:

W = [w_1, w_2, ..., w_k]    where  w_i = [f_i ; (p_i - p_cog) × f_i]
0 ∈ interior(ConvexHull(W))

A grasp is force closure iff the origin of the 6D wrench space lies strictly inside the convex hull of the contact wrenches. In practice, for a two-finger antipodal grasp on a convex object, this reduces to a cheap geometric test: the line connecting the two contacts must pass through the object, and both contact normals must lie inside each other's friction cones.

Why `condim=4` Matters

MuJoCo's condim parameter controls how many wrench components a contact carries. condim=3 gives normal + 2D tangential friction — enough to prevent sliding, not enough to prevent an object rotating in place between the fingers. Lab 5 uses condim=4, which adds torsional friction, so the cube cannot spin around the grip axis during fast transport moves. Without it, Coriolis coupling in EXEC_TRANSPORT would rotate the box out of the grip within a few hundred steps.

Implementation

Gripper MJCF

The gripper attaches to tool0 and has two slide joints mirrored by an equality constraint — 2 DOF collapsed to 1 actuated DOF:

<body name="gripper_base" pos="0 0 0.020">
  <body name="left_finger" pos="0 0.015 0.060">
    <joint name="left_finger_joint" type="slide"
           axis="0 1 0" range="0 0.030"
           damping="2.0" armature="0.0001"/>
    <geom name="left_pad" type="box"
          size="0.010 0.005 0.008" pos="0 0.009 0.025"
          friction="1.5 0.005 0.0001"
          solimp="0.99 0.99 0.001" solref="0.002 1"
          condim="4" mass="0.005"/>
  </body>
  <!-- right_finger mirrored on -Y -->
</body>

<equality>
  <joint joint1="left_finger_joint" joint2="right_finger_joint"
         polycoef="0 1 0 0 0"/>
</equality>

<actuator>
  <position name="gripper" joint="left_finger_joint"
            kp="200" ctrllimited="true" ctrlrange="0 0.030"/>
</actuator>

The critical geometry invariant: at GRIPPER_CLOSED the pad inner face sits at Y = ±0.019 m. The box half-width is 0.020 m. That 1 mm overlap is what MuJoCo's stiff contact (solref="0.002 1", solimp="0.99 0.99 0.001") turns into holding force. Miss this by a millimetre in either direction and the gripper either passes through the cube or bounces off it.

Grasp Configurations via DLS IK

Five joint configurations are computed offline using Damped Least Squares on a Pinocchio model of the UR5e + gripper:

Δq = α · Jᵀ (J Jᵀ + λ² I)⁻¹ · e

α   = 0.5       step size
λ²  = 1e-4      damping (avoids singularity blow-up)
tol = 1e-4      ‖e‖ convergence threshold
e   = [Δp ; Δθ] 6D task error

All configurations share the same top-down orientation R_topdown taken from FK at Q_HOME, so the gripper Z axis always points world -Z. Targets are offset by GRIPPER_TIP_OFFSET = 0.090 m to account for the distance from tool0 to the fingertip plane, and pregrasp/preplace add a 0.150 m clearance.

Pick-and-Place State Machine

The planner, controller, and gripper are composed by an 11-state FSM. The core transition logic:

class GraspStateMachine:
    def step(self, m, d):
        if self.state == State.PLAN_APPROACH:
            self.traj = self._plan_and_smooth(Q_HOME, self.cfg.q_pregrasp)
            self.state = State.EXEC_APPROACH

        elif self.state == State.EXEC_APPROACH:
            if self._run_joint_impedance(m, d, self.traj):
                self.state = State.DESCEND

        elif self.state == State.DESCEND:
            if self._run_cartesian_impedance(m, d, z_target=GRASP_Z):
                close_gripper(d)
                self.state = State.CLOSE

        elif self.state == State.CLOSE:
            if is_gripper_settled(m, d) and is_gripper_in_contact(m, d):
                self.state = State.LIFT
            elif self.elapsed > CLOSE_TIMEOUT:
                raise GraspFailure("gripper closed without contact")

        elif self.state == State.LIFT:
            if self._run_cartesian_impedance(m, d, z_target=PREGRASP_Z):
                self.state = State.PLAN_TRANSPORT
        # ... PLAN_TRANSPORT → EXEC_TRANSPORT → DESCEND_PLACE → RELEASE → RETRACT → DONE

Each PLAN_* state runs RRT* (6000 iterations) + shortcut smoothing + TOPP-RA parameterization with VEL_LIMITS = [3.14]*3 + [6.28]*3 rad/s and ACC_LIMITS = [8.0]*3 + [16.0]*3 rad/s². Each EXEC_* state runs Lab 3's joint-space impedance controller with gravity compensation from Pinocchio RNEA:

τ = Kp · (q_d - q) + Kd · (q̇_d - q̇) + g(q)
Kp = diag([200, 200, 200, 100, 100, 100])
Kd = 2 · sqrt(Kp)

DESCEND, LIFT, and DESCEND_PLACE swap to Cartesian impedance so only the Z-axis is actively commanded while X and Y are soft-regulated — the gripper cannot drift laterally during the 15 cm vertical moves even if IK has a small residual error.

Results

Cube: 40 mm aluminium, 150 g. Start: world [0.35, +0.20, 0.335]. Goal: world [0.35, -0.20, 0.335]. 1 kHz simulation.

Metric	Value
DLS IK position error (all 4 targets)	< 0.1 mm
DLS IK orientation error	< 1e-4 rad
Pick success rate (50 runs)	50 / 50
Place accuracy (RMS XY error)	1.4 mm
Joint tracking error during TOPP-RA	< 5 mrad
Mean cycle time (IDLE → DONE)	8.2 s
RRT* planning time per segment	200–600 ms
Path waypoints after shortcut	5–15 (from 50–200 raw)
Contact forces during LIFT	≈ 1.5 N total (= m·g)
Torque saturation events	None

Common Failure Modes

Mode	Symptom	Root cause	Fix
IK divergence	"IK failed" at startup	`GRIPPER_TIP_OFFSET` sign flipped	Verify `tool0_target = box + [0,0,+0.090]`
Box slip in LIFT	FSM stalls, box falls	μ_slide too low or gap > cube	μ_slide ≥ 1.5, pad_inner_face < half_width
Transport collision	`data.ncon > 0` mid-flight	RRT* skimmed table edge	Add Y-margin in collision checker
Contact oscillation	Box vibrates in grip	`solref` timeconst too large	`solref="0.002 1"`

Key Takeaways

Force closure is cheap for parallel jaws. For a convex object with antipodal contacts and μ ≥ 1.0, the force-closure test reduces to a geometric check — you do not need a full wrench-space convex hull at runtime.
Contact parameters matter more than control gains. Most failed grasps in this lab were fixed in XML (friction, condim, solref, solimp), not in the controller.
Sub-millimetre geometry discipline. A 1 mm overlap between pad and cube is what generates holding force. Always prototype gripper geometry statically before wiring up control.
A state machine is the right abstraction. Pick-and-place is not one trajectory; it is seven controllers stitched together by explicit termination conditions. Each phase must be able to detect its own success or failure.
Reuse everything. Lab 3's impedance controller, Lab 4's RRT* and TOPP-RA, Pinocchio's RNEA — the FSM is the only new component. The rest of the stack falls into place.
Stiff contact beats compliant control for rigid grasping. solimp="0.99 0.99 0.001" + kp=200 on the gripper position actuator holds 150 g with zero visible penetration and no tuning drama.