## 2 Via-Point Trajectory Generalization and Initial Distributions Encoding
### Model Joint Space Distributions with Probabilistic Movement Primitives  
Probabilistic Movement Primitives (ProMPs) [10] are probabilistic methods based on hierarchical Bayesian models to encode variances in Joint or Cartesian space. To promote integrating all nonlinear task constraints for the robot end effector and other links analytically, we model the Joint space distributions with ProMPs. Given the positions and orientations \(\{\mathbf{x}_{i},\mathbf{y}_{t},\mathbf{z}_{t},\mathbf{o}_{i}\}_{i=1}^{N}\) generalized in Section 2.1, we obtain all feasible joint sets \(\{\mathbf{q}_{i}\}_{i=1}^{N}=F_{\text{IK}}\Big{(}\{\mathbf{x}_{i},\mathbf{y}_{t},\mathbf{z}_{ t},\mathbf{o}_{i}\}_{i=1}^{N}\Big{)}\) by the Jacobian inverse kinematic (IK) algorithm [38]. \(\mathbf{q}_{t}\in\mathbb{R}^{D}\), \(t\in[0,T]\), \(N\) is the number of demonstrations, and \(D\) is the joint DOFs.  
ProMPs model joint positions as a linear combination of Gaussian basis functions \(\mathbf{\
abla}(t)\) and a weight vector \(\mathbf{w}_{i}\) under the presence of a zero-mean Gaussian noise \(\mathbf{\varepsilon}_{n}\),  
\[\mathbf{q}_{i}(t)=\mathbf{\
abla}(t)^{\mathrm{T}}\mathbf{w}_{i}+\mathbf{\varepsilon}_{n} \tag{10}\]  
Joint positions in one demonstration are expressed with the weight vector \(\mathbf{w}_{i}\) via linear ridge regression [39]. Using parameters \(\mathbf{\theta}=\{\mathbf{\mu}_{\mathbf{w}},\mathbf{\Sigma}_{\mathbf{w}}\}\) to estimate the weight vector \(\mathbf{w}\) with Gaussian distributions \(p(\mathbf{w};\mathbf{\theta})=\mathcal{N}(\mathbf{w}|\mathbf{\mu}_{\mathbf{w}},\mathbf{\Sigma}_{\mathbf{ w}})\), the probability of observing a trajectory \(\mathbf{\tau}\) given \(\mathbf{\theta}\) becomes:  
\[\begin{split}& p(\mathbf{\tau};\mathbf{\theta})=p(\mathbf{\tau}|\mathbf{w})p( \mathbf{w};\mathbf{\theta})d\mathbf{w}\\ &=\prod_{t}\mathcal{N}\Big{(}\mathbf{q}(t)|\mathbf{\
abla}(t)^{\mathrm{T }}\mathbf{\mu}_{\mathbf{w}},\mathbf{\
abla}(t)^{\mathrm{T}}\mathbf{\Sigma}_{\mathbf{w}}\mathbf{\
abla} (t)+\mathbf{\Sigma}_{n}\Big{)}\end{split} \tag{11}\]  
where \(\mathbf{\mu}_{\mathbf{w}}\) and \(\mathbf{\Sigma}_{\mathbf{w}}\) denote the mean and covariance of the weight vector, which are derived by maximum likelihood estimation, and \(\mathbf{\Sigma}_{n}\) is the covariance of the Gaussian noise. When it comes to new task conditions \(\mathbf{Q}^{*}=\Big{[}\mathbf{q}^{*},\mathbf{\Sigma}_{q}^{*}\Big{]}\), where \(\mathbf{q}^{*}\) and \(\mathbf{\Sigma}_{q}^{*}\) are the expected mean and covariance of the joint positions at time \(t\), the updated parameters \(\mathbf{\theta}^{\text{new}}=\{\mathbf{\mu}_{\mathbf{w}}^{\text{new}},\mathbf{\Sigma}_{\mathbf{w} }^{\text{new}}\}\) can be computed based on Bayesian theory as follows:  
\[p(\mathbf{w}|\mathbf{Q}^{*})\ \propto\mathcal{N}\Big{(}\mathbf{Q}^{*}|\mathbf{\
abla}^{* \mathrm{T}}\mathbf{w},\mathbf{\Sigma}_{q}^{*}\Big{)}p(\mathbf{w}) \tag{12}\]  
\[\begin{split}\mathbf{\mu}_{\mathbf{w}}^{\text{new}}&=\mathbf{ \mu}_{\mathbf{w}}+\mathbf{\Sigma}_{\mathbf{w}}\mathbf{\
abla}^{*}\Big{(}\mathbf{\Sigma}_{q}^{*}+ \mathbf{\
abla}^{*\mathrm{T}}\mathbf{\Sigma}_{\mathbf{w}}\mathbf{\
abla}^{*}\Big{)}^{-1}\Big{(} \mathbf{q}^{*}-\mathbf{\
abla}^{*\mathrm{T}}\mathbf{\mu}_{\mathbf{w}}\Big{)}\\ \mathbf{\Sigma}_{\mathbf{w}}^{\text{new}}&=\mathbf{\Sigma}_{\mathbf{ w}}-\mathbf{\Sigma}_{\mathbf{w}}\mathbf{\
abla}^{*}\Big{(}\mathbf{\Sigma}_{q}^{*}+\mathbf{\
abla}^{* \mathrm{T}}\mathbf{\Sigma}_{\mathbf{w}}\mathbf{\
abla}^{*}\Big{)}^{-1}\mathbf{\
abla}^{* \mathrm{T}}\mathbf{\Sigma}_{\mathbf{w}}\end{split} \tag{13}\]  
where \(\mathbf{\
abla}^{*}\) denotes the basis function at time \(t\). The Joint positions under new task conditions \(\mathbf{p}^{\text{new}}\) are sampled from Gaussian distributions as follows:  
\[\begin{split}\mathbf{w}^{\text{new}}&\sim\mathcal{N}( \mathbf{\mu}_{\mathbf{w}}^{\text{new}},\mathbf{\Sigma}_{\mathbf{w}}^{\text{new}})\\ \mathbf{p}^{\text{new}}&=\mathbf{\
abla}^{\mathrm{T}}\mathbf{w}^ {\text{new}}\end{split} \tag{14}\]