Dynamic movement primitives based on positive and negative demonstrations

Abstract

Dynamic motion primitive has been the most prevalent model-based imitation learning method in the last few decades. Gaussian mixed regression dynamic motion primitive, which draws upon the strengths of both the motion model and the probability model to cope with multiple demonstrations, is a very practical and conspicuous branch in the dynamic motion primitive family. As Gaussian mixed regression dynamic motion primitive only learns from expert demonstrations and requires full environmental information, it is incapable of handling tasks with unmodeled obstacles. Aiming at this problem, we proposed the positive and negative demonstrations-based dynamic motion primitive, for which the introduction of negative demonstrations can bring additional flexibility. Positive and negative demonstrations-based dynamic motion primitive extends Gaussian mixed regression dynamic motion primitive in three aspects. The first aspect is a new maximum log-likelihood function that balances the probabilities on positive and negative demonstrations. The second one is the positive and negative demonstrations-based expectation–maximum, which involves iteratively calculating the lower bound of a new Q-function. And the last is the application framework of data set aggregation for positive and negative demonstrations-based dynamic motion primitive to handle unmodeled obstacles. Experiments on several typical robot manipulating tasks, which include letter writing, obstacle avoidance, and grasping in a grid box, are conducted to validate the performance of positive and negative demonstrations-based dynamic motion primitive.

Introduction

Traditional robots have aided humans to accomplish laborious work in factories for several decades. They need merely repeat some preplanned motions in limited or structured environments. In the last few years, the development of deep learning has facilitated substantial progress in the perception abilities of robots. Some challenges once thought unsolvable, such as 3D object recognition and localization, 6D pose estimation, and grasp position prediction, have been solved partly with deep convolutional neural networks. ¹ With a stronger perception, the robots can observe and learn skills from human actions actively and directly. That is the skill transfer learning (STL), or imitation learning, of robots.

STL models can be split into three categories ^2,3 : motion, policy, and procedural models. Motion models, such as the dynamic movement primitives (DMPs) ⁴ or hidden Markov model (HMM), ⁵ treat STL tasks as trajectory encoding and recovering problems. Once the analytical and structured task configuration is available, either predefined manually or fed via additional independent perception modules, motion models are able to learn skills from a few or even only one demonstration. By contrast, policy models, ^6,7 such as behavior clone or generative adversarial imitation learning, handle STL tasks as sequential decision problems. Policy models output commands for the actuators (joint angles, Cartesian position, or servo torques) at each control step. Policy models have a superior capability in feature expression and generalization and can produce end-to-end policies. Nonetheless, they rely on numerous demonstrations and a long learning time. Procedural models represent ² the task structure and configuration at a higher level of semantics, making them more suitable for logical inference in complex tasks. A perfect STL model should possess all abilities and advantages of all three models. Motion models, on the other hand, are most likely to be widely used shortly soon.

DMP ^8,9 is the most prevalent motion model nowadays. With a second-order damp-string system that is driven by a virtual force term, DMP encodes the demonstrations first and transfers the shape information to new tasks. DMP and its improved variants, such as Gaussian mixed regression (GMR) DMP (GMRDMP) ⁴ and compliant parametric DMP (CPDMP), ¹⁰ have been thoroughly demonstrated to be capable of extracting and transferring shape information with a few expert demonstrations. When coping with unexpected obstacles, a compensatory force associated with the obstacle can be introduced into the damp-string system to adjust the recovered trajectory. However, the structured representation for unexpected obstacles is not always available in practice, which the DMP methods cannot handle. Could we address this issue through human intervention?

GMRDMP is a very typical and practical member of the DMP family. It draws upon the strengths of both the motion model and the probability model to deal with multiple demonstrations. Thus, we have chosen GMRDMP as the basic method to be improved. Because the optimization problem behind GMRDMP is to recover the demonstration as much as possible, there is no space for the user to modify the recovered trajectory by adjusting hyperparameters. It means that GMRDMP lacks a flexible interaction channel for the user to adjust the recovered trajectory.

Aiming at this problem, we extend GMRDMP with negative demonstrations to improve its flexibility, that is, the positive and negative demonstration-based DMP (PNDMP). Positive demonstrations are the original expert demonstrations, whereas negative demonstrations are failed executions by either the experts or the robot itself. Positive ones highlight what the student robot should perform, while negative ones indicate what it cannot. In supervised learning and reinforcement learning, negative samples are necessary for providing specific annotation information or reward value. Particularly, some researchers have utilized them to filter the nonexpert demonstrations as a preprocessing step for motion models. ^11,12 However, to the best of the authors’ knowledge, few studies have sought to directly improve DMP with negative demonstrations.

PNDMP consists of three essential extensions to GMRDMP:

A new log-likelihood function is defined for PNDMP to balance the probabilities on positive and negative demonstrations. With it, we can establish an adjusting mechanism for the recovered trajectories.

A positive and negative demonstrations-based expectation–maximum (PNEM) algorithm is proposed to solve the optimization problem with respect to the new log-likelihood function. A new Q-function and its lower bound are deduced to obtain an iterative solution.

PNDMP is applied in the data set aggregation (DAgger) ¹³ framework to tackle unmodeled obstacles. In the PNDMP DAgger framework, negative demonstrations implying unmodeled obstacles are gathered and annotated during the training process, and then used to retrain the Gaussian mixed model (GMM). DAgger is a fundamental paradigm designed primarily for policy models, however, it has not been applied to motion models yet.

The rest of the article is organized as follows. The second section reviews recent developments of DMP. The third section introduces some prerequisite knowledge and arise the optimization problem of PNDMP. The fourth section presents the main algorithms. Experimental results are analyzed in the fifth section. At last, some conclusions are drawn in the sixth section.

Related work

DMP uses a spring-damping dynamic system to fit the evolution of the observed states. ^8,14 The dynamic system is driven by a nonlinear force term that is encoded in the form of a weight vector and a group of Gaussian basis functions. Typically, the weights are optimized via some regression methods such as linear weighted regression, ¹⁵ linear weighted projection regression, ¹⁶ and Gaussian process regression (GPR). ¹⁷ DMP can ensure that the system converges to a new target state theoretically. To consider the random characteristics of multiple demonstrations, probabilistic models have been introduced into the DMP framework. With an appropriate probabilistic model, such as GMM, ¹⁸ HMM, ¹⁰ hierarchical Bayesian model, ^19,20 and kernel model, ²¹ the spatial and temporal correlations among demonstrations can be represented by a few parameters. A typical probabilistic DMP framework can be summarized as three steps. First, the virtual force terms of all demonstrations are extracted via the dynamic equation of DMP. Second, force trajectories are encoded with a probabilistic model. Finally, the virtual force is retrieved via the probabilistic model for a new task, based on which the motion is then evolved via the dynamic equation of DMP. A recent comprehensive survey about the DMP family can be referred to in the study of Saveriano et al. ²²

