**4. Benchmarking**

#### *4.1. Implementation Details*

The objective of this section is to compare the trajectories computed by Algorithm 1 with that obtained by re-solving the trajectory optimization for the perturbed parameters with warm-start initialization. We consider the same three benchmarks presented in Figures 1–3 implemented on a 7dof Franka Panda Arm, but for a diverse range of perturbations magnitude. For each benchmark, we created a data set of 180 trajectories by generating random perturbations in the task parameters. For the benchmark of Figure 1, the parameters are the joint angles, but in the following we use the forward kinematics to derive equivalent representation for the parameters in terms of end-effector position values.

Each joint trajectory is parameterized by a 50-dimensional vector of way-points. Thus, the underlying task constrained trajectory optimization involves a total of 350 variables. We use Scipy-SLSQP [3] to obtain the prior trajectory and also to re-solve the trajectory optimization for the perturbed parameters. We did our implementation in Python using Jax-Numpy [17] to compute the necessary Jacobian and Hessian matrices. We also used the just-in-time compilation ability of JAX to create an on-the-fly compiled version of our codes. The line-search in Algorithm 1 (line 2) was done through a parallelized search over a set of discretized *η* values. The entire implementation was done on a 32 GB RAM i7-8750 desktop with RTX 2080 GPU (8GB). To foster further research in this field and ensure reproducibility, we open-source our implementation for review at https://rebrand.ly/argmin-planner (First released on 15 September 2020).

#### *4.2. Quantitative Results*

**Orientation Metric:** For this analysis, we compared the pitch and roll angles at each time instant along trajectories obtained with Algorithm 1 and the resolving approach. Specifically, we computed the maximum of the absolute difference (or *L*∞ norm) of the two orientation trajectories. The yaw orientation in all these benchmarks was a free variable and is thus not included in the analysis. The results are summarized in Figures 4–6. The histogram plot in these figures are generated for the medium perturbation ranges (note the figure legends). For the Figure 1 benchmark related to cost function (13), Figure 4 shows that all the trajectories obtained by Algorithm 1 have *L*∞ norm of the orientation difference less than 0.1 rad. For the benchmark of Figure 2, which we recall involves perturbing the via-point of the end-effector trajectory, the histograms of Figure 5 show similar trends. All the trajectories computed by Algorithm 1 managed a similar orientation difference. For the benchmark of Figure 3 pertaining

to the perturbation of the final position, 69.41% of the trajectories obtained by Algorithm 1 managed to maintain a orientation difference of 0.1 rad with the resolving approach.

**Figure 4.** Performance of Algorithm 1 for different perturbation ranges on the benchmark of Figure 1 that involves perturbing the final joint configuration (recall cost function (13)). Note that the perturbation in the final joint is converted to position values by forward kinematics. The (**a**,**c**,**e**,**g**) column shows the histogram of orientation, smoothness and task residual ratio metrics for the medium range perturbation. The (**b**,**d**,**f**,**h**) column quantifies the metrics for different perturbation ranges.

**Figure 5.** Performance of Algorithm 1 for different perturbation ranges on the benchmark of Figure 2 that involves perturbing the via-point of the end-effector trajectory (recall cost function (14)). The (**a**,**c**,**e**,**g**) and (**b**,**d**,**f**,**h**) columns show similar benchmarking as those of Figure 4.

**Task residuals ratio metric:** For this analysis, we compare the task residual between trajectories obtained from Algorithm 1 and the resolving approach. For example, for the benchmark of Figure 1, we want the manipulator final configuration to be close to the specified value (recall cost (13)) while maintaining the desired orientation at each time instant. Thus, we compute the *L*<sup>∞</sup> residual of **q***<sup>t</sup>* − **q***<sup>m</sup>* for Algorithm 1 and compare it with that obtained from the resolving approach. Now, as previously, and to be consistent with the other benchmarks, we convert the residual of the joint angles to position values through forward kinematics. Similar analysis follow for the other benchmarks as well. For the ease of exposition, we divide the task residual of Algorithm 1 by that obtained with the resolving approach. A ratio greater than 1 implies that the former led to a higher task residual than the latter and vice-versa. Similarly, a ratio closer to 1 implies that both the approaches performed equally well.

The results are again summarized in Figures 4–6. From Figure 4, we notice that 97.05% of trajectories have a residual ratio less than 1.2. For the experiment involving via-point perturbation in Figure 5, the performance drops to 62.50% for the same value of residual ratio. Meanwhile, as shown in Figure 6, around 82.94% of the trajectories have a residual ratio less than 1.2 in the case of the final position perturbation benchmark of Figure 3.

**Velocity Smoothness Metric:** For this analysis, we computed the difference in the velocity smoothness cost (*L*<sup>2</sup> norm of first-order finite difference) between the trajectories obtained with Algorithm 1 and the resolving approach. The results are again summarized in Figures 4–6. For all the benchmarks, in around 65% of the examples, the difference was less than 0.05. This is 35% of the average smoothness cost observed across all the trajectories from both the approaches.

**Scaling with Perturbation Magnitude:** The line plots in Figures 4–6 represent the first quartile, median and the third quartile of the three metrics discussed above for different perturbation ranges.

