This section describes the methods in DJ-GOMP. Underlying DJ-GOMP is a jerk- and time-optimizing constrained motion planner based on an SQP. Because of the complexity of solving this SQP, computation time can far exceed the trajectory’s execution time. DJ-GOMP uses this SQP on a random set of pick-and-place inputs to generate training data (trajectories) to train a neural network. During pick-and-place operation, DJ-GOMP uses the neural network to compute an approximate trajectory for the given pick and place frames, which it then uses to warm start the SQP.

### Jerk- and time-optimized trajectory generation

To generate a jerk- and time-optimized trajectory, DJ-GOMP extends the SQP formulated in GOMP (*2*). The solver for this SQP, following the method in TrajOpt (*3*) and summarized in Algorithm 1, starts with a discretized estimate of the trajectory τ as a sequence of *H* waypoints after the starting configuration, in which each waypoint represents the robot’s configuration **q**, velocity **v**, acceleration **a**, and jerk **j** at a moment in time. The waypoints are sequentially separated by *t*_{step} seconds. This discretization is collected into **x**^{(0)}, where the superscript represents a refinement iteration. Thus

The choice of *H* and *t*_{step} is application specific, although in physical experiments, we set *t*_{step} to match (an integer multiple of) the control frequency of the robot, and we set *H* such that *H* · *t*_{step} is an estimate of the upper bound of the minimum trajectory time for the workspace and task input distribution.

The initial value of **x**^{(0)} seeds (or warm starts) the SQP computation. Without the approximation generated by the neural network (e.g., for training data set generation), this trajectory can be initialized to all zeros. In practice, the SQP can converge faster by first computing a trajectory between inverse kinematic solutions to **g**_{0} and **g*** _{H}* with only the convex kinematic and dynamic constraints (described below).

In each iteration *k* = (0,1,2, …, *m*) of the SQP, DJ-GOMP linearizes the nonconvex constraints of obstacles and pick-and-place locations and solves a QP of the following form

$$\mathrm{s}.\mathrm{t}.\text{Ax}\le \mathrm{b}$$

where **A** defines constraints enforcing the trust region, joint limits, and dynamics, and **P** is defined such that **x**^{T}**Px** is a sum-of-squared jerks. To enforce the linearized nonconvex constraints, DJ-GOMP adds constrained nonnegative slack variables penalized using appropriate coefficients in **p**. As DJ-GOMP iterates over the SQP, it increases the penalty term exponentially, terminating on the iteration *m* at which **x**^{(m)} meets the nonconvex constraints.

**Algorithm 1:** Jerk-limited Motion Plan

**Require:** **x**^{(0)} is an initial guess of the trajectory, *h* + 1 is the number of waypoints in **x**^{(0)}, *t*_{step} is the time between each waypoint, **g**_{0} and **g*** _{H}* are the pick and place frames, β

_{shrink}∈ (0,1), β

_{grow}> 1, and γ > 1

1: μ ← initial penalty multiple

2: ϵ_{trust} ← initial trust region size

3: *k* ← 0

4: **P**, **p**, **A**, **b** ← linearize **x**^{(0)} as a QP

5: **while** μ < μ_{max}**do**

6:

${\mathbf{x}}^{(k+1)}\leftarrow \text{arg}{\text{min}}_{\mathbf{x}}\frac{1}{2}{\mathbf{x}}^{\top}\mathbf{\text{Px}}+{\mathbf{p}}^{\top}\mathbf{x}\mathrm{s}.\mathrm{t}.\mathbf{\text{Ax}}\le \mathbf{b}$ /* warm start with **x**^{(k)} */

7: **if** sufficient decrease in trajectory cost **then**

8: *k* ← *k* + 1 /*accept trajectory */

9: ϵ_{trust} ← ϵ_{trust}β_{grow} /* grow trust region */

10: **A**, **b** ← update linearization using **x**^{(k)}

11: **else**

12: ϵ_{trust} ← ϵ_{trust}β_{shrink} /* shrink trust region */

13: **b** ← update trust region bounds only

14: **if** ϵ_{trust} < ϵ_{min_trust}**then**

15: μ ← γμ /* increase penalty */

16: ϵ_{trust} ← initial trust region size

17: **p** ← update penalty in QP to match μ

