## 3 Offline Optimization and Adaptation Under Nonlinear Task Constraints
### Optimization and Adaptation Procedure
#### 3.3.2 Adaptation Procedure
)\); the expected end effector pose can be derived with velocity integration:

\[\mathbf{x}_{d}=\mathbf{\xi}_{\text{mod}}+\mathbf{\xi}_{\text{mod}}dt=\mathbf{x}_{v}+\mathbf{x}_{v }dt \tag{67}\]

we then express the feed-back controller error \(\mathbf{e}(\mathbf{x})\) as follows:

\[\mathbf{e}(\mathbf{x})=\mathbf{x}_{d}-T(\mathbf{q}) \tag{68}\]

and further denote the resulted joint gradient \(\mathbf{d}\mathbf{q}\) as follows:

\[\mathbf{d}\mathbf{q}=\mathbf{J}^{\mathbf{\tilde{\mathbf{e}}}}\mathbf{e}(\mathbf{x})+\gamma\mathbf{N}\Bigl{(} \mathbf{N}^{T}\mathbf{N}+\lambda\mathbf{N}^{T}\mathbf{\Sigma}_{\mathbf{q}_{v}}{}^{-1}\mathbf{N}\Bigr{)} ^{-1}\mathbf{N}^{T}\Bigl{(}\mathbf{u}_{\mathbf{q}_{v}}-\mathbf{J}^{\mathbf{\tilde{\mathbf{e}}}}\mathbf{x} _{v}\Bigr{)}+\rho\mathbf{(J}_{0}\mathbf{N})^{\mathbf{\tilde{\mathbf{e}}}}\Bigl{(}\mathbf{x}_{v}^{ 0}-\mathbf{J}_{0}\mathbf{J}^{\mathbf{\tilde{\mathbf{e}}}}\mathbf{x}_{v}\Bigr{)} \tag{69}\]

We derive \(\mathbf{q}\) with integration \(\mathbf{q}=\mathbf{q}+\mathbf{d}\mathbf{q}\mathbf{t}\), and continue the iteration process until the error meets the stopping criterion to obtain the modulated joint positions \(\mathbf{q}_{\text{mod}}\) for real-time task execution. To integrate the offline and real-time robot movement adaptation methods presented in Sections 3 and 4, we summarize the overall control framework in Figure 4.

Given an offline task execution trajectory \(\left\{\mathbf{x},\mathbf{y},\mathbf{z},\mathbf{o}\right\}_{t=1}^{T}\) obtained from Algorithm 1, the initial velocity \(\mathbf{\xi}_{\text{init}}^{t}\) is computed using position differencing. In the presence of \(O\) dynamic obstacles, represented as \(\left\{\mathbf{\xi}_{k}^{c},\mathbf{a}_{k},\mathbf{\xi}_{k}^{L},\mathbf{\xi}_{k}^{R}\right\}_ {k=1}^{O}\), we derive the modulation matrix \(\mathbf{M}(\mathbf{\xi}_{k})\in\mathbb{R}^{3\times 3}\) and the weight coefficient \(\mathbf{w}_{k}(\mathbf{\xi}_{k})\) for each obstacle. Additionally, we identify the closest point \(C_{0}\) on the robot's non-end-effector links and define the corresponding vector \(\mathbf{d}_{0}\). The avoidance velocity \(\mathbf{x}_{v}^{0}=v_{0}\mathbf{n}_{0},\mathbf{n}_{0}=\frac{\mathbf{d}_{0}}{\|\mathbf{d}_{0}\|}\) is then determined accordingly from Equation (66). Figure 4: The integrated offline and real-time robot movement adaptation control diagram. For online end-effector adaptation, we compute the equivalent modulation magnitude and direction for each obstacle, denoted as \(\boldsymbol{q}(\boldsymbol{\tilde{\xi}})_{k}\|\boldsymbol{Q}(\boldsymbol{\tilde{ \xi}})\|_{k}\). These are then combined using weighted summation with \(\boldsymbol{w}_{k}(\boldsymbol{\tilde{\xi}}_{k})\), along with hyper-sphere projection \(\boldsymbol{\kappa}\in\mathbb{R}^{2}\), to obtain the overall modulation effect \(\boldsymbol{q}(\boldsymbol{\tilde{\xi}})_{\text{all}}\|\boldsymbol{Q}( \boldsymbol{\tilde{\xi}})\|_{\text{all}}\). The resulting modulated velocity \(\boldsymbol{\dot{\xi}}_{\text{mod}}\) serves as the desired end-effector velocity. The expected position \(\boldsymbol{x}_{d}\) at the next timestep is determined via velocity integration. Note that if \(\boldsymbol{x}_{d}\) collides, we treat it as the initial position and modulate it iteratively until safe. For online non-end-effector adaptation, given the mean and diagonal covariance matrix \(\left(\boldsymbol{\mu}_{\boldsymbol{q}_{v}}\boldsymbol{\Sigma}_{\boldsymbol{q }_{v}}\right)\) from offline optimization with 7 DoF Joint ProMPs, along with the null-space velocity component, Equation (69) is constructed to design the control diagram. Given the expected position \(\boldsymbol{x}_{d}\), the system iterates through a control loop, minimizing the error defined in Equations (67)-(69) until the stopping criteria are met. If any joint configuration results in a collision, the nominal velocity norm \(v_{0}\) is iteratively increased, and the control loop is repeated until a safe motion is achieved.