## 3 Offline Optimization and Adaptation Under Nonlinear Task Constraints
### Optimization and Adaptation Procedure
#### 3.3.2 Adaptation Procedure
```
0:\(\bullet\) Obstacles information \(\left\{\text{obs}_{\mathbf{x}},\text{obs}_{\mathbf{y}},\text{obs}_{\mathbf{z}},\mathbf{R}\right\}\) \(\bullet\) Mode template trajectory \(\left\{\mathbf{x},\mathbf{y},\mathbf{z},\mathbf{\alpha}_{\text{x}},\mathbf{\mathbf{y}},\mathbf{



## 4 Real-Time Whole-Body Movement Adaptation

Following offline movement adaptation under the static scene, the robot needs to react in real-time to immediate changes in the dynamic environment, for instance, multiple stationary or moving obstacles, new target points, etc. The analytical optimization process in Section 3 can not fulfill the real-time demands. In this context, real-time dynamic modulation upon velocity control provides a promising solution. To be specific, the offline task execution trajectory can be treated as an initial dynamic system; the objective is to obtain the modulated system for real-time robot whole-body movement adaptation including end-effector and other links (non-end-effector). We present the end-effector and non-end-effector adaptation methods in Sections 4.1 and 4.2. ### Real-Time End-Effector Adaptation

In this paper, we assume \(O\) convex obstacles around the robotic arm. Movement adaptation for the end-effector is achieved in Cartesian space by modulating the 3D positions while keeping the original orientations. Consider a state variable \(\mathbf{\xi}\in\mathbb{R}^{3}\) denoting the end effector 3D positions; its temporal dynamics is governed by an autonomous (time-invariant) and continuous function \(f(\mathbf{\xi})\) as follows:

\[\mathbf{\dot{\xi}}=\mathbf{f}(\mathbf{\xi}),\mathbf{f}:\mathbb{R}^{3}\mapsto\mathbb{R}^{3} \tag{33}\]

Given the initial state \(\mathbf{\xi}_{0}\), the trajectory can be iteratively computed by integrating \(f(\mathbf{\xi})\) according to:

\[\mathbf{\xi}_{t}=\mathbf{\xi}_{t-1}+\mathbf{f}\big{(}\mathbf{\xi}_{t-1}\big{)}dt \tag{34}\]

where \(t\) is a positive integer and \(dt\) is the integration time step. For each obstacle \(k(k=1\dots O)\), we define a distance function \(\Gamma(\mathbf{\xi}_{k})\) with a continuous first-order partial derivative and monotonically increasing value away from the obstacle center \(\mathbf{\xi}_{k}^{c}\) to distinguish the space around an obstacle into three regions:

\[ \begin{split}\text{Exterior points}:\mathcal{H}^{e}& =\Big{\{}\mathbf{\xi}_{k}\in\mathbb{R}^{3}:\Gamma(\mathbf{\xi}_{k})>1 \Big{\}}\\ \text{Boundary points}:\mathcal{H}^{b}&=\Big{\{} \mathbf{\xi}_{k}\in\mathbb{R}^{3}:\Gamma(\mathbf{\xi}_{k})=1\Big{\}}\\ \text{Interior points}:\mathcal{H}^{i}&=\Big{\{} \mathbf{\xi}_{k}\in\mathbb{R}^{3}:\Gamma(\mathbf{\xi}_{k})<1\Big{\}}\end{split}  \tag{35}\]

To envelop obstacles, we construct the ellipsoid-shaped function \(\Gamma(\mathbf{\xi}_{k})\) as follows:

\[\Gamma(\mathbf{\xi}_{k})=\sum_{i=1}^{3}\left(\|\mathbf{\xi}_{k}-\mathbf{\xi}_{k}^{c}\|/a_ {i}\right)^{2p_{i}^{k}} \tag{36}\]

where \(a_{i}\) is the \(i\)-th axis length and \(p_{i}^{k}\) is a positive curvature parameter. As shown in Figure 2, we parametrize the \(k\)-th obstacle (static or moving) as a vector \(\mathbf{P}_{k}=\left\{\mathbf{\xi}_{k}^{c},\mathbf{a}_{k},\mathbf{\xi}_{k}^{L},\mathbf{\xi}_{k}^{ R}\right\}\), where \(\mathbf{a}_{k}\) is the axis length, \(\mathbf{\xi}_{k}^{L}\) and \(\mathbf{\xi}_{k}^{R}\) are the linear and angular velocity with respect to the center \(\mathbf{\xi}_{k}^{c}\). Note that the static obstacle is a special case where the angular and linear velocities equal 0. Real-time obstacle avoidance for the end effector is achieved via a dynamic modulation matrix \(\mathbf{M}(\mathbf{\xi}_{k})\in\mathbb{R}^{3\times 3}\) given that the local modulation matrix will not bring in new extrema with a cheap computational cost and closed-form solution [44].