Many trajectory encoding methods have been proposed to improve the adaptability of probabilistic DMP. GMRDMP, ⁴ which adopts the expectation–maximization (EM) algorithm to obtain a GMM of the virtual force and the GMR algorithm to recover the force trajectories, is a prevalent probabilistic DMP method. Gribovskaya et al. ¹⁸ used a one-order nonlinear multivariate system to replace the spring-damping system, and this system can ensure robustness to external spatial-temporal perturbations through online adaptation of a motion. Colome and Torras ²³ proposed a probabilistic dimensionality reduction method to decrease the computation burden of probabilistic movement primitives and extended it to learn the joint couplings. Tanwani and Calinon ²⁴ designed a semi-tied GMM to associate or tie the covariance matrices of the mixture model with a common latent space to cope with perturbations. Lioutikov et al. ²⁵ segmented the demonstrations and established a primitive library, which can be reused in different tasks. Li et al. ²⁶ introduced a dynamic time warp into the GMRDMP ⁴ to ensure that the sampling time is identical.

Meanwhile, some researchers are devoted to enhancing the robustness when the new task is beyond the demonstration regions. Considering that some shape information would be lost when the target state is not covered by the demonstrations, Calinon ²⁷ proposed the multi-coordinate GMRDMP. Huang et al. ²⁸ introduced confidence weights for each frame of multi-frame GMRDMP and designed an iterative algorithm to choose the frames highly relevant to a task. Pervez et al. ²⁹ encoded the force term of visual demonstrations directly with a convolutional neural network and changed the traditional discriminative model to a generative model ³⁰ to solve the extrapolation problem. The model was trained with synthetic demonstrations to improve its generalization capability. Mei et al. ³¹ developed a continuous DMP to track dynamic targets, which was referred to as an online navigation method. Karlsson et al. ³² proved the exponential convergence of temporally coupled DMP which is used to solve perturbations. Yang et al. ³³ proposed a framework that considers the trajectory tracking module and DMP simultaneously.

Some methods for establishing the relationship between task variables and encoding parameters have also been proposed. Akgun and Thomaz ³⁴ designed a framework to estimate the actions and goals from demonstrations, which can serve for acquiring the relationship. Alizadeh et al. ³⁵ proposed a partial GPR algorithm for the situation that task parameters are partially observed. Liu et al. ³⁶ designed an approximation regression approach to learn the relationship and proposed a post-correction algorithm to improve accuracy.

Safe learning control has attracted considerable attention for a long time, especially in those fields which concern safety extremely. For different tasks, the safety criteria are different. There are four widely used criteria which include the stability of dynamic systems, ^37,38 obstacle avoidance, ³⁹ robustness to noise, ³⁹ and the gap between the training set and real tasks. ⁴⁰ The safety of learning control can be improved in three manners. The first is to utilize an external safety module ³⁹ or manual intervention ⁴¹ to abort or modify dangerous motions. The external module requires full obstacle information to evaluate the risk probability and generate a control command on the learning process with an independent algorithm, while the manual intervention requires the framework owns a channel of interaction. The second is to modify the learning process. ⁴² Guidance or regulation about the safe region is introduced into the learning strategy based on a specific safety model. Although this manner has been used in policy models, it has not yet been used in any motion models directly. The third is to modify the learning goal according to the safety criterion. ^43,44

For DMP, the safety criteria should be contained in the spring-damping system in the form of another additional force term. Pervez and Lee ³⁰ combined a generative model with DMP to learn the relation between external configuration and the model parameters, while Liu et al. ³⁶ used GPR to learn the relation. Uger and Girgin ¹⁰ extended the nonlinear shaping function to a parametric form using a parametric hidden Markov instead of a weighted sum of radial basis functions. Park et al. ⁴⁵ added the gradient of a potential field centered on the obstacle, that is, a repellent force, to the dynamic equation. Ginesi et al. ⁴⁶ proposed some super-quadric potential functions that include the velocity in the form of the potential of obstacles. As all these methods require full information about obstacles, they are unsuitable for tasks in which the structured representation of obstacles is not available.

Preliminaries and problem description

DMP can extract the shape information from one single demonstration, and GMRDMP is developed to learn from multiple demonstrations to improve the generality. In this section, discrete DMP and GMR are introduced first. Then, the reversibility and continuity of DMP and GMRDMP are discussed. At last, a formal representation of the model of PNDMP is presented.

In all figures of this article, “circle,” “X,” and “square” markers represent the starting, end, and target points of a trajectory, respectively.

Dynamic movement primitives

DMP models the kinematics of a demonstration as a spring-damping system and enforces a nonlinear virtual force to it to make the state converge to a target. Up to now, a variety of DMPs have been investigated. ²² In this article, we have considered a basic but relatively well-developed one. ⁴⁷ The dynamic system can be represented as

$\tau \ddot{x}=\alpha (\beta \left(x_{t_f}-x\right)-\dot{x})+f\left(s\right)$

1

where x and $\dot{x}$ are position and velocity, respectively; x ₀ and $x_{t_f}$ are the starting point and endpoint, respectively. $\tau$ is the scale coefficient of time, α is the spring coefficient, $\beta$ is the damping coefficient. Usually, we let $\beta =\alpha /4$ to guarantee the system is critically damped. $f(x)$ is the virtual force to control the system converge to $x_{t_f}$ . s is the phase variable that describes the normalized time and is subjected to a one-order canonical system

$\tau \dot{s}=-{\alpha }_{s}s$

2

s will decrease from 1 to 0 as the state moves from the starting point to the endpoint, and the decreasing speed is determined by the coefficient ${\alpha }_s$ . $f(s)$ is encoded in form of function about s as below

$\begin{array}{l}f\left(s\right)=\frac{{\displaystyle \sum _{i=1}^N}{w}_i{\varphi }_i\left(s\right)}{{\displaystyle \sum _{i=1}^N}{\varphi }_i\left(s\right)}\cdot s\cdot \left(x_{t_f}-x_0\right),\hfill \\ {\varphi }_i\left(s\right)=e^{-h_i{\left(s-c_i\right)}^2}\hfill \end{array}$

3

where ${\varphi }_i(s)$ is the $i\text{th}$ basic Gaussian function with center c_i and bandwidth h_i , w_i is the weight of ${\varphi }_i(s)$ , and N is the number of basic functions. In general, larger N will generate a smoother trajectory.

For a multi-degree of freedom (DOF) system, each DOF corresponds to an independent spring-damping model, and all DOF share one canonical system. DMP can transfer the shape feature of trajectory from the source task to the target task based on its time and special invariance.

