A two-link arm that draws a square with inverse kinematics

What you will build

This project puts three beginner ideas into one program that you can run in under a second. A two-link planar arm, defined in a few lines of MJCF, holds a pen at its tip. You give it a square in Cartesian coordinates. The program converts each corner and edge point into joint angles with closed-form inverse kinematics, feeds those angles to two simulated position servos, and steps the MuJoCo physics engine while it records where the pen really goes. At the end it prints the tracking error and the drawn path, and sweeps the drawing speed to show where the error comes from.

The result is a square of 160 mm sides drawn with a steady lag of about 4 mm at 80 mm/s. That lag is not a bug in the IK. It is the servo, and you can predict its size from two numbers in the model file. Working that out is the main lesson of the project.

You should have read the lessons on forward kinematics and inverse kinematics. The lesson on simulating an arm in MuJoCo covers the MuJoCo calls used here in more detail, and PID control explains the proportional-derivative law inside each servo.

What you need

• Python 3.10 or later with numpy and mujoco (MuJoCo 3.x, pip install mujoco numpy). We ran it with MuJoCo 3.16.0 and NumPy 2.5.4 on an Apple Silicon Mac mini.
• Nothing else. The model is an inline string, there is no viewer and nothing is downloaded.
• About 2 hours if you want to work through the design and try the variations at the end. The program itself runs in well under a second.

Design

Geometry and inverse kinematics

The arm lies in the horizontal x-y plane of the world frame. Both joints are hinges about z, so gravity (along negative z) puts no load on them and the problem stays purely kinematic. Link 1 is 0.30 m and link 2 is 0.25 m. The shoulder angle q1 is measured from the world x axis. The elbow angle q2 is measured from link 1. Angles are in radians in the code.

For a target (x, y), the law of cosines gives the elbow angle. With c2 = (x² + y² - L1² - L2²) / (2 L1 L2), the elbow is q2 = atan2(±√(1 - c2²), c2). The sign picks one of the two solutions: the elbow on one side of the line from shoulder to pen, or the other. If |c2| is greater than 1 the point is outside the workspace and the function raises an error. The shoulder angle follows as q1 = atan2(y, x) - atan2(L2 sin q2, L1 + L2 cos q2).

Two choices matter in practice. First, use the same elbow branch for every waypoint. If the solver flipped between branches halfway along an edge, the servo would swing the elbow through a large angle and the pen would leave the square. Second, check the IK with forward kinematics before you simulate. The program does this and prints the worst mismatch, which should be at the level of floating-point rounding.

The square has its centre at (0.32, 0.10) m. With the elbow-positive branch it needs shoulder angles from -49.0 to -9.1 degrees and elbow angles from 74.6 to 129.1 degrees. That keeps the elbow well away from 0 and 180 degrees, where the arm is fully stretched or folded and the Jacobian loses rank (a singularity).

Control and the lag you should expect

Each joint has a MuJoCo position actuator. It applies a torque of kp (q_ref - q) - kv dq/dt, a proportional-derivative law with the derivative acting on the measured velocity. The shoulder uses kp = 400 N m/rad and kv = 20 N m s/rad. The elbow uses half of each. Each joint also has 0.5 N m s/rad of passive damping.

Now consider a joint following a reference that moves at a constant rate. Once the motion has settled there is no acceleration, so the servo torque must exactly cancel the damping torques: kp e = (kv + 0.5) dq/dt, where e is the lag. Divide by the rate and the lag becomes a fixed delay of (kv + 0.5) / kp. For the shoulder that is 20.5 / 400 = 51 ms. For the elbow it is 10.5 / 200, which is also about 51 ms. Because both joints lag by the same time, the pen follows the right path but sits about 51 ms behind the reference. At 80 mm/s that is about 4.1 mm. The simulation should agree with this estimate, and the speed sweep at the end tests it.

Two settings that are easy to get wrong

