Reachable Sets for Safe, Real-Time Manipulator Trajectory Design

# Reachable Sets for Safe, Real-Time Manipulator Trajectory Design

## Abstract

For robotic arms to operate in arbitrary environments, especially near people, it is critical to certify the safety of their motion planning algorithms. However, there is often a trade-off between safety and real-time performance; one can either carefully design safe plans, or rapidly generate potentially-unsafe plans. This work presents a receding-horizon, real-time trajectory planner with safety guarantees, called ARMTD (Autonomous Reachability-based Manipulator Trajectory Design). The method first computes (offline) a reachable set of parameterized trajectories for each joint of an arm. Each trajectory includes a fail-safe maneuver (braking to a stop). At runtime, in each receding-horizon planning iteration, ARMTD constructs a parameterized reachable set of the full arm in workspace and intersects it with obstacles to generate sub-differentiable, provably-conservative collision-avoidance constraints on the trajectory parameters. ARMTD then performs trajectory optimization over the parameters, subject to these constraints. On a 6 degree-of-freedom arm, ARMTD outperforms CHOMP in simulation, never crashes, and completes a variety of real-time planning tasks on hardware.

## I Introduction

To maximize utility in arbitrary environments, especially when operating near people, robotic arms should plan collision-free motions in real time. Such performance requires sensing and reacting to the environment as the robot plans and executes motions; in other words, it must perform \defemphreceding-horizon planning, where it iteratively generates a plan while executing a previous plan. This paper addresses guaranteed-safe receding-horizon trajectory planning for robotic arms. We call the proposed method Autonomous Reachability-based Manipulator Trajectory Design, or ARMTD, introduced in Fig. 1.

Motion planning can be broadly split into three paradigms, depending on whether safety is enforced by (1) a path planner, (2) a trajectory planner, or (3) a tracking controller.

The first paradigm is commonly used for robotic arm planning, wherein the path planner is responsible for safety. One generates a collision-free path, then smooths it and parameterizes it by time (i.e., converts it into a trajectory) [24, 29]. Such methods often have a tradeoff between safety and real-time performance because they represent paths with discrete points in configuration space [16, 12]. Ensuring safety requires approximations such as buffering the volume of the arm at each discrete point to account for the discretization, or computing the swept volume along the path assuming, e.g., straight lines between points [17]. If one treats the path as a decision variable in a nonlinear optimization program, the gradient of the distance between the arm’s volume and obstacles may “push” each configuration out of collision [32, 26, 23]. This means the output path can be treated directly as a trajectory, if the optimization uses path smoothness as the cost. However, this relies on several approximations to achieve real-time performance: finite differencing to bound joint speeds and accelerations, collision penalties in the cost instead of hard constraints, and finite differencing [32] or linearization [26] for the collision-avoidance penalty gradient. This necessitates finer discretization to faithfully represent the robot’s kinematics. To enable real-time performance without gradients, one can compute many paths offline, then collision-check at runtime [21, 15]; but for arbitrary tasks, it can be unclear how many paths are necessary, or how to ensure safety if the arm’s volume changes (e.g., by grasping an object). Another approach to real-time performance is to plan iteratively in a receding-horizon either by gradient descent (with the same drawbacks as above) [23] or assuming the underlying path planner is safe [9]. In summary, in this paradigm, one must discretize finely, or buffer by a large amount, to achieve safety at the expense of performance.

In the second paradigm, the path planner generates a (potentially unsafe) path, then the trajectory planner attempts to track the path as closely as possible while maintaining safety. In this paradigm, one computes a \defemphreachable set (RS) for a family of trajectories instead of computing a swept volume for a path. Methods in this paradigm can achieve both safety and real-time performance in receding-horizon planning by leveraging sums-of-squares programming [19, 13, 28] or zonotope reachability analysis [14]. Unfortunately, the methods in this paradigm suffer from the curse of dimensionality, preventing their use with the high-dimensional models of typical arms.

In the third paradigm, one attempts to ensure safety via the tracking controller, instead of in a path or trajectory. Here, one builds a supervisory safety controller for pre-specified trajectories [1] or a set of safe states [27]. Another approach is to compute a safety buffer and associated controller using Hamilton-Jacobi reachability analysis [10, 5], but the curse of dimensionality has prevented applying this to arms.