Gaussian mixture regression

GMR provides a probabilistic retrieval of movements or policies with a GMM based on the theory of marginal and conditional distribution. Recovering a trajectory can be formalized as a regression problem. GMR computes the next action on the fly with a computation time independent of the number of data points that are used to train the model.

A GMM with K components is written as $GM{M}^K$ with parameters $\theta ={\left\{{\pi }_k,{\mu }_k,{\Sigma }_k\right\}}_{k=1}^K$ and written as

$P\left(S\right)\sim {\displaystyle \sum _{k=1}^K}{\pi }_k\mathcal{N}\left({\mu }_k,{\Sigma }_k\right)$

4

where S is the multidimensional variable, ${\pi }_k$ , ${\mu }_k$ , and ${\Sigma }_k$ are the mixing coefficients (i.e. weights), expectation vector, and covariance matrix of the ith Gaussian component, respectively.

For a regression task, S, ${\mu }_k$ , and ${\Sigma }_k$ can be written in the block form as

$S=\left[\begin{array}{c}{S}^I\\ S^O\end{array}\right],{\mu }_k=\left[\begin{array}{c}{\mu }_k^I\\ {\mu }_k^O\end{array}\right],{\Sigma }_k=\left[\begin{array}{cc}{\Sigma }_k^I& {\Sigma }_k^{IO}\\ {\Sigma }_k^{OI}& {\Sigma }_k^O\end{array}\right]$

5

where the superscripts I and O represent the input and output blocks, respectively.

When given a GMM model $GM{M}^K$ and the input S_I , we can compute the output according to the conditional distribution

$P\left(S^O|S^I\right)=\mathcal{N}\left(S^O|{\widehat{\mu }}^O,{\widehat{\Sigma }}^O\right)$

6

where

$\begin{array}{l}{\widehat{\mu }}_k^O={\mu }_k^O+{\Sigma }_k^{OI}{\left({\Sigma }_k^I\right)}^{-1}(S^I-{\mu }_k^I),\hfill \\ \hfill \\ {\widehat{\Sigma }}_k^O={\Sigma }_k^O-{\Sigma }_k^{OI}{\left({\Sigma }_k^I\right)}^{-1}{\Sigma }_k^{IO},\hfill \\ \hfill \\ h_k=\frac{{\pi }_k\mathcal{N}\left(S^I|{\mu }_i^I,{\Sigma }_i^I\right)}{{\displaystyle \sum _{k=1}^K}{\pi }_k\mathcal{N}\left(S^I|{\mu }_k^I,{\Sigma }_k^I\right)},\hfill \\ {\widehat{\mu }}^O={\displaystyle \sum _{k=1}^K}{h}_k{\widehat{\mu }}_i^O,\hfill \\ {\widehat{\Sigma }}^O={\displaystyle \sum _{k=1}^K}{h}_i\left[{\widehat{\Sigma }}_k^O+{\widehat{\mu }}_k^O{\left({\widehat{\mu }}_k^O\right)}^T\right]-{\widehat{\mu }}^O{\left({\widehat{\mu }}^O\right)}^T\hfill \end{array}$

7

In contrast to other regression methods such as locally weighted regression, locally weighted projection regression, or GPR, GMR models the regression function indirectly. It models the joint probability density function of data points and derives the regression function from the joint density model. The estimation of the model parameters is thus achieved in an offline phase that depends linearly on the number of data points. One advantage of GMR is that its regression is independent of this number and can be computed very rapidly. The other advantage is that any variables can be chosen as the input freely without modification of the model. For example, once a GMM about the joint probability $[s,x,y,z]$ is trained, the user can treat s as the input variable and $x,y,z$ as the output, or the user can treat x as the input variable and z as the output. GMR is very suitable for those applications which specify input and output dimensions at run time (e.g. to handle missing sensory inputs or to react swiftly by retrieving partial outputs).

In GMRDMP, $GM{M}^K$ is obtained from the demonstrations and used to recover the trajectory for a new task according to equation (6). The input S^I is the phase variable s, and the output S^O is the virtual forces.

Reversibility and continuity of DMP and GMRDMP

Denote the set of recovered trajectories as S ₂ which is obtained from a given set of demonstrations S ₁, that is, $S_2=STL\left(S_1\right)$ . The reversibility criterion means that S ₁ can be recovered from $S_1=STL(STL\left(S_2\right))$ . Continuity means that the recovered trajectory should change continuously when the starting or end point of the new task changes continuously. Failure in reversibility or continuity would lead to counterintuitive results and confuse the user.

Reversibility of DMP

In this test, a DMP model with parameters $N=500$ , $\alpha =10$ , $\beta =\alpha /4$ , and integration step $d_t=0.01$ is used for STL. As shown in Figure 1, the demonstrations are encoded and transferred to new tasks with different starting or end points. The recovered trajectory is encoded and transferred to a recycled trajectory in the same way. When the start or the end point changes, recovered2 always nearly coincides with demo1, which means that the transfer process is reversible.

Figure 1.

DAgger based on PNDMP. (a) Whole trajectories, (b) first half trajectory, and (c) second half trajectory.

Reversibility of GMRDMP

In this test, two demonstrations are given. Their starting and end points are generated randomly from two regions. The starting region is ${ℛ}_1=\{x\in [0.75,0.85],y\in [0.15,0.25]\}$ , and the end region is ${ℛ}_2=\{x\in [0.35,0.45],y\in [-0.25,-0.15]\}$ . We obtained a GMM model from them to recover a trajectory with starting region ${ℛ}_2$ and end region ${ℛ}_1$ . Then we obtained the recycled trajectory with starting region ${ℛ}_1$ and end region ${ℛ}_2$ based on another model learning from the recovered trajectory. Both models have four components and the hyperparameters of GMRDMP are ${\alpha }_s=1.0$ , $\alpha =10$ , $\beta =\alpha /4$ , and $d_t=0.01$ . As shown in Figure 2, although the recycled trajectory is in accord with the demonstrations, the recovered trajectory fails to preserve the shape. The reason is that the trajectory is encoded in the world coordinate.

Figure 2.

Reversibility of GMRDMP. GMRDMP: Gaussian mixed regression dynamic motion primitive.

Continuity of DMP

In this test, the starting and end points of the demonstration are at $\left(0.8,0.2,0.0\right)$ and $\left(0.4,-0.2,0.01\right)$ , respectively. In the new task, the starting point is at $\left(0.2,0.2,0.0\right)$ and the target varies from $\left(-0.2,-0.2,-0.03\right)$ to $\left(-0.2,-0.2,0.02\right)$ . The results of DMP are shown in Figure 3. The recovered trajectories are very sensitive to the slight change of z. This is because the virtual force is proportional to $x_{t_f}-x_0$ . This problem has been discussed in some literature. ⁴⁷