The integrator. MuJoCo’s default integrator is semi-implicit Euler. The MuJoCo XML reference recommends the implicitfast or implicit integrator whenever an actuator uses kv. We found out why while writing this project. With the default integrator, the 2 ms step and the small inertia of link 2, the elbow damping term is close to the stability limit of explicit integration. The joints went into a fast oscillation, with velocities of tens of rad/s while the angles stayed close to the reference. The mean error still looked plausible, but at the two slowest speeds the maximum error jumped above 35 mm near one corner. One attribute, integrator="implicitfast", treats the damping implicitly and removes the problem.

Interpolation in joint space. The program solves IK only at the 161 waypoints, 4 mm apart, and interpolates the joint angles linearly between them. Between two close waypoints, a straight line in joint space maps to a curve in Cartesian space that is very close to the straight edge. If you space the waypoints far apart, for example only the four corners, the pen draws arcs instead of edges. Things to try asks you to see this.

Code

The whole project is one file. Save it as draw_square.py.

# draw_square.py: a two-link planar arm in MuJoCo draws a square from analytic IK waypoints.
import math

import mujoco
import numpy as np

L1, L2 = 0.30, 0.25          # link lengths (m)
DT = 0.002                   # physics time step (s)

ARM_XML = f"""
<mujoco model="two_link_arm">
  <option timestep="{DT}" integrator="implicitfast"/>
  <worldbody>
    <geom name="table" type="plane" size="1 1 0.01" pos="0 0 -0.05"/>
    <body name="link1" pos="0 0 0">
      <joint name="shoulder" type="hinge" axis="0 0 1" range="-170 170" damping="0.5"/>
      <geom type="capsule" fromto="0 0 0 {L1} 0 0" size="0.02" mass="1.0"/>
      <body name="link2" pos="{L1} 0 0">
        <joint name="elbow" type="hinge" axis="0 0 1" range="-170 170" damping="0.5"/>
        <geom type="capsule" fromto="0 0 0 {L2} 0 0" size="0.015" mass="0.6"/>
        <site name="pen" pos="{L2} 0 0" size="0.01"/>
      </body>
    </body>
  </worldbody>
  <actuator>
    <position name="shoulder_servo" joint="shoulder" kp="400" kv="20"/>
    <position name="elbow_servo" joint="elbow" kp="200" kv="10"/>
  </actuator>
</mujoco>
"""


def ik_2link(x: float, y: float, elbow_up: bool = False) -> tuple[float, float]:
    """Analytic IK for a planar two-link arm. Returns (q1, q2) in radians."""
    c2 = (x * x + y * y - L1 * L1 - L2 * L2) / (2 * L1 * L2)
    if abs(c2) > 1.0:
        raise ValueError(f"target ({x:.3f}, {y:.3f}) is outside the workspace")
    s2 = math.sqrt(1.0 - c2 * c2)
    if elbow_up:
        s2 = -s2
    q2 = math.atan2(s2, c2)
    q1 = math.atan2(y, x) - math.atan2(L2 * s2, L1 + L2 * c2)
    return q1, q2


def fk_2link(q1: float, q2: float) -> tuple[float, float]:
    return (L1 * math.cos(q1) + L2 * math.cos(q1 + q2),
            L1 * math.sin(q1) + L2 * math.sin(q1 + q2))


def square_path(cx: float, cy: float, side: float, per_side: int) -> np.ndarray:
    """Points around a square, anticlockwise from the lower-left corner, closed."""
    h = side / 2
    corners = np.array([[cx - h, cy - h], [cx + h, cy - h], [cx + h, cy + h], [cx - h, cy + h]])
    pts = []
    for i in range(4):
        a, b = corners[i], corners[(i + 1) % 4]
        for s in np.linspace(0.0, 1.0, per_side, endpoint=False):
            pts.append(a + s * (b - a))
    pts.append(corners[0])
    return np.array(pts)