18: **return x**^{(k)}

To enforce joint limits and dynamic constraints, Algorithm 1 creates a matrix **A** and a vector **b** that enforce the following linear inequalities on joint limits

$$-{\mathbf{v}}_{\text{max}}\le {\mathbf{v}}_{t}\le {\mathbf{v}}_{\text{max}}$$

$$-{\mathbf{a}}_{\text{max}}\le {\mathbf{a}}_{t}\le {\mathbf{a}}_{\text{max}}$$

$$-{\mathbf{j}}_{\text{max}}\le {\mathbf{j}}_{t}\le {\mathbf{j}}_{\text{max}}$$

and the following equalities that enforce dynamic constraints between variables

$${\mathbf{q}}_{t+1}={\mathbf{q}}_{t}+{t}_{\text{step}}{\mathbf{v}}_{t}+\frac{1}{2}{t}_{\text{step}}^{2}{\mathbf{a}}_{t}+\frac{1}{6}{t}_{\text{step}}^{3}{\mathbf{j}}_{t}$$$${\mathbf{v}}_{t+1}={\mathbf{v}}_{t}+{t}_{\text{step}}{\mathbf{a}}_{t}+\frac{1}{2}{t}_{\text{step}}^{2}{\mathbf{j}}_{t}$$

$${\mathbf{a}}_{t+1}={\mathbf{a}}_{t}+{t}_{\text{step}}{\mathbf{j}}_{t}$$

In addition, Algorithm 1 linearizes nonconvex constraints by adding slack variables to implement *L*_{1} penalties. Thus, for a nonconvex constraint *g _{j}*(

**x**) ≤

*c*, the algorithm adds the linearization

as a constraint in the form

$${\overline{g}}_{j}(\mathbf{x})-\mathrm{\mu}{y}_{j}^{+}+\mathrm{\mu}{y}_{j}^{-}\le c$$where μ is the penalty, and the slack variables are constrained such that

${y}_{j}^{+}\ge 0$and

${y}_{j}^{-}\ge 0$.

In the QP, obstacle avoidance constraints are linearized on the basis of the waypoints of the current iteration’s trajectory (Algorithm 2). To compute these constraints, the algorithm evaluates the spline

$${\mathbf{q}}_{\text{spline}}(s;t)={\mathbf{q}}_{t}+s{\mathbf{v}}_{t}+\frac{1}{2}{s}^{2}{\mathbf{a}}_{t}+\frac{1}{6}{s}^{3}{\mathbf{j}}_{t}$$between each pair of waypoints (**x*** _{t}*,

**x**

_{t + 1}) against a depth map of obstacles to find the time

*s*∈ [0,

*t*

_{step}) and corresponding configuration

that minimizes signed distance separation from any obstacle. In this evaluation, a negative signed distance means that the configuration is in collision. The algorithm then uses this

${\widehat{\mathbf{q}}}_{t}$ to computes a separating hyperplane in the form **n**^{T}**q** + *d* = 0. The hyperplane is either the top plane of the obstacle it is penetrating or the plane that separates

from the nearest obstacle (see Fig. 8). By selecting the top plane of the penetrated obstacle, this pushes the trajectory above the obstacle, which is a specialization of TrajOpt’s more general obstacle avoidance approach that is useful in bin picking. By selecting the hyperplane of the nearest obstacle, the algorithm keeps the trajectory from entering the obstacle. The linearize constraint for this point is

$${\mathbf{n}}^{\mathrm{T}}{\widehat{\mathbf{J}}}_{t}^{(k)}{\widehat{\mathbf{x}}}_{t}^{(k+1)}\ge -d-{\mathbf{n}}^{\mathrm{T}}p({\widehat{\mathbf{x}}}_{t}^{(k)})+{\mathbf{n}}^{\mathrm{T}}{\widehat{\mathbf{J}}}_{t}^{(k)}{\widehat{\mathbf{x}}}_{t}^{(k)}$$where

${\widehat{\mathbf{J}}}_{t}$is the Jacobian of the robot’s position at

${\widehat{\mathbf{q}}}_{t}$. Because

${\widehat{\mathbf{q}}}_{t}$and

${\widehat{\mathbf{J}}}_{t}$ are at an interpolated state between optimization variables at **x*** _{t}* and

