## 3 Offline Optimization and Adaptation Under Nonlinear Task Constraints
### Optimization and Adaptation Procedure
#### 3.3.2 Adaptation Procedure
To be generic, we derive the modulated trajectory from the original dynamics system (DS) in Equation (33) as follows:

\[\mathbf{\dot{\xi}}_{k}=\mathbf{M}(\mathbf{\xi}_{k})\Big{(}\mathbf{f}(\mathbf{\xi}_{k})-\mathbf{\dot{ \xi}}_{k}\Big{)}+\mathbf{\dot{\xi}}_{k}\ \ \text{with}\ \mathbf{\dot{\xi}}_{k}=\mathbf{\xi}_{k}^{L}+\mathbf{\xi}_{k}^{R}\times(\mathbf{\xi}-\mathbf{\xi}_ {k}^{c}) \tag{37}\]where \(\xi_{k}\) denotes the relative velocity induced by the obstacle motion. The modulation matrix \(\mathbf{M}(\xi_{k})\) is composed of the orthogonal basis matrix \(\mathbf{E}(\xi_{k})\) and the corresponding eigenvalue matrix \(\mathbf{D}(\xi_{k})\) as follows:

\[\mathbf{M}(\xi_{k})=\mathbf{E}(\xi_{k})\mathbf{D}(\xi_{k})\mathbf{E}(\xi_{k})^{-1} \tag{38}\]

For notational brevity, we define the relative position to the obstacle center \(\xi_{k}=\mathbf{\zeta}-\xi_{k}^{c}\). Based on Equation (36), we derive the deflection hyperplane at point \(\xi\) with the normal vector \(\mathbf{n}\big{(}\xi_{k}\big{)}\) denoted as the distance function gradient \(d\Gamma(\xi)/d\xi\):

\[\mathbf{n}\big{(}\tilde{\xi}_{k}\big{)}=\left[\frac{\partial\Gamma_{k}\big{(}\xi_ {k}\big{)}}{\partial(\xi)_{1}},\frac{\partial\Gamma_{k}\big{(}\tilde{\xi}_{k} \big{)}}{\partial(\xi)_{2}},\frac{\partial\Gamma_{k}\big{(}\tilde{\xi}_{k} \big{)}}{\partial(\xi)_{3}}\right]^{T} \tag{39}\]

Given the normal vector, we formulate the basis matrix as follows:

\[\mathbf{E}(\xi_{k})=\big{[}\mathbf{n}\big{(}\xi_{k}\big{)},\mathbf{e}_{1}\big{(}\xi_{k} \big{)},\mathbf{e}_{2}\big{(}\xi_{k}\big{)}\big{]} \tag{40}\]

where the tangent vectors are orthonormal to the normal as follows:

\[ \begin{cases}\mathbf{e}_{1}\big{(}\tilde{\xi}_{k}\big{)}=&\big{[}-\mathbf{n}\big{(} \tilde{\xi}_{k}\big{)}_{2},\mathbf{n}\big{(}\tilde{\xi}_{k}\big{)}_{1},0\big{]}\\ \mathbf{e}_{2}\big{(}\tilde{\xi}_{k}\big{)}=&\mathbf{n}\big{(}\tilde{\xi}_{k}\big{)} \times\mathbf{e}_{1}\big{(}\tilde{\xi}_{k}\big{)}\end{cases}  \tag{41}\]

We define the related diagonal eigenvalue matrix \(\mathbf{D}(\xi_{k})\) to modify the normal and tangent directions independently with eigenvalues. The normal eigenvalue is selected to ensure impenetrability, that is, the eigenvalues should be zero on the obstacle surface. Meanwhile, the tangent eigenvalue should have the same magnitude in each direction to increase the velocity for moving around the obstacle. We formulate \(\mathbf{D}(\xi_{k})\) as follows:

\[\mathbf{D}(\xi_{k})=\mathbf{diag}(\lambda_{\mathbf{n}}(\xi_{k}),\lambda_{e}(\xi_{k}), \lambda_{e}(\xi_{k})) \tag{42}\]

Considering the safety margin, reactivity, and tail-effect in [30], we define the eigenvalue as follows:

\[ \begin{cases}\lambda_{\mathbf{n}}(\xi_{k})&=\begin{cases}1-\frac{1}{|\Gamma(\xi_ {k})|^{\frac{1}{R}}}&\mathbf{n}\big{(}\tilde{\xi}_{k}/\eta_{k}\big{)}^{T}\tilde{ \xi}_{k}<0\\ 1&\mathbf{n}\big{(}\tilde{\xi}_{k}/\eta_{k}\big{)}^{T}\tilde{\xi}_{k}\geqslant 0 \end{cases}\\ \lambda_{e}(\xi_{k})&=1+\frac{1}{|\Gamma(\xi_{k})|^{\frac{1}{R}}}\end{cases}  \tag{43}\]

where \(\eta_{k}\) is the safety magnitude for each direction, and \(\rho_{k}\) is the reactivity parameter to determine the velocity modulation amplitude, that is, the larger the \(\rho_{k}\), the earlier a robot reacts to the obstacle. To remedy the tail-effect, we check \(\mathbf{n}\big{(}\tilde{\xi}_{k}/\eta_{k}\big{)}^{T}\tilde{\xi}_{k}\) to evaluate whether the end effector is moving towards (negative) or away (positive) from the obstacle. When it extends to multiple obstacles, as in Figure 2, we encapsulate all obstacles to create a single virtual obstacle by the weighted mean of each modulated DS in Equation (37). The benefit is that we only need to apply modulation once to ensure collision avoidance.