To the best of our knowledge, RSs in manipulator planning have only been used for either collision-checking a single, precomputed trajectory [1, ?], or for controlling to a predefined setpoint [?]. In contrast, our proposed ARMTD method generates RSs for a continuum of trajectories, allowing optimization over sets of safe trajectories. Computing such RSs directly is challenging because of the high-dimensional configuration space and nonlinear transformation to workspace used for a typical arm [13, 5].

Our proposed ARMTD method overcomes these challenges by composing a high-dimensional RS in workspace from low-dimensional reachable sets of joint configurations. ARMTD extends the second planning paradigm above by using these RSs to plan safe trajectories in real time. The RS also provides subdifferentiable collision-avoidance, self-intersection, and joint limit constraints for trajectory optimization. Importantly, the RS composition, constraint generation, and gradient evaluation are all parallelizable.

We now provide an overview of ARMTD, also shown in Fig. 2. ARMTD begins by specifying a parameterized continuum of kinematic configuration space trajectories, each of which includes a fail-safe maneuver. Offline, ARMTD computes parameterized joint reachable sets, or JRSs, of these trajectories in configuration space. At runtime (in each receding-horizon), it constructs a parameterized RS from the precomputed JRSs. ARMTD intersects the RS with obstacles to generate provably-correct safety constraints. ARMTD then performs trajectory optimization over the parameters, subject to the safety constraints. If it cannot find a feasible solution within a prespecified time limit, the arm continues executing the trajectory from its previous planning iteration (which includes a fail-safe maneuver), guaranteeing perpetual safety [13, 9]. In this work, we only discuss static environments, but this approach can extend to dynamic environments [28].

### I-a Contributions

We make the following contributions. First, a method to conservatively construct the RS of high-dimensional redundant robotic manipulators (Sections IIIIV). Second, a parallelized method to perform real-time, provably-safe, receding-horizon trajectory optimization (Section IV). Third, a demonstration in simulation and on hardware, with no collisions (Section V and Supplemental Video), plus a comparison to CHOMP [32]. The remaining sections are Section II (Arm, Obstacles, and Trajectory Parameters) and Section VI (Conclusion). See our video: youtu.be/ySnux2owlAA. Our code is available: github.com/ramvasudevan/arm_planning. All proofs, plus additional explanations, are available in the appendices included at the end of this document.

### I-B Notation

The -dimensional real numbers are , natural numbers are , the unit circle is , and the set of rotation matrices is . Vectors are either or depending on if the size/shape is relevant. Let . For a point , is the set containing . The power set of is . The Minkowski sum is . For a matrix , . For matrices, performs right multiplication with increasing index (e.g., ). Greek lowercase letters in angle brackets are indeterminate variables (e.g., ). Superscripts on points index elements of a set. Subscripts are joint indices or contextual information.

## Ii Arm, Obstacles, and Trajectory Parameters

The goal of this work is to plan collision-free trajectories for a robotic arm operating around obstacles in a receding-horizon framework. We now discuss the arm and its environment, then our receding-horizon framework and parameterized trajectories.

### Ii-a Arm and Obstacles

#### Arm

