Simulate an arm in MuJoCo

Kinematics tells you where an arm’s tip is for given joint angles. It does not tell you what happens when motors push, gravity pulls and the joints have friction. For that you need a physics simulator. This lesson is for readers who have done forward kinematics and PID control and want to see both inside a simulated robot. We build a two-link arm in MuJoCo, check the simulator against our own kinematics, and then close a joint-space PD loop around it. Everything runs headless from a Python script with no window, so it works the same on a laptop, a server or a CI runner.

MuJoCo in one paragraph

MuJoCo (Multi-Joint dynamics with Contact) is a physics engine for articulated bodies such as robot arms, legged robots and human models. It was developed by Roboti LLC, acquired by Google DeepMind in October 2021 and open-sourced in May 2022, according to the MuJoCo documentation. Models are written in MJCF, an XML format, and the official mujoco Python package wraps the C library. The code on this page was tested with version 3.16.0 of that package. The MuJoCo platform page covers installation and the wider ecosystem, and the URDF and MJCF cheat sheet compares the two model formats.

Two Python objects do all the work. MjModel holds everything that does not change during a run: the kinematic tree, masses, joint limits, actuator gains, the time step. MjData holds the state and everything computed from it: joint positions qpos, joint velocities qvel, actuator commands ctrl, the world positions of bodies and sites, and the forces. You change MjData, call a function, and read MjData again.

The model

The arm moves in the vertical x-z plane, so gravity matters. The upper link is 0.30 m long with a mass of 1.0 kg, the forearm 0.25 m and 0.5 kg. Both are capsules, one of MJCF’s primitive shapes. Each joint is a hinge with a small viscous damping, and each has a motor actuator that applies a torque equal to its control input. A site is a massless marker frame. We put one at the tip so MuJoCo reports its world position for us.

Lesson 3 put its arm in the horizontal x-y plane with 0.4 m and 0.3 m links and called the joint angles theta1 and theta2. Here the arm stands in the vertical x-z plane so that gravity acts on it, the links are shorter, and the angles follow MuJoCo’s name for joint positions, q. The formula is the same, with z in place of y. The first block defines the model as a string, loads it, and compares the tip position MuJoCo computes with that forward kinematics at three joint configurations. mj_forward runs the kinematics and dynamics for the current state without advancing time, which is what you want when you only set qpos and read positions.

import mujoco
import numpy as np

L1, L2 = 0.30, 0.25   # link lengths, m

ARM_XML = f"""
<mujoco model="two_link_arm">
  <option timestep="0.002" gravity="0 0 -9.81"/>
  <worldbody>
    <body name="upper" pos="0 0 0">
      <joint name="shoulder" type="hinge" axis="0 -1 0" damping="0.05"/>
      <geom type="capsule" fromto="0 0 0 {L1} 0 0" size="0.02" mass="1.0"/>
      <body name="fore" pos="{L1} 0 0">
        <joint name="elbow" type="hinge" axis="0 -1 0" damping="0.05"/>
        <geom type="capsule" fromto="0 0 0 {L2} 0 0" size="0.015" mass="0.5"/>
        <site name="tip" pos="{L2} 0 0" size="0.01"/>
      </body>
    </body>
  </worldbody>
  <actuator>
    <motor name="shoulder_motor" joint="shoulder" ctrllimited="true" ctrlrange="-20 20"/>
    <motor name="elbow_motor" joint="elbow" ctrllimited="true" ctrlrange="-10 10"/>
  </actuator>
</mujoco>
"""

model = mujoco.MjModel.from_xml_string(ARM_XML)
data = mujoco.MjData(model)
print(f"nq={model.nq} nv={model.nv} nu={model.nu} timestep={model.opt.timestep} s")

tip_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "tip")

def fk(q1: float, q2: float) -> np.ndarray:
    """Planar forward kinematics in the x-z plane (lesson 3), angles from the +x axis towards +z."""
    return np.array([L1 * np.cos(q1) + L2 * np.cos(q1 + q2),
                     L1 * np.sin(q1) + L2 * np.sin(q1 + q2)])

for q in [(0.0, 0.0), (0.5, 0.8), (1.2, -1.0)]:
    data.qpos[:] = q
    mujoco.mj_forward(model, data)          # kinematics only, no time step
    tip = data.site_xpos[tip_id]
    print(f"q={q}  MuJoCo tip x={tip[0]:.4f} z={tip[2]:.4f}   FK x={fk(*q)[0]:.4f} z={fk(*q)[1]:.4f}")

