Fahad Islam.

# Provably Constant-time Planning and Replanning for Real-time Grasping
Objects off a Conveyor Belt

Fahad Islam11affiliationmark:  and Oren Salzman22affiliationmark:  and Aditya
Agarwal11affiliationmark:  and Maxim Likhachev11affiliationmark:
11affiliationmark:  The Robotics Institute, Carnegie Mellon University
22affiliationmark:  Technion-Israel Institute of Technology
<EMAIL_ADDRESS>
###### Abstract

In warehouse and manufacturing environments, manipulation platforms are
frequently deployed at conveyor belts to perform pick and place tasks. Because
objects on the conveyor belts are moving, robots have limited time to pick
them up. This brings the requirement for fast and reliable motion planners
that could provide provable real-time planning guarantees, which the existing
algorithms do not provide. Besides the planning efficiency, the success of
manipulation tasks relies heavily on the accuracy of the perception system
which is often noisy, especially if the target objects are perceived from a
distance. For fast moving conveyor belts, the robot cannot wait for a perfect
estimate before it starts executing its motion. In order to be able to reach
the object in time, it must start moving early on (relying on the initial
noisy estimates) and adjust its motion on-the-fly in response to the pose
updates from perception. We propose a planning framework that meets these
requirements by providing provable constant-time planning and replanning
guarantees. To this end, we first introduce and formalize a new class of
algorithms called _Constant-Time Motion Planning algorithms (CTMP)_ that
guarantee to plan in constant time and within a user-defined time bound. We
then present our planning framework for grasping objects off a conveyor belt
as an instance of the CTMP class of algorithms. We present it, give its
analytical properties and show experimental analysis both in simulation and on
a real robot.

###### keywords:

Motion Planning, Automation, Manipulation

##  1 Introduction

Conveyor belts are widely used in automated distribution, warehousing, as well
as for manufacturing and production facilities. In the modern times robotic
manipulators are being deployed extensively at the conveyor belts for
automation and faster operations Zhang et al. (2018). In order to maintain a
high-distribution throughput, manipulators must pick up moving objects without
having to stop the conveyor for every grasp. In this work, we consider the
problem of motion planning for grasping moving objects off a conveyor. An
object in motion imposes a requirement that it should be picked up in a short
window of time. The motion planner for the arm, therefore, must compute a path
within a bounded time frame to be able to successfully perform this task.

Fig. 1:  A scene demonstrating the PR2 robot picking up a moving object (sugar
box) off a conveyor belt.

Manipulation relies on high quality detection and localization of moving
objects. When the object first enters the robot’s field of view, the initial
perception estimates of the object’s pose are often inaccurate. Consider the
example of an object (sugar box) moving along the conveyor towards the robot
in Fig. 1, shown through an image sequence as captured by the robot’s Kinect
camera in Fig. 2. The plot in Fig. 2 shows the variation of the error between
the filtered input point cloud and a point cloud computed from the predicted
pose from our Iterative Closest Point (ICP)-based perception strategy Islam et
al. (2019) as the object gets closer to the camera. We observe that the error
decreases as the object moves closer, indicating that the point clouds overlap
more closely due to more accurate pose estimates closer to the camera.

However, if the robot waits too long to get an accurate estimate of the object
pose, the delay in starting plan execution could cause the robot to miss the
object. The likelihood of this occurring as the speed of the conveyor
increases. Therefore, the robot should start executing a plan computed for the
initial pose and as it gets better estimates, it should repeatedly replan for
the new goals. However, for every replanning query, the time window for the
pickup shrinks. This makes the planner’s job difficult to support real-time
planning.

Furthermore, the planning problem is challenging because the motion planner
has to account for the dynamic object and thus plan with time as one of the
planning dimensions. It should generate a valid trajectory that avoids
collision with the environment around it and also with the target object to
ensure that it does not damage or topple it during the grasp. Avoiding
collisions with the object requires precise geometric collision checking
between the object geometry and the geometry of the manipulator. The robot
arms also have kinodynamic constraints such as torque and velocity limits that
the motion planner may have to account for while computing the plans,
especially when the robots must move at high speeds. The resulting complexity
of the planning problem makes it infeasible to plan online for this task.

Motivated by these challenges, we propose an algorithm that leverages offline
preprocessing to provide bounds on the planning time when the planner is
invoked online. Our key insight is that in our domain the manipulation task is
highly repetitive. Even for different object poses, the computed paths are
quite similar and can be efficiently reused to speed up online planning. Based
on this insight, we derive a method that precomputes a representative set of
paths with some auxiliary datastructures offline and uses them online in a way
that provides _constant-time planning guarantee_. We present it as an instance
of a new class of algorithms which we call Constant-time Motion Planning
(CTMP) algorithms. To the best of our knowledge, our approach is the first to
provide constant-time planning guarantee on generating motions for indefinite
horizons i.e. all the way to the goal.

We experimentally show that constant-time planning and replanning capability
is necessary for a successful conveyor pickup task. Specifically if we only
perform one-time planning, that is planning only once either for the very
first and potentially inaccurate pose estimate or for a delayed but accurate
pose estimate of the object, the robot frequently fails to pick the object.

(a)

(b)

Fig. 2:  (2(a)) Depiction of an object moving along a conveyor towards the
robot. (2(b)) Pose error as a function of the distance from the conveyor’s
start. Specifically we use ADD-S error Hinterstoisser et al. (2012).

###  1.1 Statement of Contributions

We make the following contributions in this work:

  1. 1.

We develop a provably constant-time planning and replanning framework for
grasping moving objects off a conveyor belt.

  2. 2.

We prove that the algorithm is complete and is guaranteed to run in constant
time and within a user specified time bound.

  3. 3.

We provide experimental analysis of our approach in simulation as well as on a
physical robotic system.

  4. 4.

We introduce and formalize a new class of algorithms called Constant-Time
Motion Planning (CTMP) algorithms and show that the proposed approach for
grasping objects off the conveyor is an instance of CTMP class of algorithms.

  5. 5.

We develop a kinodynamic motion planner to account for the dynamic constraints
of the robot including joint torque and velocity limits.

This article is in continuation of our previous work presented in Islam et al.
(2020) and the contributions 4 and 5 specifically, are the extensions. In
addition to these extensions, we provide space complexity analysis of our
approach and report detailed preprocessing statistics of our experiments to
highlight the improvement over the brute force method. We also remove one of
the assumptions of Islam et al. (2020) which says that the environment remains
static up to a certain time in execution.

##  2 Related work

###  2.1 Motion planning for conveyor pickup task

Existing work on picking moving objects has focused on different aspects of
the problem ranging from closed-loop controls, servoing to object perception
and pose estimation, motion planning and others Zhang et al. (2018); Allen et
al. (1993); Han et al. (2019); Stogl et al. (2017). Here, we focus on motion-
planning related work. Time-configuration space representation was introduced
to avoid moving obstacles Fraichard (1993); Cefalo et al. (2013); Yang et al.
(2018). Specifically in Yang et al. (2018), a bidirectional sampling-based
method with a time-configuration space representation was used to plan motions
in dynamic environments to pickup moving objects. While their method showed
real-time performance in complex tasks, it used fully-specified goals; namely
knowing the time at which the object should be picked, which weakens the
completeness guarantee. Furthermore their method is probablistically complete
and therefore, does not offer constant-time behavior. Graph-search based
approaches have also been used for the motion-planning problem Menon et al.
(2014); Cowley et al. (2013). The former uses a kinodynamic motion planner to
smoothly pick up moving objects i.e., without an impactful contact. A
heuristic search-based motion planner that plans with dynamics and could
generate high-quality trajectories with respect to the time of execution was
used. While this planner provides strong guarantees on the solution quality,
it is not real-time and thus cannot be used online. The latter work
demonstrated online real-time planning capability. The approach plans to a
pregrasp pose with pure kinematic planning and relies on Cartesian-space
controllers to perform the pick up. The usage of the Cartesian controller
limits the types of objects that the robot can grasp. It also provided no
guarantee to be able to generate a plan in time to execute.

###  2.2 Preprocessing-based planning

Preprocessing-based motion planners often prove beneficial for real-time
planning. They analyse the configuration space offline to generate some
auxiliary information that can be used online to speed up planning. Probably
the best-known example is the Probablistic Roadmap Method (PRM) Kavraki et al.
(1996) which precomputes a roadmap that can answer any query by connecting the
start and goal configurations to the roadmap and then searching the roadmap.
PRMs are fast to query yet they do not provide constant-time guarantees. To
account for dynamic environments, several PRM method-based extensions have
been proposed Yoshida and Kanehiro (2011); Belghith et al. (2006). However,
these methods often require computationally expensive repairing operations
which cause additional overheads.

A provably constant-time planner was recently proposed in Islam et al. (2019).
Given a start state and a goal region, it precomputes a compressed set of
paths that can be utilised online to plan to any goal within the goal region
in bounded time. As we will see, our approach while algorithmically different,
draws some inspiration from this work. Both of the above two methods (Kavraki
et al. (1996); Islam et al. (2019)) mainly target pure kinematic planning and
thus they cannot be used for the conveyor-planning problem which is dynamic in
nature.

Another family of preprocessing-based planners utilises previous experiences
to speed up the search Berenson et al. (2012); Coleman et al. (2015); Phillips
et al. (2012). Experience graphs Phillips et al. (2012), provide speed up in
planning times for repetitive tasks by trying to reuse previous experiences.
These methods are also augmented with sparsification techniques (see e.g.,
Dobson and Bekris (2014); Salzman et al. (2014)) to reduce the memory
footprint of the algorithm. Unfortunately, none of the mentioned algorithms
provide fixed planning-time guarantees that we strive for in our application.

###  2.3 Online replanning and real time planning

The conveyor-planning problem can be modelled as a Moving Target Search
problem (MTS) which is a widely-studied topic in the graph search-based
planning literature Ishida and Korf (1991, 1995); Koenig et al. (2007); Sun et
al. (2010). These approaches interleave planning and execution incrementally
and update the heuristic values of the state space to improve the distance
estimates to the moving target. Unfortunately, in high-dimensional planning
problems, this process is computationally expensive which is why these
approaches are typically used for two-dimensional grid problem such as those
encountered in video games. More generally, real-time planning is widely
considered in the search community (see, e.g., Koenig and Likhachev (2006);
Koenig and Sun (2009); Korf (1990)). However, as mentioned, these works are
typically applicable to low-dimensional search spaces.

##  3 Constant-Time Motion Planning (CTMP) for Indefinite Horizons

Before we formally define our motion-planning problem, we introduce a new
_class_ of algorithms that we call _Constant-Time Motion Planning algorithms
(CTMP)_ that are specially designed for repetitive robotic tasks. We will see
that the algorithm we propose for real-time grasping objects off a conveyor
belt is an instance of a CTMP algorithm. However, we believe that their
definition is of independent interest regardless of the specific problem
instance that is considered. While limited-horizon planners like real-time
heuristic search-based planners also plan in constant-time, this paper
specifically addresses the indefinite horizon planning problem, that is to
plan all the way to the goal Koenig and Likhachev (2006); Koenig and Sun
(2009); Korf (1990).

###  3.1 CTMP Problem Definition

We consider the problem setting where the robot has a fixed start state
$s_{\textrm{start}}$ and a set of goals $G$, the representation and
specification of which are domain dependent. The algorithm is queried for a
pair ($s_{\textrm{start}},g$) where $g\in G$ and attempts to compute a
feasible path from $s_{\textrm{start}}$ to $g$ within a (small) constant time.

######  Definition 1 (CTMP Algorithm).

Let ALG be a motion-planning algorithm, $T_{\textrm{bound}}$ a user-controlled
time bound and $T_{\textrm{const}}<T_{\textrm{bound}}$ a small time constant
whose value is independent of the size and complexity of the motion-planning
problem. ALG is said to be a _CTMP algorithm_ if it is guaranteed to answer
any motion planning query within $T_{\textrm{bound}}$.

The input time $T_{\textrm{bound}}$ is a tunable parameter that can be used to
trade off query time with the memory footprint and preprocessing efforts of
the algorithm. It is important to note that an algorithm that returns
NO$\\_$PATH for any query is CTMP. Thus, unless endowed with additional
properties CTMP is a weak algorithmic property. We now define one such
property, namely CTMP-Completeness, that makes a CTMP algorithm interesting
and useful.

###  3.2 CTMP-Completeness

We assume a CTMP algorithm has access to a regular motion-planning algorithm
$\mathcal{P}$ (that is not necessarily CTMP). We introduce a new notion of
completeness for CTMP algorithms. Prior to that, we need to define some
preliminaries.