Consider an arm with joints (i.e., DOFs) and links, including the \tsth link, or \defemphbaselink. We make the following assumptions/definitions. Each joint is a single-axis revolute joint, attached between a \defemphpredecessor link and a \defemphsuccessor link. The arm is a single kinematic chain from baselink to end effector; link is joined to link by joint for . One can create multi-DOF joints using virtual links of zero volume. The \defemphconfiguration space is , containing \defemphconfigurations . The space of joint velocities is . There exists a default configuration . The \defemphworkspace, , is the all points in space reachable by any point on the arm in any configuration. The robot’s physical limits are as follows. Each joint has a minimum and maximum position and , maximum absolute speed and maximum absolute acceleration .

 \FOi(q) = {∑j

Let give the occupancy of the entire arm: . Note, the first expression in (1) gives the position of joint and the second gives the rotated volume of link . See Appendix D-A for an example.

#### Obstacles

We denote an \defemphobstacle as a set . If the arm’s volume at is intersecting the obstacle, we say the arm is in \defemphcollision, i.e. . We assume the following about obstacles. Each obstacle is compact and static with respect to time (note, one can extend ARMTD to dynamic obstacles [28]). At any time, there are at most , obstacles in the workspace, and the arm has access to a conservative estimate of the size and location of all such obstacles (we are only concerned with planning, not perception). Let denote a set of obstacles.

### Ii-B Receding-Horizon Planning and Timing

ARMTD plans in a receding-horizon way, meaning it generates a short plan, then executes it while generating its next short plan. Every such plan is specified over a compact time interval . Without loss of generality (WLOG), since time can be shifted to at the beginning of any plan, we denote . We further specify that ARMTD must generate a new plan every seconds. If a collision-free plan cannot be found within s, the robot must continue the plan from the previous receding-horizon iteration; therefore, we include a fail-safe (braking) maneuver in each plan. The durations and are chosen such that is large enough for the arm to stop from its maximum joint speeds given its maximum accelerations. This ensures every plan can include a fail-safe maneuver. We abuse notation to let denote a trajectory plan and denote the trajectory of the joint. A plan is \defemphcollision-free if . Next, we specify the form of each plan.

### Ii-C Trajectory Parameterization

ARMTD plans using parameterized trajectories. We describe the theory, then present our implementation.

#### Theory

Let , , be a compact space of \defemphtrajectory parameters, meaning each maps to a trajectory . We use to denote the configuration parameterized by at time . So, in each receding-horizon planning iteration, ARMTD attempts to select a single (via trajectory optimization with obstacles represented as constraints on .

{defn}

We require to satisfy three properties for all . First, is at least once-differentiable w.r.t. time. Second, . Third, .

The second property uses the fact that all joints are revolute, so WLOG. The third property guarantees each parameterized trajectory includes a fail-safe braking maneuver.

Note, the parameterized trajectories are kinematic, not dynamic. This is common in motion planning [32, 26, 23, 21, 15], because existing controllers can track such trajectories closely (e.g., within 0.01 rad for revolute joints [22, 7]) in the absence of disturbances such as collisions. We find these trajectories sufficient to avoid collision in real-world hardware demonstrations (Sec. V). Also, methods exist for quantifying tracking error [7, 14] and accounting for it at runtime [13, 28].

#### Implementation

We choose a parameterization that is simple yet sufficient for safe planning in arbitrary scenarios (see Sec. V). We define a \defemphvelocity parameter for the initial velocity , and an \defemphacceleration parameter that specifies a constant acceleration over . We write and similarly for . We denote , where . The trajectories are given by

 ˙q(t;k)={\kv+\kat,t∈[0,t\plan)\kv+\kat\plan\tfin−t\plan(\tfin−t),t∈[t\plan,\tfin],, (2)

with for all to satisfy Def. II-C1. These trajectories brake to a stop over with constant acceleration.

We require that is compact to perform reachability analysis (Sec. III). Let denote the parameters for joint . For each joint , we specify , where

 \Kvi=[\avgkvi−\delkvi, \avgkvi+\delkvi],\Kai=[\avgkai−\delkai, \avgkai+\delkai], (3)

with , , , and . To implement acceleration limits (i.e., to bound ), we ensure

 \Kai=[max{−\ddqlim,\ol\kai−Δ\kai},min{\ddqlim,\ol\kai+Δ\kai}]. (4)

Next, we use these parameterized trajectories to build parameterized reachable sets of joint configurations.

## Iii Offline Reachability Analysis

ARMTD uses short parameterized trajectories of joint angles for trajectory planning. We now describe a Joint Reachable Set (JRS) containing all such parameterized trajectories. All computations in this section are performed offline.

#### Theory

Since each represents a rotation, we examine trajectories of and , as shown in Fig. 2. By Def. II-C1, is at least once differentiable. We can write a differential equation of the sine and cosine as a function of the joint trajectory, where is a constant:

 ddt⎡⎢⎣cos(qi(t;k))sin(qi(t;k))k⎤⎥⎦=⎡⎢⎣−sin(qi(t;k))˙qi(t;k)cos(qi(t;k))˙qi(t;k)0⎤⎥⎦. (5)

We then define the parameterized JRS of the \tsth joint:

 \frsi={(c,s,k)∈\R2×K∣∃t∈T \regtexts.t. qi\regtextasinDef.???,c=cos(qi(t;k)), s=sin(qi(t;k)),\regtextandddt(cos(qi(t;k)),sin(qi(t;k)),k) \regtextasin(???)}. (6)

We account for different initial joint angles, and use the JRSs to overapproximate the forward occupancy , in Sec. IV.

#### Implementation

We represent (6) using zonotopes, a subclass of polytopes amenable to reachable set computation [?]. A \defemphzonotope is a set in in which each element is a linear combination of a \defemphcenter and \defemphgenerators :

 Z={y∈Rn ∣∣ y=x+p∑i=1\btigi, −1≤\bti≤1}. (7)

We denote as shorthand for a zonotope with center , a set of generators , and a set of indeterminate coefficients corresponding to each generator. When an indeterminate coefficient is \defemphevaluated, or assigned a particular value, we write (i.e., without angle brackets).

To represent the JRS, we first choose a time step such that and partition into closed intervals each of length , indexed by . We represent with one zonotope per time interval, which is returned by . For example, the zonotope corresponds to the time interval . We abuse notation and let index the subinterval of that contains it, so that where rounds down to the nearest integer. We use similar notation for the center, generators, and indeterminates.

Next, we make an initial condition zonotope :

 \Zio=(\cio,{\givo,\giao},{\sym\lkvio,\sym\lkaio}), (8)

with , , . The indeterminates and correspond to and . contains and in the and dimensions.

Finally, we use an open-source toolbox [2] with the time partition, differential equation (5) and (2), and initial set to overapproximate (6). Importantly, by [3, Thm. 3.3 and Prop. 3.7], one can prove the following:

 \frsi⊆⋃t∈T\Zit. (9)

JRSs are illustrated in Fig. 2. Next, we use the JRSs online to build an RS for the arm and identify unsafe plans in each receding-horizon iteration.

## Iv Online Planning

We now present ARMTD’s online algorithm for a single receding-horizon iteration (see Alg. 3 and Fig. 2). First, we construct the parameterized RS of the entire arm from the JRS of each joint. Second, we identify unsafe trajectory plans. Third, we optimize over the safe plans to minimize an arbitrary cost function. If no solution is found, we execute the previous plan’s fail-safe maneuver. Note, we present self-intersection constraints in Appendix C.

### Iv-a Reachable Set Construction

#### Theory

Recall that ARMTD plans while the robot is executing its previous plan. Therefore, ARMTD must estimate its future initial condition as a result of its previous plan by integrating (5) for seconds. At the beginning of each online planning iteration, we use to compose the RS of the arm from the low-dimensional JRSs. Denote each link’s RS , formed from all with :

 \linkfrsi={(Y,k)∈\lx@paragraphsign(W)×K ∣∣ ∃t∈T \regtexts.t.˙qi(0;k)=\initdqi, Y=\FOi(q(t;k)+\initq),\regtextand (cos(qj(t;k)),sin(qj(t;k)),k)∈\frsj ∀ j≤i} (10)

with as in (1). Each is formed by trajectories which start at the given initial conditions . The RS of the entire arm, , is then .

#### Implementation

It is important that we overapproximate to guarantee safety when planning. To do this, we overapproximate for all configurations in each (see Alg. 2).

First, we fix by obtaining subsets of the JRSs containing trajectories with the given initial velocity. To do so, we note a property of the zonotope JRS: {lem} There exist that overapproximate as in (9) such that, for each , has only one generator with a nonzero element, equal to , in the dimension corresponding to ; we denote this generator . Similarly, has only one generator (distinct from ) with a nonzero element, , for . Note, the zonotopes created by the open-source toolbox [2] satisfy Lem. IV-A2. For each , we denote the center , the generators , and the corresponding indeterminates for . We write since the number of generators is not necessarily the same for each [2]. For all except , and may have nonzero elements in the cosine and sine dimensions, due to nonzero dynamics and linearization error. The generators and are important because they let us obtain a subset of the JRS corresponding to a particular choice of parameters and . We refer to this operation as \defemphslicing, and we call and \defemph-sliceable and \defemph-sliceable, respectively.

To this end, we define in Alg. 1. We slice a zonotope by taking in a set of indeterminate coefficients and corresponding values with which to evaluate them. We evaluate an indeterminate by multiplying its associated generator by the given value. We then \defemphremove the corresponding indeterminate from the set. Since any zonotope generator has only one indeterminate, once its indeterminate is evaluated, it is called \defemphfully-sliced, and added to the center of the zonotope. Later in this section (Def. IV-A2), we construct zonotope-like objects called \defemphrotatotopes, which have multiple indeterminates per generator (so, a generator could be sliced without being fully-sliced). For additional explanation of slicing, see Appendix D-B.

For each joint , recall that each has generator , with indeterminate and nonzero element corresponding to the dimension. Also, (the center of ) has the value in that same dimension. We use to slice each :

 \Zitv=\slice(\Zit, \sym\lkvit, (\initdq−\avgkvi)/\delkvi) (11)

Note, we ensure later in this section. We denote , where is the new (shifted) center and is the new number of generators, other than , left after slicing. contains a set of and reachable for a single value of , but for a range of . Denote the components of as , and for each . Note from Lem. IV-A2 that and are generally non-zero, and is constant.

The forward occupancy map uses rotation matrices formed from the cosine and sine of each joint. By overapproximating these matrices, we can overapproximate . To this end, we represent sets of rotation matrices with matrix zonotopes. A \defemphmatrix zonotope is a set of matrices parameterized by a center and generators :

 M={A∈\Rn×n ∣∣ A=X+m∑j=1Gjλj,−1≤λj≤1}. (12)

We use as shorthand for a matrix zonotope with center , generators , and indeterminate coefficients . Note, superscripts are indices, not exponentiation, of matrix zonotope generators.

We use each sliced zonotope to produce a matrix zonotope that overapproximates the rotation matrices for each joint at each time . We do so by reshaping the center and generators of (and keeping its indeterminates), then rotating the resulting matrix zonotope by the initial joint angle ; we call this the makeMatZono function in Alg. 2. See Appendix D-C for an example of .

Importantly, satisfies the following property: {lem} For any parameterized trajectory with , every .

Now we use to overapproximate the link RS . Given the joint displacements and link volumes , we specify as a zonotope with center and no generators, and as a zonotope overapproximating the volume of link . We multiply the matrix zonotopes by to overapproximate a swept volume, hence the following definition: {defn} Let be a zonotope and be a matrix zonotope. Let . We call a \defemphrotatotope, which can be written:

 MZ={y∈\Rn ∣y=Xx+\tsumi\bt\upiXg\upi+\tsumjλ\upjG\upjx++\tsumi,j\bt\upiλ\upjG\upjg\upi, −1≤(\bt,λ)≤1}, (13)

where and .

We use the shorthand where , , and the generator and coefficient sets are

 {^gr}sr=1 ={Xg1,⋯,Xgp,G1x,⋯,Gmx,G1g1,⋯,Gmgp} {\sym\gmr}sr=1 ={\sym\bt1,⋯,\sym\btp,\sym\lm1,⋯,\sym\lmm,\sym\bt1\lm1,⋯,\sym\btp\lmm}.

Rotatotopes are a special class of polynomial zonotopes [2]. Each for is a product of indeterminate coefficients from and . For a pair of indeterminate coefficients and , the notation indicates the product . We call and the \defemphfactors of .

As noted earlier, we use with rotatotopes, for which we now define removing factors generically. We denote the \defemphremoval of the \tsth indeterminate coefficient of as:

 \sym\gm1\gm2⋯\gmn∖\sym\gmi=\sym\gm1\gm2⋯\gmi−1\gmi+1⋯\gmn. (14)

We define . We write to denote that is a factor of .

Two useful properties follow from the rotatotope definition:

{lem}

A matrix zonotope times a rotatotope is a rotatotope.

{lem}

(Zono/rotatotope Minkowski sum) Consider two zonotopes and . Then , which is a zonotope centered at with all the generators and indeterminates of both and . Similarly, for two rotatotopes, and ,

 V⊕W=(v+w,{giV,gjW},{\symμi,\sym\omj})i=n,j=mi=1,j=1. (15)

That is, the Minkowski sum is given by the sum of the centers and the union of the generators/indeterminate sets.

We use rotatotopes to overapproximate the forward occupancy map of each link by \defemphstacking rotatotopes representing link volume on top of rotatopes representing joint positions:

{lem}

For any and , , where

 (16)

Lem. IV-A2 lets us overapproximate the RS: , as shown in Fig. 2. Alg. 2 computes (see Appendix D-D for further computational details).

Though , many of its generators are -sliceable, because they are the product of -sliceable matrix zonotope generators. Denote . Formally, the \tsth generator is -sliceable if there exists at least one with . This means, by slicing by , we can obtain a subset of corresponding to that parameter. We make the distinction that a generator is fully--sliceable if all of its indeterminates are evaluated when sliced by , i.e. . Fully--sliceable generators are created by multiplying -sliceable generators with each other or with centers in (16). These generators are important because all of their indeterminates are evaluated by the trajectory optimization decision variable , which we use in Sec. IV-B2.

### Iv-B Constraint Generation

#### Theory

With the RS composed, we now use to find all unsafe trajectory parameters that could cause collisions with obstacles. We treat as a constraint for trajectory optimization, shown in Fig. 2. Recall , , and are joint limits. Let be a set of obstacles. At each planning iteration, the unsafe trajectory parameters are , where

 K\jlim={k | ∃ t∈T \regtexts.t. q(t;k)<\qlim− \regtextor q(t;k)>\qlim+\regtextor |˙q(t;k)|>\dqlim} (17) K\obs ={k | Y∩O≠∅,(Y,k)∈\linkfrs,O∈\obsset}. (18)

#### Implementation

We represent with functions . Notice in (2) that is piecewise quadratic in and is piecewise linear in , so the parameterized trajectory extrema can be computed analytically. We construct from , , and , such that when feasible.

To represent (depicted in Fig. 2), first consider a particular . We test if the corresponding subset of each rotatotope could intersect any obstacle . We overapproximate each by a zonotope, which is always possible for compact, bounded sets [3] that appear in common obstacle representations such as octrees [20] or convex polytopes [18]. To proceed, we must test if two zonotopes intersect: {lem} [8, Lem. 5.1] For two zonotopes and , iff is in the zonotope , where the subscript indicates is buffered by the generators of .

Since zonotopes are convex polytopes [8], by [3, Theorem 2.1], one can implement Lem. IV-B2 by computing a \defemphhalf-space representation of for which , where the inequality is taken elementwise. Using this representation, . We can use Lem. IV-B2 for collision avoidance by replacing (resp. ) with a zonotope representing the arm (resp. an obstacle).

However, since we use rotatotopes, we need the following: {lem} Any rotatotope as in (13) can be overapproximated by a zonotope. So, we can overapproximate the intersection of each , sliced by , with each . Note, we only slice the fully--sliceable generators of , and treat all other generators conservatively by applying Lemma IV-B2. That is, we do not slice any generators that have any indeterminates in addition to , and instead use those generators to (conservatively) buffer obstacles.

To check intersection, we separate into two rotatotopes,

 \Vitslc=(\cit,g\slcj,\sym\kp\slcj) \regtextand \Vitbuf=(0,g\bufn,\sym\bt\bufn), (19)

such that , where has only fully--sliceable generators. That is, each is a product of only for one or more . Note, the number of generators/indeterminates in and is omitted to ease notation. For any , since every generator of is -sliceable, slicing by returns a point. We express this with for which

 (20)

where . Note, can be implemented as the evaluation of polynomials.

Now, let and be the halfspace representation of , and let . Then,

 ({x}⊕\Vitbuf)∩O=∅⟺−max{A\obsx−b\obs}<0 (21)

where is overapproximated as a zonotope by applying Lem. IV-B2. We use (21) to overapproximate the parameters (18) with for which

 h\obs(∗,\ka)=−max{A\obs(∗)\eval(\Vitslc,\ka)−b\obs(∗)}. (22)

where for space. Here, and return the halfspace representation of . Importantly, for each obstacle, time, and joint, is a max of a linear combination of polynomials in (per (20) and Alg. 1), so we can take its subgradient with respect to [4] (also see [?, Thm. 5.4.5]). This constraint conservatively approximates : {lem} If , then there exists , , and such that .

### Iv-C Trajectory Optimization

#### Theory

ARMTD performs trajectory optimization over for an arbitrary user-specified cost function (which encodes information such as completing a task). ARMTD attempts to solve the following within :

 k\opt=\regtextargmink{\costfunc(k) | k∉K\unsafe}. (23)

If no solution is found in time, the robot tracks the fail-safe maneuever from its previous plan.

#### Implementation

We implement (23) as a nonlinear program, denoted optTraj in Alg. 3.

 \regtextargmin\ka∈\Ka{\costfunc(\ka) | h\obs(i,t,O,\ka)<0, h\jlim(\ka)<0} (24)

where the constraints hold for all .

{thm}

Any feasible solution to (24) parameterizes a trajectory that is collision-free and obeys joint limits over the time horizon .

ARMTD uses Alg. 3 at each planning iteration. If the arm does not start in collision, this algorithm ensures that the arm is always safe (see Appendix B, also see [13, Remark 70] or [9, Theorem 1]).

## V Demonstrations

We now demonstrate ARMTD in simulation and on hardware using the Fetch mobile manipulator (Fig. 1). ARMTD is implemented in MATLAB, CUDA, and C++, on a 3.6 GHz computer with an Nvidia Quadro RTX 8000 GPU. See our video: youtu.be/ySnux2owlAA. Our code is available: github.com/ramvasudevan/arm_planning.

### V-a Implementation Details

#### Manipulator

The Fetch arm has revolute DOFs [31]. We consider the first DOFs, and treat the body as an obstacle. The \tsth DOF controls end effector orientation, which does not affect the volume used for collision checking. We command the hardware via ROS [25] over WiFi.

#### Comparison

To assess the difficulty of our simulated environments, we ran CHOMP [32] via MoveIt [6] (default settings, straight-line initialization). We emphasize that CHOMP is not a receding-horizon planner [6]; it attempts to find a plan from start to goal with a single optimization program. However, CHOMP provides a useful baseline to measure the performance of ARMTD. To the best of our knowledge, no open-source, real-time receding-horizon planner is available for a direct comparison. Note, we report solve times to illustrate that ARMTD is real-time feasible, but the goal of ARMTD is not to solve as fast as possible; instead, we care about finding provably collision-free trajectories in the allotted time .

#### High-level Planner

Recall that ARMTD performs trajectory optimization using an arbitrary user-specified cost function. In this work, in each planning iteration, we create a cost function for ARMTD using an intermediate waypoint between the arm’s current configuration and a global goal. These waypoints are generated by a high-level planner (HLP). Note, the RS and safety constraints generated by ARMTD are independent of the HLP, which is only used for the cost function. To illustrate that ARMTD can enforce safety, we use two different HLPs, neither of which is guaranteed to generate collision-free waypoints. First, a straight-line HLP that generates waypoints along a straight line between the arm and a global goal in configuration space. Second, an RRT* [11] that only ensures the arm’s end effector is collision-free. Thus, ARMTD can act as a safety layer on top of RRT*. Note, we allot a portion of to the HLP in each iteration, and give ARTMD the rest of . We cannot use CHOMP as a receding-horizon planner with these HLP waypoints, because it requires a collision-free goal configuration. For further discussion of the comparison to CHOMP, see Appendix D-G.

#### Algorithm Implementation

Alg. 2 runs at the start of each ARMTD planning iteration. We use a GPU with CUDA to execute Alg. 2 in parallel, taking approximately ms to compose a full RS. The constraint generation step in Alg. 3 is also parallelized across obstacles and time steps (this takes approximately ms for obstacles).

We solve ARMTD’s trajectory optimization (24) using IPOPT [30]. The cost function is , where is the waypoint specified by the HLP (straight-line or RRT*) at each planning iteration. We compute analytic gradients/sub-gradients of the cost function and constraints, and evaluate the constraints in parallel. IPOPT takes ms when it finds a feasible solution in a scene with random obstacles.

#### Hyperparameters

To reduce conservatism, we partition into equally-sized intervals and compute one JRS for each interval. At runtime, for each joint, we pick the JRS containing the initial speed . In each JRS, we set , with so the range of accelerations scales with the absolute value of the mean velocity of each JRS. This reduces conservativism at low speeds, improving maneuverability near obstacles.

We also use these values: s, s, s, , , , , , and . For collision checking, we overapproximate the Fetch’s links with cylinders of radius 0.146 m. For further discussion of design choices and hyperparamters, see Appendix D-F.

### V-B Simulations

#### Setup

We created two sets of scenes. The first set, Random Obstacles, shows that ARMTD can handle arbitrary tasks (see Fig. 3). This set contains 100 tasks with random (but collision-free) start and goal configurations, and random box-shaped obstacles. Obstacle side lengths vary from to cm, with 10 scenes for each .

The second set, Hard Scenarios, shows that ARMTD guarantees safety where CHOMP converges to an unsafe trajectory. There are seven tasks in the Hard Scenarios set: (1) from below to above a table, (2) from one side of a wall to another, (3) between two vertical posts, (4) from one set of shelves to another, (5) from inside to outside of a box on the ground, (6) from a sink to a cupboard, (7) through a small window. These scenarios are shown in Fig. 4 in Appendix D-E.

#### Results

Table I presents ARMTD (with a straight-line HLP) and CHOMP’s results for the Random Obstacles scenarios. ARMTD reached goals and had crashes, meaning ARMTD stopped safely times without finding a new safe trajectory. CHOMP reached goals and had crashes. CHOMP always finds a trajectory, but not necessarily a collision-free one; it can converge to infeasible solutions because it considers a non-convex problem with obstacles as areas of high cost (not as hard constraints). We did not attempt to tune CHOMP to only find feasible plans (e.g., by buffering the arm), since this incurs a tradeoff between safety and performance. Note, in MoveIt, infeasible CHOMP plans are not executed (if detected by an external collision-checker).

We report the mean solve time (MST) of ARMTD over all planning iterations, while the MST for CHOMP is the mean over all 100 tasks. Directly comparing timing is not possible since ARMTD and CHOMP use different planning paradigms; we report MST to confirm ARMTD is capable of real-time planning (note that that ARMTD’s MST is less than ).

We also report the mean normalized path distance (MNPD) of the plans produced by each planner (the mean is taken over all 100 tasks). The normalized path distance is a path’s total distance (in configuration space), divided by the distance between the start and goal. For example, the straight line from start to goal has a (unitless) normalized path distance of . ARMTD’s MNPD is smaller than CHOMP’s, which may be because CHOMP’s cost rewards path smoothness, whereas ARMTD’s cost rewards reaching an intermediate waypoint at each planning iteration (note, path smoothness could be included in ARMTD’s cost function).

Table II presents results for the Hard Scenarios. With the straight-line HLP, ARMTD does not complete any of the tasks but also has no collisions. With the RRT* HLP [11], ARMTD completes scenarios. CHOMP converges to trajectories with collisions in all of the Hard Scenarios.

### V-C Hardware

See our video: youtu.be/ySnux2owlAA. ARMTD completes arbitrary tasks while safely navigating the Fetch arm around obstacles in scenarios similar to Hard Scenarios (1) and (4). We demonstrate real-time planning by suddenly introducing obstacles (a box, a vase, and a quadrotor) in front of the moving arm. The obstacles are tracked using motion capture, and treated as static in each planning iteration. Since ARMTD performs receding-horizon planning, it can react to the sudden obstacle appearance and continue planning without crashing.

## Vi Conclusion

This work proposes ARMTD as a real-time, receding-horizon manipulator trajectory planner with safety guarantees. The method proposes novel reachable sets for arms, which enable safety. ARMTD can enforce safety on top of an unsafe path planner such as RRT*, shown in both simulation and on hardware. Of course, ARMTD has limitations: it may not perform in real time without parallelization, is only demonstrated on 6-DOF planning problems, and has not yet been demonstrated planning around humans. However, because ARMTD uses time-varying reachable sets, it can readily extend to dynamic environments, uncertainty such as tracking error, and planning with grasped objects. The results in this work show promise for practical, safe robotic arm trajectory planning.

\printbibliography

## Appendix A Proofs

Here, we provide the proof of each mathematical claim in the paper, plus a short explanation of how each claim is useful.

First, we examine the structure of the JRS zonotope representation. This structure enables the creation of fully--sliceable generators when we use the JRS to produce rotatotopes. That is, this lemma enables us to slice the arm’s RS to find subsets corresponding to particular trajectory parameters.

###### Lemma Iv-A2.

There exist that overapproximate as in (9) such that, for each , has only one generator with a nonzero element, equal to , in the dimension corresponding to , and only one (distinct) generator with a nonzero element, , for .

###### Proof.

Given , the subsequent zonotope is computed as , where is found by linearizing the dynamics (5) at and is a set that overapproximates the linearization error and the states reached over the interval [3, Section 3.4.1]. This linearized forward-integration and error-bounding procedure is applied to to produce , and so on, to compute all in (9). Since , we have that equals in the dimensions (and therefore each does as well, and similarly for ). Since the zero dynamics have no linearization error, one can define to have zero volume in the dimensions [3, Proposition 3.7], meaning no generator of any has a nonzero element in the dimensions, except for and (which are defined with such nonzero elements). ∎

We now note that all rotation matrices are contained in the matrix zonotopes , which are produced by slicing and reshaping the JRS zonotopes. This enables us to conservatively approximate the forward occupancy map.

###### Lemma Iv-A2.

For any parameterized trajectory with , every .

###### Proof.

By [3, Thm. 3.3 and Prop. 3.7], all values attained by the sines and cosines of the joint angles are contained in each . By Alg. 1 and (11), each only contains the values of sine and cosine of for which . Since only reshapes , the proof is complete. ∎

The following lemma confirms that the product of multiple matrix zonotopes times a zonotope is still a rotatotope. This is necessary to overapproximate the forward occupancy map, wherein the arm’s joint rotation matrices are multiplied together (and, analogously, the matrix zonotopes are multiplied together).

###### Lemma Iv-A2.

A matrix zonotope times a rotatotope is a rotatotope.

###### Proof.

This follows from the rotatotope definition. ∎

We use the Minkowski sums of zonotopes and rotatotopes to enable stacking, which is how we build an RS of the entire arm from the low-dimensional JRSs. We also use the Minkowski sum to dilate obstacles, which is necessary to check for intersection with our arm’s RS per Lem. IV-B2 (which we do not prove here, as it is proven in [8]).