Robot Task-Constrained Optimization and Adaptation with Probabilistic Movement Primitives Authors: Guanwen Ding, Xizhe Zang, Xuehe Zhang, Changle Li, Yanhe Zhu, Jie Zhao Published: 2024 ABSTRACT Enabling a robot to learn skills from a human and adapt to different task scenarios will enable the use of robots in manufacturing to improve efficiency. Movement Primitives (MPs) are prominent tools for encoding skills. This paper investigates how to learn MPs from a small number of human demonstrations and adapt to different task constraints, including waypoints, joint limits, virtual walls, and obstacles. Probabilistic Movement Primitives (ProMPs) model movements with distributions, thus providing the robot with additional freedom for task execution. We provide the robot with three modes to move, with only one human demonstration required for each mode. We propose an improved via-point generalization method to generalize smooth trajectories with encoded ProMPs. In addition, we present an effective task-constrained optimization method that incorporates all task constraints analytically into a probabilistic framework. We separate ProMPs as Gaussians at each timestep and minimize Kullback–Leibler (KL) divergence, with a gradient ascent–descent algorithm performed to obtain optimized ProMPs. Given optimized ProMPs, we outline a unified robot movement adaptation method for extending from a single obstacle to multiple obstacles. We validated our approach with a 7-DOF Xarm robot using a series of movement adaptation experiments. 1. Introduction With modern technological advancements, artificial intelligence (AI) is playing a pivotal role in transforming various domains, including smart cities and industrial manufacturing. In smart cities, AI, coupled with the Internet of Things (IoT) and blockchain, enables data-driven decision-making, real-time monitoring, and secure data sharing. These capabilities empower smart cities to optimize resource allocation, enhance public safety, improve traffic management, and provide personalized services to residents [1,2]. By analyzing vast amounts of data collected from sensors, IoT devices, and other sources, AI facilitates intelligent automation, helping urban systems address complex challenges efficiently [3,4]. Similarly, in industrial manufacturing, robotics is undergoing a parallel transformation driven by the need for adaptability and automation [5]. Traditionally, robotic tasks in manufacturing have been performed in carefully structured environments with repetitive trajectories. However, when the environment changes, robots often require extensive reprogramming to adapt. Addressing this limitation, AI-driven approaches, such as learning from human demonstrations, provide robots with the ability to adapt to varying task scenarios, making them more versatile and resilient [6]. In this context, Movement Primitives (MPs) emerge as a well-established method in robotics. MPs enable the generalization of movements through temporal and spatial modulation, allowing robots to sequence and combine different primitives to accomplish complex tasks efficiently [7]. The core challenge lies in how to learn and adapt MPs from finite human demonstrations. Current methods can be categorized into two groups: (i) probabilistic approaches and (ii) dynamical systems. Probabilistic approaches commonly involve methods such as Hidden Markov Models–Gaussian Mixture Regression (HMM-GMR) [8] and Gaussian Mixture Models–Gaussian Mixture Regression (GMM-GMR) [9], which model the joint probability distributions of multiple demonstrations and derive the desired trajectories via regression. Although these methods capture the statistical variability of demonstrations, they struggle to adapt to new task scenarios. To address this, Task-Parameterized Gaussian Mixture Models (TP-GMMs) [10] incorporate task parameters into GMMs, enabling adaptation to novel conditions. However, TP-GMMs fail when the task parameters are not included in the learning process. Probabilistic Movement Primitives (ProMPs) [11] represent movements via hierarchical Bayesian models with probabilistic conditioning to achieve via-point generalization, capturing correlations from different demonstrations. However, ProMPs need a great number of demonstrations to obtain reliable distributions, and the extrapolation capability is limited once via-points are out of the original range. On the other hand, dynamical systems approaches, such as Dynamic Movement Primitives (DMPs) [12,13], describe movements using nonlinear differential equations modulated by external forcing terms. DMPs are well suited for generalizing start and end points from a single demonstration but are unable to handle intermediate via-points effectively. To enhance their capabilities, some works have combined DMPs with reinforcement learning (RL) to learn couplings across different control variables [14]. Recently, hybrid methods that combine DMPs and ProMPs have been proposed. Probabilistic Dynamic Movement Primitives (ProDMPs) [15] convert DMPs’ numerical integration into basis functions to denote trajectories with ProMPs. However, the number of DMP trajectories required to learn ProMPs affects the performance. Via-point Movement Primitives (VMPs) [16] denote movements with elementary and shape trajectories, adapting to single or multiple demonstrations. A threshold is defined to distinguish DMP extrapolation and ProMP interpolation. However, VMPs may result in high accelerations at transition points with nonsmooth trajectories. When adapting MPs to different task scenarios, via-point generalization is not sufficient. Task constraints manifested as inequalities like position and velocity limits, obstacles, and virtual walls are imposed on MPs. The objective is to generate trajectories in close proximity to the original trajectories while satisfying task constraints. MP adaptation methods are extensively researched; for instance, DMPs extend obstacle avoidance by imposing repulsive forces in the acceleration domain [17]. Position limits are ensured by transforming trajectories into exogenous states [18]. However, DMPs are prone to becoming trapped in local minima when multiple forces act mutually. Moreover, DMPs are adapted to a particular trajectory, losing additional freedom to execute tasks in different ways. In contrast, ProMPs encode variations with a covariance matrix, and the variations can be further exploited to satisfy task constraints. Ref. [19] combines multiple ProMPs to achieve obstacle avoidance. However, it is impractical to foresee all situations by adding primitives. Another way is to exclude specific regions from the original ProMPs via optimization; for instance, ref. [20] formulates optimization as a policy search problem with Kullback–Leibler (KL) divergence and reward functions defined to derive the policy, while the work in [21] frames optimization by minimizing the Mahalanobis distance to the original ProMPs in conjunction with obstacle distance computation. However, the methods mentioned above only optimize ProMPs for the robot end-effector, with no regard to possible collisions for other robot links. Moreover, they only handle obstacle constraints, and other task constraints, like virtual walls and joint limits, are not considered. Ref. [22] incorporates multiple task constraints into an integrated probabilistic framework; they directly optimize ProMPs with gradient ascent–descent algorithms. However, ProMPs will affect Gaussians in each timestep, and the timesteps satisfying task constraints will be optimized, leading to a long optimization time. In this work, we propose an improved via-point generalization method that allows a robot to acquire skills from only one demonstration. ProMPs are encoded by generalizing trajectories with via-points rather than providing more demonstrations. Furthermore, we propose an improved task-constrained ProMP adaptation method by separating each timestep with Gaussians and only optimizing feature timesteps violating task constraints. To summarize, our contributions are as follows: (1) We propose an improved via-point generalization method that allows a robot to acquire skills from only one human demonstration with smooth trajectories generalized to learn ProMPs; (2) We propose an effective task-constrained ProMP optimization method that incorporates all task constraints analytically into a probabilistic framework; (3) Given optimized ProMPs, we propose a unified robot movement adaptation method extending from a single obstacle to various obstacles. This paper is organized as follows: Section 2 provides derivations of the theoretical formulas and methods involved in this paper, with Section 2.1 presenting via-point trajectory generalization and ProMP encoding, Section 2.2 formulating the task-constrained optimization problem, and Section 2.3 introducing the optimization procedure; Section 3.2 outlines the complete robot movement adaptation procedure in principle. We validate and compare our method through experiments in Section 4. Finally, we conclude this work and outline future research directions. For clarity, all definitions and notations employed in this paper are summarized in Table 1. 2. Theoretical Formulation and Methodology 2.1. Generating Trajectories and Encoding Distributions 2.1.1. Via-Point Trajectory Generalization Via-point trajectory generalization enables robots to quickly generate a diverse set of feasible motions for a given skill while retaining key features from human demonstrations. This process includes generalizing both positions and orientations, allowing the robot to efficiently generate task-specific trajectories while maintaining task diversity by teaching the robot new skills. We provide three modes, namely, moving above, left, and right around the obstacle, as shown in Figure 1. Only one demonstration is required for each mode, and it is obtained by tracking human hand movements with the Kinect FORTH system [23]. We select the human hand centroid as the tracking point, with position and orientation sets x,y,z,qx,qy,qz,qw recorded in T timesteps. To ensure trajectory smoothness, we apply a Moving Average Filter (MAF) with a window width n and transform the trajectory into the robot base frame. To learn and generalize human demonstration features, we express the trajectory as a combination of the elementary trajectory h and shape modulation f. (1)Position Generalization Position Generalization Regarding 3D positions, we denote position sets p by yp=hp+fp. The elementary trajectory hp is a line directly connecting the start point p1 and the end point pend, with the shape modulation defined as fp=p−p1+rp1pend→, where r is the scale factor denoting the proportion of projective points on the line hp. If no intermediate via-points are required, we directly generate new projective points between a new start point p1new and end point pendnew according to r, with the learned shape modulation fp added to generalize new trajectory sets pnew=p1new+rp1newpendnew→+fp. Once a via-point tvia,pvia is needed, we scale the shape modulation with the coefficient k=pvia−p1new+rp1newpendnew→fp−1. Subsequently, new trajectory sets are generalized to pnew=p1new+rp1newpendnew→+kfp. (2)Orientation Generalization Orientation Generalization Considering that orientation dimensions are affected by each other, we define the trajectory as yo=ho×fo, where o=qw,qx,qy,qz represents quaternion sets, and × denotes quaternion multiplication. Spherical linear interpolation (SLERP) provides an efficient way to interpolate between two quaternions while preserving their spherical geometry. It ensures a constant angular velocity along the shortest path on the unit sphere, which results in smooth and natural transitions between orientations. We thus derive the elementary trajectory ho by SLERP with the ratio set uniformly from 0 to 1 to learn fo. Once new start and end quaternion sets are required during generalization, we compute a new elementary trajectory honew with the learned shape modulation fo superimposed to generate a new orientation trajectory yonew. Note that there is no need to generalize orientation via-points, as we can generalize 3D positions with via-points while keeping the original orientations. 2.1.2. Modeling Joint Distributions with Probabilistic Movement Primitives Probabilistic Movement Primitives (ProMPs) [24] are probabilistic methods for encoding variances in joint or Cartesian space. To integrate all task constraints analytically, we encode ProMPs in joint space. Given the position and orientation sets generalized in Section 2.1.1, we obtain all joint sets qti=1N via the Jacobian inverse kinematics (IK) algorithm, where qt∈RD, t∈0,T, N is the demonstration number, and D is the DoF number. ProMPs model joint positions as a linear combination of Gaussian basis functions Ψt and a weight vector wi in the presence of zero-mean Gaussian noise as εn, qit=ΨtTwi+εn. Joint positions in one demonstration are expressed with the weight vector wi via linear ridge regression [25]. Using the parameters θ=μw,Σw to estimate the weight vector w with Gaussian distributions, the probability of observing a trajectory τ given θ becomes (1)pτ;θ=pτ|wpw;θdw=∏tNqtΨtTμw,ΨtTΣwΨt+Σn where μw and Σw denote the mean and covariance of the weight vector, which are derived by maximum likelihood estimation, and Σn is the Gaussian noise covariance. To model joint distributions with ProMPs, we first transform the human hand trajectory into the robot base frame and derive the corresponding joint values. These joint values are then represented as a combination of K Gaussian basis functions and their associated weights. We compute the mean and covariance of the weights. The weight distributions capture the variability in human demonstrations and enable the robot to execute tasks flexibly. During task execution, the robot samples trajectories from the joint distribution at each timestep, ensuring adaptability while preserving the demonstrated variability. 2.2. Optimizing Distributions with Task Constraints 2.2.1. Problem Formulation Given the initial ProMPs p0w=Nw;μw0,Σw0, the main objective is to obtain optimized ProMPs that are as close as possible to the original ProMPs while satisfying task constraints. Achieving this trade-off requires a method to quantify the similarity between distributions while effectively encoding task constraints. In this context, KL divergence is particularly suitable due to its closed-form solution for Gaussian distributions, ensuring computational efficiency. Additionally, the cumulative distribution function (CDF) offers a probabilistic framework that incorporates variability and uncertainty into the constraints, considering the stochastic nature of ProMPs. As a result, we use KL divergence to denote the similarity and formulate task constraints with a cumulative distribution function (CDF). Since ProMPs model each timestep as a Gaussian distribution, we separate each timestep with Gaussian distributions by converting the initial ProMPs to the mean and standard deviation μt0,σt0 of each timestep t, with the new mean and standard deviation μt∗,σt∗ optimized under task constraints separately. Let ck,t=Cμt,σt denote the kth task constraint function related to μt,σt, and let Fck,tμt,σt denote the CDF of ck,t. We can rewrite the optimization problem as a Lagrangian function:(2)Lμt,σt,λk,t=DKLNqt;μt,σt||Nqt0;μt0,σt0+∑k,tλk,tαk,t−Fck,tμt,σt where αk,t is the confidence level to represent the task constraint probability Pμt,σtHck,t⩾αk,t, H represents one-sided or two-sided inequality constraints, and the optimized μt∗,σt∗ are derived by gradient ascent–descent methods: μt∗,σt∗,λk,t∗=maxλk,t⩾0minμt,σtLμt,σt,λk,t. Subsequently, given μt∗,σt∗, we implement another optimization step to obtain the optimized ProMPs as μw∗=minμwΨtTμw−μt∗,Σw∗=minΣwΨtTΣwΨt−σt∗. In the next section, we formulate every task constraint by defining the function ck,t=Cμt,σt related to μt,σt, expressing the inequality constraint function H and approximating the CDF Fck,tμt,σt. 2.2.2. Constraint Definitions in Joint and Cartesian Space (1)Joint Range Limit Joint Range Limit In this paper, we focus on retaining the original joint ranges learned from ProMPs. We formulate the constraint function ck,t=qtμt,σt and denote two-sided constraints by Pμt,σtqk,tmin