## 3 Offline Optimization and Adaptation Under Nonlinear Task Constraints
### Optimization and Adaptation Procedure
#### 3.3.2 Adaptation Procedure
where \(\mathbf{R}_{f}\) is the projection matrix formulated from the unit vector \(\mathbf{n}^{f}(\mathbf{\xi})\) for the init vel \(\mathbf{\hat{\xi}}_{\rm init}\) along with two orthonormal bases \(\mathbf{e}_{1}^{f}(\mathbf{\xi}),\mathbf{e}_{2}^{f}(\mathbf{\xi})\):

\[\mathbf{R}_{f}(\mathbf{\xi})=\left[\mathbf{n}^{f}(\mathbf{\xi}),\mathbf{e}_{1}^{f}(\mathbf{\xi}),\mathbf{e}_ {2}^{f}(\mathbf{\xi})\right] \tag{50}\]

We then evaluate the weighted mean in this \(\mathbf{\kappa}\) space as follows:

\[\mathbf{\kappa}(\mathbf{\xi})_{\rm all}=\sum_{k=1}^{O}\mathbf{w}_{k}(\mathbf{\xi}_{k})\mathbf{ \kappa}\Big{(}\mathbf{\hat{\xi}}_{k},\mathbf{\tilde{\xi}}\Big{)} \tag{51}\]

with the combined direction \(\mathbf{q}(\mathbf{\xi})_{\rm all}\) expressed back to the original 3D space as follows:

\[\dot{\xi}_{\text{all}}=\sum_{k=1}^{O}\exp\left(-\frac{1}{\rho_{k}}(\Gamma(\xi_{k })-1)\right)\mathbf{w}_{k}(\xi_{k})\Big{(}\dot{\xi}_{k}^{L}+\dot{\xi}_{k}^{R}\times \big{(}\dot{\xi}_{k}/\eta_{k}\big{)}\Big{)} \tag{45}\]0

The final modulated velocity and position for the \(O\) obstacles can be computed with Equation (46) and velocity integration (Equation (34)). To sum up, the complete real-time robot end effector adaptation method comprises the following steps: First, we obtain the initial task execution trajectory with \(T\) timesteps and \(dt\) integration steps after the offline optimization and adaptation procedure in Section 3.3.2. Given the initial velocity at \(t=0\) with position differencing, the modulated velocity is computed from Equation (46) with the presence of dynamic obstacles. We then apply velocity integration (Equation (34)) to obtain the expected position for the next timestep. The offline trajectory beyond the current timestep is treated as a template trajectory. Given the current position and the changing target point, we utilize the proposed trajectory generalization method in Section 2.1 to generate the remaining trajectory and compute the velocity via position differencing. The velocity is treated as the initial velocity at the next timestep, and we apply modulation iteratively to obtain the whole modulated trajectory with the expected velocity for the robot end-effector. Note that if the modulated positons collide, we treat them as init positions and modulate them iteratively until safe. During the real-time end-effector modulation process, we leverage the demonstration information from the offline optimization phase through trajectory generalization, preserving the human demonstration characteristics. The initial velocity at the next timestep is determined based on the generalized trajectory, rather than being arbitrarily chosen, which integrates offline optimization and online modulation. The end-effector modulation provides the modulated velocity and the desired position at each timestep, which are utilized for subsequent real-time non-end-effector adaptation. ### Real-Time Non-End-Effector Adaptation

We ensure real-time obstacle avoidance for the robot end effector in Section 4.1; however, the potential collisions between other robot links (non-end-effector) and obstacles are not considered. To achieve real-time non-end-effector adaptation, given the expected velocity in Cartesian space, we utilize the robot joint redundancy with null-space velocity control. In particular, the 7-DoF manipulator is considered in this paper; its end effector velocity \(\mathbf{x}_{v}\) and joint velocity \(\mathbf{q}_{v}\) conform to:

\[\dot{\xi}_{\text{all}}=\sum_{k=1}^{O}\exp\left(-\frac{1}{\rho_{k}}(\Gamma(\xi_{k })-1)\right)\mathbf{w}_{k}(\xi_{k})\Big{(}\dot{\xi}_{k}^{L}+\dot{\xi}_{k}^{R}\times \big{(}\dot{\xi}_{k}/\eta_{k}\big{)}\Big{)} \tag{45}\]1

where \(\mathbf{J}\) is a \(6\times 7\) Jacobian matrix.