ProMPs provide a parametric representation of trajectories which can be executed in multiple ways through the use of a probability distribution. A set of basis functions are used to reduce the model parameters and aid learning over the demonstrated trajectories. The trajectory distribution can be defined and generated in any space that accommodates the system (e.g., joint space or task space) (Paraschos et al., 2018a). In this work we consider joint space trajectories and assume the demonstrations to be normally distributed.

Let q i ( t ) ∈ R be the ith state variable. Then q i ( k ) ∈ R is q _i (t) sampled at time k, where k ∈ {t ₁, … , t _K } is a discrete set of sampling times. Within a ProMP, the execution of a trajectory is modeled as the set of robot positions, ζ _i = {q _i (k)}. Let w i ∈ R 1 × L be a weight matrix with L terms. A linear basis function model is then given by x i k = q i k q i ̇ k = Φ k w i + ξ x i , where Φ ( k ) = ϕ ( k ) ϕ ̇ ( k ) ⊤ ∈ R 2 × L is the time-dependent basis function matrix and L is the number of basis functions. Gaussian noise is described by ξ x i ∼ N ( 0 , Σ x i ) . Thus, the ProMP trajectory is represented by a Gaussian distribution over the weight vector w _i and the parameter vector θ i = { μ w i , Σ w i } , which simplifies the estimation of the parameters.

We marginalize out w _i to create the trajectory distribution p ζ i , θ i = ∫ p ζ i | w i p w i ; θ i d w i . (1)

Here, the distribution p (ζ _i , θ _i ) defines a hierarchical Bayesian model over the trajectories ζ _i (Paraschos et al., 2018a) and p ( w i | θ i ) = N ( w i | μ w i , Σ w i ) . In an MP representation, the parameters of a single primitive must be easy to obtain from demonstrations. The distribution of the state p (x _i (k); θ _i ) is p x i k ; θ i = N x i k | Φ k μ w i , Φ k Σ w i Φ k ⊤ + Σ x i . (2)

The trajectory can be generated from the ProMP distribution using w _i , the basis function Φ(k), and (2). The basis function is chosen based on the type of robot movement which can be either rhythmic or stroke-based. From (Eq. 2), the mean μ ̃ i ( k ) ∈ R 2 of the ProMP trajectory at k is Φ ( k ) μ w i and the covariance Σ _i (k) is Φ ( k ) Σ w i Φ ( k ) ⊤ + Σ x i .

Multiple demonstrations are needed to learn a distribution over w _i . To train a ProMP we use a combination of radial basis and polynomial basis functions. From the demonstrations, the parameters θ _i can be estimated using maximum likelihood estimation (Lazaric and Ghavamzadeh, 2010). However, when there are insufficient demonstrations this may result in unstable estimates of the ProMP parameters. Therefore, similar to (Gomez-Gonzalez et al., 2020), our method uses a regularization to estimate the ProMP distribution. We maximize θ _i for the posterior distribution over the ProMP using expectation maximization, p θ i | x i k ∝ p θ i p x i k | θ i . (3)

In addition, we make use of Normal-Inverse-Wishart as a prior distribution p (θ _i ) to increase stability when training the ProMP parameters (Gomez-Gonzalez et al., 2020).

Consider the following control affine nonlinear system x ̇ = f x + G ̃ x u , (4) where x ∈ R n denotes the state, u ∈ R m is the control input, G ̃ = [ g 1 , … , g m ] , and f : R n → R n and g i : R n → R n are locally Lipschitz vector fields. It is assumed that the system in (Eq. 4) is controllable.

The model (4) encompasses the dynamic model of robotic manipulators. We consider the following description of robot motion given by the general form by the Euler-Lagrange equations, D q q ̈ + H q , q ̇ = E u , (5) where q ∈ R n are generalized coordinates of the robot, D ( q ) ∈ R n × n is the inertia matrix, R n × n ∋ H ( q , q ̇ ) = C ( q , q ̇ ) q ̇ + K ( q ) is a vector containing the Coriolis and gravity terms, and E ∈ R n × p is the actuation matrix that determines the way in which the inputs u actuate the system. In this work, we consider the system to be fully actuated (i.e., p = n), which is typical for robot manipulators. Then, the system description in (Eq. 5) may be converted to an ODE of the form in (Eq. 4) where x = [ q , q ̇ ] ⊤ and f x = q ̇ − D − 1 q H q , q ̇ , G ̃ x = 0 D − 1 q E . (6)