def draw(model: mujoco.MjModel, targets: np.ndarray, joint_targets: np.ndarray,
         seg_time: float) -> tuple[np.ndarray, np.ndarray]:
    """Track the waypoints with the position servos. Returns (pen xy per waypoint, error per step)."""
    data = mujoco.MjData(model)
    pen_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "pen")
    data.qpos[:] = joint_targets[0]          # start at rest on the first waypoint
    data.ctrl[:] = joint_targets[0]
    mujoco.mj_forward(model, data)
    steps_per_seg = round(seg_time / DT)
    drawn, errors = [], []
    for k in range(1, len(joint_targets)):
        for s in range(steps_per_seg):
            a = (s + 1) / steps_per_seg
            data.ctrl[:] = (1 - a) * joint_targets[k - 1] + a * joint_targets[k]
            mujoco.mj_step(model, data)
            ref_xy = (1 - a) * targets[k - 1] + a * targets[k]
            errors.append(np.linalg.norm(data.site_xpos[pen_id][:2] - ref_xy))
        drawn.append(data.site_xpos[pen_id][:2].copy())
    return np.array(drawn), np.array(errors) * 1000.0


model = mujoco.MjModel.from_xml_string(ARM_XML)
targets = square_path(cx=0.32, cy=0.10, side=0.16, per_side=40)
joint_targets = np.array([ik_2link(x, y) for x, y in targets])

# Check IK against forward kinematics before simulating anything.
fk_err = max(math.dist(fk_2link(*q), p) for q, p in zip(joint_targets, targets))
print(f"waypoints: {len(targets)}, worst IK/FK mismatch: {fk_err:.1e} m")
print(f"joint range used: shoulder {np.degrees(joint_targets[:, 0]).min():.1f} to {np.degrees(joint_targets[:, 0]).max():.1f} deg, "
      f"elbow {np.degrees(joint_targets[:, 1]).min():.1f} to {np.degrees(joint_targets[:, 1]).max():.1f} deg")

drawn, err_mm = draw(model, targets, joint_targets, seg_time=0.05)
print(f"seg_time 0.05 s: {len(err_mm)} steps, {len(err_mm) * DT:.2f} s simulated")
print(f"tracking error: mean {err_mm.mean():.2f} mm, rms {np.sqrt((err_mm ** 2).mean()):.2f} mm, max {err_mm.max():.2f} mm")
print("every 4th waypoint (m): target_x target_y drawn_x drawn_y")
for k in range(0, len(drawn), 4):
    print(f"{targets[k + 1][0]:.4f} {targets[k + 1][1]:.4f} {drawn[k][0]:.4f} {drawn[k][1]:.4f}")

print("speed sweep: seg_time (s), pen speed (mm/s), rms error (mm), max error (mm)")
perimeter_mm = 4 * 160.0
for seg_time in (0.2, 0.1, 0.05, 0.025):
    _, e = draw(model, targets, joint_targets, seg_time)
    speed = perimeter_mm / (seg_time * (len(targets) - 1))
    print(f"{seg_time:.3f} {speed:.0f} {np.sqrt((e ** 2).mean()):.2f} {e.max():.2f}")

draw() creates fresh MjData each time, so every speed in the sweep starts from the same state. data.site_xpos holds the pen position after each step. That makes the error measurement independent of the IK code: it comes from MuJoCo’s own forward kinematics of the simulated joint angles.

Run

python -m pip install mujoco numpy
python draw_square.py

The program prints the summary lines, 40 rows of target and drawn coordinates (every fourth waypoint) and the speed sweep. It takes well under a second on a laptop.

Test

This is the output from our run. MuJoCo is deterministic on a given machine, so you should see the same numbers to the last printed digit, or within a few hundredths of a millimetre on another CPU or MuJoCo version.