**x**

_{t + 1}, linearizing this constraint requires computing the chain rule for the Jacobian

where

${\mathbf{J}}_{p}({\widehat{\mathbf{q}}}_{t})$ is the Jacobian of the position at configuration **q*** _{t}*, and

**J**

*(*

_{q}*s*) is the Jacobian of the configuration on the spline at

*s*

We observe that linearization at each waypoint will safely avoid obstacles with a sufficient buffer around obstacles (e.g., via a Minkowski difference with obstacles); however, slight variations in pick or place frames can shift the alignment of waypoints to obstacles. This shift of alignment (see Fig. 8C) can lead to solutions that vary disproportionately to small changes in input. Although this may be acceptable in operation, it can lead to data that can be difficult for a neural network to learn.

**Algorithm 2:** Linearize Obstacle-Avoidance Constraint

1: **for** *t* ∈ [0, *H*) **do**

2: (**n**_{min}, *d*_{min}) ← linearize obstacle nearest to *p*(**q*** _{t}*)

3: **q**_{min} ← **q**_{t}

4: **for all** *s* ∈ [0, *t*_{step}) **do** /* line search *s* to desired resolution */

5:

${\mathbf{q}}_{s}\leftarrow {\mathbf{q}}_{t}+s{\mathbf{v}}_{t}+\frac{1}{2}{s}^{2}{\mathbf{a}}_{t}+\frac{1}{6}{s}^{3}{\mathbf{j}}_{t}$6: (**n*** _{s}*,

*d*)← linearize obstacle nearest to

_{s}*p*(

**q**

*)*

_{s}7: **if**

**then** /* compare signed distance */

8: (**n**_{min}, *d*_{min}, **q**_{min}) ← (**n**_{s}, *d*_{s}, **q**_{s})

9: **J*** _{q}* ← Jacobian of

**q**

_{s}10: **J*** _{p}* ← Jacobian of position at

**q**

_{min}

11:

${\widehat{\mathbf{J}}}_{t}\leftarrow {\mathbf{J}}_{p}{\mathbf{J}}_{q}$12:

${b}_{t}\leftarrow -{d}_{\text{min}}-{\mathbf{n}}_{\text{min}}^{\top}p({\mathbf{q}}_{\text{min}})+{\mathbf{n}}_{\text{min}}^{\top}{\widehat{\mathbf{J}}}_{t}{\mathbf{x}}_{t}-\mathrm{\mu}{y}_{t}^{+}$/* lower bound with slack

${y}_{t}^{+}$*/

13: Add

$({\mathbf{n}}_{\text{min}}^{\top}{\widehat{\mathbf{J}}}_{t}{\mathbf{x}}_{t}\ge {b}_{t})$and

$({y}_{t}^{+}\ge 0)$to set of linearconstraints in QP

As with GOMP, DJ-GOMP allows degrees of freedom in rotation and translation to be added to start and goal grasp frames. Adding this degree of freedom allows DJ-GOMP to take a potentially shorter path when an exact pose of the end effector is unnecessary. For example, a pick point with a parallel-jaw gripper can rotate about the axis defined by antipodal contact points (see Fig. 2), and the pick point with a suction gripper can rotate about the normal of its contact plane. Similarly, a task may allow for a place point anywhere within a bounded box. The degrees of freedom about the pick point can be optionally added as constraints that are linearized as

$${\mathbf{w}}_{\text{min}}\le {\mathbf{J}}_{0}^{(k)}{\mathbf{q}}_{0}^{(k+1)}-({\mathbf{g}}_{0}-p({\mathbf{q}}_{0}^{(k)}))+{\mathbf{J}}_{0}^{(k)}{\mathbf{q}}_{0}^{(k)}\le {\mathbf{w}}_{\text{min}}$$where

${\mathbf{q}}_{0}^{(k)}$and

${\mathbf{J}}_{0}^{(k)}$are the configuration and Jacobian of the first waypoint in the accepted trajectory,

${\mathbf{q}}_{0}^{(k+1)}$ is one of variables the QP is minimizing, and **w**_{min} ≤ **w**_{max} defines the twist allowed about the pick point. Applying a similar set of constraints to **g*** _{H}* allows degrees of freedom in the place frame as well.