First, we define the trajectory and error vectors as μ = [ μ 1 , … , μ n ] ⊤ and e = [ e 1 , … , e n ] ⊤ , respectively. Using (Eq. 4) and taking the derivative two times along f(x) and G ̃ ( x ) , we obtain e ̈ x = L f 2 e x + L G ̃ L f e x ︸ Γ u x − μ ̈ , e ̈ x = L f 2 e x + Γ u x − μ ̈ . (10)

Next, assume that the decoupling matrix Γ is well-defined and has full rank (Hsu et al., 2015) ¹ . This implies that the system in (Eq. 4) is feedback linearizable and we can prescribe the following control law, u x = Γ − 1 − L f 2 e x + μ ̈ + v , (11) where v is an auxiliary feedback control value. This yields the second order linear system from input v to output e, e ̈ = v . (12)

By defining η = [ e , e ̇ ] ⊤ , (Eq. 12) can be written as a linear time invariant system η ̇ = 0 I 0 0 η + 0 I v . (13)

From there, n decoupled systems can be obtained from (Eq. 13), η ̇ i = F η i + G v i , i = 1 , … , n , (14) where η i = [ e i , e ̇ i ] ⊤ , F = 0 1 0 0 , and G = [0 1] ^⊤ .

It should be noted that the derivative of the tracking error in (Eq. 14) can be obtained from q ̇ and μ ̇ . Indeed, we can measure q ̇ and we know μ from the ProMP. Since μ is a stored trajectory there is no noise in this signal. Therefore, μ ̇ can be calculated via backwards difference and it can be made as smooth as necessary (i.e., by creating spline curve from fit points in μ). Moreover, the backward difference method is again used for approximating μ ̈ , in (Eq. 10) and (Eq. 11).

To accomplish problem objective 3, it is sufficient to ensure e _i → 0. This is accomplished by designing an appropriate CLF. To satisfy problem objective 4, it is adequate to make |e _i | < σ _i . This objective is satisfied by defining suitable CBFs. In the following subsections, the CLFs and CBFs are defined for the system in (Eq. 14). Moreover, the ith controller for each system in (Eq. 14) is designed by combining the corresponding CLFs and CBFs via a QP problem.

Consider the following rapidly exponentially stabilizing-CLF (RES-CLF) (Zhao et al., 2014), V ϵ i η i = η i ⊤ 1 / ϵ i 0 0 1 P 1 / ϵ i 0 0 1 η i , (15) where ϵ _i is a positive scalar and P ∈ R 2 × 2 is a symmetric positive definite matrix that can be obtained by solving the continuous time algebraic Riccati equation F ⊤ P + P F − P G G ⊤ P + I = 0 . (16)

In order to exponentially stabilize the system, we want to find v _i such that V ̇ ϵ i η i = L F V ϵ i η i + L G V ϵ i η i v i ≤ − c 3 i ϵ i V ϵ i η i , (17) where c _3i is a positive constant value. To guarantee a feasible solution for the QP, the CLF constraint can be relaxed by δ _i > 0 (Ames et al., 2014) resulting in L F V ϵ i η i + L G V ϵ i η i v i + c 3 i ϵ i V ϵ i η i ≤ δ i . (18)

This relaxation parameter will be minimized in the QP cost function. It is worth mentioning that by providing a weighting factor on the relaxation parameter δ _i , the QP can prioritize how close the system should track of a specific trajectory while ensuring that safety is always satisfied.

We propose two safety constraints for each system in (Eq. 14). More specifically, each system should satisfy − σ _i < e _i < σ _i . Consequently, we have the following two safety constraints, h i 1 = e i + σ i , h i 2 = − e i + σ i . (19)

In this work, we assume that the initial conditions satisfy the safety constraints, i.e., the initial tracking errors are in the safe region. From (Eq. 19), it is clear that we have multiple time-varying constraints that should be satisfied simultaneously. Moreover, it is trivial to verify that L _G e _i = 0 and L _F L _G e _i ≠ 0, thus the safety constraint has a relative degree of 2. For relative degree-two constraints, the reciprocal CBF is defined as (Hsu et al., 2015), B j η i = − ln h i j η i 1 + h i j η i + a E i j b E i j h ̇ i j η i 2 1 + b E i j h ̇ i j η i 2 , (20) where j ∈ {1, 2} and a _Eij , b _Eij are positive scalars. The following control barrier condition should be satisfied for time varying constraints which leads to time varying CBFs, L F B j η i + L G B j η i v i + ∂ B j η i ∂ t − γ i B j η i ≤ 0 . (21)

Remark 2

Note that by choosing small values for a _Eij and b _Eij , the system will stop far from the constraint surfaces. On the other hand, by choosing large parameters the system will stop close to the constraints. In some cases, especially in the presence of uncertainties, choosing a _Eij and b _Eij to be too large may cause constraint violations (i.e., no solution exists to the QP problem). As a result, based on the given application, a compromise must be considered for choosing these parameters.

