Project 4 — Inverse Kinematics¶
Overview¶
In Project 3 your runtime answered "where is every part of my robot, given its joint angles?" (forward kinematics). Project 4 answers the inverse question: which joint angles put the robot's end effector where I want it? That is inverse kinematics (IK).
For a general robot there is no closed-form answer, so you will solve IK the way the lectures derive it: by gradient descent on the manipulator Jacobian. You will build three services on top of your Project 3 runtime:
/ik/robot_jacobian— the manipulator Jacobian of any chain of a loaded URDF robot, at any configuration. This is the checkpoint./ik/solve_robot_step— one gradient-descent step toward an end-effector target, using either the Jacobian transpose or the Jacobian pseudoinverse, plain or damped (damped least squares)./ik/solve_robot— the full solve: iterate that step until the end effector reaches the target position (and, optionally, orientation), or report honestly that it did not.
Project 4 is kinematics only: no mass, inertia, gravity, simulation or control gains.
Dates. Released Mon Nov 9, 2026. The checkpoint is due Fri Nov 13, 2026 and the project is due Wed Nov 18, 2026.
The robot arrives exactly as in Project 3: as the URDF text of robot_description, set on your
parameter server. Everything speaks the same ROS-like protocol on the same
127.0.0.1:9095 TCP/JSON gateway as Projects 1–3: newline-delimited JSON over a plain TCP
socket, not HTTP and not WebSocket. Bring your own Project 1 middleware and gateway and your
Project 3 parameter server, URDF reader and forward kinematics. The starters do not include
them.
Do not use an external kinematics, linear-algebra or URDF-model library. Implement the Jacobian, the small linear solve and the iteration yourself. The course image has no numeric library anyway (no Eigen, BLAS, LAPACK or numpy). The one piece of plumbing the course supplies is the same XML parser as Project 3. You may use Python, C, C++, or Rust, and internal process/thread topology is your choice, as in the earlier projects.
The exact contract is spec/IK_API.md; the URDF rules are
spec/ROBOT_DESCRIPTION.md (the same as Project 3's). Where this
handout and the spec differ, the spec wins.
Learning goals¶
- Build the manipulator Jacobian from a robot's kinematic tree: which joints move a given link, and how each one moves it (rotation about an axis, or translation along it).
- Turn an end-effector error into joint motion with three methods: the Jacobian transpose
(
Δq = α·Jᵀ·e), the Jacobian pseudoinverse (Δq = α·Jᵀ(JJᵀ)⁻¹e), and its damped form, damped least squares (Δq = α·Jᵀ(JJᵀ + λ²I)⁻¹e). See why the plain pseudoinverse blows up near a singularity and how damping keeps the step bounded. The services offer the last two as one method,"JacobianPseudoinverse"with a damping λ: λ = 0 is the plain pseudoinverse, the special case of damped least squares. - Iterate to a solution, respect joint limits, and report honestly when a target is out of reach.
- Extend position IK to a full pose with an orientation error that does not depend on how rotations are represented.
System architecture¶
your tooling ──set_param robot_description──► param_server
│ get_param
▼
external client ─call─► /ik/robot_jacobian ┐
/ik/solve_robot_step ├─ your IK node(s)
/ik/solve_robot ┘ (URDF reader + FK from Project 3,
plus the Jacobian, step and solve)
| Piece | Status |
|---|---|
Gateway on 127.0.0.1:9095 and middleware |
yours, from Project 1 |
param_server (/param_server/set_param, /param_server/get_param) |
yours, from Project 3, unchanged |
| URDF reader and forward kinematics | yours, from Project 3 |
/ik/robot_jacobian, /ik/solve_robot_step, /ik/solve_robot |
new: one node or several, your choice |
Project 3's robot_state_publisher and robot_world_state_publisher |
optional: they may run, nothing in Project 4 uses them |
Every IK request carries everything it needs: the configuration, the end-effector link, the
target, the method, the step length, the damping, the tolerances and the iteration limit.
Nothing in Project 4 reads /joint_states or any other topic.
Starter projects¶
Each starter (project4/starter/{python,c,cpp,rust}) is a placeholder, as in Projects 2 and 3:
a Makefile (build, run, clean, test), a main program that prints a TODO line and
idles, and the same small example of reading a URDF with that language's supplied XML parser as
in Project 3:
| Language | Supplied XML parser | Example |
|---|---|---|
| Python | standard library xml.etree.ElementTree |
src/urdf_example.py |
| C | ezxml 0.8.6 (vendored in third_party/ezxml/) |
src/urdf_example.c |
| C++ | pugixml 1.14 (vendored in third_party/pugixml/) |
src/urdf_example.cpp |
| Rust | roxmltree 0.20.0 (vendored in vendor/roxmltree/) |
print_links in src/main.rs |
The starters contain no middleware and no nodes. The natural route is to copy your
Project 3 runtime into your chosen starter directory, keep its gateway, middleware,
param_server and URDF reader, and add the IK services. Extend the Makefile so make build
compiles everything and make run starts what "Submission, building, and running" lists. See
starter/README.md.
The student kit¶
This handout, the specs it links to, the four starters, and a submission script are available
pre-packaged as the student kit: a bundle with no other course-repo material, distributed by
course staff. Its README.md lists what is inside.
The kit ships no robot description. Start from the two-link arm in
spec/ROBOT_DESCRIPTION.md (its Jacobian is worked in full in spec/IK_API.md), then write
small URDFs of your own that cover what that one does not: a prismatic joint with a non-unit
axis, a fixed joint inside the chain, a branch off the chain, and a chain of more than six
movable joints. Real robots' published URDF files are valid descriptions too, under the rules of
spec/ROBOT_DESCRIPTION.md.
Submission, building, and running¶
Submit a project root with a top-level Makefile providing:
Choose one supported implementation language: your submission.tar.gz extracts to one
project root whose direct contents include that submission's Makefile. If you have the student
kit, ./make_submission.sh <c|cpp|python|rust> at its root builds a correctly shaped
submission.tar.gz from your completed starter directory.
make buildis noninteractive, repeatable, and offline: the grading environment has no network. It must finish within the setup time limit (180 s, including extraction). Use only what the course environment provides (listed below) plus source you include in your submission, such as the vendored XML parser.make runlaunches your runtime in the foreground, exposing127.0.0.1:9095: your gateway,param_server, and whatever answers the three IK services. It needs no files, environment variables, or command-line arguments, and it starts with no robot loaded: every robot arrives as aset_paramofrobot_description. The services must answer within 10 seconds ofmake runstarting.make runmust not start anything that publishes/joint_statesor that setsrobot_description. During grading, robots come only from outside your runtime.make demois optional and never graded: use it for anything you want to show off, such as a viewer.make cleanremoves generated artifacts.
What the course environment provides (the same for every project; nothing else is installed, and nothing can be downloaded):
| Available | |
|---|---|
| All languages | Ubuntu 24.04; GNU Make, Bash, standard Unix process tools; CMake if you want it |
| Python | Python 3.12 and its standard library only: no pip packages (no numpy) |
| C | GCC 13 with C17, libc/POSIX, sockets and pthreads; cJSON (#include <cjson/cJSON.h>, link -lcjson) |
| C++ | G++ 13 with C++20, libc/POSIX, sockets and std::thread; nlohmann JSON (#include <nlohmann/json.hpp>, header-only) |
| Rust | Rust 1.85.1 and its standard library; one crate, serde_json 1.0.151 (with the crates it depends on), resolved offline: declare it as serde_json = { version = "=1.0.151", features = ["float_roundtrip"] }. Any other crate fails to resolve, unless you include its source in your submission as a path dependency, as the starter does with roxmltree. starter/README.md lists the exact crate versions for your Cargo.lock |
As the overview says, do not add an external kinematics, linear-algebra or URDF-model library to your submission either.
Your own tests (optional, not graded). You may also submit up to 25 pass/fail tests of your
own in a tests/ folder, run with make test while make run is running in another terminal.
They come with your project into the course's mutation challenge, a separate activity that course
staff describe when it starts; the README's notes about it (your container, autograde, comments
removed) apply only there. The README in your starter's tests/ folder gives the rules. It is
the same in every project, so its examples call earlier projects' services (such as
/heap_sort); test_jacobian_example.py next to it is the Project 4 example (the Jacobian of
the spec's worked two-link arm).
Autograder.io¶
Grading is black-box using Autograder.io, as in Projects 1–3: it extracts your project, runs
the Make targets in the offline course environment, launches your runtime with make run, and
connects externally to 127.0.0.1:9095 using the documented TCP/JSON protocol. It loads robots
through /param_server/set_param and calls your three IK services. It never reads your files,
logs, or output. No frontend or visualization is part of grading.
There are two hosted projects: the checkpoint (38 points: categories A and B, except B's near-1 MiB description) and the final project (100 points; see "Grading"). Each test is all-or-nothing and has a time limit that covers starting your runtime and several calls, so keep startup fast, and treat the 5-second solve guidance below as a ceiling for one solve, not a target.
Do not hard-code version numbers. Other robots may already have been loaded into your
runtime before the one you care about. A version means something only relative to the version
set_param returned.
Raise your gateway's service-call timeout. Your Project 1 gateway gives up on a service call after a few seconds. A solve on a long chain can take a while in Python, so make your gateway wait longer than your slowest solve (the spec recommends that a solve reply arrive within 5 s for chains of up to 10 movable joints and 2000 iterations).
Request line size¶
Every hop in your runtime MUST carry lines of at least 1 MiB (checkpoint and final), as in Project 3. A URDF travels as one JSON string inside one line: the
set_paramrequest, and again theget_paramreply your IK node reads. Real robots' request lines are 20–110 KiB, and a description can come close to 1 MiB. A fixed 64 KiB line buffer will fail; read until the newline, in the gateway and in every node-to-node connection.
Project checkpoint — Robot Jacobian Service¶
Project checkpoint: due Fri Nov 13, 2026.
Given a robot description, a configuration and an end-effector link, your runtime returns the
manipulator Jacobian. The checkpoint uses your parameter server (/param_server/set_param
and /param_server/get_param) and /ik/robot_jacobian, and nothing else: /ik/solve_robot_step
and /ik/solve_robot need not exist yet. It is a separate hosted project and grades categories
A and B below, except B's near-1 MiB description, which is graded only in the final project.
To pass it you need:
- your parameter server, from Project 3;
- an
/ik/robot_jacobianthat answers for the newestrobot_description(read it withget_param, or any way your internal design allows), reports thedescription_versionit answered for, and picks up a new description within 2 seconds; - the chain from the root link to the requested link, and its movable joints in order;
- the Jacobian rows for revolute, continuous and prismatic joints, in the root-link frame;
- error replies for bad requests, and real robot URDFs (including ones over 64 KiB). The 1 MiB line requirement holds from the checkpoint on; only its near-1 MiB test waits for the final.
The step and the solve are not part of the checkpoint. The checkpoint's features are graded again in the final project, so this work counts toward both.
Reading the robot¶
Your IK node answers for the robot in robot_description. Read it with
/param_server/get_param on each request, or poll in the background; either is fine.
- Every reply carries
values.description_version: therobot_descriptionversion you computed it for (0if none was ever set). - Within 2 seconds of a
set_paramthat returned versionV, every reply must carry a version>= V, and every reply must be computed for the description whose version it echoes. A runtime that fetches the description on each request meets this automatically; one that polls must poll often enough. - If the newest description is invalid (by
spec/ROBOT_DESCRIPTION.md, "Validity", the same rules as Project 3), your services may either reject every request or keep answering for the previous valid robot, and they must not crash. The next valid description must be picked up.
Frames and the chain¶
Everything in Project 4 is in the root-link frame of the loaded robot: the Jacobian, the target pose, and every error. There is no world frame or global pose.
For an end-effector link e, the chain is the path of joints from the root link down to
e. Its movable joints are its revolute, continuous and prismatic joints, in
root-to-end-effector order. Fixed joints still contribute their <origin> to forward
kinematics but have no degree of freedom, and joints on other branches of the tree are not part
of the chain at all. The end-effector point p_e is the origin of link e.
A request's joint_state ({"name": [...], "position": [...]}) gives the configuration: a
movable joint it does not name is at 0; names that are not movable joints are ignored;
positions are used exactly as given, never clamped to the limits.
The Jacobian¶
Each movable joint i has a frame (after its <origin>, before its motion) with origin p_i
and rotation R_i in the root frame, and the axis a written in the URDF:
| Joint | Its row of the Jacobian |
|---|---|
| revolute, continuous | [ ẑ × (p_e − p_i) ; ẑ ], with ẑ = R_i·a / ‖a‖ |
| prismatic | [ R_i·a ; 0, 0, 0 ], with a as written (not normalized) |
The first three numbers are the linear velocity of p_e, the last three the angular velocity
of link e, per unit rate of that joint. The prismatic row uses the axis as written because
Project 3's forward kinematics translates a prismatic joint by q · axis with the axis as
written: the Jacobian is the derivative of your FK.
The reply lists joint_names (the chain's movable joints, root first) and jacobian, one
row per joint in the same order. This N×6 layout is the transpose of the 6×N textbook
Jacobian: row i is joint joint_names[i]'s column. The spec works a two-link example in full:
at shoulder = 0, elbow = π/2 the rows are [-1, 1, 0, 0, 0, 1] and [-1, 0, 0, 0, 0, 1].
Every entry must be within 1e-4 of the exact value. An unknown end-effector link, mismatched
name/position lengths and a non-numeric position are errors (result: false and a
status), and your service must keep answering after them.
One gradient-descent step¶
/ik/solve_robot_step takes the current configuration, the end-effector link, a target pose
(position plus a unit quaternion, in the root frame), ik_position_only, a method, a
step_length α and a damping λ. It returns the next configuration. It is stateless: the
answer depends only on the request, never on earlier calls.
For a position-only step, with e = p_target − p_e and J_v the first three entries of every
row (as a 3×n matrix):
method |
Δq |
|---|---|
"JacobianTranspose" |
α · J_vᵀ · e |
"JacobianPseudoinverse" |
α · J_vᵀ (J_v J_vᵀ + λ² I)⁻¹ e (or the identical α · (J_vᵀ J_v + λ² I)⁻¹ J_vᵀ e) |
"JacobianPseudoinverse" with λ = 0 is the plain pseudoinverse, α · J_vᵀ (J_v J_vᵀ)⁻¹ e; any
λ > 0 makes it damped least squares.
Expected default values. Every graded request carries all of method, step_length,
ik_position_only and damping. If your runtime fills in one that a client left out, use the
course's defaults: method "JacobianTranspose", step_length 0.1, ik_position_only true,
and damping 0 (the plain pseudoinverse). Rejecting such a request is also fine.
Solve the small linear system yourself, for example by Gaussian elimination with partial
pivoting: in the J_v J_vᵀ form it is 3×3 whatever the number of joints (6×6 for a full-pose
step), while the J_vᵀ J_v form is n×n. Every joint's q + Δq must be within 1e-4 of the
exact value. Note that λ appears squared, and that without damping J_v J_vᵀ is singular
whenever the chain cannot move the end effector in all three directions: with fewer than three
movable joints, and for any planar arm, the worked two-link example included.
With ik_position_only: false the step uses the 6-vector error [e ; e_ω] and the whole 6×n
Jacobian. The spec recommends e_ω = the rotation vector (axis × angle) of R_target · R_eᵀ.
Other orientation errors, such as differences of Euler angles, may also be used, but the
rotation vector is the one that does not suffer from gimbal lock.
Inverse kinematics: the solve¶
/ik/solve_robot takes the step's fields plus position_tolerance, orientation_tolerance and
max_iterations, and iterates from the given configuration:
- Compute the position error
‖p_target − p_e‖and, for a full-pose solve, the orientation error: the angle of the rotationR_targetᵀ · R_e(in[0, π]; it does not depend on how you represent rotations). - If both are below their tolerances, stop:
converged: true. - If you have taken
max_iterationssteps, stop:converged: false. - Otherwise take one step, then clamp every revolute and prismatic joint to its limits, and go back to 1.
Reply with the final configuration_solution, converged, final_error (the position error
and the orientation error, or null for a position-only solve) of the configuration you
return, and description_version, and optionally iterations; result equals converged. A target the
robot cannot reach is a normal converged: false reply, not an error.
What is graded:
- Honesty. If you say
converged: true, the configuration really is within tolerance, andfinal_erroris the true error of the configuration you return. - Limits. Every revolute and prismatic joint you return is within its limits.
- Convergence. A reachable target that the iteration above reaches within
max_iterationswithout any joint hitting a limit must be reported converged. You may use a faster or smarter iteration, as long as it still does that. - Unreachable targets are reported as not converged, honestly.
- Full pose. With
ik_position_only: false, both errors must be below their tolerances.
Testing¶
Test the way Autograder.io does: as an external TCP/JSON client of your running runtime on
127.0.0.1:9095. A few lines of Python load a robot and ask for one Jacobian:
import itertools, json, socket, sys, time
sock = socket.create_connection(("127.0.0.1", 9095))
stream = sock.makefile("rwb")
ids = itertools.count()
def call(service, args):
call_id = f"call-{next(ids)}"
stream.write((json.dumps({"op": "call_service", "service": service, "id": call_id, "args": args}) + "\n").encode())
stream.flush()
for line in stream: # skip other traffic until our reply arrives
reply = json.loads(line)
if reply.get("op") == "service_response" and reply.get("id") == call_id:
return reply
with open(sys.argv[1], encoding="utf-8") as f: # any .urdf file
version = call("/param_server/set_param", {"name": "robot_description", "value": f.read()})["values"]["version"]
# An empty joint_state puts every joint at 0; name joints here to pick a configuration.
request = {"joint_state": {"name": [], "position": []}, "endeffector_frame": sys.argv[2]}
deadline = time.monotonic() + 3 # replies must reflect a new description within 2 s
while True: # wait until the IK node answers for our version
reply = call("/ik/robot_jacobian", request)
if reply["values"].get("description_version", 0) >= version or time.monotonic() > deadline:
break
time.sleep(0.1)
if not reply["result"]:
sys.exit(f"error: {reply['status']} (description_version {reply['values'].get('description_version')})")
for name, row in zip(reply["values"]["joint_names"], reply["values"]["jacobian"]):
print(name, [round(v, 4) for v in row])
Things worth testing yourself:
- The worked example in
spec/IK_API.md, and small robots you can do on paper: one revolute joint, a planar two- and three-link arm, a prismatic joint whose axis is2 0 0under a rotated origin. - Finite differences. Your own FK is the ground truth: move one joint by a small
hand check that the end-effector point moves by abouthtimes that joint's row. This catches almost every Jacobian bug. - Chains in trees. A robot with two arms: the left gripper's chain must not include right-arm joints. Fixed joints in the middle of a chain.
- Real robots. The real robots used in grading are not in the kit, but everything their URDFs
contain is covered by
spec/ROBOT_DESCRIPTION.md: give your own test URDFs those features (a long arm chain with a prismatic joint in it, hollow<joint>elements inside<gazebo>blocks, comments, namespace declarations). For your line size, a robot of your own with a few thousand fixed links, well over 64 KiB. - Steps by hand. A two-link arm at
q = 0toward a nearby target: computeα·J_vᵀ·eon paper and compare. Then the damped step with two values of λ. - Solves. A reachable target (pick a configuration, compute its FK, and solve for it from a nearby start), a target out of reach, a target that needs a joint at its limit, and a full pose on a 6- or 7-joint arm.
Common pitfalls¶
- The sign of the error. It is
e = p_target − p_e. Withp_e − p_targetevery step goes the wrong way. p_e − p_i, notp_i − p_e, inside the cross product.- The joint's position.
p_iis the joint frame's origin after its<origin>transform, not the parent link's origin. - The axis in the wrong frame. Rotate it into the root frame with
R_ibefore using it. - Normalizing. Normalize revolute and continuous axes; do not normalize prismatic ones.
- Fixed joints and other branches. They get no rows.
- The end-effector point. It is the origin of the requested link, which may sit behind fixed joints after the last movable joint.
- λ, not λ². The damping term is
λ²I. - Hard-coding α, λ or a tolerance. Use the values in each request.
- A step that remembers.
/ik/solve_robot_stepmust not continue a previous call's solve. final_errorfor the wrong configuration. Report the error of the configuration you return, not the one before your last step.- Claiming convergence you did not reach. Check the tolerance before you say
true. - Clamping inside the Jacobian. The Jacobian uses the configuration exactly as given; only the solve's steps clamp to the limits.
- Ignoring
ik_position_only. For a position-only solve, ignore the target orientation. - A short gateway timeout. Raise it above your slowest solve (see "Autograder.io").
- A fixed-size line buffer anywhere. Read until the newline.
- Hard-coding version numbers. Compare against the version
set_paramreturned.
Grading¶
| Category | Weight |
|---|---|
| A. Description and chain | 10% |
| B. Jacobian correctness | 30% |
| C. Gradient step | 20% |
| D. IK solve, position | 30% |
| E. IK solve, orientation | 10% |
| Total | 100% |
Every category is graded by Autograder.io. The checkpoint grades categories A and B, except B's near-1 MiB description, which is graded only in the final project.
A. Description and chain covers description_version and picking up each new
robot_description, the chain's joints and their order on robots with branches and fixed
joints, and error replies that leave your service running.
B. Jacobian correctness checks the rows for planar arms, spatial chains with rotated origins
and non-unit axes, prismatic joints, positions used exactly as given, partial joint_state, a
long chain, real robot URDFs sent unmodified (small and large), and a description near the
1 MiB line bound.
C. Gradient step checks single Jacobian-transpose and damped-least-squares steps exactly, with several step lengths and damping values, and that a step depends only on its request.
D. IK solve, position checks solves of reachable targets with both methods on synthetic and real robot arms, joint limits, honest reporting of unreachable targets, and a reloaded robot.
E. IK solve, orientation checks full-pose solves on 6- and 7-joint arms, synthetic and real.
Optional, ungraded. An IK action node (drive the robot to a solved target) and a timed trial node (a sequence of random targets) are natural next steps, and a starting point for the IK setpoint competition later in the term. They are not part of this project's grading.