Learning from Successful and Failed Demonstrations via Optimization

The algorithm starts by receiving a set of labeled demonstrations $\mathcal{D}$ that includes two classes of successful and failed demonstrations, $\mathcal{D}=\{\mathcal{D}_{s},\mathcal{D}_{f}\}$. We assume these subsets include $M$ and $N$ demonstrations denoted by $\mathcal{D}_{s}=\{\bm{X}_{s}^{1},\dots,\bm{X}_{s}^{M}\}$ and $\mathcal{D}_{f}=\{\bm{X}_{f}^{1},\dots,\bm{X}_{f}^{N}\}$, respectively. Later, we show that our approach can handle cases where either one of these subsets is empty.

Each demonstration in $\mathcal{D}$ is a discrete finite-length trajectory $\bm{X}^{j}=[\bm{x}_{1},\bm{x}_{2},...,\bm{x}_{T}]^{\top}\in\mathbb{R}^{T\times n}$ in robot task space where $\bm{x}_{t}=[x_{t(1)},x_{t(2)},...,x_{t(n)}]\in\mathbb{R}^{n}$ is a single $n$-D observation at time $t$. We use kinesthetic teaching for capturing demonstrations, however, any other demonstration technique such as shadowing and teleoperation can also be used. We then align the raw demonstrations through interpolation and resampling. Other methods such as Dynamic Time Warping (DTW) [10] also could be utilized. We also assume that the human teacher constructs the dataset $\mathcal{D}$ by labeling each collected demonstration either as successful or failed.

After constructing the demonstration set $\mathcal{D}$, we estimate the joint distribution $p(t,x)$ between spatial samples $x$ and time $t$ for each subset $\mathcal{D}_{s}$ and $\mathcal{D}_{f}$ independently. Although these joint distributions can be estimated using various probabilistic approaches, we use a mixture model of $K$ $n$-dimensional Gaussian components defined by the joint distribution $p(t,x)=\sum_{k=1}^{K}\pi^{k}p(t,x|k)$, where $\pi^{k}$ and $p(t,x|k)\sim\mathcal{N}(.;\mu^{k},\Sigma^{k})$ denote the prior and conditional distribution, respectively. We encode each subset of temporally-aligned demonstrations as a Gaussian mixture model (GMM) for which the prior $\pi^{k}$, mean $\hat{\mu}^{k}=[\mu_{t}^{k},\mu_{x}^{k}]^{\top}$ and conditional covariances $\hat{\Sigma}^{k}=\begin{bmatrix}\Sigma_{t,t}&\Sigma_{t,x}\\ \Sigma_{x,t}&\Sigma_{x,x}\end{bmatrix}$ can be estimated using the Expectation-Maximization algorithm [9]. This process results in two sets of GMM parameters $\mathcal{P}_{s}=\{\pi_{s}^{1},\dots,\pi_{s}^{K_{s}};\hat{\mu}_{s}^{1},\dots,\hat{\mu}_{s}^{K_{s}};\hat{\Sigma}_{s}^{1},\dots,\hat{\Sigma}_{s}^{K_{s}}\}$ and $\mathcal{P}_{f}=\{\pi_{f}^{1},\dots,\pi_{f}^{K_{f}};\hat{\mu}_{f}^{1},\dots,\hat{\mu}_{f}^{K_{f}};\hat{\Sigma}_{f}^{1},\dots,\hat{\Sigma}_{f}^{K_{f}}\}$ representing the maximum likelihood solution for the corresponding subset of demonstrations. The optimum number of Gaussian components for each subset ($K_{s}$ and $K_{f}$) that maximizes the likelihood while minimizing the number of parameters can be determined using Bayesian Information Criterion (BIC) [11].

