Intelligent Systems for Robotics · Week 5
Week 4 gave the robot a vocabulary, and a reasoner that draws conclusions from what you wrote in it. This week the knowledge graph starts to drive behaviour. It gains the map the robot navigates, and the compositions of skills that the planner will reason over from week 6. Both live in the same graph. It also gains a path from detections to facts, so that the room types you defined in week 4 come out of what the robot sees from places you chose.
In week 1 you drove a closed square and asked whether you believed the pose the robot
reported. On the bare motor-test floor, with arc turns, the square finished 0.174 m
from its start after 4 m of travel. Wheel odometry with an inertial yaw is the only
pose estimate isfr.robot offers, and the course gives you no ground-truth
channel. A route across the apartment is several times longer than that square, and it
passes through doorways a few tenths of a metre wider than the base. If drift grows in
proportion to the distance driven, a whole route accumulates more of it than a doorway
leaves on either side of the base.
The map you build this week therefore holds no pose in a world frame. Every metric quantity in it is relative to something the robot can perceive from where it stands, such as a doorway or a piece of furniture. The robot never needs to know where it is in the apartment. It needs to know where the next recognisable thing is relative to itself. Each time it arrives at a place and perceives that place's anchor, it measures its pose against the anchor afresh, and the odometric error from the edge it has just driven no longer matters. Re-anchoring bounds drift to one edge. That is the only scale at which week 1's evidence says odometry is usable.
The choice has a cost, which the section on route search meets. With no common frame, the search has no straight-line distance to the goal to guide it.
The design has a parallel in how people find their way. You walk to a colleague's office by recognising a door, a corridor junction and a stairwell, and by knowing roughly where the next one lies from where you stand. Warren, Rothman, Schnapp and Ericson (2017) tested whether people's knowledge of navigation space is a Euclidean map or a labelled graph. Participants walked a virtual hedge maze that held two wormholes, which rotated and teleported them. Their shortcuts were biased by the wormholes, with mean constant errors of 37 and 41 degrees where 45 were expected. A labelled-graph model fitted the shortcuts decisively better than a Euclidean map model. The authors conclude that knowledge of navigation space is a labelled graph, whose local metric information is approximate, inconsistent, and not in a common coordinate system.
Read this as a lens, in the way you read Endsley's levels in week 1. The flat claim that people hold no metric map is contested. Peer, Brunec, Newcombe and Epstein (2021) review the evidence and report that map-like and graph-like representations both exist, and that they can operate together or apart. They name grid cells as a possible neural substrate for a Euclidean coordinate system, and they give the evidence that people's spatial memories violate Euclidean rules. The defensible form of the claim is narrower. People navigate well without a globally consistent metric map, and grid cells may still carry a metric code. The robot's map does not depend on either claim. Its reason is the odometry you measured in week 1.
A place is a node of the map. It is somewhere the robot can recognise and stop, and the
base ontology has had the class isfr:Place since week 4. This week every
place gains an anchor, given by isfr:anchoredBy. The anchor is what the
robot perceives at the place. Two kinds exist.
A gap anchor, isfr:GapAnchor, is an opening in the lidar scan, with an
expected width in isfr:width. Doorways and corridor junctions are gap
anchors. A doorway reads as a gap of its own width in TIAGo's lidar. In a probe run on
2026-09-11, a 0.8 m doorway read between 0.793 and 0.812 m from 1.0 to
3.0 m away. An object anchor, isfr:ObjectAnchor, is an object the
detector names, such as a sink, a fridge or a dining table. It names the ontology class
of its object with isfr:anchorClass. The section on grounding explains why
the anchor names an ontology class.
The detector limits the choice. It returns COCO class names, and COCO includes
sink, oven, refrigerator,
dining table, couch, bed, bottle
and cup. It has no door and no worktop. A doorway is therefore anchored by
its gap, and a worktop by the sink or the oven on it, with the offset carried by the
approach. A room centre is never a place, since nothing at the centre of a room can be
recognised.
Every place defines a local frame, and the hints between places are written in those frames. A frame has to meet two conditions. It must be observable from the anchor on arrival, so that the robot can measure its own pose in it. It must also be independent of the direction the robot arrived from. Each edge carries one hint, and the reverse hint is computed from it. If a place's frame turned with the direction of arrival, the hint of every edge leaving that place would depend on which edge the robot came in by, and one hint per edge could not be right for every arrival.
A gap meets both conditions. Its frame has its origin at the midpoint of the opening and
its x axis along the normal to the opening. The gap's authored isfr:joins
pair of regions fixes the sign, and the axis points from the first region into the
second. Both ends of the opening are visible in the lidar scan, so the frame is
observable. The robot knows which region it came from, so it can resolve the sign. The
approach of a gap place is the frame's origin, facing along x, so the robot stops in
the opening. A gap place belongs to the first region of its pair.
An object meets only the first condition. A detection gives an object's position and not its orientation. An object place's frame therefore has its origin at the approach pose, a stand-off distance from the object, and its x axis along the line of sight to the object on arrival. For an object the robot can reach from one side only, such as a sink against a wall, the line of sight is nearly the same whatever direction the robot came from. For a dining table, which it can approach from any side, the line of sight changes with the arrival, and hints leaving such a place are less accurate. The design tolerates this. Hints are coarse by construction, and once the next anchor is in view the robot steers by what it sees.
Here are two places from this week's example, a small flat with a hall and two corners:
:hall_door a isfr:Place ;
rdfs:label "hall door" ;
isfr:inRoom :hall ;
isfr:anchoredBy [ a isfr:GapAnchor ; isfr:width 1.0 ;
isfr:joins ( :hall :kitchen_corner ) ] .
:counter_place a isfr:Place ;
rdfs:label "counter place" ;
isfr:inRoom :kitchen_corner ;
isfr:anchoredBy [ a isfr:ObjectAnchor ; isfr:anchorClass isfr:KitchenSink ] .
Regions group places. isfr:Region has the subclasses
isfr:Room and isfr:Corridor, and a place belongs to one region
through isfr:inRoom, as in week 4. The hall door is a gap from the hall
into the kitchen corner, so it belongs to the hall. The counter place belongs to the
room its sink stands in.
You author the map this week. The same graph could be built by exploration, with the robot recording anchors and hints as it drives. The course authors it to save time, and building it by exploration is outside its scope.
An edge joins two places. It is an individual of class isfr:Edge, with
isfr:from and isfr:to, because it carries a hint and runtime
state and a plain triple can carry neither. The hint, isfr:hint, is the
approximate pose of the target place's frame in the source place's frame. It has
isfr:x and isfr:y in metres, x ahead and y to the left, and
isfr:yaw in radians, anticlockwise. These are the keys
robot.pose() returns. A hint such as 2 m ahead, 0.5 m to the left
and turned a quarter turn left is enough. It does two jobs. It gives the robot a goal to
drive towards until the target's anchor is in view, and it tells the robot which of two
similar openings is the intended one.
A hint is an element of SE(2), the group of rigid motions in the plane. Write a hint as $h = (x, y, \theta)$, with $\theta$ the yaw. Let $a$ be the pose of frame $B$ in frame $A$, and $b$ the pose of frame $C$ in frame $B$. The pose of $C$ in $A$ is their composition:
$$a \oplus b = \begin{pmatrix} x_a + x_b \cos\theta_a - y_b \sin\theta_a \\ y_a + x_b \sin\theta_a + y_b \cos\theta_a \\ \theta_a + \theta_b \end{pmatrix}$$
The translation of $b$ is rotated into $A$'s orientation and added to the translation of $a$, and the yaws add. Composition is associative, and it is not commutative. The inverse of a hint is the pose of the source's frame in the target's frame:
$$\ominus h = \begin{pmatrix} -x \cos\theta - y \sin\theta \\ x \sin\theta - y \cos\theta \\ -\theta \end{pmatrix}$$
Composing a hint with its inverse gives the identity,
$h \oplus (\ominus h) = (0, 0, 0)$. Expand the first row to check it.
isfr/knowledge/topology.py implements the two operations as compose and invert, and wraps each yaw
into $(-\pi, \pi]$.
Take an edge from the example. The edge from the arch to the bed place has the hint $(1.38, 0.60, -1.57)$. With $\theta = -1.57$, $\cos\theta \approx 0$ and $\sin\theta \approx -1$, so the inverse is $(0.60, -1.38, 1.57)$. Seen from the bed place, the arch is 0.60 m ahead and 1.38 m to the right, and its frame is turned a quarter turn anticlockwise from the bed place's.
You author each edge in one direction only. The reverse is derived by inversion, so the
two directions cannot disagree. Authoring both would give two hints for one pair of
places, each with its own error, and nothing would say which one to trust.
edges() raises an error, naming both edges, when a pair of places is joined
in both directions.
Composition is also how the robot uses a hint while it drives. Let $p_0$ be the odometric pose at which the robot left the source place, and $p_t$ its odometric pose now. The robot leaves from the source's approach pose, which is the origin of the source place's frame, facing along its x axis. The change $\Delta = (\ominus p_0) \oplus p_t$ is therefore the robot's pose in the source place's frame, and the target, in the robot's own frame, is
$$g = (\ominus \Delta) \oplus h$$
Only the change in odometry enters, so the origin of the odometry frame cancels and
drift accumulates over this one edge. Once the target's anchor is in view, the perceived
pose of the anchor replaces $g$. If the anchor does not appear within the hint's
distance and a margin, go-to, the learned skill that drives one edge, is
designed to end in failure with the reason that the expected landmark was not seen.
That is monitoring in week 1's sense, noticing that what the robot expected has stopped
being true, at the lowest layer of the stack.
Kuipers (2000) describes the Spatial Semantic Hierarchy in four levels: control, causal, topological and metrical. Its topological level introduces places, paths and regions by abduction from the states and actions of the causal level. Local geometric frames, linked by topological connections, form what Kuipers calls a patchwork map, and they are merged into a single global frame only when enough information and computation are available. Your map is a patchwork map in that sense, with a frame at every place and a hint on every edge. The course never takes the last step of merging the frames.
A route is a sequence of edges from one place to another. Its length is not fixed, so a
route is found by search when it is needed, and it is never written into the graph.
The provided skill navigate-to is designed to run the search and then drive
the route with go-to, one edge at a time. This week you call the search
directly, as route(kg, start, goal), which returns the edges in order, or
None when no route exists.
Each edge costs the Euclidean length of its hint:
$$c(e) = \sqrt{x_h^2 + y_h^2}$$
Here $x_h$ and $y_h$ are the translation of the edge's hint. That cost is as coarse as the hint, and the route found is the cheapest under those costs. The search is uniform-cost search. Poole and Mackworth, section 3.5.4, describe it under the name lowest-cost-first search. It keeps a frontier of paths ordered by cost, always expands the cheapest, and stops when the cheapest path on the frontier ends at the goal. It is the simplest method guaranteed to find a minimum-cost path.
A* would usually expand fewer paths. It orders the frontier by the cost so far plus a heuristic estimate $H(n)$ of the cost from node $n$ to the goal. Poole and Mackworth, section 3.6, call a heuristic admissible when it never exceeds the cost of a lowest-cost path from a node to a goal. Their Proposition 3.1, in section 3.6.1, shows that, when a solution exists, A* with an admissible heuristic returns an optimal one if the branching factor is bounded and every arc cost exceeds some positive constant. A finite map whose edges all have positive length meets both conditions. On a metric map the straight-line distance to the goal is the usual admissible heuristic. It needs both positions in one frame, and this map has no common frame. You could compose hints along some path to estimate where the goal lies. That needs a path first, and two paths would give two different estimates, since the hints are approximate and do not agree with one another. Neither estimate is guaranteed to stay below the true cost. The one heuristic known to be admissible here is $H(n) = 0$, and A* with a zero heuristic is uniform-cost search.
The maps in this course have tens of places, so the search is cheap.
reachable(kg) returns every ordered pair of places joined by a
route, each place with itself included. Week 6's planner never sees a route. Its domain holds
navigate-to, with the precondition (reachable ?from ?to), and
the reachable facts are computed from the graph when the problem is
generated. Route finding stays outside PDDL.
The knowledge graph is now one rdflib dataset that holds three named
graphs. KnowledgeGraph, in isfr/knowledge/graph.py, wraps it.
KnowledgeGraph.load(*paths) fills it, and it does not change during a
run.
close() recomputes it when the belief graph changes, and never once per
control tick.
asserted() returns the map and belief graphs together, and
union() returns all three. Keeping them apart lets you inspect separately
what was written, what was seen and what was concluded. As in week 4, a query on
asserted() shows what the reasoner had to work with, and the same query on
union() shows its answers. The closure takes about 0.1 s for the
example's flat and about 0.3 s for the micro-project's map, measured on the
instructor's laptop (Apple M4 Max).
When the robot finds an edge impassable, block(edge) writes one triple to
the belief graph:
:hatch isfr:blocked true .
The map graph does not change. The derived reverse of an edge shares the authored edge's IRI, so blocking an edge closes it in both directions, and the route search skips it. A blocked edge stays blocked for the rest of the run. Letting it expire would need a timing policy, and nothing in the course teaches one. In the example, the hatch joins the bed place to the counter place directly:
route: bed place -> counter place
after blocking the hatch: bed place -> arch -> hall door -> counter place
The hatch is one edge of 2.0 m. The way round, through the arch and the hall door,
is 6.0 m. After block, the same call to route returns the
way round.
Perception is tooling in this course, and grounding what it returns is a course topic. A
detection is a COCO class name, a confidence and a position in the robot's base frame,
x ahead and y to the left, in metres. ground(kg, place, detections), in
isfr/perception/grounding.py, turns the detections made at a place into
individuals in the belief graph. isfr:objectAt puts each individual at
the place, with a pose relative to the place's frame. It never receives
a pose in a world frame.
The detector names COCO classes, and the graph holds ontology classes. Week 4's
annotation isfr:cocoClass joins the two. A detection takes the most general
class that carries its COCO name, so a bottle becomes an
isfr:Bottle and a sink an isfr:Sink. A detection
whose COCO name no class carries is left ungrounded, and ground returns it
in a separate list.
The sink from week 4 returns here. The base ontology splits isfr:Sink into
isfr:KitchenSink and isfr:BathroomSink, and keeps the COCO name
on the parent, because the detector cannot tell the two apart. A sink grounded from its
COCO name alone is therefore an isfr:Sink. The kitchen axiom needs an
isfr:KitchenSink, so it would never fire, and no kitchen would ever be
inferred from a detection.
The anchor class settles it. When you anchor a place on a kitchen sink, you write
isfr:anchorClass isfr:KitchenSink. A sink detected at that place is
explained by the anchor, and it takes the anchor's class. The rule is that a detection
takes the anchor class when that class, or a superclass of it, carries the detection's
COCO name, and the detection is the nearest of that name to the robot. At a place
anchored on a bathroom sink, the same detection becomes an
isfr:BathroomSink, and no kitchen follows. The fact you author is about the
fixture, as in week 4, where you asserted which sink was which. The room type is still
inferred, and you never write that a room is a kitchen. The example shows the anchor at
work at its counter place:
at the counter place the stub sees:
bottle: 0.80 m ahead, 0.30 m left
sink: 1.00 m ahead, -0.30 m left
grounded as:
counter_place-bottle-1: Bottle
counter_place-sink-1: KitchenSink
Data association is one rule. A detection of the same class, at the same place, within 0.3 m of an existing individual's pose, is that individual. Any other detection is a new individual. The rule is kept to one line because data association is a perception problem, and perception is tooling here. Its cost follows from the map. Poses are relative to the place they were seen from, and two places share no frame in which their poses could be compared, only a hint too coarse to tell one bottle from two. The same bottle seen from two places therefore becomes two individuals. The 0.3 m radius is a proposal. The stub detector has no noise, and YOLO's depth error has not been measured.
This week every detection comes from StubDetector, in
isfr/perception/stub.py. It reads the apartment from
data/apartment/world.json and reports what the head camera would see from a
pose. It sees a bottle, a cup or a wine glass within 1.0 m, and anything else within
4.0 m, inside a field of view 1.0 rad wide, and never through a wall. The
1.0 m follows a probe of YOLO on Webots models. A beer bottle read as
bottle with confidence 0.746 from 0.6 m with the head tilted down, and
from 1.4 m no bottle was labelled at a confidence of 0.25 or more. The field of view
is assumed until the apartment world reports the camera's. The world coordinates the
stub reads stay in its input, and none reaches a detection or the graph.
The reasoner works under the open-world assumption, as in week 4. When no sink was seen
from any of a room's places, the room's type is unknown. The robot may not have looked
from the right place, or the sink may lie outside the field of view or more than
4.0 m away. A query for rooms that are not kitchens, written with
FILTER NOT EXISTS, reads absence as falsehood, and its answer depends on
where you stood the robot.
Week 8 shows a failure repaired at the lowest layer able to repair it, at one of three
levels. The go-to policy drives round an obstacle, navigate-to
blocks the edge and finds another route round it, and the task planner replans when a
skill ends in failure.
The micro-project builds this week's part of the fetch system. You map the kitchen and the two living rooms of the apartment, in 8 to 12 places that include four the README names, and compute each hint from approach poses you read off the floor plan. Then the check script stands the stub detector at each of your places, grounds what it sees, closes the graph, and runs two queries you write, one for room types and one for the drinks the robot has seen. If you anchored a place on the kitchen sink and stood the robot where it can see it, the kitchen comes out a kitchen. Nothing in it needs Webots.
Week 6 plans over the skills, with the reachable facts computed from a map
like yours. Before then, run the example, read Poole and Mackworth, sections 3.5 and
3.6, and start the micro-project with the map.
Run from the repository root:
PYTHONPATH=. python3 examples/05-topological-map/main.py
It loads a small flat of four places and four edges, prints every edge with its hint
and marks the derived reverses, finds a route, blocks the hatch and finds the route
again. It then stands the stub detector at two places, grounds what it sees, and asks
which rooms have a type before and after reasoning. Nothing in it needs Webots. Read
flat.ttl next to the base ontology's week 5 terms, and compare each
authored edge with its derived reverse.
This week's exercise is exercises/05-topological-map/. Its
README.md states the task and ends in a Self Check list you can verify
unaided. The instructor's map of the whole apartment is published in
data/reference/week-05/ after the deadline.
The micro-project runs from the inputs it provides. It does not need any earlier week's output.
Further, optional, and not examined:
More background, optional: further reading.
Intelligent Systems for Robotics · course materials