######  Definition 2 (Reachability).

A goal $g\in G$ is said to be reachable from a state $s_{\textrm{start}}$ if
$~{}\mathcal{P}$ can find a path to it within a (sufficiently large) time
bound $T_{\textrm{$\mathcal{P}$}}$.

######  Definition 3 (Goal Coverage).

A reachable goal $g\in G$ is said to be covered by the CTMP algorithm for a
state $s_{\textrm{start}}$ if (in the query phase) it can plan from
$s_{\textrm{start}}$ to $g$ while satisfying Def. 1.

We are now equipped to define CTMP-Completeness.

######  Definition 4 (CTMP-Completeness).

An algorithm is said to be CTMP-complete if it covers all the reachable goals
$g\in G$ for $s_{\textrm{start}}$.

A CTMP-algorithm provides a planning time bound reduction of
$T_{\mathcal{P}}:T_{\textrm{bound}}$, where $T_{\textrm{bound}}\ll
T_{\textrm{$\mathcal{P}$}}$, while still guaranteeing a success rate no worse
than what $\mathcal{P}$ would provide with a time bound of $T_{\mathcal{P}}$.
A CTMP-complete algorithm distinguishes from a CTMP algorithm for it must
_cover_ all _reachable_ goals in $G$ for $s_{\textrm{start}}$ in the
preprocessing phase, so that in the query phase, it can find a plan to any
reachable goal within $T_{\textrm{bound}}$ time. A CTMP algorithm which is not
CTMP-complete however, may return failure even if the queried goal is
reachable.

Note that the notion of CTMP-completeness is decoupled from the completeness
guarantee (by conventional definition) of the underlying motion planner
$\mathcal{P}$, hence we can view a CTMP algorithm as a meta planner. The CTMP
algorithm only provides guarantees for the subset of $G$ which is reachable
and hence its completeness properties differ from the completeness properties
of $\mathcal{P}$. However, the size of the reachable set of goals is largely
dependent on the performance of $\mathcal{P}$.

##  4 CTMP for Grasping Objects off A Conveyor Belt—Problem Setup

Our system is comprised of a robot manipulator, a conveyor belt moving at some
known velocity, a set of known objects $\mathcal{O}$ that need to be grasped
and a perception system that is able to estimate the type of object and its
location on the conveyor belt. Here, we assume that the geometric models of
the target objects are known apriori and we are given a set of feasible grasps
for each object $o\in\mathcal{O}$.

Given a pose $g$ of an object $o\in\mathcal{O}$, our task is to plan the
motion of the robot such that it grasps $o$ from the conveyor belt at some
future time. Unfortunately, the perception system may give inaccurate object
poses. Thus, the pose $g$ will be updated by the perception system as the
robot is executing its motion. To allow for the robot to move towards the
updated pose in real time, we introduce the additional requirement that
planning should be done within a user-specified time bound
$T_{\textrm{bound}}$. For ease of exposition, when we say that we plan to a
pose $g$ of $o$ that is given by the perception system, we mean that we plan
the motion of the robot such that it will be able to pick $o$ from the
conveyor belt at some future time. This is explained in detail in Sec. 6.2.1
and in Fig. 9.

We denote by $G^{\textrm{full}}$ the discrete set of initial object poses on
the conveyor belt that the perception system can perceive. Finally, we assume
that the robot has an initial state $s_{\textrm{home}}$ corresponding to the
time $t=0$ from which it starts planning to grasp any object.

Roughly speaking, the objective is to enable planning and replanning to any
goal pose $g\in G^{\textrm{full}}$ in bounded time $T_{\textrm{bound}}$
regardless of the robot’s current state. Specifically we aim to develop a
CTMP-complete algorithm, so that for any state $s$ that the system can be in
and every reachable goal pose $g\in G^{\textrm{full}}$ from $s$ updated by the
perception system, $g$ is covered by $s$.

We assume that the pose estimation error of the perception system is bounded
by a distance $\varepsilon$. We also specify a replan cutoff time
$t=t_{\textrm{rc}}$ after which the planner does not replan and continues to
execute the last planned path until the goal is reached. The time
$t_{\textrm{rc}}$ could be chosen based on when the perception system is
expected to send an accurate pose estimate, and therefore no replanning is
needed from there on.

##  5 Algorithmic framework

Our approach for constant-time planning relies on a _preprocessing_ stage that
allows to efficiently compute paths in a _query_ stage to any goal. Before we
describe our approach, we start by describing a naïve method that solves the
aforementioned problem but requires a prohibitive amount of memory. This can
be seen as a warmup before describing our algorithm which exhibits the same
traits but does so in a memory-efficient manner.

###  5.1 Straw man approach

(a)

(b)

Fig. 3:  The figures show paths discretized from timesteps $t_{0}$ to
$t_{\textrm{rc}}$ with steps of size $\delta_{t}$. (3(a)) At $t_{0}$, the
algorithm computes $n_{\rm goal}$ paths, from $s_{\textrm{home}}$ to every
$g\in G^{\textrm{full}}$. (3(b)) At $t_{1}=\delta_{t}$, the algorithm computes
$n_{\rm goal}^{2}$ paths, from all $n_{\rm goal}$ replanable states at $t_{1}$
to every $g\in G^{\textrm{full}}$ (here we only show paths from three states).
Thus, the number of paths increases exponentially at every timestep.

We first compute from $s_{\textrm{home}}$ a path $\pi_{g}$ to every reachable
$g\in G^{\textrm{full}}$. These paths can be stored in a lookup (hash) table
which can be queried in constant time $T_{\textrm{const}}$ (assuming perfect
hashing Czech et al. (1997)). For the straw man approach, since there is no
utility of having a $T_{\textrm{bound}}$ larger than $T_{\textrm{const}}$ as
it only performs look up operations at query time, we have
$T_{\textrm{bound}}=T_{\textrm{const}}$. By storing paths to all the goals,
every goal is covered by $s_{\textrm{home}}$ and this allows us to start
executing a path once the perception system gives its initial pose estimate.
However, we need to account for pose update while executing $\pi_{g}$. This
only needs to be done up until time $t_{\textrm{rc}}$, since no future
improved estimates are expected from the perception system. Thus, we
discretize each path uniformly with resolution $\delta_{t}$. We call all
states that are less than $t_{\textrm{rc}}$ time from $s_{\textrm{home}}$
_replanable states_.

Next, for every replanable state along each path $\pi_{g}$, we compute a new
path to all goals. This will ensure that all goals are covered by all
replanable states. Namely, it will allow to immediately start executing a new
path once the goal location is updated by the perception system.
Unfortunately, the perception system may update the goal location more than
once. Thus, this process needs to be performed recursively for the new paths
as well.

The outcome of the preprocessing stage is a set of precomputed collision-free
paths starting at states that are at most $t_{\textrm{rc}}$ from
$s_{\textrm{home}}$ and end at goal states. The paths are stored in a lookup
table $\mathcal{M}:S\times
G^{\textrm{full}}\rightarrow\\{\pi_{1},\pi_{2},\ldots\\}$ that can be queried
in $T_{\textrm{bound}}=T_{\textrm{const}}$ time to find a path from any given
$s\in S$ to $g\in G^{\textrm{full}}$.

In the query stage we obtain an estimation $g_{1}$ of the goal pose by the
perception system. The algorithm then retrieves the path
$\pi_{1}(s_{\textrm{home}},g_{1})$ (from $s_{\textrm{home}}$ to $g_{1}$) from
$\mathcal{M}$ and the robot starts executing
$\pi_{1}(s_{\textrm{home}},g_{1})$. For every new estimation $g_{i}$ of the
goal pose obtained from the perception system while the system is executing
path $\pi_{i-1}(s_{i-1},g_{i-1})$, the algorithm retrieves from $\mathcal{M}$
the path $\pi_{i}(s_{i},g_{i})$ from the nearest state $s_{i}$ along
$\pi_{i-1}(s_{i-1},g_{i-1})$ that is least $T_{\textrm{bound}}$ away from
$s_{i-1}$. The robot then starts executing $\pi_{i}(s_{i},g_{i})$ once it
reaches $s_{i}$. Hence, the straw man algorithm is trivially a CTMP-complete
algorithm.

Clearly, every possible goal is covered for every possible configuration that
the robot might be in during execution by this brute force approach, however
it requires a massive amount of memory and prepreprocessing time. Let $n_{\rm
goal}=|G^{\textrm{full}}|$ be the number of goals and $\ell$ be the number of
states between $s_{\textrm{home}}$ and the state that is $t_{\textrm{rc}}$
time away. This approach requires precomputing and storing $O(n_{\rm
goal}^{\ell})$ paths which is clearly infeasible (see Fig. 3). In the next
sections, we show how we can dramatically reduce the memory footprint of the
approach without compromising on the system’s capabilities.

###  5.2 Algorithmic approach

While the straw man algorithm presented allows for planning to any goal pose
$g\in G^{\textrm{full}}$ within $T_{\textrm{const}}$ time, its memory
footprint is prohibitively large. We suggest to reduce the memory footprint by
building on the observation that many paths to close-by goals traverse very
similar parts of the configurations space. Instead of generating a plan
strictly within $T_{\textrm{const}}$ time, our approach trades off
preprocessing efforts with the bound on the planning time and guarantees to
generate a solution within the user-specified time bound
$T_{\textrm{bound}}(>T_{\textrm{const}})$.

The key idea of our approach is that instead of computing (and storing) paths
to all reachable goals in $G^{\textrm{full}}$, we compute a relatively small
subset of so-called “root paths” that can be reused in such a way that we can
still cover $G^{\textrm{full}}$ fully. Namely, at query time, we can reuse
these paths to plan to any $g\in G^{\textrm{full}}$ within
$T_{\textrm{bound}}$. The idea is illustrated in Fig. 5.

First, we compute a set of root paths $\\{\Pi_{1},\ldots,\Pi_{k}\\}$ from
$s_{\textrm{home}}$ to cover $G^{\textrm{full}}$ by $s_{\textrm{home}}$ (here
we will have that $k\ll n_{\rm goal})$ Next, for all replanabale states along
these root paths, the algorithm recursively computes additional root paths so
that their reachable goals are also covered. During this process, additional
root paths are computed only when the already existing set of root paths does
not provide enough guidance to the search to cover $G^{\textrm{full}}$ i.e.,
to be able to compute a path to any $g\in G^{\textrm{full}}$ within
$T_{\textrm{bound}}$. The remainder of this section formalizes these ideas.

###  5.3 Algorithmic building blocks

We start by introducing the algorithmic building blocks that we use.
Specifically, we start by describing the motion planner $\mathcal{P}$ that is
used to compute the root paths and then continue to describe how they can be
used as _experiences_ to efficiently compute paths to other goals. We
implemented two types of motion planners. One operates in a time-configuration
space and the other is a kinodynamic motion planner that plans with additional
state dimensions of joint velocities in order to satisfy the kinodynamic
constraints of the robot. While the former planning framework is simpler and
works successfully on a physical robot in our experiments, the latter might be
more desirable for operating a robot closer to its maximum performance limits.

We use a heuristic search-based planning approach with motion primitives (see,
e.g, Cohen et al. (2010, 2011); Likhachev and Ferguson (2009)) as it allows
for deterministic planning time which is key in our domain. Moreover, such
planners can easily handle under-defined goals as we have in our setting—we
define a goal as a grasp pose for the goal object. The grasp pose for a target
object $o$ is manually selected.

####  5.3.1 Time-Configuration Motion Planner

State space and graph construction. We define a state $s$ as a pair $(q,t)$
where $q=(\theta_{1},...,\theta_{n})$ is a configuration represented by the
joint angles for an $n$-DOF robot arm (in our setting $n=$7) and $t$ is the
time associated with $q$. Given a state $s$ we define two types of motion
primitives which are short kinodynamically-feasible motions that the robot can
execute.

The first type of motion primitives are predefined primitives. These are small
individual joint movements in either direction as well as _wait_ actions.
These primitives have non-uniform resolution. For each joint, we define two
motion primitives of distance $\pm$4∘. In addition, for the first four of the
seven robot joints we define two additional primitives each, of distance
$\pm$7∘. We only allow moving one joint at a time which makes it a total of 23
predefined primitives. For each motion primitive, we compute its duration by
using a nominal constant velocity profile for the joint that is moved.