As shown in (Eq. 19), two safety constraints need to be satisfied simultaneously for each linearized, decoupled system. Due to this fact, a single controller can be obtained in such a way that guarantees adherence to both constraints (Rauscher et al., 2016). In this subsection, n QPs are proposed to unify RES-CLF and CBFs for each system in (Eq. 14) into a single controller. The n QPs for i ∈ {1, … , n} are defined as m i n v i = v i , δ i ⊤ ∈ R 2 v i ⊤ H i v i , s u b j e c t t o L F V ϵ i η i + L G V ϵ i η i v i + c 3 i ϵ i V ϵ i η i ≤ δ i C L F L F B j η i + L G B j η i v i + ∂ B j η i ∂ t ≤ γ i B j η i C B F s (22) where H i = 1 0 0 p s c i and p s c i ∈ R + is a variable that can be chosen based on the designer’s assessment of weighting the control inputs. Based on the QP problem, if the system states η _i are far away from the boundary of the safe set, then the control objective that is represented by RES-CLF will be satisfied. However, as the states get close to the boundary the control performance will be violated by the CBF.

Remark 3

If feedback linearization is not feasible, or in the case that feedback linearization does not result in independent systems, we should encode the coupling between the joints to train a single ProMP for the multidimensional system. Our proposed approach can be extended to address such cases by defining the safety constraints based on the entire ProMP covariance matrix, not just the diagonal elements. This is an avenue of future exploration.

We consider a rigid, two-link robot with the dynamic model of (Eq. 5) and the following parameters (Kolhe et al., 2013), D q = m 1 l 1 2 + m 2 l 1 2 + l 2 2 + 2 l 1 l 2 ⁡ cos q 2 ⋆ m 2 l 2 2 + l 1 l 2 ⁡ cos q 2 m 2 l 2 2 , C q , q ̇ = − m 2 l 1 l 2 ⁡ sin q 2 q ̇ 2 2 q ̇ 1 + q ̇ 2 m 2 l 1 l 2 q ̇ 1 2 ⁡ sin q 2 , K q = m 1 + m 2 g l 1 ⁡ sin q 1 + m 2 g l 2 ⁡ sin q 1 + q 2 m 2 g l 2 ⁡ sin q 1 + q 2 , where m ₁ and m ₂ are the link masses, l ₁ and l ₂ are the lengths of the links, and g is the gravitational acceleration. For the simulations, the values of these variables are selected as m ₁ = 1, m ₂ = 1, l ₁ = 1, l ₂ = 1, and g = 9.8.

We generated 50 trajectories that achieve a goal position from various starting positions while avoiding three obstacles. Using this dataset, we trained a ProMP with Algorithm 1 from (Gomez-Gonzalez et al., 2020). We used L = 2 basis functions consisting of five radial basis parameters. The results of the ProMP training are presented in Figure 3, where the 50 input trajectories are shown in red. The ProMP mean joint trajectories, μ _i , are shown in dark green, and in a light-green fill we show μ _i ± σ _i . A visualization of the ProMP in the workspace, based on (Sakai et al., 2018), is displayed in Figure 4, where the black circles indicate the location of obstacles. The robot link positions over time are highlighted in red, with different colors representing key end-effector trajectories from the ProMP. The green trajectory is the mean of the ProMP distribution. The other four trajectories result from combinations of μ _i ± σ _i , i ∈ {1, 2}.

Three sets of simulations were conducted. In each simulation, the CLF parameters were selected as ϵ _i = 0.1 and c _3i = 0.5. In scenario 1, greater priority was given to the CLF than CBF by choosing a high weight, i.e., p _sc1 = p _sc2 = 200. Moreover, the CBF design parameters were set to a _E11 = a _E12 = 20.1, a _E21 = a _E22 = 20, b _E11 = b _E12 = 1, b _E21 = b _E22 = 0.9, γ ₁ = 10.1, and γ ₂ = 9. In scenario 2, p _sci was chosen as p _sc1 = p _sc2 = 0.02 which implies that less priority was given to the CLF in comparison with the CBF. Moreover, the CBF parameters in this scenario are the same as the first scenario. To show the effects of changing the CBF parameters a _Eij , b _Eij , and γ _i , we consider another scenario. In scenario 3, a _E11 = a _E12 = 1.1, a _E21 = a _E22 = 1.1, b _E11 = b _E12 = 0.4, b _E21 = b _E22 = 0.5, γ ₁ = 1.3, and γ ₂ = 1.51, with p _sci = 0.02 as in the second scenario. Consequently, the effects of changing the CBF parameters can be analyzed by comparing the second and third scenarios.

