IEEE Robotics & Automation Magazine - September 2018 - 66

shows how the scene graph edges correspond to symbolic
multiplication or chaining of transformations in the Cartesian
space. Starting from the global root zero (see Figure 5) and
multiplying the relative pose of each local coordinate frame
along a chain yields the global pose of the frame at the end of
the chain. Picking and placing objects, common operations in
TMP, are represented by reparenting a frame in the scene
graph, i.e., changing the object's parent between the hand and
a support surface, such as a table.
Our scene graph implementation offers a unique set of
features that make it suitable for both TMP and real-time
execution. We use two variations on the structure: a mutable
version and a persistent version, i.e., a purely functional one.
Both variations can be efficiently updated at runtime, e.g.,
when the robot picks up a tray of objects, and they share
underlying data for geometric objects via reference counting
so that data for large meshes are not copied. The mutable
version is based on indexed arrays and avoids heap allocations, which may impose unacceptable pauses, after construction, making it suitable for real-time operation. The
persistent version is based on weight-balanced binary trees
that efficiently create partial copies on updates, useful during
planning when we backtrack to a previous point in the
search and a previous version of the scene graph. To enable
efficient multithreaded access, e.g., when performing inverse
kinematics, motion planning, and visualization in separate
threads, we separate the scene graph object from the data for
states and configurations.
We also provide a compiler (see Figure 6) enabling scene
graphs to be specified in domain-specific languages. Our

Task Domain
We represent the task domain by the task language in Figure 4.
Generally, task domains are specified using a variety of notations and logics, but, at a fundamental level, all these representations define some type of transition system, automaton,
or formal language. The de facto standard syntax for task
planning is the planning domain definition language (PDDL)
[19], which our framework also takes as input. The PDDL
(see Figure 3) defines parameterized actions with preconditions and effects based on first-order logic. Our task planning
algorithm [1], [2], however, is not specific to PDDL and
assumes only that the state space is finite and compactly represented with a set of variables. Thus, new task domains can
be created in PDDL, and the underlying algorithm is adaptable to other notions.
Motion Domain
The motion domain is represented by the motion scene graph
in Figure 4. Motion planning algorithms are typically defined
in terms of abstract configuration spaces [9], while robot
manipulators are modeled as kinematic trees or scene graphs
of joints and links in packages such as OpenRAVE [22], Orocos KDL [23], and MoveIt! [24]. Existing implementations,
however, focus on only a subset of the TMP pipeline shown in
Figure 2. Consequently, TMKit uses a new, streamlined scene
graph representation that enables direct TM translation, efficient updates, and real-time kinematics.
The scene graph is a tree representing relative Cartesian
poses, with data attached at each node for geometry (e.g.,
meshes), inertial parameters, joint limits, and such. Figure 5

0

rw2

=0

rs0

rs0

rs1

rs1

re0

re0

re1
rw0
rw1
re1
rw0
rw1
rw2

0

lw2

=0

ls0

ls0

ls1

ls1

le0

le0

(a)

le1

le1

lw0

lw0

lw1

lw1

lw2

(b)

r w0
r w1

r w2

r e1

le0

r e0

B
C

r s1

ls1

r s0

ls0

T
0

(c)

le1

lw0
lw1

holding(A)
∧
on(B,C)
∧
ontable(C,0,0)
clear(B)

lw2

∧

A

(d)

Figure 5. The scene graph abstraction for a simplified version of the Baxter robot and the corresponding task state: (a) a kinematic
equation to compute the right wrist pose; (b) a kinematic equation to compute the left wrist pose; (c) the local coordinate frames of
the corresponding scene graph overlaid on the Baxter; and (d) the task state corresponding to the scene graph for a pick-and-place
task domain, partially shown in Figure 3. This state abstracts the object relationships in the scene graph. Note that the task state is
computed automatically via the domain semantics and that additional or different task predicates may be used by modifying the
domain semantics. The transform a S b is the relative Cartesian pose between parent a and child b. The coordinate frame labels for
Baxter consist of the left (l ) or right (r) arm; the shoulder (s), elbow (e), or wrist (w); and the zeroth (0), first (1), and second (2)
joint. The other frame labels represent the global root (zero), table (T ), or blocks (A, B, and C).

66

*

IEEE ROBOTICS & AUTOMATION MAGAZINE

*

september 2018
IEEE Robotics & Automation Magazine - September 2018

Table of Contents for the Digital Edition of IEEE Robotics & Automation Magazine - September 2018