The second type of primitives are dynamic primitives. They are generated by
the search only at the states that represent the arm configurations where the
end effector is close to the object. These primitives correspond to the actual
grasping of the object while it is moving. The dynamic primitives are
generated by using a Jacobian pseudo inverse-based control law similar to what
Menon et al. (2014) used. The desired velocity of the end effector is computed
for which the end-effector minimizes the distance to the grasp pose. Once the
gripper encloses the object, it moves along with the object until the gripper
is closed. Some examples of the dynamic primitives are shown in Fig. 4. During
the search, all motion primitives are checked for validity with respect to
collision and joint limits.

Fig. 4:  The figures show the dynamic motion primitive for two different
initial poses of the sugar box. (Only the gripper poses are visualized.) The
sugar box shown at the rear of the conveyor belt in the examples depicts its
initial pose.

Heuristic Function The search is guided by an efficient and fast-to-compute
heuristic function which in our case has two components. The first component
drives the search to intercept the object at the right time and the second
component guides the search to correct the orientation of the end effector as
it approaches the object. Mathematically, our heuristic function is given by

$h(s,g)=\max(\Delta t(s,g),\lambda\cdot\textsc{AngleDiff}(s,g)).$ (1)

Here, $\Delta t(s,g)$ is the expected time to intercept the object which can
be analytically computed from the positions of the target object and the end-
effector, the velocity of the target object and the speed of the end-effector.
We pick a nominal speed for the end-effector to solve the problem.
AngleDiff($s,g$) gives the magnitude of angular difference between the end-
effector’s current pose and the grasp pose. The coefficient $\lambda$ is used
as a weighting factor.

####  5.3.2 Kinodynamic Motion Planner

The time-configuration motion planner does not guarantee that the plan when
transformed into a trajectory will satisfy robot’s torque limits. To this end,
we extend the previous motion planner framework to be able to plan within the
kinodynamic constraints of the robot, while still being able to handle the
conveyor speed of 0.2$m/s$. In this section we describe the modified state
space and the heuristic function for the kinodynamic motion planner.

State space and graph construction. We modify the state space to include joint
velocities. A state $s$ is a tuple $(q,\dot{q},t)$ where
$\dot{q}=(\dot{\theta_{1}},...,\dot{\theta_{n}})$ represent velocities for
each joint. The dimensionality of the state space hence becomes 15 in our
experiments. Similar to $q$, the velocities $\dot{q}$ are also discretized.

We modify the set of predefined motion primitives. The primitives are
specified as magnitudes of accelerations for each joint. Specifically, to
generate a successor of a state, for the desired accelerations (specified in
the primitive), first we use inverse dynamics to compute the required torques.
Second, we integrate using the forward dynamics for a fixed time step $dt$ to
get the resulting state using Runge-Kutta integration. We use the Orocos
Kinematics and Dynamics Library (KDL) to solve for forward/inverse dynamics
and the integration111KDL:https://orocos.org/kdl.html.. Let a motion primitive
be specified by a vector of accelerations $\ddot{q}$ of size $n$. The two
steps to compute the primitive are

$\displaystyle\tau=\textsc{ComputeInverseDynamics}(s,\ddot{q})$ $\displaystyle
s^{\prime}=\textsc{Integrate}(s,\tau,dt)$

Since the robot dynamics is a function of the state $s$ of the robot, these
primitives need to be computed on-the-fly during search. In addition to
performing kinematic and collision checks to verify motion validity, we
discard successors for which $\tau$ exceeds the robot’s torque or velocity
limits.

In our experiments, we use 6 motion primitives for each of the 7 joints. These
are accelerations $\pm$(4, 8, 12) $\deg/s^{2}$. We only accelerate or
decelerate one joint at a time under these acceleration profiles, thus
resulting in 42 primitives. In addition to these primitives, we use a
“coasting” primitive that assigns zero acceleration to all the joints.

Heuristic Function. The heuristic function we used for the time-configuration
space planner gives no guidance for the velocity profiling which is crucial in
the case of kinodynamic planning. We, therefore, modify the heuristic in Eq. 1
by introducing an additional term $\Delta\dot{x}$ that guides the search with
respect to the velocity profile.

$\Delta\dot{x}(s)=\|\mathbf{\dot{x^{o}}-\dot{x^{e}}}(s)\|.$ (2)

Namely, $\Delta\dot{x}$ is the magnitude of the difference of the target
object’s velocity $\mathbf{\dot{x^{o}}}$ and the robot end-effector’s velocity
$\mathbf{\dot{x^{e}}}$ at state $s$ in 3D. $\mathbf{\dot{x^{e}}}$ is computed
using forward velocity kinematics.

The new heuristic function is given by

$h(s,g)=\max(\Delta t(s,g),\\\
\lambda_{1}\cdot\textsc{AngleDiff}(s,g)+\lambda_{2}\cdot\Delta\dot{x}(s)).$
(3)

where $\lambda_{1}$ and $\lambda_{2}$ are the weighting factor. Intuitively,
this additional term guides the search to match the end-effector velocity with
$o$’s velocity as the end-effector approaches the object. This increases the
likelihood of generating a dynamic primitive that satisfies the kinodynamic
constraints of the robot.

####  5.3.3 Graph Search

The states and the transitions implicitly define a graph $\mathcal{G}=(S,E)$
where $S$ is the set of all states and $E$ is the set of all transitions
defined by the motion primitives. We use Weighted A* (wA*) Pohl (1970) to find
a path in $\mathcal{G}$ from a given state $s$ to a goal $g$. wA* is a
suboptimal heursitic search algorithm that allows a tradeoff between
optimality and greediness by inflating the heuristic function $h$ by a given
weight $w$. The cost of an edge is the time of its traversal.

####  5.3.4 Planning with Experience Reuse

We now show how previously-computed paths which we named as root paths can be
reused as experiences in our framework. Given a heuristic function $h$, we
define for a root path $\Pi$ and a goal $g\in G^{\textrm{full}}$ the
_shortcut_ state $s_{\textrm{sc}}(\Pi,g)$ as the state on the path $\Pi$ that
is closest to $g$ with respect $h$. Namely,

$s_{\textrm{sc}}(\Pi,g):=\operatorname*{arg\,min}\limits_{s_{i}\in\Pi}h(s_{i},g).$
(4)

Now, when searching for a path to a goal $g\in G^{\textrm{full}}$ using root
path $\Pi$ as an experience, we add $s_{\textrm{sc}}(\Pi,g)$ as a successor
for any state along $\Pi$ (subject to the constraint that the path along $\Pi$
to $s_{\textrm{sc}}$ is collision free). In this manner the search reuses
previous experience to quickly reach a state close to $g$.

###  5.4 Algorithmic details

We are finally ready to describe our algorithm describing first the
preprocessing phase and then the query phase.

####  5.4.1 Preprocessing

(a)

(b)

(c)

Fig. 5: First step of the preprocessing stage. (5(a)) A goal $g_{1}$ is
sampled and the root path $\Pi_{1}$ is computed between $s_{\textrm{home}}$
and $g_{1}$. (5(b)) The set $G_{1}\subset G^{\textrm{full}}$ of all states
that can use $\Pi_{1}$ as an experience is computed and associated with
$\Pi_{1}$. (5(c)) The goal region covered by four root paths from
$s_{\textrm{home}}$ after the first step of the preprocessing stage
terminates.

Our preprocessing stage starts by sampling a goal $g_{1}\in G^{\textrm{full}}$
and computing a root path $\Pi_{1}$ from $s_{\textrm{home}}$ to $g_{1}$. We
then associate with $\Pi_{1}$ the set of goals $G_{1}\subset
G^{\textrm{full}}$ such that $\Pi_{1}$ can be used as an experience in
reaching any $g_{j}\in G_{1}$ within $T_{\textrm{bound}}$.222To account for
the lower bound $T_{\textrm{const}}$ time that is required for the query phase
which is consumed in operations, such as hash table lookups etc., the time
$T_{\textrm{const}}$ is subtracted from $T_{\textrm{bound}}$ for the
experience-based planner, to ensure that the overall query time is bounded by
$T_{\textrm{bound}}$. We use a conservative estimate of $T_{\textrm{const}}$
in our experiments. Additionally, the experience-based planner is constrained
to reuse the root path upto $t_{\textrm{rc}}$. Thus, all goals in $G_{1}$ are
covered by $s_{\textrm{home}}$. We then repeat this process but instead of
sampling a goal from $G^{\textrm{full}}$, we sample from
$G^{\textrm{full}}\setminus G_{1}$, thereby removing covered goals from
$G^{\textrm{full}}$ in every iteration. At the end of this step, we obtain a
set of root paths. Each root path $\Pi_{i}$ is associated with a goal set
$G_{i}\subseteq G^{\textrm{full}}$ such that (i) $\Pi_{i}$ can be used as an
experience for planning to any $g_{j}\in G_{i}$ in $T_{\textrm{bound}}$ and
(ii)
$\bigcup_{i}G_{i}=\textsc{Reachable}(s_{\textrm{home}},G^{\textrm{full}})$
(i.e., all reachable goals for $s_{\textrm{home}}$ in $G^{\textrm{full}}$).
Alg. 1 details this step (when called with arguments
($s_{\textrm{home}},G^{\textrm{full}}$)). It also returns a set of unreachable
goals that are left uncovered. The process is illustrated in Fig. 5.

Algorithm 1 Plan Root Paths

1:procedure PlanRootPaths($s_{\textrm{start}},G^{\textrm{uncov}}$)

2: $\Psi_{s_{\textrm{start}}}\leftarrow\emptyset$ $\triangleright$ a list of
pairs ($\Pi_{i},G_{i})$

3: $G^{\textrm{uncov}}_{s_{\textrm{start}}}\leftarrow\emptyset$; $i=0$

4: while $G^{\textrm{uncov}}\neq\emptyset$ do $\triangleright$ until all
reachable goals are covered

5: $g_{i}\leftarrow$SampleGoal($G^{\textrm{uncov}}$)

6: $G^{\textrm{uncov}}\leftarrow G^{\textrm{uncov}}\setminus\\{g_{i}\\}$

7: if $\Pi_{i}\leftarrow$ PlanRootPath($s_{\textrm{start}},g_{i}$) then
$\triangleright$ planner succeeded

8: $G_{i}\leftarrow\\{g_{i}\\}$ $\triangleright$ goals reachable

9: for each $g_{j}\in G^{\textrm{uncov}}$ do

10: if
$\pi_{j}\leftarrow$PlanPathWithExperience($s_{\textrm{start}},g_{j},\Pi_{i}$)
then

11: $G_{i}\leftarrow G_{i}\cup\\{g_{j}\\}$

12: $G^{\textrm{uncov}}\leftarrow G^{\textrm{uncov}}\setminus\\{g_{j}\\}$

13:
$\Psi_{s_{\textrm{start}}}\leftarrow\Psi_{s_{\textrm{start}}}\cup\\{(\Pi_{i},G_{i})\\}$;
$i\leftarrow i+1$

14: else

15: $G^{\textrm{uncov}}_{s_{\textrm{start}}}\leftarrow
G^{\textrm{uncov}}_{s_{\textrm{start}}}\cup\\{g_{i}\\}$ $\triangleright$ goals
unreachable

16: return $\Psi_{s_{\textrm{start}}},G^{\textrm{uncov}}_{s_{\textrm{start}}}$

Algorithm 2 Preprocess

1:procedure
TryLatching($s,\Psi_{s_{\textrm{home}}}G^{\textrm{uncov}},G^{\textrm{cov}}$)

2: for each $(\Pi_{i},G_{i})\in\Psi_{s_{\textrm{home}}}$ do

3: if CanLatch($s,\Pi_{i}$) then

4: $G^{\textrm{uncov}}\leftarrow G^{\textrm{uncov}}\setminus G_{i}$

5: $G^{\textrm{cov}}\leftarrow G^{\textrm{cov}}\cup G_{i}$

6: return $G^{\textrm{uncov}},G^{\textrm{cov}}$

7:procedure
Preprocess($s_{\textrm{start}},G^{\textrm{uncov}},G^{\textrm{cov}}$)

8:
$\Psi_{s_{\textrm{start}}},G^{\textrm{uncov}}_{s_{\textrm{start}}}\leftarrow$
PlanRootPaths($s_{\textrm{start}},G^{\textrm{uncov}}$)

9: if $s_{\textrm{start}}=s_{\textrm{home}}$ then
$\Psi_{s_{\textrm{home}}}=\Psi_{s_{\textrm{start}}}$

10: $G^{\textrm{cov}}_{s_{\textrm{start}}}\leftarrow
G^{\textrm{cov}}\cup(G^{\textrm{uncov}}\setminus
G^{\textrm{uncov}}_{s_{\textrm{start}}})$

11: if $t(s_{\textrm{start}})\leq t_{\textrm{rc}}$ then