The simulation results are exhibited in Figure 5. In scenario 1, by choosing a large value for p _sci (more priority given to the CLF than CBF), the system output remains close to the mean trajectory. However, in scenario 2, by considering a small value for p _sci (more priority given to the CBF than CLF), the system remains safely inside the distribution but does not necessarily stay close to the mean. In scenario 3, it can be seen that by choosing smaller values for a _Eij , b _Eij , and γ _i , the system output will maintain more distance from the constraint surfaces, resulting in remaining closer to the mean trajectory. In short, our proposed method provides a valuable option to the system designer by permitting fine-grained administration of the trajectories while ensuring safety.

The primary computational cost of our controller, with respect to time, comes from the fact that it must solve a set of QPs at every time step. In the evaluation simulations, two QP problems are solved in real time (one for each link). The average required time (T _ave), maximum time (T _max), and the standard deviation (std) for solving the QP problems are T _ave = {0.0 015s, 0.0 011s}, T _max = {0.1 148s, 0.0 119s}, std = {0.0 053s, 0.0 006s}. From these results, it is clear that the expected execution time of the QP problems is very small (e.g., in the range of 1 ms). The large maximum times correspond to instances of a single outlier. Hence, the controller is applicable for real-time implementation.

One specific aspect of the CLF/CBF-based ProMP controller developed in this work is minimizing the control effort in the optimization problem (Eq. 22) at each time step. This leads to a lower control effort in comparison with a traditional ProMP controller. Another set of simulations were conducted to compare our proposed methodology with the results of (Paraschos et al., 2018a; Mathew, 2018), which is representative as one of the primary works in this field. To provide a fair comparison with our method, the ProMP controller is also applied to the feedback linearizable model of the two-link robot.

We conducted three sets of simulations (designated as the fourth, fifth and sixth scenarios). In scenario 4, we implemented the CLF/CBF-based controller with more priority accorded to the CLF rather than the CBF (similar to scenario 1). In scenario 5, we considered the CLF/CBF-based controller with more priority bestowed to the CBF instead of the CLF (similar to scenario 2). The ProMPs in scenarios 4 and 5 were trained using the library in (Gomez-Gonzalez, 2019). However, we were not able to successfully implement the original controller in (Gomez-Gonzalez, 2019). Therefore, in scenario 6, we trained and implemented the traditional ProMP controller presented in (Paraschos et al., 2018a) using the library presented in (Mathew, 2018). While the ProMPs for our CLF/CBF-based ProMP controller and traditional ProMP controller used the same training set, the resulting ProMP mean and covariance are slightly different. For each scenario, the simulations were run for 100 different initial conditions that were randomly selected within the bound [μ ₁ ± 0.12, μ ₂ ± 0.12], where μ ₁ (0) = − 1.292 5, and μ ₂ (0) = 0.6.

The simulation results corresponding to these three scenarios are, respectively, shown in Figures 6–8, where the mean of ProMP is highlighted by a dashed orange line, and the mean ± variance bounds are shown with dotted black lines. These results show that all the controllers are robust against uncertainties in the initial conditions. From Figure 6, it can be concluded that by using the CLF/CBF-based controller with high p _sci , the system quickly converges and tracks the mean trajectory. The system deviates from the mean to take a shorter path in Figure 7, but it is clear that by considering small values for p _sci the system remains safely inside the distribution. Based on Figure 8, it can be seen that when using a traditional ProMP controller, the system tracks the mean with an error larger than scenario 4. Moreover, the second joint does not remain inside the distribution during certain periods. We posit that the ProMP controller has some lag, akin to a PID controller with proportional gains that are too low. The feedback and feedforward gains of the ProMP controller are determined as functions of the system parameters and ProMP parameters, and they cannot be tuned to reduce tracking error. The ProMP can be “tuned” through the use of via points. To this end, we have added a via point as the last element of the mean trajectory to ensure convergence. These results show that the CLF/CBF-based ProMP controller has better tracking performance when compared to a traditional ProMP controller.

Figure 9 and Figure 10 summarize the root mean square (RMS) values of the control variables for scenarios 4, 5, and 6 in the form of a boxplot. In these figures, 50% of all the RMS values are placed in the boxes and the median is shown by a red line that divides the box into two parts. The black bars indicate the maximum and minimum values, and the dashed “whiskers” indicate 25% of the values between the box and max/min. Outliers, if any, are indicated by red crosses. From Figure 9, it can be concluded that scenario 4 has larger control values in comparison to scenario 5, i.e., more control effort is needed to be able to effectively track the ProMP mean. This indicates one strength of leveraging trajectory distributions over a single trajectory; the system is given more freedom to reduce the control effort while maintaining safety. Moreover, in contrast with a traditional ProMP controller our methodology has a remarkably lower control effort as shown in Figure 9 and Figure 10.