nq=2 nv=2 nu=2 timestep=0.002 s
q=(0.0, 0.0)  MuJoCo tip x=0.5500 z=0.0000   FK x=0.5500 z=0.0000
q=(0.5, 0.8)  MuJoCo tip x=0.3301 z=0.3847   FK x=0.3301 z=0.3847
q=(1.2, -1.0)  MuJoCo tip x=0.3537 z=0.3293   FK x=0.3537 z=0.3293

nq and nv are both 2 because each hinge contributes one position and one velocity. (A free-floating body would contribute 7 positions, a 3-D position plus a quaternion in MuJoCo’s (w, x, y, z) order, and 6 velocities, which is why the two counts are kept separate.) nu is 2 for the two motors. The tip positions agree with our own kinematics to every printed digit. That agreement is worth checking for every new model: a hinge axis with the wrong sign, or a body offset in the wrong place, shows up immediately as a mismatch here and is much harder to find once a controller is running.

Stepping the simulation

mj_step advances the state by one time step. The default step is 0.002 s, so 500 calls simulate one second. Inside, MuJoCo computes the forces from gravity, damping, actuators and contacts, solves for the joint accelerations and integrates. The default integrator is a semi-implicit Euler method, and it treats joint damping implicitly, which keeps strongly damped joints stable at this step size. The step size is the main speed-accuracy trade-off in any simulator. Halve it and the run costs twice as much.

The control loop pattern is always the same: read the state from MjData, compute commands, write them to data.ctrl, call mj_step, repeat. The second block restates the model so that it runs on its own, then does two experiments.

First, with the motors off, it releases the arm from horizontal and lets it fall for 5 s, once with the damping in the model (0.05 N m s/rad) and once with ten times more. Second, it drives both joints to a target of q = (0.8, 0.6) rad with a PD controller, tau = Kp (q_target - q) - Kd qdot, run once as is and once with a gravity compensation term added. The motors’ control range limits the shoulder to 20 N m and the elbow to 10 N m, and MuJoCo clamps commands outside that range.

import mujoco
import numpy as np

L1, L2 = 0.30, 0.25
ARM_XML = f"""
<mujoco model="two_link_arm">
  <option timestep="0.002" gravity="0 0 -9.81"/>
  <worldbody>
    <body name="upper" pos="0 0 0">
      <joint name="shoulder" type="hinge" axis="0 -1 0" damping="0.05"/>
      <geom type="capsule" fromto="0 0 0 {L1} 0 0" size="0.02" mass="1.0"/>
      <body name="fore" pos="{L1} 0 0">
        <joint name="elbow" type="hinge" axis="0 -1 0" damping="0.05"/>
        <geom type="capsule" fromto="0 0 0 {L2} 0 0" size="0.015" mass="0.5"/>
        <site name="tip" pos="{L2} 0 0" size="0.01"/>
      </body>
    </body>
  </worldbody>
  <actuator>
    <motor name="shoulder_motor" joint="shoulder" ctrllimited="true" ctrlrange="-20 20"/>
    <motor name="elbow_motor" joint="elbow" ctrllimited="true" ctrlrange="-10 10"/>
  </actuator>
</mujoco>
"""

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

# 1. Passive: motors off, release the arm horizontal and let gravity and joint damping act.
for damping in (0.05, 0.5):
    model.dof_damping[:] = damping
    mujoco.mj_resetData(model, data)        # qpos = 0: both links along +x, at rest
    data.ctrl[:] = 0.0
    while data.time < 5.0:
        mujoco.mj_step(model, data)
    print(f"passive, damping {damping} N m s/rad: after 5 s q1={data.qpos[0]:+.3f} rad "
          f"q1dot={data.qvel[0]:+.3f} rad/s (hanging straight down is q1 = {-np.pi / 2:.3f})")
model.dof_damping[:] = 0.05

# 2. Joint PD control towards a target, with and without gravity compensation.
q_target = np.array([0.8, 0.6])
kp = np.array([40.0, 15.0])                 # N m/rad
kd = np.array([4.0, 1.0])                   # N m s/rad