Figure 3.

Continuity of DMP. DMP: dynamic motion primitive.

Continuity of GRMDMP

In this test, only one demonstration is given. The targets of the new tasks vary from $(0.4,0.2,-0.3)$ to $(0.4,0.2,-0.5)$ . The GMM model has four components and the hyperparameters of GMRDMP are ${\alpha }_s=1.0$ , $\alpha =10$ , $\beta =\alpha /4$ , and $d_t=0.01$ . Given that the encoded force only has a relation with the world coordinate, its continuity is guaranteed. The results of GMRDMP are shown in Figure 4.

Figure 4.

Continuity of GRMDMP. GMRDMP: Gaussian mixed regression dynamic motion primitive.

The above tests illustrate that DMP has good reversibility but poor continuity while GMRDMP is just the opposite. The discontinuity of DMP and the reversibility of GMRDMP can be solved by coordinate transformation or normalization. ²⁷ However, different transformation or normalization methods may produce very different results. Many other members of the DMP family also suffer from this problem. This kind of uncertainty is not allowed in industrial applications. Thus, some manual intervention, such as DAgger, is necessary for the application of GMRDMP. As GMRDMP lacks flexibility for interactive adjustment, we intended to utilize the negative demonstrations to improve flexibility.

Problem description

PNDMP is an improved version of GMRDMP with negative demonstrations. Without loss of generality, this study considers the typical motion planning task of the end effector of a robot manipulator. The methodology can be extended to other tasks easily.

Denote the set of demonstrations as

$\mathcal{D}=\left\{d_{i,j}|i=1,2,\cdots ,N_p+N_q,j=1,\cdots ,M\right\}$

8

where N_p and N_q are the number of demonstrations in each set, M is the length of demonstrations. The sampling point is

$d_{i,j}=[s_{i,j},x_{i,j},y_{i,j},z_{i,j},{\dot{x}}_{i,j},{\dot{y}}_{i,j},{\dot{z}}_{i,j},{\ddot{x}}_{i,j},{\ddot{y}}_{i,j},{\ddot{z}}_{i,j}]$

9

$\{d_{i,j}|i=1,2,\cdots ,N_p\}$ and $\{d_{i,j}|i=N_p+1,N_p+2,\cdots ,N_p+N_q\}$ are the positive and negative subsets, respectively.

With equation (1), the virtual force for the ith demonstration in the x direction can be calculated as

$f_{i,j}^{\left(x\right)}=\tau {\ddot{x}}_{i,j}-\alpha \left(\beta \left(x_{i,M}-x_{i\mathrm{,0}}\right)-{\dot{x}}_{i,j}\right)$

10

$f_{i,j}^{(y)}$ and $f_{i,j}^{(z)}$ can be calculated in the same manner.

Then, we can obtain the new positive and negative sets of virtual force as below

$\begin{array}{l}\mathcal{U}=\left\{\right(s_{i,j},f_{i,j}^{(x)},f_{i,j}^{(y)},f_{i,j}^{(z)})|i=1,2,\cdots ,N_p\},\hfill \\ \mathcal{V}=\left\{\right(s_{i,j},f_{i,j}^{(x)},f_{i,j}^{(y)},f_{i,j}^{(z)})|i=N_p+1,N_p+2,\cdots ,N_p+N_q\}\hfill \end{array}$

11

For the convenience of the following expression, we can rewrite them as $\mathcal{U}=\{u_i,i=1,2,\cdots ,m_u\}$ and $\mathcal{V}=\{v_j,j=1,2,\cdots ,m_v\}$ , where $m_u=N_{p}M$ and $m_v=N_{q}M$ .

The pipeline of GMRDMP can be summarized as follows:

Obtain $\mathcal{U}$ and $\mathcal{V}$ from $\mathcal{D}$ ;

Optimize $GM{M}^K=\{{\pi }_i,{\mu }_i,{\Sigma }_i{\}}_{k=1}^K$ with EM algorithm based on $\mathcal{U}$ and $\mathcal{V}$ ;

Retrieve the force trajectory $(\widehat{s},f^{(x)}(\widehat{s}),f^{(y)}(\widehat{s}),f^{(z)}(\widehat{s}))$ via GMR with the model $GM{M}^K$ ;

For a new task, subject the recovered force trajectory and the new starting and end points into equation (1) to recover a new motion.

In PNDMP, the optimization of $GM{M}^K$ is replaced by maximizing a new log-likelihood function for adjustment of the recovered trajectory, in which $\mathcal{U}$ and $\mathcal{V}$ are annotated with different labels. The log-likelihood function is defined as below

$L\left(\theta \right)\text{=}(1-\lambda ){\displaystyle \sum _{i=1}^{m_u}}\text{log}P\left(u_i|\theta \right)+\lambda {\displaystyle \sum _{j=1}^{m_v}}\text{log}P\left(v_j|\theta \right)$

12

where $\lambda \in [0,1]$ is a scalar weight, and U and V are the corresponding variables of $\mathcal{U}$ and $\mathcal{V}$ . The optimization objective changes to maximize the probability on U and minimize the probability on V simultaneously.

Because the difference between discrete and rhythmic GMRDMP lies in the calculation of $\mathcal{U}$ from $\mathcal{D}$ , PNDMP can be easily extended to rhythmic tasks by replacing the motion equation. Furthermore, we can also plan one single cycle of the rhythmic motion with the discrete GMRDMP first and repeat that cycle infinitely to accomplish rhythmic tasks. Thus, we only focus on the discrete GMRDMP in this paper.

Main algorithms of PNDMP

In this section, we developed a new PNEM algorithm to maximize the log-likelihood function (12). In addition, we devised a new positive and negative demonstration-based Kmeans (PNKmeans) algorithm for determining the initial value for PNEM. The DAgger framework of PNDMP is then put forward at last.

PNEM

In GMRDMP, EM algorithm ^48,49 provides an iterative solution for $\text{arg max}{\displaystyle \sum }\text{log}P\left(u_i|\theta \right)$ . It introduces a latent variable to establish the Q-function and uses Kullback–Leibler divergence to calculate the lower bound of the Q-function, in which the latent variables represent the stage of the trajectory. On this basis, the E-step and M-step can be executed iteratively. The derivation process of PNEM refers to that of the standard EM and the standard EM is a special case of PNEM.

In this subsection, we mainly focused on the procedure from the optimization problem $\widehat{\theta }=\text{arg}\underset{\theta }{{\displaystyle \text{max}}}L(\theta )$ to the Q-function. The rest derivation can be found in the Appendix 1. When the positive and negative latent variables $\zeta$ and $\eta$ are introduced, $L(\theta )$ can be rewritten as