Given a time vector and a learned GMM parameter set, like [9], we can use Gaussian mixture regression (GMR) to construct a mean value $m_{t}=\mathbb{E}[x|t]$ and a corresponding covariance matrix $\Sigma_{t}=\text{Var}[x|t]$ at each time step $t$. The mean and covariances can be calculated as $m_{t}=\sum_{k=1}^{K}\beta^{k}(\mu^{k}+\Sigma^{k}_{x,t}(\Sigma^{k}_{t,t})^{-1}(t-\mu_{t}^{k}))$ and $\Sigma_{m}=\sum_{k=1}^{K}(\beta^{k})^{2}(\Sigma_{x,x}-\Sigma_{x,t}(\Sigma_{t,t})^{-1}\Sigma_{t,x})$, where $\beta^{k}=p(t|k)/\sum_{l=1}^{K}p(t|l)$. We use a single time vector and apply GMR to the learned GMM parameter sets $\mathcal{P}_{s}$ and $\mathcal{P}_{f}$ independently. As a result, we obtain two mean trajectories $\bm{m}_{s}$ and $\bm{m}_{f}$ together with their corresponding covariance matrices $\bm{\Sigma}_{f}$ and $\bm{\Sigma}_{s}$.

In the next step, we use the estimated joint distributions to construct two quadratic cost functions $J^{Q}_{s}$ and $J^{Q}_{f}$ using:

To reproduce trajectories that resemble successful demonstrations, our approach penalizes deviations from the conditional mean of the successful subset by minimizing $J^{Q}_{s}$, and rewards deviations from the conditional mean of the failed subset by maximizing $J^{Q}_{f}$. The combined cost can be obtained through scalarization as $J^{Q}=J^{Q}_{s}-J^{Q}_{f}$.

To find an optimal reproduction of the demonstrated skill, we formalize the optimization problem using (2) and (3) as

In the first experiment, we show that our approach can deal with (i) an empty failed set, (ii) an empty successful set, and (iii) when both sets include demonstrations of the skill. As illustrated in Fig. 4(a), we recorded and labeled two successful and two failed demonstrations. The statistical mean for each set was also estimated as explained in Sec. III-B and plotted. It can be seen that all the demonstrations approach the same final point. However, to show the generalization capabilities of our approach, we conditioned our algorithm to an initial point which is close to the failed mean, a via-point, and the common endpoint.

The first reproduction (dotted) was generated using only successful demonstrations, assuming the failed set was empty. This reproduction satisfies three spatial constraints while following the GMR mean of successful demonstrations. Since this trajectory does not use the information from the failed demonstration set, it passes through the failed set from the initial point to the via-point and hence could be considered incorrect by the user. The next reproduction (dashed) was generated using only the failed demonstration set, assuming the successful set was empty. This reproduction avoids the failed demonstrations while satisfying the constraints but cannot imitate the desired shape of the skill without knowledge of the successful demonstration set. The final reproduction (solid) uses both failed and successful demonstration sets. Since the initial point is close to the failed set, the algorithm reproduces an optimum solution that diverges from the failed set while at the same time satisfying the shape of the successful set and the given constraints.

This experiment shows the importance of considering failed demonstrations in a specific task where a reaching skill was demonstrated in the presence of an obstacle. We assume that the obstacle is unknown and undetectable by the robot. It can be seen in Fig. 1 that the user has given two demonstrations that avoid the obstacle (successful set) and two demonstrations that collide with the obstacle (failed set). Our results show that given only the successful demonstration set, the algorithm generates a trajectory that tries to preserve a mean between the two demonstrations and hence collides with the obstacle. However, when the algorithm uses information from both the failed and successful demonstrations, it learns to generate trajectories that avoid the obstacle. It should be noted that we do not rely on any object detection or obstacle avoidance method, instead assuming that the information about the obstacle was only provided through failed demonstrations. Thus, to guarantee that the reproductions of the skill avoid the obstacle, the given failed set should cover the boundaries of the obstacle. Alternatively, different solutions can be found by tuning the hyper-parameters of the algorithm accordingly.

In this experiment, we compared TLFSD against Gaussian Mixture Models/Gaussian Mixture Regression with weighted Expectation Maximization (GMM/GMR-wEM) [3], which was designed for iterative skill refinement. GMM/GMR-wEM enables the user to refine a skill, by assigning higher weights to newer (more optimal) demonstrations and forgetting older (sub-optimal) ones. We conducted an experiment on a pushing skill for closing a drawer as can be seen in Fig. 4(b) and 4(c). This task has an implicit constraint that requires the trajectory to remain horizontal when pushing the drawer. Note that we did not impose this constraint on the optimization problem and expect the algorithm to learn it from demonstrations. As depicted in Fig. 4(b), we recorded three demonstrations that fail to complete the task because of an upward movement at the end of the trajectory and one successful demonstration. Given these demonstrations sets, TLFSD is able to find a reproduction that successfully completes the task without any iteration.