The SQP establishes trust regions to constrain the optimized trajectory to be within a box with extents defined by a shrinking trust region size. Because convex constraints on dynamics enforce the relationship between configuration, velocity, and acceleration of each waypoint, we observe that trust regions only need to be defined as box bounds around one of the three for each waypoint. In experiments, we established trust regions on configurations.

**Algorithm 3:** Time-optimal Motion Plan

**Require**: **g**_{0} and **g*** _{H}* are the start and end frames, γ > 1 is the search bisection ratio

1: *H*_{upper} ← fixed or estimated upper limit of maximum time

2: *H*_{lower} ← 3

3: *v*_{upper} ← ∞ /* constraint violation */

4: **while** *v*_{upper}> tolerance **do** /* find upper limit */

5: (**x**_{upper}, *v*_{upper}) ← call Alg. 1 with cold-start trajectory for *H*_{upper}

6: *H*_{upper} ← max(*H*_{upper} + 1, ⌈γ *H*_{upper}⌉)

7: **while** *H*_{lower} < *H*_{upper}**do** /* search for shortest *H* */

8: *H*_{min} ← *H*_{lower} + ⌊(*H*_{upper} − *H*_{lower})/γ⌋

9: (**x**_{mid}, *v*_{mid}) ← call Alg. 1 with warm-start trajectory **x**_{upper} interpolated to *H*_{mid}

10: **if v**_{mid}≤ tolerance **then**

11: (*H*_{upper}, **x**_{upper}, *v*_{upper}) ← (*H*_{mid}, **x**_{mid}, *v*_{mid})

12: **else**

13: *H*_{lower} ← *H*_{mid} + 1

14: **return x**_{upper}

To find the minimum time-time trajectory, J-GOMP searches for the shortest jerk-minimized trajectory that solves all constraints. This search, shown in Algorithm 3, starts with a guess of *H* and then performs an exponential search for the upper bound, followed by a binary search for the shortest *H* by repeatedly performing the SQP of Algorithm 1. The binary search warm starts each SQP with an interpolation of the trajectory of the current upper bound of *H*. The search ends when the upper and lower bounds of *H* are the same.

### Deep learning of trajectories

To speed up motion planning, we add a deep neural network to the pipeline. This neural network treats the trajectory optimization process as a function *f*_{τ} to approximate

where the arguments to the function are the pick and place frames, and the output is a discretized trajectory of variable length *H** waypoints, each of which has a configuration, velocity, acceleration, and jerk for all *n* joints of the robot. We assume that the neural network

can only approximate *f*_{τ}, thus

for some unknown error distribution *E*(τ). Hence, the output of

may not be dynamically or kinematically feasible. To address this potential issue, we use the network’s output to warm start a final pass through the SQP. This process can be thought of as polishing the output of the neural network approximation to overcome any errors in the network’s output.

In this section, we describe a proposed neural network architecture, its loss function, training and testing dataset generation, and the training process. Although we posit that a more general approximation could include the amount of pick and place degrees of freedom as inputs, for brevity, we assume that *f*_{τ} and its neural network approximation are parameterized by a preset amount of pick and place degrees of freedom. In practice, it may also be appropriate to train multiple neural networks for different parameterizations of *f*_{τ}.

*Architecture*

The deep neural network architecture we propose is depicted in Fig. 3. It consists of an input layer connected through four fully connected blocks to multiple output blocks. The input layer takes in the concatenated grasp frames

${[\begin{array}{cc}{\mathbf{g}}_{0}^{\mathrm{T}}& {\mathbf{g}}_{H}^{\mathrm{T}}\end{array}]}^{\mathrm{T}}$. Because the optimal trajectory length *H** can vary, the network has multiple output heads for each of the possible values for *H**. To select which of the outputs to use, we use a separate classification network with two fully connected layers with one-hot encoding trained using a cross-entropy loss.

We refer to the horizon classification and multiple-output network as a HYDRA (Horizon Yielding Distillation through Retained Activations) network. The network yields both an optimal horizon length and the trajectory for that horizon. To train this network (detailed below), the trajectory output layers’ activation values for horizons not in the training sample are retained using a zero gradient so as to weight the contribution of the layers during backprop to the input layers.