waypoints: 161, worst IK/FK mismatch: 1.2e-16 m
joint range used: shoulder -49.0 to -9.1 deg, elbow 74.6 to 129.1 deg
seg_time 0.05 s: 4000 steps, 8.00 s simulated
tracking error: mean 4.08 mm, rms 4.09 mm, max 4.24 mm
every 4th waypoint (m): target_x target_y drawn_x drawn_y
0.2440 0.0200 0.2414 0.0200
0.2600 0.0200 0.2558 0.0200
0.2760 0.0200 0.2718 0.0200
0.2920 0.0200 0.2878 0.0201
0.3080 0.0200 0.3038 0.0201
0.3240 0.0200 0.3198 0.0201
0.3400 0.0200 0.3358 0.0201
0.3560 0.0200 0.3518 0.0201
0.3720 0.0200 0.3678 0.0201
0.3880 0.0200 0.3838 0.0201
0.4000 0.0240 0.3985 0.0214
0.4000 0.0400 0.4000 0.0359
0.4000 0.0560 0.4000 0.0519
0.4000 0.0720 0.4000 0.0679
0.4000 0.0880 0.4000 0.0839
0.4000 0.1040 0.4000 0.0999
0.4000 0.1200 0.4000 0.1159
0.4000 0.1360 0.4000 0.1319
0.4000 0.1520 0.4000 0.1479
0.4000 0.1680 0.4000 0.1639
0.3960 0.1800 0.3985 0.1787
0.3800 0.1800 0.3842 0.1800
0.3640 0.1800 0.3682 0.1800
0.3480 0.1800 0.3522 0.1800
0.3320 0.1800 0.3362 0.1800
0.3160 0.1800 0.3202 0.1800
0.3000 0.1800 0.3042 0.1800
0.2840 0.1800 0.2882 0.1800
0.2680 0.1800 0.2722 0.1800
0.2520 0.1800 0.2562 0.1801
0.2400 0.1760 0.2415 0.1787
0.2400 0.1600 0.2401 0.1641
0.2400 0.1440 0.2401 0.1481
0.2400 0.1280 0.2401 0.1321
0.2400 0.1120 0.2401 0.1161
0.2400 0.0960 0.2401 0.1001
0.2400 0.0800 0.2401 0.0841
0.2400 0.0640 0.2401 0.0681
0.2400 0.0480 0.2401 0.0521
0.2400 0.0320 0.2401 0.0361
speed sweep: seg_time (s), pen speed (mm/s), rms error (mm), max error (mm)
0.200 20 1.03 1.06
0.100 40 2.06 2.12
0.050 80 4.09 4.24
0.025 160 8.38 8.82

Read it from the top. The IK and FK agree to 1e-16 m, so the joint targets are exact. The joint ranges stay away from the singular elbow angles. The tracking error has a mean of 4.08 mm and a maximum of 4.24 mm, which matches the 4.1 mm predicted from (kv + 0.5) / kp in the Design section. The maximum is only slightly larger than the mean, so the error is a steady lag rather than an overshoot at the corners.

The drawn rows show the lag directly. Along the bottom edge the pen is about 4 mm behind in x. On the right edge it is about 4 mm behind in y. At each corner it cuts slightly inside, because a lagging pen starts the next edge before it reaches the corner.

The speed sweep is the test of the explanation. The root-mean-square (RMS) error doubles each time the speed doubles: 1.03, 2.06, 4.09 and 8.38 mm at 20, 40, 80 and 160 mm/s. A constant time delay gives exactly that. Dividing the error by the speed gives about 51 ms in every row, the delay predicted from the servo gains.

If your numbers are far from these, check three things. A maximum error much larger than the mean usually means the integrator is still Euler. An IK/FK mismatch larger than 1e-9 m means the formula has a sign error. A ValueError means a waypoint is out of reach, so move the square towards the shoulder.

On a real robot

A desk-sized version of this arm is a common first build: two hobby or smart servos, two links cut from aluminium or printed in plastic, and a pen in a holder at the tip. The IK and the waypoint code carry over unchanged. Several other things change.