We then repeated the experiment using GMM/GMR-wEM by assigning a higher weight to the successful demonstration and smaller weights to failed ones (GMM/GMR-wEM weights cannot be negative). It can be seen that GMM/GMR-wEM was not able to pull the regression mean towards the successful demonstration. After adding two additional successful demonstrations, as shown in Fig. 4(c), the reproduced trajectory using GMM/GMR-wEM was able to successfully close the drawer.

Additionally, we compared TLFSD against two conventional single-demonstration LfD representations: Dynamic Movement Primitives (DMPs) [16] and Laplacian Trajectory Editing (LTE) [17]. Neither DMPs nor LTE has been designed to encode failed demonstrations, and they are only given information of successful demonstrations. As depicted in Fig. 4(d), a single successful and a single failed demonstration of a 2D writing skill were provided. The endpoints of the two demonstrations are the same, while the initial point during reproduction is constrained to the initial point of the failed demonstration. We use three dissimilarity metrics: Sum of Squared Errors (SSE), Swept Error Area (SEA) [18], and SSE in curvature space (CRV). These metrics were selected as they each measure different properties: SSE measures spatial and temporal displacement, SEA measures spatial displacement invariant of time, and CRV measures the difference in curvature between the two trajectories. Fig. 4(d) depicts reproductions using DMP, LTE, and TLFSD. Results of the comparison between these reproductions and the successful demonstration are reported in Table I. Based on SSE and SEA, TLFSD generates a reproduction with higher similarity in Cartesian space. In curvature space, however, TLFSD is the most dissimilar of the three reproductions which may be considered as failure by the user, especially in a writing task where shape curvature must be preserved. Based on CRV, the LTE reproduction is best, because LTE encodes the skill in Laplacian coordinates and preserves shape properties including curvature. In Section V, we propose an extension of TLFSD to address this shortcoming.

In this experiment, we employ TLFSD in an iterative learning manner to find a successful reproduction when only failed demonstrations are given. As it can be seen in Fig. 6(left) a single demonstration of a skill was provided which collides with an obstacle. The algorithm was not given any information regarding the obstacle, only the demonstration was labeled as failed. The first reproduction in Fig. 6(left) that satisfies the initial and final conditions also collided with the obstacle and was labeled as failed by the user. The failed reproduction then was added as a new demonstration in the failed set. As shown in Fig. 6(middle) the algorithm incorporates the new observation into the model and generates a ‘better’ reproduction. After three iterations, the algorithm generates a reproduction which does not collide with the obstacle, resulting in a successful reproduction of the skill as seen in Fig. 6(right). This experiment shows that TLFSD is able to reproduce better trajectories (i.e., trajectories that diverge from failed demonstrations) iteratively using labels provided by the human user.

We tested TLFSD in a real-world 3D reaching task in an environment similar to Fig. 1(right). As illustrated in Fig. 5(a), first we captured four demonstrations and labeled them as successful. Four reproductions of the skill using TLFSD from different initial points are plotted. All reproductions were able to successfully achieve the goal of the skill. Then, the box shown in Fig. 5(b) was added to the scene as an obstacle that intersected with some of the given demonstrations which were then labeled as failed. New TLFSD reproductions from the initial point of each given demonstration were generated, all of which adapted to be able to complete the task and avoid the obstacle²²2Accompanying video at: https://youtu.be/sRdOm_9nJ8g.

Additionally, we tested a pushing skill in 3D similar to the skill shown in Fig. 4(b) and 4(c). In this experiment, as shown in Fig. 5(c) and 5(d), two successful and two failed demonstrations were given. The two failed demonstrations were unable to complete the skill successfully as they raise in elevation before fully closing the drawer. The TLFSD reproduction is able to complete the task by encoding the important implicit features of the movement from successful and failed demonstrations. Fig. 5(d) shows the execution of the successful reproduction on a UR5e manipulator.