## 3 Offline Optimization and Adaptation Under Nonlinear Task Constraints
### Optimization and Adaptation Procedure
#### 3.3.2 Adaptation Procedure  
Subsequently, given optimized ProMPs distribution \(\big{(}\mathbf{\mu}_{\mathbf{w}}^{*},\mathbf{\Sigma}_{\mathbf{w}}^{*}\big{)}\), we present the complete offline robot movement adaptation procedure. During the learning phase, the human first demonstrates three skills to avoid a single obstacle. The human hand centroid positions and orientations are equivalent to the robot end effector trajectories. For each mode, we generalize trajectory sets to the same start and end points, normalized to the same timestep \(T\), to formulate the template trajectory. Aiming to generate feasible collision-free trajectory sets, we choose feature 3D via-points with equal intervals to generalize the initial trajectories (see Section 2.1). We check the collisions and fine-tune the via-points with threshold to construct initial collision-free trajectories \(\big{\{}\mathbf{x},\mathbf{y},\mathbf{z},\mathbf{o}_{\mathbf{x}},\mathbf{o}_{\mathbf{y}},\mathbf{o}_{\mathbf{z}}, \mathbf{o}_{\mathbf{w}}\big{\}}_{i=1}^{N}\) and obtain corresponding joint position sets \(\{\mathbf{q}\}_{i=1}^{N}\). We encode the initial joint distributions with ProMPs (see Section 2.2) as \(p_{0}(\mathbf{w})=\mathcal{N}\big{(}\mathbf{w};\mathbf{\mu}_{\mathbf{w}}^{0},\mathbf{\Sigma}_{\mathbf{ w}}^{0}\big{)}\). To speed up optimization, we first find feature timesteps that actually violate task constraints, then impose task constraints at those timesteps \(\mathbf{t}_{\text{feature}}\) and optimize the ProMPs distributions (see Section 3.3.1). The optimized ProMPs \(p(\mathbf{w}^{*})=\mathcal{N}\big{(}\mathbf{w}^{*};\mathbf{\mu}_{\mathbf{w}}^{*},\mathbf{\Sigma}_{ \mathbf{w}}^{*}\big{)}\), the mean and standard deviation \(p(\mathbf{q}_{t})=\mathcal{N}\big{(}\mathbf{q}_{t};\mathbf{\mu}_{t}^{*},\mathbf{\sigma}_{t}^{*} \big{)}\) are treated as safe joint distributions. During task execution, the robot first determines the start and end point by visual information, next randomly selects the running mode from three modes \(k\sim\{above,left,right,right\}\), then samples the via-points with Gaussian distributions as \(\mathbf{p}_{via}=\mathcal{N}\big{(}\mathbf{\mu}_{xyz},\mathbf{\sigma}_{xyz}\big{)}\). The mean \(\mathbf{\mu}_{xyz}\) and standard deviation \(\mathbf{\sigma}_{xyz}\) are computed from the initial via-points range. The trajectory is then generalized and fine-tuned to ensure safety according to the sampled via-point. The optimized \(\mathbf{\mu}_{t}^{*}\) and \(\mathbf{\sigma}_{t}^{*}\) represent the joint limit range as \(\mathbf{q}_{t}^{\text{safe}}\in(\mathbf{\mu}_{t}^{*}-2\mathbf{\sigma}_{t}^{*},\mathbf{\mu}_{t}^ {*}+2\mathbf{\sigma}_{t}^{*})\) with the initial joint value denoted as \(\mathbf{q}_{t}^{0}=\mathbf{\Psi}_{t}^{T}\mathbf{w}^{*}\). The joint limited range and initial value are passed to the Jacobian IK algorithm to test whether the solution is calculated successfully. The succeed number of all feature timesteps decides whether the robot can perform the task successfully; that is, if the succeed number is more than a threshold, the robot directly runs the generalized trajectory, if else, the robot continues sampling until success. To obtain the threshold automatically, we perform equal space sampling from the initial via-points range, record the success result and corresponding succeed number, then determine the threshold at the turning point between success and failure. Note that if there are no feature timesteps that violate the task constraints, the robot only needs to generalize the trajectory without optimizing the distribution. Meanwhile, if any of the three modes can not be optimized, that is, the probability satisfying the task constraints always equals 0, we eliminate the mode from the optional modes. The complete offline optimization and adaptation procedure is outlined in Algorithm 1.