$\begin{array}{l}L\left(\theta \right)=(1-\lambda ){\displaystyle \sum _{i=1}^{m_u}}q\left({\zeta }_i\right){\displaystyle \sum _{z_i}}\text{log}\frac{P\left(u_i,{\zeta }_i|\theta \right)}{P\left({\zeta }_i|u_i,\theta \right)}+\lambda {\displaystyle \sum _{j=1}^{m_v}}q\left({\eta }_j\right){\displaystyle \sum _{z_j}}\text{log}\frac{P\left(v_j,{\eta }_j|\theta \right)}{P\left({\eta }_j|v_j,\theta \right)}\hfill \\ =(1-\lambda ){\displaystyle \sum _{i=1}^{m_u}}q\left({\zeta }_i\right){\displaystyle \sum _{z_i}}\text{log}\frac{P\left(u_i,{\zeta }_i|\theta \right)/q\left({\zeta }_i\right)}{P\left({\zeta }_i|u_i,\theta \right)/q\left({\zeta }_i\right)}+\lambda {\displaystyle \sum _{j=1}^{m_v}}q\left({\eta }_j\right){\displaystyle \sum _{z_j}}\text{log}\frac{P\left(v_j,{\eta }_j|\theta \right)/q\left({\eta }_j\right)}{P\left({\eta }_j|v_j,\theta \right)/q\left({\eta }_j\right)}\hfill \\ =(1-\lambda ){\displaystyle \sum _{i=1}^{m_u}}q\left({\zeta }_i\right){\displaystyle \sum _{z_i}}\text{log}\frac{P\left(u_i,{\zeta }_i|\theta \right)}{q\left({\zeta }_i\right)}+\lambda {\displaystyle \sum _{j=1}^{m_v}}q\left({\eta }_j\right){\displaystyle \sum _{z_j}}\text{log}\frac{P\left(v_j,{\eta }_j|\theta \right)}{q\left({\eta }_j\right)}+(1-\lambda ){\displaystyle \sum _{i=1}^{m_u}}q\left({\zeta }_i\right){\displaystyle \sum _{z_i}}\text{log}\frac{q\left({\zeta }_i\right)}{P\left({\zeta }_i|u_i,\theta \right)}\hfill \\ +\lambda {\displaystyle \sum _{j=1}^{m_v}}q\left({\eta }_j\right){\displaystyle \sum _{z_j}}\text{log}\frac{q\left({\eta }_j\right)}{P\left({\eta }_j|v_j,\theta \right)}\hfill \\ =(1-\lambda ){\displaystyle \sum _{i=1}^{m_u}}q\left({\zeta }_i\right){\displaystyle \sum _{z_i}}\text{log}\frac{P\left(u_i,{\zeta }_i|\theta \right)}{q\left({\zeta }_i\right)}+\lambda {\displaystyle \sum _{j=1}^{m_v}}q\left({\eta }_j\right){\displaystyle \sum _{z_j}}\text{log}\frac{P\left(v_j,{\eta }_j|\theta \right)}{q\left({\eta }_j\right)}+(1-\lambda )KL\left(q\left(\zeta \right)||P\left(\zeta |U,\theta \right)\right)\hfill \\ +\lambda \cdot KL\left(q\left(\eta \right)||P\left(\eta |V,\theta \right)\right)\hfill \end{array}$

13

where $KL(a\parallel b)\ge 0$ is the Kullback–Leibler divergence that measures the distance between distributions a and b. Only when $a=b$ , there exists $KL(a\parallel b)=0$ . As conditional probabilities $P(\zeta |U,\theta )$ and $P(\eta |V,\theta )$ are statistical variables, they can be obtained when the parameter $\theta$ is given. Then, we have

$\begin{array}{rr}\hfill & \hfill \\ \hfill L\left(\theta \right)\ge (1-\lambda ){\displaystyle \sum _{i=1}^{m_u}}q\left({\zeta }_i\right){\displaystyle \sum _{{\zeta }_i}}\text{log}\frac{P\left(u_i,{\zeta }_i|\theta \right)}{q\left({\zeta }_i\right)}\\ \hfill +\lambda {\displaystyle \sum _{j=1}^{m_v}}q\left({\eta }_j\right){\displaystyle \sum _{{\eta }_j}}\text{log}\frac{P\left(v_j,{\eta }_j|\theta \right)}{q\left({\eta }_j\right)}\end{array}$

14

When $q(\zeta )=P(\zeta |U,\theta )$ and $q(\eta )=P(\eta |V,\theta )$ , the equivalence of (14) is true. Thus, we can construct a constrained Q-function for $L(\theta )$ , and calculate z and $\theta$ alternatively in the same manner as the standard EM algorithm. Denoting ${\theta }^{(t)}$ as the estimated result of iteration $t-1$ , then the positive and negative demonstrations-based Q-function (PNQ-function) is defined as

$\begin{array}{r}\hfill Q\left(\theta ,{\theta }^{\left(t\right)}\right)=(1-\lambda ){\displaystyle \sum _{i=1}^{m_u}}P\left({\zeta }_i|u_i,{\theta }^{\left(t\right)}\right){\displaystyle \sum _{{\zeta }_i}}\text{log}P\left(u_i,{\zeta }_i|\theta \right)\\ \hfill +\lambda {\displaystyle \sum _{j=1}^{m_v}}P\left({\eta }_j|v_j,{\theta }^{\left(t\right)}\right){\displaystyle \sum _{{\eta }_j}}\text{log}P\left(v_j,{\eta }_j|\theta \right)\end{array}$

15

With the new PNQ-function, we can obtain the posterior probabilities of the latent variables in the E-step

${\gamma }_{u,ik}^{\left(t+1\right)}=\frac{{\pi }_k^{(t)}\varphi \left(u_i|{\mu }_k^{(t)},{\Sigma }_k^{(t)}\right)}{{\displaystyle \sum _{l=1}^K}{\pi }_l^{(t)}\varphi \left(u_i|{\mu }_l^{(t)},{\Sigma }_l^{(t)}\right)}$

16

${\gamma }_{v,jk}^{\left(t+1\right)}=\frac{{\pi }_k^{(t)}\varphi \left(v_j|{\mu }_k^{(t)},{\Sigma }_k^{(t)}\right)}{{\displaystyle \sum _{l=1}^K}{\pi }_l^{(t)}\varphi \left(v_j|{\mu }_l^{(t)},{\Sigma }_l^{(t)}\right)}$

17

And in the M-step, we can obtain

${\pi }_k^{(t+1)}=\frac{\left(1-\lambda \right){\displaystyle \sum _{i=1}^{m_u}}{\gamma }_{u,ik}^{\left(t+1\right)}+\lambda {\displaystyle \sum _{j=1}^{m_v}}{\gamma }_{v,jk}^{\left(t+1\right)}}{\left(1-\lambda \right)m_u+\lambda m_v}$