In experiments, a neural network with a single output head was unable to produce a consistent result for predicting varied length horizons. We conjecture that the discontinuity between trajectories of different horizon lengths made it difficult to learn. In contrast, we found that a network was able to accurately learn a function for a single horizon length but was computationally and space inefficient, because each network should be able to share information about the function between the horizons. This led to the proposed design in which a single network with multiple output heads shares weights through multiple shared input layers.

*Dataset generation*

We propose generating a training dataset by randomly sampling start and end pairs from the likely distribution of tasks. For example, in a warehouse pick-and-place operation, the pick frames will be constrained to a volume defined by the picking bin, and the place frames will be constrained to a volume defined by the placement or packing bin. For each random input, we generate optimized trajectories for all time horizons from *H*_{max} to the optimal *H**. Although this process generates more trajectories than necessary, generating each trajectory is efficient because the optimization for a trajectory of size *H* warm starts from the trajectory of size *H* + 1. Overall, this process is efficient and, with parallelization, can quickly generate a large training dataset.

This process can also help detect whether the analysis of the maximum trajectory duration was incorrect. If all trajectories are significantly shorter than *H*_{max}, then one may reduce the number of output heads. If any trajectory exceeds *H*_{max}, then the number of output heads can be increased.

We also note that in the case where the initial training data do not match the operational distribution of inputs, the result may be that the neural network produces suboptimal motions that are substantially, kinematically, and dynamically infeasible. In this case, the subsequent pass through the optimization (see “Fast pipeline for trajectory generation” section) will fix these errors, although with a longer computation time. We propose, in a manner similar to DAgger (*48*), that trajectories from operation can be continually added to the training dataset for subsequent training or refinement of the neural network.

*Training*

To train the network in a way that encourages matching the reference trajectory while keeping the output trajectory kinematically and dynamically feasible, we propose a multipart loss function ℒ. This loss function includes a weighted sum of MSE loss on the trajectory

${\mathcal{L}}_{\mathrm{T}}$, a boundary loss

${\mathcal{L}}_{\mathrm{B}}$, which enforces the correct start and end positions, and a dynamics loss

${\mathcal{L}}_{\text{dyn}}$that enforces the dynamic feasibility of the trajectory. The MSE loss is the mean of the sum of squared differences of the two vector arguments:

${\mathcal{L}}_{\text{MSE}}(\tilde{\mathbf{a}},\underset{\xaf}{\mathbf{a}})=\frac{1}{n}{\Sigma}_{i=1}^{n}{({\tilde{\mathbf{a}}}_{i}-{\underset{\xaf}{\mathbf{a}}}_{i})}^{2}$. The dynamics loss attempts to mimic the convex constraints of the SQP. Given the predicted trajectories

$\tilde{X}=({\tilde{\mathbf{x}}}^{{H}_{\text{min}}},\dots ,{\tilde{\mathbf{x}}}^{{H}_{\text{max}}})$, where

${\tilde{\mathbf{x}}}^{h}={(\tilde{\mathbf{q}},\tilde{\mathbf{v}},\tilde{\mathbf{a}},\tilde{\mathbf{j}})}_{t=0}^{h}$and the ground-truth trajectories from dataset generation

$\underset{\xaf}{X}=({\underset{\xaf}{\mathbf{x}}}^{{H}^{*}},\dots ,{\underset{\xaf}{\mathbf{x}}}^{{H}_{\text{max}}})$, the loss functions are

$${\mathcal{L}}_{\mathrm{T}}={\mathrm{\alpha}}_{q}{\mathcal{L}}_{\text{MSE}}(\tilde{\mathbf{q}},\underset{\xaf}{\mathbf{q}})+{\mathrm{\alpha}}_{v}{\mathcal{L}}_{\text{MSE}}(\tilde{\mathbf{v}},\underset{\xaf}{\mathbf{v}})+{\mathrm{\alpha}}_{a}{\mathcal{L}}_{\text{MSE}}(\tilde{\mathbf{\alpha}},\underset{\xaf}{\mathbf{a}})+{\mathrm{\alpha}}_{j}{\mathcal{L}}_{\text{MSE}}(\tilde{\mathbf{j}},\underset{\xaf}{\mathbf{j}})$$$${\mathcal{L}}_{\mathrm{B}}={\mathcal{L}}_{\text{MSE}}({\tilde{\mathbf{q}}}_{0},{\underset{\xaf}{\mathbf{q}}}_{0})+{\mathcal{L}}_{\text{MSE}}({\tilde{\mathbf{q}}}_{H},{\underset{\xaf}{\mathbf{q}}}_{H})$$