The equation of motion of the UR5 robot can be written in the form of (Eq. 5), with the following parameters (Spong and Vidyasagar, 2008), D q = ∑ i = 1 6 m i J v i ⊤ J v i + J w i ⊤ R i I m i R i ⊤ J w i , (23) where m i ∈ R is the mass of ith link, and J v i ∈ R 3 × 6 and J w i ∈ R 3 × 6 are the linear and angular part of the Jacobian matrix J _i , respectively. R i ∈ R 3 × 3 is the rotation matrix and I m i ∈ R 3 × 3 is the inertia tensor. The elements of C ( q , q ̇ ) are obtained from the inertia matrix as c i j = ∑ k = 1 6 1 2 ∂ m i j ∂ q k + ∂ m i k ∂ q j − ∂ m k j ∂ q i q ̇ k , (24) where m _ij are the entries of the inertia matrix. Moreover, the elements of the gravity vector are obtained from K i q = ∂ P ∂ q i , (25) where P is the total potential energy of the robot. Additional information on these equations can be found in (Katharina, 2014).

We generated 90 joint-space trajectories with defined goals, obstacles, and starting positions. The 90 UR5 trajectories were then used to train a joint-space ProMP using the same parameters as in the two-link robot case study. The following set of CLF and CBF parameters were chosen: ϵ ₁ = ϵ ₂ = ϵ ₃ = ϵ ₄ = ϵ ₅ = 0.1, ϵ ₆ = 0.01, and c _3i = 1.1, a _Eij = 20.1, b _Eij = 1, and γ _i = 10.1, i ∈ {1, … , 6}, j ∈ {1, 2}. The simulation environment is depicted in Figure 1 and Figure 11. We consider two different scenarios. In scenario 1, p _sci = 200, which gives higher importance to the CLF. For scenario 2, p _sci = 0.001, which implies that the design interest and priority is on the CBF. As is clear from Figure 11, in both scenarios the robot can effectively track the mean of ProMP and simultaneously avoid colliding with environmental obstacles. The running time statistics for solving the QP problems are T _ave = [0.001 4, 0.001 1, 0.001 0, 0.001 0, 0.001 1, 0.001 0], T _max = [0.119 3, 0.012 0, 0.012 8, 0.005 8, 0.031 4, 0.002 8], std = [0.005 3, 0.000 5, 0.000 5, 0.000 3, 0.001 4, 0.000 2]. The large maximum values are again due to a single outlier. Thus, the expected operational time is well within the demands of a robotic system. The cause of the outlier occurrences is an avenue of future research to offer improved performance guarantees.

To conclude this section, we evaluated our CLF/CBF-based ProMP controller using a physical robot with static obstacles. The robot used was a Universal Robots UR5e six-link manipulator with a Robotiq two-finger gripper. In addition to the robot, our environmental setup included the following three static obstacles: a robotic arm, box, and the table that the robot was mounted on.

We tasked a human teacher with demonstrating a pick-and-place procedure to the robot. In our setup, the robot must move from a starting bin to a goal bin located on the other side of a box. We installed a gravity compensation controller on the robot enabling the teacher to directly affect the robot’s joints via kinesthetic teaching. Using this directed learning from demonstration approach allows us to bypass the correspondence problem (Argall et al., 2009). Additionally, kinesthetic teaching retains parity between the demonstration, learning, and execution space of the trajectories. In all, seven demonstrations of the task were conducted. From these demonstrations, we collected the joint angles, velocities, and the associated time steps. It is worth mentioning that care must be taken to collect data that is roughly Gaussian, or at least unimodal. Methods to handle non-Gaussian or multimodal data is an issue to be addressed in future work.

Using this demonstration data, we trained a joint-space ProMP using the same parameters as described in the previous sections. In Figure 12, we see the demonstration data (in green), the respective ProMP mean (red), and one standard deviation of the ProMP (light red). Note that q ₆, which corresponds to the final wrist joint, was not purposefully actuated by the teacher and is largely static during training. We implemented the CLF/CBF-based ProMP controller with p _sci = 0.5. The other parameters were selected to be the same as the previous simulations in Section 4.3. Figure 13 depicts the demonstration operation as the human teacher moves the robot arm during the task. The trajectory from the CLF/CBF-based ProMP controller on the UR5e robot is shown in Figure 14.