18

${\mu }_k^{\left(t+1\right)}=\frac{(1-\lambda ){\displaystyle \sum _{i=1}^{m_u}}{u}_i{\gamma }_{u,ik}^{\left(t+1\right)}+\lambda {\displaystyle \sum _{j=1}^{m_u}}{v}_i{\gamma }_{v,jk}^{\left(t+1\right)}}{(1-\lambda ){\displaystyle \sum _{i=1}^{m_u}}{\gamma }_{u,ik}^{\left(t+1\right)}+\lambda {\displaystyle \sum _{j=1}^{m_u}}{\gamma }_{v,jk}^{\left(t+1\right)}}$

19

${\Sigma }_k^{\left(t+1\right)}=\frac{(1-\lambda ){\Sigma }_a+\lambda {\Sigma }_b}{(1-\lambda ){\displaystyle \sum _{i=1}^{m_u}}{\gamma }_{u,ik}^{(t+1)}+\lambda {\displaystyle \sum _{j=1}^{m_v}}{\gamma }_{v,jk}^{(t+1)}}$

20

where

$\begin{array}{ll}{\Sigma }_a={\displaystyle \sum _{i=1}^{m_u}}\left(u_i-{\mu }_k\right){\left(u_i-{\mu }_k\right)}^T{\gamma }_{u,ik},\hfill & \hfill \\ {\Sigma }_b={\displaystyle \sum _{j=1}^{m_v}}\left(v_j-{\mu }_k\right){\left(v_j-{\mu }_k\right)}^T{\gamma }_{v,jk}\hfill & \hfill \end{array}$

21

In the E-step, ${\gamma }_{u,ik}$ and ${\gamma }_{v,jk}$ are calculated with ${\pi }_k$ , ${\mu }_k$ , and ${\Sigma }_k$ being frozen. In the M-step, it is the opposite. E-step and M-step are repeated till some stopping conditions (threshold of iterations or tolerance) are triggered.

PNKmeans

EM is sensitive to the initial value. Thus, Kmeans is often adopted to generate a preferable initial value ${\mu }_k^{\left(0\right)}$ for EM. Kmeans is also an iterative clustering algorithm that only considers the distance between data points. In this section, we propose the PNKmeans method to introduce negative demonstrations into Kmeans.

Given the point data sets U and V defined as before and K to-be-calculated center points as $o_k^{\left(t\right)}$ , in the iteration $t+1$ we can obtain

$\begin{array}{l}{d}_{ik}^{\left(t\right)}={\Vert u_i-o_k^{\left(t\right)}\Vert }^2,\hfill \\ {\overline{d}}_{jk}^{\left(t\right)}={\Vert v_j-o_k^{\left(t\right)}\Vert }^2\hfill \end{array}$

22

The sample space can be divided into K subspace $S_k^{\left(t\right)}$ by $o_k^{\left(t\right)}$ . A point x belongs to $S_k^{\left(t\right)}$ if ${\Vert x-o_k^{\left(t\right)}\Vert }^2\le {\Vert x-o_{j\ne k}^{\left(t\right)}\Vert }^2$ , which is denoted as $x\in S_k^{\left(t\right)}$ . The points on the bound of two different subspaces belong to both subspaces.

The optimization objective is to minimize J which is defined as

$J=(1-\lambda )\underset{︸J_1}{{\displaystyle \sum _{i=1}^{m_u}}{\displaystyle \sum _{k=1}^K}{r}_{ij}{d}_{ik}}+\lambda \underset{︸J_2}{{\displaystyle \sum _{j=1}^{m_v}}{\displaystyle \sum _{k=1}^K}{r}_{ij}{\overline{d}}_{jk}}$

23