$$\begin{array}{c}{\mathcal{L}}_{\text{dyn}}=\frac{1}{h}{\displaystyle \underset{t=0}{\overset{h-1}{\Sigma}}}{\Vert {\tilde{\mathbf{q}}}_{t}+{t}_{\text{step}}{\tilde{\mathbf{v}}}_{t}+\frac{1}{2}{t}_{\text{step}}^{2}{\tilde{\mathbf{a}}}_{t}+\frac{1}{6}{t}_{\text{step}}^{3}{\tilde{\mathbf{j}}}_{t}-{\tilde{\mathbf{q}}}_{t+1}\Vert}^{2}\\ +\frac{1}{h}{\displaystyle \underset{t=0}{\overset{h-1}{\Sigma}}}{\Vert {\tilde{\mathbf{v}}}_{t}+{t}_{\text{step}}{\tilde{\mathbf{a}}}_{t}+\frac{1}{2}{t}_{\text{step}}^{2}{\tilde{\mathbf{j}}}_{t}-{\tilde{\mathbf{v}}}_{t+1}\Vert}^{2}\hfill \\ +\frac{1}{h}{\displaystyle \underset{t=0}{\overset{h-1}{\Sigma}}}{\Vert {\tilde{\mathbf{a}}}_{t}+{t}_{\text{step}}{\tilde{\mathbf{j}}}_{t}-{\tilde{\mathbf{a}}}_{t+1}\Vert}^{2}\hfill \\ +\frac{1}{h}{\displaystyle \underset{t=0}{\overset{h-1}{\Sigma}}}{\Vert \frac{1}{{t}_{\text{step}}}({\underset{\xaf}{\mathbf{j}}}_{t+1}-{\underset{\xaf}{\mathbf{j}}}_{t})-\frac{1}{{t}_{\text{step}}}({\tilde{\mathbf{j}}}_{t+1}-{\tilde{\mathbf{j}}}_{t})\Vert}^{2}\hfill \end{array}$$

$${\mathcal{L}}^{h}={\mathrm{\alpha}}_{\mathrm{T}}{\mathcal{L}}_{\mathrm{T}}^{h}+{\mathrm{\alpha}}_{B}{\mathcal{L}}_{B}^{h}+{\mathrm{\alpha}}_{\text{dyn}}{\mathcal{L}}_{\text{dyn}}^{h}$$

where values of α* _{q}* = 10, α

*= 1, α*

_{v}*= 1, α*

_{a}*= 1, α*

_{j}_{B}= 4 × 10

^{3}, and α

_{dyn}= 1 were chosen empirically. This loss is combined into a single loss for the entire network by summing the losses of all horizons while multiplying by an indicator function for the horizons that are valid

$$\mathcal{L}=\underset{h={H}_{\text{min}}}{\overset{{H}_{\text{max}}}{\mathrm{\Sigma}}}{\mathcal{L}}^{h}{\mathrm{\U0001d7d9}}_{[{\underset{\xaf}{H}}^{*},{H}_{\text{max}}]}(h)$$

Because the ground-truth trajectories for horizons shorter than *H** are not defined, we must ensure that some finite data are present so that the multiplication of an indicator value of 0 results in 0 (and not NaN). Multiplying by indicator function in this way results in a zero gradient for the part of the network that is not included in the trajectory data.

During training, we observed that the network would often exhibit behavior of coadaptation in which it would learn either

${\mathcal{L}}_{\mathrm{T}}$or

${\mathcal{L}}_{\text{dyn}}$ but not both. This showed up as a test loss for one going to small values, whereas the other remained high. To address this problem, we introduced dropout layers (*49*) with dropout probability *p*_{drop} = 0.5 between each fully connected layer, shown in Fig. 3. We annealed (*50*) *p*_{drop} to 0 over the course of the training to reduce the loss further.