12: for each $(\Pi_{i},G_{i})\in\Psi_{s_{\textrm{start}}}$ do

13: $G_{i}^{\textrm{cov}}\leftarrow G_{i}$; $G_{i}^{\textrm{uncov}}\leftarrow
G^{\textrm{cov}}_{s_{\textrm{start}}}\setminus G_{i}$;

14: for each $s\in\Pi_{i}$ (from last to first) do $\triangleright$ states up
to $t_{\textrm{rc}}$

15: $G^{\textrm{uncov}}_{i},G^{\textrm{cov}}_{i}\leftarrow$
TryLatching($s,\Psi_{s_{\textrm{home}}},G^{\textrm{uncov}}_{i},G^{\textrm{cov}}_{i}$)

16: if $G_{i}^{\textrm{uncov}}=\emptyset$ then

17: break

18: $G_{i}^{\textrm{uncov}},G_{i}^{\textrm{cov}}\leftarrow$
Preprocess($s,G_{i}^{\textrm{uncov}},G_{i}^{\textrm{cov}}$)

19: if $G_{i}^{\textrm{uncov}}=\emptyset$ then

20: break

21: return
$G^{\textrm{uncov}}_{s_{\textrm{start}}},G^{\textrm{cov}}_{s_{\textrm{start}}}$

So far we explained the algorithm for one-time planning when the robot is at
$s_{\textrm{home}}$ ($t=0$); we now need to allow for efficient replanning for
any state $s$ between $t=0$ to $t_{\textrm{rc}}$. In order to do so, we
iterate through all the states on these root paths and add additional root
paths so that these states also cover their respective reachable goals. This
has to be done recursively since newly-added paths generate new states which
the robot may have to replan from. The complete process is detailed in Alg. 2.
The Preprocess procedure takes in a state $s_{\textrm{start}}$, the goal
region that it has to cover $G^{\textrm{uncov}}$ and region that it already
has covered $G^{\textrm{cov}}$. Initially Preprocess is called with arguments
($s_{\textrm{home}},G^{\textrm{full}},\emptyset$) and it runs recursively
until no state is left with uncovered reachable goals.

At a high level, the algorithm iterates through each root path $\Pi_{i}$ (loop
at line 12) and for each state $s\in\Pi_{i}$ (loop at line 14) the algorithm
calls itself recursively (line 18). The algorithm terminates when all states
cover their reachable goals. The pseudocode in blue constitute an additional
optimization step which we call “latching” and is explained later in Sec.
5.4.3.

In order to minimise the required computation, the algorithm leverages two key
observations:

  1. O1

If a goal is not reachable from a state $s\in\Pi$, it is not reachable from
all the states after it on $\Pi$.

  2. O2

If a goal is covered by a state $s\in\Pi$, it is also covered by all states
preceeding it on $\Pi$.

O1 is an assumption that we make about the planner $\mathcal{P}$. We use O1 to
initialize the uncovered set for any state; instead of attempting to cover the
entire $G^{\textrm{full}}$ for each replanable state $s$, the algorithm only
attempts to cover the goals that could be reachable from $s$, thereby saving
computation. O2 is used by iterating backwards on each root path (loop at line
14) and for each state on the root path only considering the goals that are
left uncovered by the states that appear on the path after it.

Specifically, O2 is used to have a single set of uncovered goals
$G^{\textrm{uncov}}_{i}$ for all states that appear on $\Pi_{i}$ instead of
having individual sets for each state and the goals that each $s\in\Pi_{i}$
covers in every iteration of loop 14 are removed from
$G^{\textrm{uncov}}_{i}$. O1 is used to initialize $G^{\textrm{uncov}}_{i}$
(in line 13). Namely, it is initialized not by the entire $G^{\textrm{full}}$
but by the set of goals covered by $s_{\textrm{start}}$. $G_{i}$ is excluded
since it is already covered via $\Pi_{i}$. The iteration completes either when
all goals in $G^{\textrm{uncov}}_{i}$ are covered (line 19) or the loop
backtracks to $s_{\textrm{start}}$. The process is illustrated in Fig. 6

Thus, as the outcome of the preprocessing stage, a map $\mathcal{M}:S\times
G^{\textrm{full}}\rightarrow\\{\Pi_{1},\Pi_{2},\ldots\\}$ is constructed that
can be looked up to find which root path can be used as an experience to plan
to a goal $g$ from a state $s$ within $T_{\textrm{bound}}$.

(a)

(b)

(c)

(d)

Fig. 6: Preprocess loop for $\Pi_{1}$ without latching. (6(a)) Initially the
state $s$ covers $G_{1}$ via $\Pi_{1}$. (6(b)) New root paths are computed
from $s$ to cover remaining uncovered region. (6(c)) This process is repeated
by backtracking along the root path. (6(d)) Outcome of a preprocessing step
for one path: $G^{\textrm{full}}$ is covered either by using $\Pi_{1}$ as an
experience or by using newly-computed root paths.

(a)

(b)

(c)

(d)

Fig. 7: Preprocess loop for $\Pi_{1}$ with latching. (7(a)) The algorithm
starts by trying to latch on to every other root path; for successful latches,
the corresponding goals are removed from uncovered region. (7(b)) New root
paths are computed from $s$ to cover remaining uncovered region. (7(c)) This
process is repeated by backtracking along the root path. (7(d)) Outcome of a
preprocessing step: $G^{\textrm{full}}$ is covered either by using $\Pi_{1}$
as an experience, latching on to $\Pi_{2},\Pi_{3}$ or $\Pi_{4}$ (at different
time steps) or by using newly-computed root paths.

####  5.4.2 Query

Alg. 3 describes the query phase of our algorithm. Again, the lines in blue
correspond to the blue pseudocode in Alg. 2 for the additional optimization
step which is explained in Sec. 5.4.3. Assume that the robot was at a state
$s_{\textrm{curr}}$ while executing a path $\pi_{\rm curr}$ when it receives a
pose update $g$ from the perception system. Alg. 3 will be called for the
first state $s_{\textrm{start}}$ that is $T_{\textrm{bound}}$ ahead of
$s_{\textrm{curr}}$ along $\pi_{\rm curr}$, allowing the algorithm to return a
plan before the robot reaches $s_{\textrm{start}}$.

Alg. 2 assures that either the first state on $\pi_{\rm curr}$ covers $g$ or
there exists one state on $\pi_{\rm curr}$ between $s_{\textrm{start}}$ and
the state at $t_{\textrm{rc}}$ that covers $g$. The algorithm first checks if
the first state on $\pi_{\rm curr}$ covers $g$ or not. If it covers $g$ then
the corresponding root path is used to find the new path $\pi_{\rm new}$.
Otherwise, it iterates over each $s\in\pi_{\rm curr}$ backwards (similar to
Alg. 2) between $s_{\textrm{start}}$ and the state at $t_{\textrm{rc}}$ and
finds the one that covers $g$ by quering $\mathcal{M}$. Once found, the
corresponding root path $\Pi_{\rm next}$ is used as an experience to plan the
path $\pi_{\rm next}$ from $s$ to $g$. Finally the paths $\pi_{\rm curr}$ and
$\pi_{\rm next}$ are merged together with $s$ being the transitioning state to
return the final path $\pi$.

Algorithm 3 Query

Inputs: $\mathcal{M},s_{\textrm{home}}$

1:procedure PlanPathByLatching($s,g,\pi_{\textrm{curr}}$)

2: if $\Pi_{\textrm{home}}\leftarrow\mathcal{M}(s_{\textrm{home}},g)$ exists
then $\triangleright$ lookup root path

3: if CanLatch($s,\Pi_{\textrm{home}}$) then

4:
$\pi_{\textrm{home}}\leftarrow$PlanPathWithExperience($s_{\textrm{home}},g,\Pi_{\textrm{home}}$)

5: $\pi\leftarrow$
MergePathsByLatching($\pi_{\textrm{curr}},\pi_{\textrm{home}},s$)

6: return $\pi$

7: return failure

8:procedure Query($g,\pi_{\textrm{curr}},s_{\textrm{start}}$)

9: $s\leftarrow\pi_{\textrm{curr}}[0]$ $\triangleright$ first state on
$\pi_{\textrm{curr}}$

10: if $\Pi_{\textrm{curr}}\leftarrow$ $\mathcal{M}(s,g)$ exists then
$\triangleright$ lookup root path

11:
$\pi_{\textrm{new}}\leftarrow$PlanPathWithExperience($s,g,\Pi_{\textrm{curr}}$)

12: return $\pi_{\textrm{new}}$

13: for each $s\in\pi_{\textrm{curr}}$ (from last to $s_{\textrm{start}}$) do
$\triangleright$ states up to $t_{\textrm{rc}}$

14: if $\Pi_{\textrm{next}}\leftarrow$ $\mathcal{M}(s,g)$ exists then
$\triangleright$ lookup root path

15:
$\pi_{\textrm{next}}\leftarrow$PlanPathWithExperience($s,g,\Pi_{\textrm{next}}$)

16: $\pi\leftarrow$ MergePaths($\pi_{\textrm{curr}},\pi_{\textrm{next}},s$)

17: return $\pi$

18: if $\pi\leftarrow$PlanPathByLatching($s,g,\pi_{\textrm{curr}}$) successful
then

19: return $\pi$

20: return failure $\triangleright$ goal is not reachable

####  5.4.3 Latching: Reusing Root Paths

We introduce an additional step called “Latching” to minimise the number of
root paths computed in Alg. 2. With latching, the algorithm tries to reuse
previously-computed root paths as much as possible using special motion
primitives that allow transitions from one root path to another.

For the time-configuration motion planner, the primitive is computed from a
state $s\in\Pi_{i}$ to $s^{\prime}\in\Pi_{j}$ such that
$t(s^{\prime})=t(s)+\delta_{t}$ by simple linear interpolation while ensuring
the feasibility of the motion. Specifically, given the nominal joint
velocities of the robot, if $s^{\prime}$ can be reached from $s$ in time
$\delta_{t}$, while respecting the kinematic and collision constraints, then
the transition is allowed.

Clearly, this approach does not ensure that the torque limits are respected.
Therefore, for the kinodynamic planner, we interpolate by fitting a cubic
polynomial from the state $s\in\Pi_{i}$ to $s^{\prime}\in\Pi_{j}$ that
satisfies the boundary conditions. We then ensure motion validity by checking
the joint velocity and torque limits of each state along the interpolated
trajectory.

In Alg. 2, before calling the Preprocess procedure for a state, the algorithm
removes the set of goals that can be covered via latching, thereby reducing
the number of goals that need to be covered by the Preprocess procedure.
Correspondingly, in Alg. 3, an additional procedure is called to check if the
path can be found via latching. These additions in the two pseudocodes are
shown in blue. An iteration of the complete algorithm with latching is
illustrated in Fig 7.

###  5.5 Theoretical Analysis

####  5.5.1 CTMP Properties

######  Lemma 1.

For any state $s_{\textrm{start}}$ of the robot during execution, provided
$t(s_{\textrm{start}})\leq t_{\textrm{rc}}$, the algorithm is CTMP.

###### Proof.

We can prove it by showing that the query stage (Alg. 3) returns within
$T_{\textrm{bound}}$ time and has a constant-time complexity. The number of
times the algorithm queries $\mathcal{M}$, which is an $O(1)$ operation
assuming perfect hashing, is bounded by $O(l)$ where
$l=t_{\textrm{rc}}/\delta_{t}$ is the maximum number of time steps from $t=0$
to $t_{\textrm{rc}}$. The number of times the algorithm attempts to latch on
to a root path (namely, a call to CanLatch which is a constant-time operation)
is also bounded by $l$. As $l$ is constant for of a fixed time cut off
$t_{\textrm{rc}}$, the execution time of the aforementioned operations
constitute $T_{\textrm{const}}$ (constant-time). Finally, Alg. 3 calls the
Plan method only once. Since the execution time of Plan is bounded by
$T_{\textrm{bound}}$\- $T_{\textrm{const}}$, the overall execution time of
Alg. 3 is $T_{\textrm{bound}}$. Hence it is a CTMP algorithm. ∎

######  Lemma 2.

For any state $s_{\textrm{start}}$ of the robot during execution, provided
$t(s_{\textrm{start}})\leq t_{\textrm{rc}}$, the algorithm is CTMP-complete.

###### Proof.

In order to prove it we need to show that for any $s_{\textrm{start}}$ that
the system can be at, (1) if a $g$ is reachable from $s_{\textrm{start}}$, it
is _covered_ by $s_{\textrm{start}}$ in Alg. 2 and (2) if $g$ is covered by
$s_{\textrm{start}}$, Alg. 3 is guaranteed to return a path within
$T_{\textrm{bound}}$ time.