def run(gravity_comp: bool, n_steps: int = 500, log_every: int = 10):
    """Simulate n_steps of 2 ms (1 s) and log (t, q1, q2) every log_every steps."""
    mujoco.mj_resetData(model, data)
    log = []
    for step in range(n_steps + 1):
        if step % log_every == 0:
            log.append((round(data.time, 2), round(float(data.qpos[0]), 3), round(float(data.qpos[1]), 3)))
        tau = kp * (q_target - data.qpos) - kd * data.qvel
        if gravity_comp:
            tau += data.qfrc_bias           # gravity plus Coriolis terms at the current state
        data.ctrl[:] = tau                  # MuJoCo clips this to ctrlrange
        mujoco.mj_step(model, data)
    return log

tip_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "tip")
for comp in (False, True):
    log = run(comp)
    err = np.round(q_target - data.qpos, 3) + 0.0
    print(f"PD, gravity compensation {comp!s:5}: final q=({data.qpos[0]:.3f}, {data.qpos[1]:.3f}) "
          f"error=({err[0]:+.3f}, {err[1]:+.3f}) rad, tip x={data.site_xpos[tip_id][0]:.3f} z={data.site_xpos[tip_id][2]:.3f} m")
print("target tip from FK: x={:.3f} z={:.3f} m".format(
    L1 * np.cos(0.8) + L2 * np.cos(1.4), L1 * np.sin(0.8) + L2 * np.sin(1.4)))
print("t, q1, q2 with gravity compensation:", log)
print("q1 without compensation:", [row[1] for row in run(False)])

passive, damping 0.05 N m s/rad: after 5 s q1=-1.156 rad q1dot=+0.724 rad/s (hanging straight down is q1 = -1.571)
passive, damping 0.5 N m s/rad: after 5 s q1=-1.570 rad q1dot=+0.000 rad/s (hanging straight down is q1 = -1.571)
PD, gravity compensation False: final q=(0.742, 0.590) error=(+0.058, +0.010) rad, tip x=0.280 z=0.446 m
PD, gravity compensation True : final q=(0.800, 0.600) error=(+0.000, +0.000) rad, tip x=0.252 z=0.462 m
target tip from FK: x=0.252 z=0.462 m
t, q1, q2 with gravity compensation: [(0.0, 0.0, 0.0), (0.02, 0.01, 0.079), (0.04, 0.075, 0.149), (0.06, 0.174, 0.232), (0.08, 0.283, 0.321), (0.1, 0.39, 0.404), (0.12, 0.485, 0.474), (0.14, 0.566, 0.529), (0.16, 0.632, 0.57), (0.18, 0.683, 0.597), (0.2, 0.722, 0.614), (0.22, 0.75, 0.624), (0.24, 0.769, 0.627), (0.26, 0.783, 0.628), (0.28, 0.791, 0.626), (0.3, 0.796, 0.622), (0.32, 0.799, 0.619), (0.34, 0.801, 0.615), (0.36, 0.802, 0.612), (0.38, 0.802, 0.609), (0.4, 0.801, 0.607), (0.42, 0.801, 0.605), (0.44, 0.801, 0.604), (0.46, 0.8, 0.603), (0.48, 0.8, 0.602), (0.5, 0.8, 0.601), (0.52, 0.8, 0.601), (0.54, 0.8, 0.6), (0.56, 0.8, 0.6), (0.58, 0.8, 0.6), (0.6, 0.8, 0.6), (0.62, 0.8, 0.6), (0.64, 0.8, 0.6), (0.66, 0.8, 0.6), (0.68, 0.8, 0.6), (0.7, 0.8, 0.6), (0.72, 0.8, 0.6), (0.74, 0.8, 0.6), (0.76, 0.8, 0.6), (0.78, 0.8, 0.6), (0.8, 0.8, 0.6), (0.82, 0.8, 0.6), (0.84, 0.8, 0.6), (0.86, 0.8, 0.6), (0.88, 0.8, 0.6), (0.9, 0.8, 0.6), (0.92, 0.8, 0.6), (0.94, 0.8, 0.6), (0.96, 0.8, 0.6), (0.98, 0.8, 0.6), (1.0, 0.8, 0.6)]
q1 without compensation: [0.0, 0.012, 0.076, 0.167, 0.266, 0.361, 0.447, 0.52, 0.579, 0.626, 0.661, 0.687, 0.706, 0.719, 0.728, 0.734, 0.738, 0.741, 0.742, 0.743, 0.743, 0.743, 0.743, 0.743, 0.743, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742]

