My work in KTH’s Introduction to Robotics course: a seven-joint arm that reaches a pose, a map built from laser scans, routes a car-like robot can actually drive, and a ROS 2 robot that recovers when it is moved. All in simulation.
From an arm’s joints to a robot that finds its way.
This course took me from the geometry of a robot arm to the uncertainty of a mobile robot. Kinematics says where the arm is; a map says what is around the robot; planning and behaviour turn both into the next move. The arm, mapping and planning code on this page is my own. The navigation assignment I first submitted in a pair and later revised on my own. The simulators, robot models and frameworks around the code belong to the course.
My submitted inverse-kinematics code, re-run unchanged for this page on 25 nearby reachable targets, with forward kinematics checking each solved pose. Geometry only: no collisions, joint limits or hardware.
01 / Forward and inverse kinematics
From joint angles to a pose, and back again.
The SCARA arm’s inverse kinematics have a closed-form answer, so I solved them analytically. The seven-joint KUKA arm has no such shortcut. I built its chain of transformations and a geometric Jacobian, then iterated with damped least squares, reducing the position and orientation errors together until both were within a tolerance of 10⁻⁴.
For this page, my submitted KUKA code was run again, unchanged, on 25 nearby target poses, and each answer was checked with forward kinematics. The largest position error was below 0.084 mm. It is a consistency check made in September 2026, not a course result.
INSPECT A SOLVED POSE
01 / 25
Drag through the solved joint positions. The fine traces retain neighbouring poses, so the arm’s movement stays legible.
Projected joint chain from the original FK code. Geometric illustration; no collision model.
The run and its limits+
My solver uses a damping of 0.01, an update step of 0.5, at most 600 iterations, and a tolerance of 10⁻⁴ on both the position error and the orientation-error vector. The check for this page also measures the actual rotation angle between target and solution. None of this tests joint limits, collisions, convergence from far away, or a real arm. The KUKA model is the course’s, not hardware I designed.
Source: IK_functions.py; course LWR4+ model; new computation-results.json.
A robot-arm simulation shown alongside its terminal. Original monitor recording, 00:06–00:25.
02 / Mapping and estimation
Making free space explicit.
My mapping code turns each laser scan into a grid. It traces every ray, marking the cells it passed through as free and the cell where it stopped as occupied, then inflates the obstacles so that the rest of the navigation stack keeps a margin from them.
Knowing where the robot is in that map is the other half. The course taught Kalman and extended Kalman filtering, where each prediction is corrected by a measurement while the uncertainty is carried along. My navigation work uses AMCL, a particle-filter localisation method, instead. They solve the same problem in different ways, and I keep them apart here rather than present one as the other.
Implementation versus course context+
My mapping.py contains the map update and the inflation. I learned Kalman and extended Kalman filtering in the course, but I have no implementation of my own to show, so I list it as knowledge rather than as something built here. The map sequence above explains the algorithm; it is not a playback of sensor logs.
Source: mapping.py; DD2410 mapping materials and localisation lecture.
The obstacle map alongside the simulated environment. Original monitor recording, 00:07–00:24.
03 / Plan a feasible motion
A route must respect how the vehicle turns.
My planner grows a tree of short motions the vehicle can actually drive: each new branch is a bounded steering command, integrated forward through a Dubins-style car model, and I check clearance along the whole segment, not only at its end.
Goal-biased and Halton samples spread the search across the space. Each new node attaches to its nearest parent without rewiring, so the planner finds feasible paths and makes no claim to the optimality of RRT*.
SAMPLE
Goal bias + low-discrepancy positions
EXTEND
Bounded steering · integrated motion
CHECK
Bounds and inflated obstacles
RETURN
Steering controls and monotonically increasing times
Why the implementation detail matters+
My source includes helpers and comments associated with rewiring, but the executed insertion step explicitly uses the nearest parent only. A feasible returned path and an asymptotically optimal planner are different claims. I have no completion-rate or optimality benchmark for it.
Source: solution.py, sampling, steer_segment and nearest-parent insertion.
04 / Integrate the behaviour
Navigate, recover, continue.
The last assignment puts it together in ROS 2. The robot starts without knowing where it is, spins in place until its particle filter settles, fetches a goal, checks it against the map and drives there with Nav2. Then comes the test that gives the assignment its edge: someone picks the robot up and moves it. It has to notice that its estimate no longer fits what it sees, relocalise and carry on. I built the mission as a behaviour tree rather than a state machine, so that the check for being moved could sit above the normal mission and interrupt it without tangling the rest.
This assignment went through two submissions: a first one in a pair with Nicholas Bosiacki, and a later revision I made on my own. I have not measured how often it recovers or how long recovery takes. The recording below shows the behaviour; it is not a measurement.
ROS 2
Nodes, topics, services and actions
LOCALISE
AMCL and pose-confidence checks
NAVIGATE
Send a goal and observe its outcome
RECOVER
Reinitialise, gather evidence and resume
Course and contribution record+
The course’s submission comments separate the earlier paired navigation submission with Nicholas Bosiacki from my later revision. The arm and planner files carry my authorship. The frameworks, robot models and navigation packages are the course’s.
Source: BT_students.py, launch/setup files, Canvas submission comments and assignment packages.
The robot and map views changing during a navigation simulation. Original monitor recording, 01:01–01:25.
Guided scroll · scroll or press Escape to take over