• Calibration. Your link lengths and joint zero offsets will be a few millimetres and a few degrees off. Measure the real L1 and L2 between joint axes, and find each servo’s zero by commanding 0 and measuring the link angle. An error of 1 degree at the shoulder moves the pen about 7 mm at 0.4 m reach.
• The servo is a black box. A hobby servo runs its own position loop at a fixed gain, and you cannot set kp and kv. Expect a lag of tens of milliseconds and some deadband and backlash. Measure the lag with the speed sweep, then either slow down or send each joint target early by the measured delay.
• Pen height. A planar arm has no z axis. You need a third actuator (a small servo or solenoid) to lift the pen between strokes, and the pen needs some compliance so that it keeps touching the paper.
• Joint limits and cables. Most hobby servos turn about 180 degrees. Check that the joint range you printed fits inside it after calibration.

The block below sends the joint targets to a microcontroller over a serial port. It needs a physical robot and the pyserial package. The text protocol (J <q1> <q2> in degrees, one line per update) is an example: you write the matching firmware, which converts angles into servo pulse widths.

# needs-hardware
import math
import time

import serial  # pyserial, not part of the project environment

PORT = "/dev/tty.usbmodem1101"   # your board's serial port
UPDATE_HZ = 50


def send_path(joint_targets: list[tuple[float, float]], seg_time: float = 0.05) -> None:
    """Stream joint targets in degrees to firmware that drives two servos."""
    with serial.Serial(PORT, 115200, timeout=1) as link:
        time.sleep(2.0)                     # many boards reset when the port opens
        steps = max(1, round(seg_time * UPDATE_HZ))
        for (a1, a2), (b1, b2) in zip(joint_targets[:-1], joint_targets[1:]):
            for s in range(1, steps + 1):
                t = s / steps
                q1, q2 = a1 + t * (b1 - a1), a2 + t * (b2 - a2)
                link.write(f"J {math.degrees(q1):.2f} {math.degrees(q2):.2f}\n".encode())
                time.sleep(1.0 / UPDATE_HZ)

Start with the arm held off the paper and a slow seg_time of 0.2 s. Keep a hand near the power switch the first time it moves.

Things to try

1. Corners only. Change per_side=40 to per_side=1 and the segment time to 2 s, so the arm interpolates in joint space between the four corners. Print the drawn path and look at the bowed edges. In our run the maximum error rose from 4.2 mm to 13.9 mm at the same pen speed. This is why straight-line moves on industrial arms interpolate in Cartesian space and solve IK at every control step.
2. Remove the lag. Shift the joint reference forward by the predicted delay: interpolate towards the waypoint 51 ms ahead of the current one. In our version of this change the mean error fell to 0.13 mm, with 2.1 mm left at the corners. This is a crude form of velocity feedforward.
3. Stiffer servos. Double both kp values and keep kv. Predict the new delay before you run it.
4. The other elbow. Call ik_2link(x, y, elbow_up=True) and compare the joint ranges. Both branches draw the same square, with a different arm posture.
5. A circle. Write circle_path(cx, cy, radius, n) and draw a circle of 0.07 m radius. The lag now shows as a circle that is slightly rotated behind the target, rather than as rounded corners.
6. Break the integrator. Remove integrator="implicitfast" and rerun the sweep. Look at the maximum error at 20 mm/s.

Summary

• Analytic IK for a two-link planar arm is two atan2 calls and a law-of-cosines check. Keep one elbow branch for the whole path and test the result against FK.
• Dense waypoints with joint-space interpolation give near-straight edges. Sparse waypoints do not.
• A PD position servo following a moving target lags by (kv + damping) / kp seconds. Here that is 51 ms, so the pen trails by 4.1 mm at 80 mm/s, and the error scales with speed.
• When an actuator uses kv, use MuJoCo’s implicitfast integrator, as the MuJoCo XML reference recommends.
• For hardware, the code is the same but you must calibrate link lengths and zero offsets and measure the servo delay.

For more on the model format, see the URDF and MJCF cheat sheet, the MuJoCo XML reference for position actuators and integrators, and the kinematics topic hub.