where $r_{ij}=\{\begin{array}{c}\begin{array}{cc}1,& if{u}_i\in o_k^{\left(t\right)}\\ 0,& else\mathrm{.}\end{array};\end{array}$ J ₁ is the optimization object of Kmeans, and J ₂ is the new term for negative demonstrations. As J ₁ and J ₂ are linearly weighted, the new center points can be updated as follows

$o_k^{\left(t+1\right)}=\left(1-\lambda \right)\underset{u_i\in O_k^{\left(t\right)}}{{\displaystyle mean}}\left\{u_i\right\}+\lambda \underset{u_j\in O_k^{\left(t\right)}}{{\displaystyle mean}}\left\{v_j\right\}$

24

Extrapolation strategy for PNDMP

From equations (18), (19), and (20), we can find that the results of PNEM are functions of the weight $\lambda$ , that is

$theta(\lambda )=\{{\pi }_k(\lambda ),{\mu }_k(\lambda ),{\Sigma }_k(\lambda {)\}}_{k=1}^K$

25

where $\lambda \in [0,1]$ , $\theta (0)$ represents the model extracted from positive demonstrations, and $\theta (1)$ from negative demonstrations. When we adjust $\lambda$ from 0 to 1, the shape of the recovered trajectory changes from positive demonstrations to negative ones gradually. However, to adjust the trajectory furthermore, that is $\lambda <0$ or $\lambda >1$ , calculating $\theta (\lambda )$ directly is unsuitable. This is because of the exponential decay characteristic of the Gaussian distribution. When a Gaussian component is close to positive demonstrations and far from negative demonstrations, the effect of negative demonstrations on this component would decline considerably. Thus, we designed an extrapolation strategy to improve the flexibility of PNDMP.

Considering that ${\pi }_k$ represents the stage division of the trajectory and ${\Sigma }_k$ represents the degree of dispersion, the extrapolation strategy to calculate $\widehat{\theta }(w_p,w_n)$ from the trained GMM model is as below

$\begin{array}{l}{\widehat{\pi }}_k(w)={\pi }_k(0.5),\hfill \\ {\widehat{\mu }}_k(w)=w_p\cdot {\mu }_k(0)+w_n\cdot {\mu }_k(1),\hfill \\ {\widehat{\Sigma }}_k(w)={\Sigma }_k(0.5)\hfill \end{array}$

26

This strategy can make the demonstrations act on the adjustment continuously and smoothly, as shown in Figure 5.

Figure 5.

Adjustment of the recovered trajectory based on extrapolation. (a) Recovering the trajectory. (b) Adjusting the trajectory via extrapolation with different w_n and $w_p=1-w_n$ . (c) Adjusting the trajectory via $\theta (\lambda )$ with different $\lambda$ .

DAgger framework of PNDMP

Unexpected situations are very common in the industrial field. For instance, some new sensors added into the manipulator’s workspace render preprogrammed trajectories unsafe, yet the operator cannot model the obstacle. In that case, DAgger is an effective solution. DAgger is one of the fundamental incremental learning methods for sequential decision tasks, as well as an efficient method to cope with unexpected situations. In DAgger, new samples are collected and annotated by experts and then added to the training data set during the iterative training process. In the DAgger framework of PNDMP, the user can treat the recovered trajectory as a negative demonstration and re-train the GMM model.

Experiments

To validate the advantage of PNDMP, we have conducted experiments on three typical robot manipulating tasks. The first task is 2D letter writing, ²⁴ with which we analyzed the properties of PNEM and PNKmeans. The second one is obstacle avoidance. ¹⁰ The last one is the grasping task in a grid box, which is conducted on a Franka Panda robot. Experiments on the last two tasks illustrate the usage and performance of PNDMP.

Letter writing tasks

In the letter writing task, we have selected the letters B, E, Q, T, and X as the samples to learn. Ten positive and two negative demonstrations of each letter are gathered, with each demonstration including 200 sampling points.

The hyperparameters of PNDMP are $K=5$ , $\alpha =10$ , ${\alpha }_s=1.0$ , $d_t=0.01$ , $w_p=1$ and $w_n=-1$ . Kmeans and EM are both limited to 100 iterations. Denote the clustering results of standard Kmeans and PNKmeans as ${\mu }_k^{\left(0\right)}$ and ${\stackrel{\sim }{\mu }}_k^{\left(0\right)}$ , $k=1,\cdots ,K$ , the results of standard EM based on ${\mu }_k^{\left(0\right)}$ and ${\stackrel{\sim }{\mu }}_k^{\left(0\right)}$ as $\left({\pi }_{k,EM}^{\left(t_f\right)},{\mu }_{k,EM}^{\left(t_f\right)},{\Sigma }_{k,EM}^{\left(t_f\right)}\right)$ and $\left({\stackrel{\sim }{\pi }}_{k,EM}^{\left(t_f\right)},{\stackrel{\sim }{\mu }}_{k,EM}^{\left(t_f\right)},{\stackrel{\sim }{\Sigma }}_{k,EM}^{\left(t_f\right)}\right)$ , the results of PNEM as $\left({\pi }_{k,PNEM}^{\left(t_f\right)},{\mu }_{k,PNEM}^{\left(t_f\right)},{\Sigma }_{k,PNEM}^{\left(t_f\right)}\right)$ and $\left({\stackrel{\sim }{\pi }}_{k,PNEM}^{\left(t_f\right)},{\stackrel{\sim }{\mu }}_{k,PNEM}^{\left(t_f\right)},{\stackrel{\sim }{\Sigma }}_{k,PNEM}^{\left(t_f\right)}\right)$ , and $T_{EM}$ , ${\stackrel{\sim }{T}}_{EM}$ , $T_{PNEM}$ , and the corresponding recovered letters as ${\stackrel{\sim }{T}}_{PNEM}$ , $T=ℬ,ℰ,\mathcal{Q},\mathcal{T},\mathcal{X}$ , respectively. The superscript t_f represents the iterations of each execution.

It is difficult to evaluate the recovered letters quantitatively. Thus, we have repeated the experiment 100 times and analyzed the statistical difference between ${\stackrel{\sim }{T}}_{EM}$ and ${\stackrel{\sim }{T}}_{PNEM}$ . As EM can only obtain a local optimal solution, the result of GMRDMP is random. PNDMP inherits this property from DMP. In the task which owns an obvious gap between the positive and negative demonstrations, PNDMP shows flexibility to adjust the recovered trajectory. As shown in Figure 6, the recovered trajectories can be divided into different patterns. The statistical results are presented in Table 1. The margin distributions of $\left(s,f_x\left(s\right)\right)$ are presented in Figure 7, in which each ellipse represents a component of GMM.

Figure 6.

Patterns of recovered motion of letters. (a) Pattern 1 of B, (b) pattern 2 of B, (c) pattern 3 of B, (d) pattern 4 of B, (e) pattern 1 of E, (f) pattern 2 of E, (g) pattern 3 of E, (h) pattern 4 of E, (i) pattern 1 of Q, (j) pattern 1 of T, (k) pattern 1 of X, and (l) pattern 2 of X.

Figure 7.

Marginal distributions of the GMM. GMM: Gaussian mixed model.

Table 1.

Statistical results of different patterns of the letter writing task.

	Pattern 1	Pattern 2	Pattern 3	Pattern 4
B	21	14	60	5
E	32	18	42	8
Q	100	—	—	—
T	100	—	—	—
S	62	38	—	—

It is worth noting that PNDMP fails to adjust the trajectory in some tasks. The scope of PNDMP is important for workers with little knowledge about the learning control of robots, but it is still challenging to estimate whether PNDMP is appropriate for a task.

Although calculating $\theta (\lambda )$ directly for $\lambda \in (-\infty \mathrm{,0})\cap (0,+\infty )$ cannot generate good trajectories, we found a special phenomenon in the process of the experiment. That is, the introduction of negative demonstrations can improve the converging speed of PNDMP. In this experiment, we analyzed the converging speed of PNDMP and PNKmeans on the letter writing task. As shown in Tables 2 and Figure 8, there are 60 configurations in all, which are the combination of five letters, three $GMM$ , two Kmeans algorithms, and two EM algorithms. The hyperparameters of algorithms are the same as before except that $\theta (\lambda =-0.25)$ is used in PNDMP. PNEM can converge with fewer iterations than EM on 80% configurations. PNKmeans can converge with fewer iterations than Kmeans on 60% configurations. It should be noted that the number of components of GMM has no obvious relation to the number of iterations.

Table 2.

Average iterations of 100 executions.

		B	E	Q	T	X
$GM{M}^5$	Kmeans + PNEM	15.29	8.04	43.49	19.52	53.63
	PNKmeans + PNEM	15.81	9.79	31.09	24.08	47.13
	Kmeans + EM	52.49	32.33	43.43	34.49	65.27
	PNKmeans + EM	54.26	31.52	33.35	36.85	56.30
$GM{M}^6$	Kmeans + PNEM	65.64	30.05	68.79	28.09	31.49
	PNKmeans + PNEM	52.20	29.15	53.98	27.19	34.29
	Kmeans + EM	73.60	27.17	74.76	33.39	33.51
	PNKmeans + EM	58.06	26.01	56.72	35.90	36.15
$GM{M}^7$	Kmeans + PNEM	46.22	36.70	19.96	32.39	51.89
	PNKmeans + PNEM	45.44	38.15	24.23	31.25	46.75
	Kmeans + EM	49.04	33.90	34.57	39.09	49.42
	PNKmeans + EM	48.77	33.21	35.95	51.12	48.26

PNEM: positive and negative demonstrations-based expectation–maximum; EM: expectation–maximization.

Figure 8.

Comparison of iterations of four methods with GMM⁵. GMM: Gaussian mixed model.

The searching process of one component of $GM{M}^5$ for letters B and Q are depicted in Figures 9 and 10, respectively. The converging step decreases gradually in both executions. In Figure 9, PNEM converges faster and achieves the stop condition much earlier than EM. In Figure 10, PNEM converges faster with a shorter searching path. Thus, negative demonstrations can impose a boundary constraint on the searching space and then make PNEM apt to reach a local optimum, as shown in the sketch diagram in Figure 11.

Figure 9.

Iteration process of one component for letter B.

Figure 10.

Iteration process of one component for letter Q.

Figure 11.

Sketch diagram of the search path.

To analyze the performance of PNKmeans, another clustering task of letters is conducted. In this task, the demonstrations are clustered with a $GM{M}^5$ without being encoded to the virtual force. The execution is repeated 100 times for each letter, and 2500 components of GMM are recorded. We let ${\Vert {\stackrel{\sim }{\mu }}_{k,PNEM}^{\left(t_f\right)}-{\mu }_{k,PNEM}^{\left(t_f\right)}\Vert }_2=e_k^{\left(t_f\right)}$ , and ${\Vert {\stackrel{\sim }{\mu }}_k^{\left(0\right)}-{\mu }_k^{\left(0\right)}\Vert }_2=e_k^{\left(0\right)}$ , the joint distribution of $e_k^{\left(t_f\right)}$ and $e_k^{\left(0\right)}$ are depicted in Figure 12. The numbers of points lying in regions 1–4 are 2317, 16, 12, and 155, respectively. Regions 1 and 4 indicate that close or different initial values lead to very close results, as B, E, and Q in Figure 13. Region 3 indicates that close initial values lead to very different results, such as the X letter in Figure 6. Region 2 indicates that different initial values lead to very different results, such as the letter T. Thus, PNKmeans can improve the converging speed slightly without affecting the cluster results in most cases.

Figure 12.

Distribution of the difference between Kmeans and PNKmeans.

Figure 13.

Effect of PNKmeans on PNDMP. PNDMP: positive and negative demonstrations-based dynamic motion primitive

Obstacle avoidance task

In this task, the recovered trajectories of $GM{M}^5$ are required to avoid two obstacles. The position and volume of the obstacles are unknown and the DAgger framework based on PNDMP is used to finish this task. Trajectories are recovered by GRMDMP first. If they do not satisfy the safety criterion, then they are treated as negative demonstrations and mixed with original positive demonstrations to retrain $GM{M}^5$ . Ten positive demonstrations are collected and each demonstration includes 200 points. GMRDMP and PNDMP have the same hyperparameters $\alpha =10$ and ${\alpha }_s=1.0$ , and Kmeans and EM are both limited to 100 iterations. Those hyperparameters make the recovered trajectories of GMRDMP collide with the spherical obstacle, as those negative demos in Figure 14. By adding them to the data set and re-training $GM{M}^5$ , we can obtain better trajectories with $w_n=-1,-2$ and $w_p=1-w_n$ .

Figure 14.

Recovered trajectories for obstacle avoidance task.

Grasping task

In the grasping task, we need to control the Franka Panda robot to grasp a bottle from one square to another of the grid box, as shown in Figure 15(a). To focus on the PNDMP algorithm, we have chosen a bottle with a red cap and sticker as the object and designed the simple pipeline in Figure 16 to detect it. As depicted in Figure 15(b), the end effector of Panda is large, and it often collided with the box when we collected the demonstrations. Because the geometrical collision volume of the grid box is very hard to be described with a few parameters, neither task-parameterized DMP (TPDMP) ²⁷ nor CPDMP is suitable for this task.

Figure 15.

Grasping task by Franka Panda. (a) The Franka Panda robot. (b) Camera above the grid box.

Figure 16.

Detection of the bottle. (a) Original RGB image. (b) Red channel. (c) Contours of the cap and sticker. (d) Position and attitude of the bottle.

In the world coordinate, the bottom centers of the squares are $\left(0.3,-0.15,0\right)$ , $\left(0.7,-0.15,0\right)$ , $\left(0.4,0.15,0\right)$ , and $\left(0.8,0.15,0\right)$ . The lengths and heights of the grid edges are 0.3 and 0.25, respectively. The starting and end points of demonstrations lie in $p_0=\left({\overline{X}}^4\pm 0.1,{\overline{Y}}^4\pm 0.1,0\right)$ and $p_5=\left({\overline{X}}^1\pm 0.1,{\overline{Y}}^1\pm 0.1,0\right)$ , where $\left({\overline{X}}^i,{\overline{Y}}^i\mathrm{,0}\right)$ is the bottom centers of grid i. Positive demonstrations are collected by nonlinear interpolation with three specified waypoints $p_1=p_0+\left(0,0,0.2\right)$ , $p_2=\left(p_0+p_5\right)/2+\left(0,0,0.35\right)$ , and $p_3=p_5+\left(0,0,0.2\right)$ . Negative demonstrations are collected with $p_1=p_0+\left(0.1,0.1,0.2\right)$ , $p_2=\left(p_0+p_5\right)/2+\left(0,0,0.25\right)$ , and $p_3=p_5+\left(-0.1,-0.1,0.2\right)$ . The negative demonstrations are much closer to the grid edges than the positive demonstrations. Ten positive and two negative demonstrations are collected and each demonstration includes 200 points. The maximum iterations of Kmeans and EM are both set to 100. The hyperparameters are $K=5$ , $\alpha =\sqrt{20}$ , and ${\alpha }_s=1.0$ , $w_n=-1$ , and $w_p=1-w_n$ . The results are shown in Figure 17.

Figure 17.

Recovered trajectories for grasping task in a gird box.

Conclusion

By introducing negative demonstrations, we developed PNDMP from GMRDMP with the new log-likelihood function, the PNEM algorithm, and the DAgger framework. PNDMP provides interactive flexibility for the user to adjust the recovered trajectories. Experiments on three typical robot manipulating tasks have been conducted to validate the performance of PNDMP from different aspects. Besides that, another experiment shows that PNEM and PNKmeans converge faster than standard EM and Kmeans when $\lambda <0$ .

Although PNDMP can handle unexpected obstacles in many tasks, there are still several works to be done in the future. The first issue is to clarify the scope of PNDMP, which is of great significance for industrial practice. The second one is to identify the task parameters based on positive and negative demonstrations. Similar to PNEM, the negative demonstrations would provide a definite boundary to improve the accuracy and speed of parameter identification. The last one is to extend the DAgger framework to combine the motion model and policy model task, which means that we can cope with the path planning task and sequential decision task with a uniform model.