Alg. 2 starts by computing a set of root paths from $s_{\textrm{home}}$ that
ensures that it covers all of its reachable goals. It then iterates over all
states on these paths and adds additional root paths ensuring that these
states also cover their reachable goals. It does it recursively until no state
$s_{\textrm{start}}$ before $t_{\textrm{rc}}$ is left with any uncovered $g$
which could be reachable.

Alg. 2 covers $g$ via at least one state between $s_{\textrm{start}}$ and the
state at $t_{rc}$ (inclusively) (loop at line 14). In query phase, Alg. 3
iterates through all states between $s_{\textrm{start}}$ and the state at
$t_{rc}$ (inclusively) to identify the one that covers $g$ (loop at line 13).
Since $g$ is covered by at least one of these states by Alg. 2, Alg. 3 is
guaranteed to find a path from $s_{\textrm{start}}$ to $g$. Moreover, from
lemma 1 we have that the path is returned within $T_{\textrm{bound}}$ time.
Hence the algorithm is CTMP-complete.

∎

####  5.5.2 Space Complexity

######  Lemma 3.

Let $n_{\Pi}$ be the maximum number of root paths needed to cover a goal
region for a given state $s_{\textrm{start}}$ and $\ell^{\prime}$ be the
number of discretized time steps at which the algorithm computes root paths,
then algorithm requires $O(n_{\Pi}^{\ell^{\prime}})$ space.

###### Proof.

At first, the algorithm stores $n_{\Pi}$ paths (worst case) from
$s_{\textrm{home}}$ to $G^{\textrm{full}}$ consuming $O(n_{\Pi})$ space. It
then computes root paths starting with the states at $t_{\textrm{rc}}$ and
iterating backwards to previous time steps (see Alg. 2 line 14). The loop
terminates when no more uncovered goals are left. If $\ell^{\prime}-1$ is the
maximum number of iterations of loop at line 14 that Alg. 2 undergoes, then it
requires $O(n_{\Pi}^{\ell^{\prime}})$ space. ∎

Note that Alg. 2 reduces the space complexity of the naive approach (Sec. 5)
primarily by (1) compressing the number of paths required to cover a goal
region from $n_{\rm goal}$ to $n_{\Pi}$ by planning with experience reuse and
(2) compressing the number of time steps for which the algorithm requires
preprocessing for from $\ell$ to $\ell^{\prime}$ by leveraging O2.
Additionally, the latching feature allows the algorithm to further reduce the
required number of paths by a significant amount.

Pertaining to the structure of our problem, the algorithm works under the
assumption that $n_{\Pi}\ll n_{\text{goal}}$ and $\ell^{\prime}\ll\ell$. To
demonstrate the magnitude of compression, in preprocessing for the time-
configuration planner (see Table. 2(a)), the algorithm covers 7197 goals with
only nine root paths for $s_{\textrm{home}}$ and only computes root paths for
a single time step $t=0$ (i.e., $\ell^{\prime}=1$) out of a total of eight
time steps up to $t_{\textrm{rc}}$.

##  6 Evaluation

We evaluated our algorithm in simulation and on a real robot. The conveyor
speed that we used for all of our results was 0.2$m/s$. We used Willow
Garage’s PR2 robot in our experiments using its 7-DOF arm. For the real robot
experiments we tested with the time-configuration planner whereas for the
simulated results we tested both for the time-configuration and kinodynamic
planners. The additional time dimension made the planning problem eight
dimensional. The experiments can be viewed at https://youtu.be/iLVPBWxa5b8.

CTMP wA* E-Graph RRT $T_{b}$ = 0.2 $T_{b}$ = 0.5 $T_{b}$ = 1.0 $T_{b}$ = 2.0
$T_{b}$ = 0.5 $T_{b}$ = 1.0 $T_{b}$ = 2.0 $T_{b}$ = 0.5 $T_{b}$ = 1.0 $T_{b}$
= 2.0 Pickup success [%] 100 0.0 0.0 18.0 0.0 0.0 80.0 0.0 0.0 18.0 Planning
success [%] 100 4.0 17.0 19.0 31.0 80.0 90.0 12.0 9.0 13.0 Planning time [s]
0.085 0.433 0.628 0.824 0.283 0.419 0.311 0.279 0.252 0.197 Planning cycles 3
2 2 2 2 2 2 2 2 2 Path cost [s] 9.49 8.19 8.28 7.60 8.54 8.22 7.90 9.68 8.96
8.04

(a) Simulation results—Time-configuration Planner.

CTMP wA* E-Graph $T_{b}$ = 0.25 $T_{b}$ = 0.5 $T_{b}$ = 1.0 $T_{b}$ = 2.0
$T_{b}$ = 0.5 $T_{b}$ = 1.0 $T_{b}$ = 2.0 Pickup success [%] 100 0.0 0.0 18.0
0.0 0.0 24.0 Planning success [%] 100 14.0 27.0 44.0 35.0 22.0 38.0 Planning
time [s] 0.122 0.325 0.732 1.45 0.1593 0.405 0.413 Planning cycles 3 2 2 2 2 2
2 Path cost [s] 9.82 8.74 9.15 9.39 8.63 8.72 8.15

(b) Simulation results—Kinodynamic Planner.

Table 1: Simulation results averaged over 50 experiments. Here $T_{b}$ denotes
the (possibly arbitrary) time bound that the algorithm uses. Note that for our
method $T_{b}=T_{\textrm{bound}}$ is the time bound that the algorithm is
ensured to compute a plan. CTMP algorithm uses a fixed $T_{b}$ (0.2$s$). For
the baselines, since their planning times are unbounded, we test them with
$T_{b}=$ 0.5, 1.0 and 2.0$s$. Our CTMP approach shows a task and planning
success rate of 100$\%$ for both the planners. Among the baselines, E-Graph
shows the best performance, however its performance drops significantly for
the kinodynamic planning problem.

###  6.1 Experimental setup

####  6.1.1 Sense-plan-act cycle

As object $o$ moves along the conveyor belt, we use the Brute Force ICP pose
estimation baseline proposed in Narayanan and Likhachev (2016) to obtain its
3-Dof pose for each captured input point cloud. We follow the classical sense-
plan-act cycle as depicted in Fig. 8. Specifically, the perception system
captures an image (point cloud) of the object $o$ at time $t_{\textrm{img}}$
followed by a period of duration $T_{\textrm{perception}}$ in which the
perception system estimates the pose of $o$. At time
$t_{\textrm{msg}}=t_{\textrm{img}}+T_{\textrm{perception}}$, planning starts
for a period of $T_{\textrm{planning}}$ which is guaranteed to be less than
$T_{\textrm{bound}}$. Thus, at $t_{\textrm{plan}}=t_{\textrm{msg}}+T_{\rm
planning}$ the planner waits for an additional duration of $T_{\rm
wait}=T_{\textrm{bound}}-T_{\rm planning}$. Finally, at
$t_{\textrm{exec}}=t_{\textrm{plan}}+T_{\rm wait}$, the robot starts executing
the plan. Note that the goal $g$ that the planner plans for is not for the
object pose at $t_{\textrm{img}}$ but its forward projection in time to
$t_{\textrm{exec}}$ to account for $T_{\textrm{perception}}$ and
$T_{\textrm{bound}}$. While executing the plan, if we obtain an updated pose
estimate, the execution is preempted and the cycle repeats.

Fig. 8: Timeline of the sense-plan-act cycle.

####  6.1.2 Goal region specification

To define the set of all goal poses $G^{\textrm{full}}$, we need to detail our
system setup, depicted in Fig. 9. The conveyor belt moves along the $x$-axis
from left to right. We pick a fixed $x$-value termed $x_{\textrm{exec}}$, such
that when the incoming $o$ reaches $x_{\textrm{exec}}$ as per the perception
information, at that point we start execution.

Recall that a pose of an object $o$ is a three dimensional point
$(x,y,\theta)$ corresponding to the $(x,y)$ location of $o$ and to its
orientation (yaw angle) along the conveyor belt. $G^{\textrm{full}}$ contains
a fine discretization of all possible $x,y$ and $\theta$ values in
$[x_{\textrm{exec}}-2\varepsilon,x_{\textrm{exec}}+2\varepsilon]$. We select
$G^{\textrm{full}}$ such that $\varepsilon=$2.5$cm$, making the goal region
10$cm$ long along x-axis. Its dimension along $y$-axis is 20$cm$, equal to the
width of the conveyor belt. The discretization in $x,y$ and $\theta$ is
1.0$cm$ and 10$\deg$ respectively.

In the example depicted in Fig. 9, the thick and the thin solid rectangles
show the ground truth and estimated poses, respectively at two time instances
in the life time of the object. The first plan is generated for the pose shown
at $x_{\textrm{exec}}$. During execution, the robot receives an improved
estimate and has to replan for it. At this point we back project this new
estimate in time using the known speed of the conveyor and the time duration
between the two estimates. This back-projected pose (shown as the dotted
rectangle) is then picked as the new goal for replanning. Recall that under
our assumption about the pose error in perception being $\varepsilon$, the
back projected pose will always lie inside $G^{\textrm{full}}$.

Fig. 9: A depiction of $G^{\textrm{full}}$-specification on a conveyor belt
(overhead view) and perception noise handling.

###  6.2 Results

State time Num. of states Unreachable goals Covered goals Covered via root
paths Covered via Latching Num. of root paths Num. of states Latching tries
Latching failures Processing time 0 1 3 7197 7197 0 9 1641 0 0 2534 3.5 9 3
7197 0 7197 0 0 8 0 0.01

(a) Preprocessing statistics—Time-configuration Planner

State time Num. of states Unreachable goals Covered goals Covered via root
paths Covered via Latching Num. of root paths Num. of states Latching tries
Latching failures Processing time 0 1 16 7184 7184 0 18 3120 0 0 25237.7 3.0
16 16 7184 0 7184 0 0 13.2 0 1062 3.5 18 38.2 7161.8 444.6 6717.2 11.7 2203.4
18 2.4 0.02

(b) Preprocessing statistics—Kinodynamic Planner

Table 2: Preprocessing statistics— We report the preprocessing statistics for
the robot states from which the algorithm either computes a latching primitive
or computes new root paths for replanning. The statistics are indexed based on
the time step of the states. For each time step, we give the number of states
at that time step and the average values for each preprocessing metric over
all the states at that time stamp.

####  6.2.1 Real-robot experiments

To show the necessity of real-time replanning in response to perception
updates, we performed three types of experiments: using our approach to replan
every time new object pose estimate arrives, single-shot planning based on the
first object pose estimate and single-shot planning using the late (more
accurate) pose estimate. For each set of experiments, we determined the pickup
success rate to grasp the moving object (sugar box) off the conveyor belt. In
addition, we report on the perception system’s success rate by observing the
overlap between the point cloud of the object’s 3D model transformed by the
predicted pose (that was used for planning) and the filtered input point cloud
containing points belonging to the object. A high (low) overlap corresponds to
an accurate (inaccurate) pose estimate. We use the same strategy to determine
the range for which the perception system’s estimates are accurate and use it
to determine the time for the best-pose planning. Further, for each method, we
determine the pickup success rate given that the perception system’s estimate
was or was not accurate.

The experimental results are shown in Table 3. Our method achieves the highest
overall pickup success rate on the robot by a large margin, indicating the
importance of continuous replanning with multiple pose estimates. First-pose
planning has the least overall success rate due to inaccuracy of pose
estimates when the object is far from the robot’s camera. Best-pose planning
performs better overall than the first pose strategy, since it uses accurate
pose estimates, received when the object is close to the robot. However it
often fails even when perception is accurate, since a large number of goals
are unreachable due to limited time remaining to grasp the object when it is
closer to the robot. A demonstration of our approach is given in Fig. 10.

|  | Success
---
rate
| Accuracy of
---
perception $[\%]$
| Success rate
---
(Accurate perception)
| Success rate
---
(Inaccurate perception)
CTMP | 69.23 | 42.31 | 83.33 | 57.14
First-pose Planning | 16.00 | 24.00 | 66.67 | 0.00
Best-pose Planning | 34.61 | 34.62 | 55.56 | 23.53
Table 3: Real-robot experiments. Success rate for the three experiments (Our
method (CTMP), First-pose planning and Best-pose planning) averaged over 50
trials .

(a)

(b)

(c)

(d)

(e)

(f)