For the benchmark of Figure 1, trajectories from Algorithm 1 maintains an orientation difference of less than 0.1 rad, with the trajectories of the resolving approach for perturbations as large as 40 cm. The difference in smoothness cost for the same range is also small, with the median value being in the order of 10−3. The median task residuals achieved by Algorithm 1 is only 2% higher than that obtained by the resolving approach. For the benchmark of Figure 2, the performance remains same on the orientation metric, but the median difference in smoothness cost and task residual ration increases to 0.04 and 9% for the largest perturbation range. The benchmark of Figure 6 follows a similar trend in orientation and smoothness metric, but performs significantly worse in task residuals. For the largest perturbation range, Algorithm 1 leads to 50% higher median task residuals. However, importantly, for perturbation up to 30 cm, the task residual ratio is close to 1, suggesting that Algorithm 1 performed as well as the resolving approach for these perturbations.

**Computation Time:** Table 1 contrasts the average timing of our Algorithm 1 with the approach of resolving the trajectory optimization with warm-start initialization. As can be seen, our Argmin differentiation based approach provides a worst-case speed up of 160x on the benchmark of Figure 3. For the rest of the benchmarks, this number varies between 500 to 1000. We believe that this massive gain in computation time offsets whatever little performance degradation in terms of orientation, smoothness, and task residual metric that Algorithm 1 incurs compared to re-solving the problem using warm-start. Note that the high computation time of the re-solving approach is expected, given that we are solving a difficult non-convex function over a long horizon of 50 steps resulting in 350 decision variables. Even highly optimized planners like [1] show similar timings on closely related benchmarks [18].

**Figure 6.** Performance of Algorithm 1 for different perturbation ranges on the benchmark of Figure 6 that involves perturbing the final end-effector position. (recall cost function (14)). The (**a**,**c**,**e**,**g**) and (**b**,**d**,**f**,**h**) columns show similar benchmarking as those of Figure 4.


**Table 1.** Computation times comparison between Algorithm 1 and resolving trajectory optimization approach on three benchmarks.

#### **5. Conclusions and Future Work**

We presented a fast, near real-time algorithm for adapting joint trajectories to task perturbation as high as 40 cm in the end-effector position, almost half the radius of the Franka Panda arm's horizontal workspace used in our experiments. By consistently producing trajectories similar to those obtained by resolving the trajectory optimization problem but in a small fraction of a time, our Algorithm 1 opens up exciting possibilities for reactive motion control of manipulators in applications like human–robot handover.

Our algorithm is easily extendable to other kind of manipulators. The only requirement is that we should know the forward kinematics of the manipulator. This would allow us to get the algebraic expressions for functions **o***e*(**q**) and **x***e*(**q**) in cost function (13) and (14), respectively. In our implementation, we derived the forward kinematics and **o***e*(**q**) and **x***e*(**q**) through the DH representation of the manipulator. The DH table is available for many commercial manipulators, e.g., UR5e besides the Franka Panda Arm used in our simulation.

Our algorithm does not depend on any specific sensing modality. For example, in collision avoidance applications, we assume that obstacle information is used by some higher level planners that provides intermediate collision-free points to the manipulator, which then uses the ArgMin differentiation to replan its prior trajectories.

There are several ways to improve our algorithm. First, the joint bounds can also be included as penalties in the cost function itself, in addition to being handled by projection (Line 11 in Algorithm 1). This would ensure that the gradient and Hessian of the optimal cost is aware of the joint limit bounds. Second, we can consider a low dimensional polynomial representation of the trajectories. For example, the joint trajectories can be represented by a 10th order Bernstein polynomial with the coefficients acting as the variables of the optimization problem. This would drastically reduce the computation cost of obtaining the Hessian of the optimal cost as compared to current way-point paramaterization of the joint trajectory that requires around 50 variables to represent one joint trajectory.

In future works, we will extend our formulation to problems with dynamic constraints, such as torque bounds. We conjecture that by coupling the way-point parametrization with a multiple-shooting like approach, we can retain the constraints as simple box-bounds on the decision variables and consequently retain the computational structure of the Algorithm 1. We are also currently evaluating our algorithm's performance on applications such as autonomous driving.

**Author Contributions:** Conceptualization, S.S., M.B., A.K.S. and K.M.K.; Methodology, S.S., M.B., A.K.S. and K.M.K.; Software, S.S., M.B. and H.M.; Validation, S.S., M.B. and H.M.; formal analysis, S.S. and M.B.; investigation, S.S., M.B. and A.K.S.; resources, S.S., M.B. and A.K.S.; data curation, S.S., M.B. and H.M.; writing—original draft preparation, A.K.S., S.S., M.B., K.K. and K.M.K.; writing—review and editing, S.S., M.B., K.K. and K.M.K.; visualization, S.S., M.B. and H.M.; supervision, A.K.S. and K.M.K.; project administration, A.K.S. and K.M.K.; funding acquisition, A.K.S. and K.K. All authors have read and agreed to the published version of the manuscript.

**Funding:** The work was supported in part by thw European Social Fund via the "ICT programme" measure, by grant PSG753 from the Estonian Research Council, by AI & Robotics Estonia (AIRE), the Estonian candidate for European Digital Innovation Hub, funded by the Ministry of Economic Affairs and Communications in Estonia.

**Data Availability Statement:** Codes to reproduce the results are available at https://rebrand.ly/ argmin-planner (accessed on 1 August 2020).

**Conflicts of Interest:** The authors declare no conflict of interest. The funders had no role in the design of the study; in the collection, analyses, or interpretation of data; in the writing of the manuscript, or in the decision to publish the results.

#### **References**