What the numbers show

Damping. With 0.05 N m s/rad the arm is still swinging after 5 s, passing through q1 = -1.156 rad at 0.72 rad/s. With 0.5 N m s/rad it has settled at -1.570 rad, hanging straight down. Joint damping in a simulator stands in for friction in the gearbox and bearings. Real values come from system identification on the physical robot. A model with too little damping looks lively in simulation and then behaves differently on hardware, one source of the sim-to-real gap.

PD without gravity compensation. The plain PD loop settles with the shoulder 0.058 rad (3.3 degrees) below the target. At rest the velocity term is zero, so the only torque the controller produces is Kp times the error. That torque has to hold the arm up against gravity, so the error cannot be zero. You can check it by hand: at the final pose, gravity pulls on the shoulder with about 2.31 N m (the upper link’s 1.0 kg at 0.15 m and the forearm’s 0.5 kg at its centre, each times 9.81 m/s² and the cosine of its angle). Dividing by Kp = 40 N m/rad gives 0.058 rad. The elbow carries only the forearm, so its error is smaller, 0.010 rad. The tip ends up 32 mm from where it should be. Lesson 5 cured steady-state error with an integral term. On an arm there is a better option, because the disturbance is known.

PD with gravity compensation. data.qfrc_bias is the joint torque needed to cancel gravity, Coriolis and centrifugal effects at the current state. Adding it to the PD output means the PD terms only have to correct errors, not hold the arm up. The final error drops to zero at three decimals and the simulated tip matches the forward kinematics target, x = 0.252 m and z = 0.462 m. Using the simulator’s own model for the compensation is the ideal case. On real hardware the compensation comes from a model with estimated masses, so a small error remains, and an integral term or an adaptive estimate deals with the rest.

The elbow overshoots slightly, to 0.628 rad, before settling. That is the coupling between the joints: when the shoulder accelerates, the reaction torque swings the forearm, and the elbow PD has to catch it. Gains tuned for each joint alone meet this coupling as soon as the joints move together. The PID tuning reference has the usual procedures.

Habits for simulation scripts

• Keep the model in a string or a file under version control, and check it against your own kinematics as in the first block before trusting any dynamics.
• Call mj_resetData before each experiment. MjData carries over state, including time, from the previous run.
• Change parameters at run time through MjModel arrays such as model.dof_damping, not by editing the XML, when you sweep a value.
• Log at a lower rate than you simulate. Here the physics ran at 500 Hz and the log at 50 Hz.
• Never open a viewer in code that runs unattended. Everything on this page runs without rendering.

Exercise

1. Set the time step to 0.02 s (model.opt.timestep = 0.02) and run the PD experiment again. What happens to the response, and at what step size does it stop matching the 0.002 s run to two decimals?
2. Replace the two motor actuators with position actuators using kp="40" and kv="4" for the shoulder and kp="15" and kv="1" for the elbow. Command the target through data.ctrl and confirm that the steady-state error matches the plain PD result.
3. Give the forearm’s capsule a 0.5 kg payload by raising its mass to 1.0 kg, but keep the compensation term computed from a copy of the original model. Measure the remaining steady-state error.

Summary

• MuJoCo separates the fixed model (MjModel) from the changing state (MjData). Set state, call mj_forward or mj_step, and read the results from MjData.
• A site at the tip gives its world position in data.site_xpos. Checking it against your own forward kinematics catches axis and offset mistakes early.
• mj_step advances by model.opt.timestep, 0.002 s by default. Joint damping models friction and strongly affects how a simulated arm settles.
• A PD controller holding an arm against gravity leaves a steady-state error equal to the gravity torque divided by Kp, here 0.058 rad at the shoulder.
• Adding data.qfrc_bias as a gravity and Coriolis compensation term removes that error when the model is exact.

Further reading

• MuJoCo documentation, Overview: the concepts behind MjModel, MjData, bodies, joints, sites and actuators.
• MuJoCo documentation, XML Reference: every MJCF element and attribute, including option/timestep, joint damping and the motor and position actuators.
• MuJoCo documentation, Computation: the equations of motion, the bias force and the integrators.
• Modern Robotics, Lynch and Park, Cambridge University Press, 2017 (updated first edition, 2019 preprint): chapter 8 (Dynamics of Open Chains) derives the equations of motion that MuJoCo solves, and chapter 11 (Robot Control) covers joint-space control.