Fig. 10: Snapshots from a real robot experiment. (10(a)) The robot receives
the first pose estimate from the perception system, generates the first plan
within $T_{\textrm{bound}}$ and starts execution. (10(b)) The robot receives
the second pose estimate with a pose correction of distance 3cm and replans to
the new goal (10(c)) The robot receives the third and last pose estimate with
no further correction and hence continues to follow the previous plan. (10(d))
and (10(e)) The robot executes the dynamic motion primitive to reach the grasp
pose and glide along with the object. (10(f)) The robot lifts up the sugar box
from the conveyor.

####  6.2.2 Simulation experiments

We simulated the real world scenario to evaluate our method against other
baselines. We compared our method with wA* Pohl (1970), E-graph Phillips et
al. (2012) and RRT LaValle (1998). For wA* and E-graph we use the same graph
representation as our method. For E-graph we precompute five paths to
randomly-selected goals in $G^{\textrm{full}}$. We adapt the RRT algorithm to
account for the under-defined goals. To do so, we sample pre-grasp poses along
the conveyor and compute IK solutions for them to get a set of goal
configurations for goal biasing. When a newly-added node falls within a
threshold distance from the object, we use the same dynamic primitive that we
use in the search-based methods to add the final grasping maneuver. If the
primitive succeeds, we return success. We also allow wait actions at the pre-
grasp locations.

For any planner to be used in our system, we need to endow it with a (possibly
arbitrary) planning time bound to compute the future location of the object
from which the new execution will start. If the planner fails to generate the
plan within this time, the robot fails to react to that pose update and such
cases are recorded as planning failure. We label a run as a pickup success if
the planner successfully replans once after the object crosses the 1.0m mark.
The mark is the mean of accurate perception range that was determined
experimentally and used in the robot experiments as described in Section
6.2.1. The key takeaway from our experiments (Table 1) is that having a known
time bound on the query time is vital to the success of the conveyor pickup
task.

Our method shows the highest pickup success rate, planning success rate
(success rate over all planning queries) and an order of magnitude lower
planning times compared to the other methods. We tested the other methods with
several different time bounds. After our approach E-graph performed decently
well. RRT suffers from the fact that the goal is under-defined and sampling
based planners typically require a goal bias in the configuration space.
Another important highlight of the experiments is the number of planning
cycles over the lifetime of an object. While the other approaches could replan
at most twice, our method was able to replan thrice due to fast planning
times.

####  6.2.3 Preprocessing

The statistics of the preprocessing stage (i.e. running Alg. 2) are shown in
Table 2. The offline time bound $T_{\mathcal{P}}$ used in all of our
experiments was 10$s$ In all experiments, we used $t_{\textrm{rc}}=$3.5$s$ and
$\delta_{t}=$0.5$s$. For the time-configuration planner the preprocessing took
2,534$s$. Only nine root paths are computed to cover 7,197 goals (three goals
being unreachable and hence uncovered). For the states at
$t=t_{\textrm{rc}}($3.5$s)$, there were no latching failures and therefore,
the algorithm terminates without preprocessing for earlier time stamps. For
the kinodynamic planner, the preprocessing takes about 7 hours. The dynamic
constraints causes latching failures and therefore, the algorithm requires
more preprocessing efforts. Due to latching failures at $t=$3.5, it needs to
compute root paths for some of the states at this time step. Finally, it
covers all the uncovered goals for states at $t=$3.0 via latching and finishes
preprocessing.

##  7 Conclusion and Discussion

To summarize, we developed a provably constant-time planning and replanning
algorithm that can be used to grasp fast moving objects off conveyor belts and
evaluated it in simulation and in the real world on the PR2 robot. Through
this work, we advocate the need for algorithms that guarantee (small)
constant-time planning for time-critical applications, such as the conveyor
pickup task, which are often encountered in warehouse and manufacturing
environments. To this end we introduce and formalize a new class of algorithms
called CTMP algorithms.

An interesting future research direction could be to leverage roadmap-based
representation instead of storing individual paths in a way that the algorithm
remains CTMP-complete, namely it maintains constant-time planning guarantee.
On the more practical side, a useful extension could be to parallelize the
preprocessing computation over multiple CPU cores. One obvious way of doing so
is within the Alg. 1. The required uncovered goal region $G^{\textrm{uncov}}$
can be divided over multiple threads.

Another useful extension to our CTMP approach is to be able to handle multiple
objects simultaneously coming on the conveyor belt. This setting is common for
a sorting task when multiple robot arms work at the same conveyor and they
have to pick up one object while avoiding collisions with the other object.
This makes the problem more challenging since the planner has to consider more
than one dynamic objects in the scene.

This work was supported by the ONR grant N00014-18-1-2775 and the ARL grant
W911NF-18-2-0218 as part of the A2I2 program. In addition, it was partially
supported by the Israeli Ministry of Science & Technology grant No. 102583 and
by Grant No. 1018193 from the United States-Israel Binational Science
Foundation (BSF) and by Grant No. 2019703 from the United States National
Science Foundation (NSF).

The authors would like to thank Andrew Dornbush for providing support for the
Search-Based Motion Planning Library (https://github.com/aurone/smpl) which is
used in our implementation. The authors would also like to thank Ellis Ratner
for fruitful discussions and Anirudh Vemula for helping out in restoring the
PR2 robot which is used in our experiments.

## References

  * Allen et al. (1993) Allen PK, Timcenko A, Yoshimi B and Michelman P (1993) Automated tracking and grasping of a moving object with a robotic hand-eye system.  _TRA_ 9(2): 152–165.
  * Belghith et al. (2006) Belghith K, Kabanza F, Hartman L and Nkambou R (2006) Anytime dynamic path-planning with flexible probabilistic roadmaps.  In: _Proceedings of the IEEE International Conference on Robotics and Automation, 2006. ICRA 2006._ IEEE, pp. 2372–2377.
  * Berenson et al. (2012) Berenson D, Abbeel P and Goldberg K (2012) A robot path planning framework that learns from experience.  In: _Proceedings of the IEEE International Conference on Robotics and Automation (ICRA)_. pp. 3671–3678.
  * Cefalo et al. (2013) Cefalo M, Oriolo G and Vendittelli M (2013) Task-constrained motion planning with moving obstacles.  In: _2013 IEEE/RSJ International Conference on Intelligent Robots and Systems_. IEEE, pp. 5758–5763.
  * Cohen et al. (2010) Cohen BJ, Chitta S and Likhachev M (2010) Search-based planning for manipulation with motion primitives.  In: _Proceedings of the IEEE International Conference on Robotics and Automation (ICRA)_. pp. 2902–2908.
  * Cohen et al. (2011) Cohen BJ, Subramania G, Chitta S and Likhachev M (2011) Planning for Manipulation with Adaptive Motion Primitives.  In: _Proceedings of the IEEE International Conference on Robotics and Automation (ICRA)_. pp. 5478–5485.
  * Coleman et al. (2015) Coleman D, Şucan IA, Moll M, Okada K and Correll N (2015) Experience-based planning with sparse roadmap spanners.  In: _Proceedings of the IEEE International Conference on Robotics and Automation (ICRA)_. pp. 900–905.
  * Cowley et al. (2013) Cowley A, Cohen B, Marshall W, Taylor CJ and Likhachev M (2013) Perception and motion planning for pick-and-place of dynamic objects.  In: _Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. pp. 816–823.
  * Czech et al. (1997) Czech ZJ, Havas G and Majewski BS (1997) Perfect hashing.  _Theoretical Computer Science_ 182(1-2): 1–143.
  * Dobson and Bekris (2014) Dobson A and Bekris KE (2014) Sparse roadmap spanners for asymptotically near-optimal motion planning.  _The International Journal of Robotics Research (IJRR)_ 33(1): 18–47.
  * Fraichard (1993) Fraichard T (1993) Dynamic trajectory planning with dynamic constraints: A’state-time space’approach.  In: _IROS_ , volume 2. pp. 1393–1400.
  * Han et al. (2019) Han SD, Feng SW and Yu J (2019) Toward Fast and Optimal Robotic Pick-and-Place on a Moving Conveyor.  _IEEE Robotics and Automation Letters (RA-L)_ 5(2): 446–453.
  * Hinterstoisser et al. (2012) Hinterstoisser S, Lepetit V, Ilic S, Holzer S, Bradski G, Konolige K and Navab N (2012) Model based training, detection and pose estimation of texture-less 3D objects in heavily cluttered scenes.  In: _Proceedings of the Asian Conference on Computer Vision_. pp. 548–562.
  * Ishida and Korf (1991) Ishida T and Korf RE (1991) Moving Target Search.  In: _Proceedings of the International Joint Conference on Artificial Intelligence (IJCAI)_ , volume 91. pp. 204–210.
  * Ishida and Korf (1995) Ishida T and Korf RE (1995) Moving-target search: A real-time search for changing goals.  _IEEE Transactions on Pattern Analysis and Machine Intelligence_ 17(6): 609–619.
  * Islam et al. (2020) Islam F, Salzman O, Agarwal A and Likhachev M (2020) Provably Constant-time Planning and Replanning for Real-time Grasping Objects off a Conveyor Belt.  In: _Proceedings of Robotics: Science and Systems (RSS)_.
  * Islam et al. (2019) Islam F, Salzman O and Likhachev M (2019) Provable Indefinite-Horizon Real-Time Planning for Repetitive Tasks.  In: _Proceedings of the International Conference on Automated Planning and Scheduling (ICAPS)_. pp. 716–724.
  * Kavraki et al. (1996) Kavraki LE, Svestka P, Latombe JC and Overmars MH (1996) Probabilistic roadmaps for path planning in high-dimensional configuration spaces.  _IEEE Robotics and Automation Letters (RA-L)_ 12(4): 566–580.
  * Koenig and Likhachev (2006) Koenig S and Likhachev M (2006) Real-time adaptive A*.  In: _Proceedings of the International Joint Conference on Autonomous Agents and Multiagent Systems (AAMAS)_. pp. 281–288.
  * Koenig et al. (2007) Koenig S, Likhachev M and Sun X (2007) Speeding up moving-target search.  In: _Proceedings of the International Joint Conference on Autonomous Agents and Multiagent Systems (AAMAS)_. pp. 1–8.
  * Koenig and Sun (2009) Koenig S and Sun X (2009) Comparing real-time and incremental heuristic search for real-time situated agents.  _Autonomous Agents and Multi-Agent Systems_ 18(3): 313–341.
  * Korf (1990) Korf RE (1990) Real-time heuristic search.  _Artificial intelligence_ 42(2-3): 189–211.
  * LaValle (1998) LaValle SM (1998) Rapidly-exploring random trees: A new tool for path planning .
  * Likhachev and Ferguson (2009) Likhachev M and Ferguson D (2009) Planning Long Dynamically Feasible Maneuvers for Autonomous Vehicles.  _The International Journal of Robotics Research (IJRR)_ 28(8): 933–945.
  * Menon et al. (2014) Menon A, Cohen B and Likhachev M (2014) Motion planning for smooth pickup of moving objects.  In: _Proceedings of the IEEE International Conference on Robotics and Automation (ICRA)_. pp. 453–460.
  * Narayanan and Likhachev (2016) Narayanan V and Likhachev M (2016) PERCH: Perception via search for multi-object recognition and localization.  In: _Proceedings of the IEEE International Conference on Robotics and Automation (ICRA)_. pp. 5052–5059.
  * Phillips et al. (2012) Phillips M, Cohen BJ, Chitta S and Likhachev M (2012) E-Graphs: Bootstrapping Planning with Experience Graphs.  In: _Proceedings of Robotics: Science and Systems (RSS)_.
  * Pohl (1970) Pohl I (1970) . _Artificial intelligence_ 1(3-4): 193–204.
  * Salzman et al. (2014) Salzman O, Shaharabani D, Agarwal PK and Halperin D (2014) Sparsification of motion-planning roadmaps by edge contraction.  _The International Journal of Robotics Research (IJRR)_ 33(14): 1711–1725.
  * Stogl et al. (2017) Stogl D, Zumkeller D, Navarro SE, Heilig A and Hein B (2017) Tracking, reconstruction and grasping of unknown rotationally symmetrical objects from a conveyor belt.  In: _IEEE International Conference on Emerging Technologies and Factory Automation (ETFA)_. IEEE, pp. 1–8.
  * Sun et al. (2010) Sun X, Yeoh W and Koenig S (2010) Moving target D* lite.  In: _Proceedings of the International Joint Conference on Autonomous Agents and Multiagent Systems (AAMAS)_. pp. 67–74.
  * Yang et al. (2018) Yang Y, Merkt W, Ivan V and Vijayakumar S (2018) Planning in time-configuration space for efficient pick-and-place in non-static environments with temporal constraints.  In: _2018 IEEE-RAS 18th International Conference on Humanoid Robots (Humanoids)_. IEEE, pp. 1–9.
  * Yoshida and Kanehiro (2011) Yoshida E and Kanehiro F (2011) Reactive robot motion using path replanning and deformation.  In: _Proceedings of the IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, pp. 5456–5462.
  * Zhang et al. (2018) Zhang Y, Li L, Ripperger M, Nicho J, Veeraraghavan M and Fumagalli A (2018) Gilbreth: A conveyor-belt based pick-and-sort industrial robotics application.  In: _International Conference on Robotic Computing (IRC)_. pp. 17–24.


# Tunable multimode lasing in a fiber ring

Eyal Buks  Andrew and Erna Viterbi Department of Electrical Engineering,
Technion, Haifa 32000 Israel

###### Abstract

We experimentally study a fiber loop laser with an integrated Erbium doped
fiber (EDF). The output optical spectrum is measured as a function of the EDF
temperature. We find that below a critical temperature of about 10K the
measured optical spectrum exhibits a sequence of narrow and unequally-spaced
peaks. Externally injected light and filtering are employed for tuning the
peaks’ wavelengths. Operation of the device as an optical memory having
storage time of about $20\,\mathrm{ms}$ is demonstrated. The multimode lasing
tunability can be exploited for novel applications in the fields of sensing,
communication, and quantum data storage.

Introduction \- Erbium doped fibers (EDF) are widely employed in a variety of
applications. Key properties of EDF can be controlled by varying the
temperature Kobyakov_1 ; Le_3611 ; Thevenaz_22 . Multimode lasing has a
variety of applications in the fields of sensing, spectroscopy, signal
processing and communication Haken_laser . Multimode lasing in the telecom
band has been demonstrated by integrating an EDF cooled by liquid nitrogen
into a fiber ring laser Yamashita_1298 ; Liu_102988 ; Lopez_085401 . It has
been recently proposed that EDF operating at low temperatures can be used for
storing quantum information Saglamyurek_83 ; Staudt_720 ; Wei_2209_00802 ;
Ortu_035024 ; Liu_2201_03692 ; Veissier_195138 . Population storage times
exceeding 10 seconds Saglamyurek_241111 , and electronic spin lifetimes
exceeding one hour Shafiei_F2A , have been demonstrated using persistent
spectral hole burning.

.

Figure 1: USOC - (a) The experimental setup. (b) The measured optical spectrum
with diode current $I_{\mathrm{D}}=120\,\mathrm{mA}$. (c) Comparison between
the measured normalized frequencies
$i_{n}=\left(f_{0}-f_{n}\right)/f_{\mathrm{L}}$ and the calculated values of
$\nu\log p_{n}$ [see Eq. 1]. The dimensionless pre-factor $\nu$ is found by
fitting to be given by $\nu=5610$. (d) Comparison between the normalized
intensities $I_{n}/I_{1}$ and the normalized wavelength gaps
$\left(\lambda_{n+1}-\lambda_{n}\right)/\left(\lambda_{1}-\lambda_{0}\right)$
[see Eq. 2].

In this work we study a fiber loop laser with an integrated EDF
Antuzevics_1149 ; haken1985laser . We measure the emitted optical spectrum as
a function of the EDF temperature Aubry_2100002 ; Pizzaia_2352 . Below a
critical temperature of about $10\,\mathrm{K}$ the measured optical spectrum
exhibits an unequally-spaced optical comb (USOC) made of a sequence of narrow
peaks. The observed USOC and multimode lasing are attributed to intermode
coupling moloney2018nonlinear ; Buks_128591 . We find that the wavelengths at
which the multimode lasing occurs can be controlled by light that is
externally injected into the fiber loop. The possibility of using the device
under study as an optical memory is explored, and a storage time of about
$20\,\mathrm{ms}$ is demonstrated. The USOC controllability, together with its
high stability, can be exploited for facilitating novel applications in the
fields of sensing, communication, and quantum data storage.

Experimental setup \- The experimental setup is schematically depicted in Fig.
1(a). EDF having length of $20\,\mathrm{m}$, absorption of $30$ dB
$\mathrm{m}^{-1}$ at $1530\,\mathrm{nm}$, and mode field diameter of
$6.5\mu\mathrm{m}$ at $1550\,\mathrm{nm}$, is cooled down using a cryogen free
cryostat. The EDF is thermally coupled to a calibrated silicon diode serving
as a thermometer, and it is pumped using a $980\,\mathrm{nm}$ laser diode (LD)
biased with current denoted by $I_{\mathrm{D}}$. The cold EDF is integrated
with a room temperature fiber loop using a wavelength-division multiplexing
(WDM) device. Two isolators [labeled by arrows in the sketch shown in Fig.
1(a)] and a 10:90 output coupler (OC) are integrated in the fiber loop. The
loop frequency $f_{\mathrm{L}}$ (inverse loop period time), which is measured
using a radio frequency spectrum analyzer and a photodetector, is given by
$f_{\mathrm{L}}=c/\left(n_{\mathrm{F}}l_{\mathrm{L}}\right)=4.963\,\mathrm{MHz}$,
where $c$ is the speed of light in vacuum, $n_{\mathrm{F}}=1.45$ is the fiber
refractive index, and $l_{\mathrm{L}}=41.7\,\mathrm{m}$ is the fiber loop
total length. An optical spectrum analyzer (OSA) is connected to the 10:90 OC.

USOC \- Near wavelength of $1540\,\mathrm{nm}$ and below a critical
temperature of about $10\,\mathrm{K}$ the measured optical spectrum exhibits
narrow peaks at a sequence of wavelengths denoted by
$\left\\{\lambda_{n}\right\\}$, where $n=0,1,2,\cdots$ [see Fig. 1(b)]. For
the data presented in Fig. 1 $\lambda_{0}=1540.629\,\mathrm{nm}$ and
$\lambda_{1}-\lambda_{0}=0.1661\,\mathrm{nm}$. The frequency $f_{n}$
associated with wavelength $\lambda_{n}$ is given by $f_{n}=c/\lambda_{n}$.
The intensity of the USOC peak occurring at wavelength $\lambda_{n}$ is
denoted by $I_{n}$.

Figure 2: Gain saturation. Optical spectrum (in dBm units) as a function of
the SE optical power $P_{\mathrm{SE}}$ feeding the 1:99 OC. The temperature is
$T=2.9\,\mathrm{K}$ and diode current is $I_{\mathrm{D}}=100\,\mathrm{mA}$.
Figure 3: Laser injection. The measured optical spectrum after the first (1),
second (2) and third (3) step. The monochromatic injected light of the
external laser has wavelength $\lambda_{\mathrm{L}}$ of (a)
$\lambda_{\mathrm{L}}=1537.917\,\mathrm{nm}$, (b)
$\lambda_{\mathrm{L}}=1540.162\,\mathrm{nm}$, (c)
$\lambda_{\mathrm{L}}=1541.784\,\mathrm{nm}$ and (d)
$\lambda_{\mathrm{L}}=1543.000\,\mathrm{nm}$. The temperature is
$T=3.0\,\mathrm{K}$, diode current is $I_{\mathrm{D}}=200\,\mathrm{mA}$, and
tunable laser power is $0.1\,\mathrm{mW}$. Figure 4: Peak moving. The tunable
laser wavelength $\lambda_{\mathrm{L}}$ is varied from $1539.988\,\mathrm{nm}$
to $1539.888\,\mathrm{nm}$ in steps of $1\,\mathrm{pm}$. During the time when
the wavelength $\lambda_{\mathrm{L}}$ is modified, the external laser optical
power is turned off (using an external attenuator). The measured optical
spectrum is shown in dBm units as a function of $\lambda_{\mathrm{L}}$. The
temperature is $T=2.9\,\mathrm{K}$, diode current is
$I_{\mathrm{D}}=200\,\mathrm{mA}$, and tunable laser power is
$0.39\,\mathrm{mW}$. Figure 5: Optical filter. A tunable filter is integrated
into the fiber loop. The plot displays the optical spectrum (in dBm units) as
a function of temperature $T$. The optical filter central wavelength
$\lambda_{\mathrm{F}}$ is tuned to the value
$\lambda_{\mathrm{F}}=1540\,\mathrm{nm}$, and the diode current is set to
$I_{\mathrm{D}}=100\,\mathrm{mA}$. Figure 6: USOC memory. Optical spectrum
before (a) and after (b) an off pulse having time duration of
$\tau_{\mathrm{off}}=22\,\mathrm{ms}$. In our experimental setup, the LD bias
current $I_{\mathrm{D}}$ can be fully modulated with frequency up to of about
$150\,\mathrm{kHz}$. Diode current (before and after the off pulse) is
$I_{\mathrm{D}}=150\,\mathrm{mA}$, and temperature is $T=3.5\,\mathrm{K}$.

The normalized frequency sequence
$i_{n}\equiv\left(f_{0}-f_{n}\right)/f_{\mathrm{L}}$, and the normalized
intensity sequence $s_{n}\equiv I_{n}/I_{1}$ are well describe by the
following empirical laws

$\displaystyle i_{n}$ $\displaystyle=$ $\displaystyle\nu\log p_{n}\ ,$ (1)
$\displaystyle s_{n}$ $\displaystyle=$
$\displaystyle\frac{\lambda_{n+1}-\lambda_{n}}{\lambda_{1}-\lambda_{0}}\ ,$
(2)

where $\nu$ is a positive constant, and $p_{n}$ is the $n$’th prime number.
The comparison between the measured values of
$i_{n}=\left(f_{0}-f_{n}\right)/f_{\mathrm{L}}$ and the calculated values of
$\nu\log p_{n}$ [see Eq. (1)] yields a good agreement [see Fig. 1(c)]. The
level of agreement is quantified by the parameter
$\varepsilon=n_{\mathrm{m}}^{-1}\sum_{n=1}^{n_{\mathrm{m}}}\left|\left(i_{n}-\nu\log
p_{n}\right)/i_{n_{\mathrm{m}}}\right|$, where $n_{\mathrm{m}}$ is the number
of peaks that can be reliably resolved. For the data shown in Fig. (1)
$n_{\mathrm{m}}=200$ and $\varepsilon=0.0043$. The comparison between
$I_{n}/I_{1}$and
$\left(\lambda_{n+1}-\lambda_{n}\right)/\left(\lambda_{1}-\lambda_{0}\right)$
[see Eq. (2)] is shown in Fig. 1(d). The underlying mechanism responsible for
USOC formation has remained mainly unknown. The connection between the
normalized frequency sequence $i_{n}=\left(f_{0}-f_{n}\right)/f_{\mathrm{L}}$
and the sequence of prime numbers [see Eq. (1)] is discussed in Buks_128591 .

Gain saturation \- The fiber loop gain can be characterized by injecting input
light having a continuous spectrum. Injection measurements [shown below in
Figs. 2, 3 and 4] are performed by integrating a 1:99 OC into the loop between
the points labeled as ’A’ and ’B’ in Fig. 1(a). For the measurements presented
in Fig. 2 below, the 1:99 OC is employed to inject into the loop spontaneous
emission (SE) light from an external pumped EDF. The optical power of the
externally injected SE light is controlled by a tunable attenuator. The plot
shown in Fig. 2 exhibits the spectrum measured by the OSA as a function of the
SE optical power $P_{\mathrm{SE}}$ feeding the 1:99 OC.

Consider two neighboring USOC peaks at wavelength $\lambda_{n}$ and
$\lambda_{n+1}$, where $n$ is a non-negative integer. As can be seen from Fig.
2, the loop gain $g_{\mathrm{L}}$ in the region
$\left[\lambda_{n},\lambda_{n+1}\right]$ is approximately a symmetric function
around the mid point
$\lambda_{\mathrm{a},n}=\left(\lambda_{n}+\lambda_{n+1}\right)/2$, at which
$g_{\mathrm{L}}$ peaks. The loop gain $g_{\mathrm{L}}$ vanishes at the end
points $\lambda_{n}$ and $\lambda_{n+1}$.

When the SE input optical power $P_{\mathrm{SE}}$ is sufficiently high, new
USOC peaks at the mid point wavelengths $\lambda_{\mathrm{a},n}$ are created
(see the regions highlighted by the overlaid red circles in Fig. 2). This
behavior is attributed to the fact that the wavelengths $\lambda_{n}$ and
$\lambda_{n+1}$, together with the mid wavelength $\lambda_{\mathrm{a},n}$,
nearly satisfy the mixing condition $f_{n}+f_{n+1}=2f_{\mathrm{a},n}$, where
$f_{\mathrm{a},n}=c/\lambda_{\mathrm{a},n}$ (recall that
$\left|\lambda_{n+1}-\lambda_{n}\right|\ll\lambda_{n}$). Note that the USOC
created with relatively intense SE injection (see Fig. 2) is significantly
distorted, and it cannot be well described by the empirical laws (1) and (2).

Laser injection \- The plot shown in Fig. 3 demonstrates external tuning of an
USOC peak. For the measurements presented in Fig. 3, the 1:99 OC is fed with a
narrow band external laser having a tunable wavelength $\lambda_{\mathrm{L}}$.
The experimental protocol has three steps, In the first one the external laser
having wavelength $\lambda_{\mathrm{L}}$ is turned on, then in the second step
the diode current is turned on, and in the last step the external laser is
turned off. After each step the optical spectrum is measured. The measured
spectrum after the first, second and third step is shown in the plots in Fig.
3 labelled by the numbers 1, 2 and 3, respectively.

In some cases, the spectrum that is measured after the third step, i.e. after
turning off the external laser, contains a peak at the injected external laser
wavelength $\lambda_{\mathrm{L}}$. This is demonstrated by plots in Fig. 3
labelled by the letters b and c. We find that this persistent effect occurs
only when $\lambda_{\mathrm{L}}$ is tuned inside the USOC region of about
$1540-1542.5\,\mathrm{nm}$ (compare to the plots in Fig. 3 labelled by the
letters a and d, for which the persistent effect does not occur).

Moving a single peak \- Each USOC peak wavelength $\lambda_{n}$ can be
individually tuned using external laser injection. The tuning method is
demonstrated for the second ($n=1$) USOC peak by the plot shown in Fig. 4.
Peak moving is achieved by first tuning the external laser wavelength
$\lambda_{\mathrm{L}}$ to match the USOC peak wavelength to be moved
$\lambda_{n}$, and then by gradually tuning $\lambda_{\mathrm{L}}$ to a target
value denoted by $\lambda_{n,\mathrm{T}}$. After reaching the target value,
the external laser is switched off. For the example shown in Fig. 4, all peaks
with $n>1$ are unaffected by the moving process applied to the $n=1$ peak.

The moving of the USOC $n$’th peak is possible when the external laser power
$P_{\mathrm{L}}$ exceeds a threshold value denoted by $P_{n}$. By carefully
tuning $P_{\mathrm{L}}$ (to be sufficiently close to $P_{n}$), the tuning
process of the $n$’th peak wavelength from the initial value $\lambda_{n}$ to
the target value $\lambda_{n,\mathrm{T}}$ can be performed without affecting
the wavelengths $\lambda_{n^{\prime}}$ of all other USOC peaks having
$n^{\prime}\neq n$.

Optical filter \- The band inside which USOC occurs can be controlled using an
optical filter. For the measurement shown in Fig. 5, a Fabry–Pérot filter
having full width at half maximum of $1.2\,\mathrm{nm}$ is integrated into the
loop between the points ’A’ and ’B’ [see the sketch in Fig. 1(a)]. The plot
shown in Fig. 5, which exhibits the temperature dependence of the created
USOC, demonstrates a transition from the low-temperature multimode lasing, to
the higher temperature single mode lasing. Note that the same above-discussed
tuning methods (see Figs. 3 and 4) can be successfully implemented to
manipulate the peaks of the filtered USOC shown in Fig. 5.

Memory \- The USOC wavelength sequence $\left\\{\lambda_{n}\right\\}$ is
highly stable. Over time periods exceeding 20 hours, changes in the sequence
$\left\\{\lambda_{n}\right\\}$ are too small to be reliably resolved by our
OSA, provided that the $980\,\mathrm{nm}$ LD bias current $I_{\mathrm{D}}$ is
kept constant. However, the sequence $\left\\{\lambda_{n}\right\\}$ is
significantly modified each time $I_{\mathrm{D}}$ is switched off, and then
switched on again, provided that the off time $\tau_{\mathrm{off}}$ is
sufficiently long. On the other hand, we find that the sequence
$\left\\{\lambda_{n}\right\\}$ remains unchanged when $\tau_{\mathrm{off}}$ is
sufficiently short.

Amplitude modulation applied to the LD bias current $I_{\mathrm{D}}$ is
employed to determine the USOC memory time, which is denoted by
$\tau_{\mathrm{M}}$. This is done by measuring the sequence
$\left\\{\lambda_{n}\right\\}$ before and after a switching off pulse having
time duration $\tau_{\mathrm{off}}$ is applied to $I_{\mathrm{D}}$. We find
that when $\tau_{\mathrm{off}}\ll\tau_{\mathrm{M}}$, the sequence
$\left\\{\lambda_{n}\right\\}$ is unaffected by the off pulse, where
$\tau_{\mathrm{M}}\simeq 20\,\mathrm{ms}$. On the other hand,
$\left\\{\lambda_{n}\right\\}$ is significantly modified for
$\tau_{\mathrm{off}}\gg\tau_{\mathrm{M}}$. In the region where
$\tau_{\mathrm{off}}\simeq 20\,\mathrm{ms}$, we find that most of the
wavelengths in the sequence $\left\\{\lambda_{n}\right\\}$ remain unchanged,
except for the first and/or the second ones (having the shortest wavelengths).
This behavior is demonstrated by the plots shown in Fig. 6, for which the off
time is $\tau_{\mathrm{off}}=22\,\mathrm{ms}$. For this case, the off pulse
shifts the first wavelength $\lambda_{0}$ by $-65\,\mathrm{pm}$, and all other
peaks are nearly unaffected. Note that the memory time $\tau_{\mathrm{M}}$ is
expected to strongly depend on the temperature Saglamyurek_83 ; Staudt_720 ;
Wei_2209_00802 ; Ortu_035024 ; Liu_2201_03692 ; Veissier_195138 ;
Saglamyurek_241111 ; Shafiei_F2A .

Summary \- Further study is needed to reveal the underlying mechanism
responsible for the USOC formation. Our measurements demonstrate several
methods that allow tuning of the wavelengths at which multimode lasing occurs.
Future study will focus on systematically characterizing spectral hole burning
Rittner_1  in the system under study, in order to explore its performance for
quantum memory applications.

Acknowledgments \- We thank Tian Zhong for useful discussions. This work was
supported by the Israeli science foundation.

## References

  * (1) Andrey Kobyakov, Michael Sauer, and Dipak Chowdhury,  “Stimulated brillouin scattering in optical fibers”,  Advances in optics and photonics, vol. 2, no. 1, pp. 1–59, 2010\.
  * (2) Julien Le Gouët, Jérémy Oudin, Philippe Perrault, Alaeddine Abbes, Alice Odier, and Alizée Dubois,  “On the effect of low temperatures on the maximum output power of a coherent erbium-doped fiber amplifier”,  Journal of Lightwave Technology, vol. 37, no. 14, pp. 3611–3619, 2019.
  * (3) Luc Thevenaz, Alexandre Fellay, Massimo Facchini, Walter Scandale, Marc Nikles, and Philippe A Robert,  “Brillouin optical fiber sensor for cryogenic thermometry”,  in Smart Structures and Materials 2002: Smart Sensor Technology and Measurement Systems. International Society for Optics and Photonics, 2002, vol. 4694, pp. 22–27.
  * (4) Hermann Haken,  Laser light dynamics, vol. 1,  North-Holland Amsterdam, 1985.
  * (5) S Yamashita and K Hotate,  “Multiwavelength erbium-doped fibre laser using intracavity etalon and cooled by liquid nitrogen”,  Electronics Letters, vol. 32, no. 14, pp. 1298–1299, 1996.
  * (6) Haochong Liu, Wei He, Yantao Liu, Yunhui Dong, and Lianqing Zhu,  “Erbium-doped fiber laser based on femtosecond laser inscribed fbg through fiber coating for strain sensing in liquid nitrogen environment”,  Optical Fiber Technology, vol. 72, pp. 102988, 2022.
  * (7) J Lopez, H Kerbertt, M Plata, E Hernandez, and S Stepanov,  “Two-wave mixing in erbium-doped-fibers with spectral-hole burning at 77k”,  Journal of Optics, vol. 22, no. 8, pp. 085401, 2020.
  * (8) Erhan Saglamyurek, Jeongwan Jin, Varun B Verma, Matthew D Shaw, Francesco Marsili, Sae Woo Nam, Daniel Oblak, and Wolfgang Tittel,  “Quantum storage of entangled telecom-wavelength photons in an erbium-doped optical fibre”,  Nature Photonics, vol. 9, no. 2, pp. 83–87, 2015.
  * (9) Matthias U Staudt, Sara R Hastings-Simon, Mikael Afzelius, Didier Jaccard, Wolfgang Tittel, and Nicolas Gisin,  “Investigations of optical coherence properties in an erbium-doped silicate fiber for quantum state storage”,  Optics Communications, vol. 266, no. 2, pp. 720–726, 2006.
  * (10) Shi-Hai Wei, Bo Jing, Xue-Ying Zhang, Jin-Yu Liao, Hao Li, Li-Xing You, Zhen Wang, You Wang, Guang-Wei Deng, Hai-Zhi Song, et al.,  “Storage of 1650 modes of single photons at telecom wavelength”,  arXiv:2209.00802, 2022.
  * (11) Antonio Ortu, Jelena V Rakonjac, Adrian Holzäpfel, Alessandro Seri, Samuele Grandi, Margherita Mazzera, Hugues de Riedmatten, and Mikael Afzelius,  “Multimode capacity of atomic-frequency comb quantum memories”,  Quantum Science and Technology, vol. 7, no. 3, pp. 035024, 2022\.
  * (12) Duan-Cheng Liu, Pei-Yun Li, Tian-Xiang Zhu, Liang Zheng, Jian-Yin Huang, Zong-Quan Zhou, Chuan-Feng Li, and Guang-Can Guo,  “On-demand storage of photonic qubits at telecom wavelengths”,  arXiv:2201.03692, 2022.
  * (13) Lucile Veissier, Mohsen Falamarzi, Thomas Lutz, Erhan Saglamyurek, Charles W Thiel, Rufus L Cone, and Wolfgang Tittel,  “Optical decoherence and spectral diffusion in an erbium-doped silica glass fiber featuring long-lived spin sublevels”,  Physical Review B, vol. 94, no. 19, pp. 195138, 2016.
  * (14) Erhan Saglamyurek, Thomas Lutz, Lucile Veissier, Morgan P Hedges, Charles W Thiel, Rufus L Cone, and Wolfgang Tittel,  “Efficient and long-lived zeeman-sublevel atomic population storage in an erbium-doped glass fiber”,  Physical Review B, vol. 92, no. 24, pp. 241111, 2015.
  * (15) Sara Shafiei, Erhan Saglamyurek, and Daniel Oblak,  “Hour-long decay-time of erbium spins in an optical fiber at milli-kelvin temperatures”,  in Quantum Information and Measurement. Optical Society of America, 2021, pp. F2A–4.
  * (16) Andris Antuzevics,  “Epr characterization of erbium in glasses and glass ceramics”,  Low Temperature Physics, vol. 46, no. 12, pp. 1149–1153, 2020.
  * (17) Hermann Haken,  Laser light dynamics, vol. 2,  North-Holland Amsterdam, 1985.
  * (18) Marine Aubry, Luciano Mescia, Adriana Morana, Thierry Robin, Arnaud Laurent, Julien Mekki, Emmanuel Marin, Youcef Ouerdane, Sylvain Girard, and Aziz Boukenter,  “Temperature influence on the radiation responses of erbium-doped fiber amplifiers”,  physica status solidi (a), vol. 218, no. 15, pp. 2100002, 2021.
  * (19) João Paulo Lebarck Pizzaia, Rodolpho Ladislau Silva, Arnaldo Gomes Leal-Junior, and Carlos Eduardo Schmidt Castellani,  “Temperature sensor based on an erbium-doped fiber sagnac interferometer”,  Applied Optics, vol. 61, no. 9, pp. 2352–2356, 2022.
  * (20) Jerome Moloney and Alan Newell,  Nonlinear optics,  CRC Press, 2018.
  * (21) Eyal Buks,  “Low temperature spectrum of a fiber loop laser”,  Physics Letters A, vol. 458, pp. 128591, 2023.
  * (22) Inga L Rittner and Peter M Krummrich,  “Measurement procedure for the inverse spectral hole burning characteristics of erbium-doped fiber amplifiers (edfa)”,  in Photonic Networks; 22th ITG Symposium. VDE, 2021, pp. 1–6.