## 5 NMPC Motion Planning Scheme
### Nonlinear Model Predictive Control for Collision Avoidance  
The robot's objective is for its end-effector to follow the estimated position of the human's right hand while avoiding collisions. To that end, we propose an NMPC formulation as a high-level control system that commands angular velocity references to a low-level control system (see Section 6.2). The overall controller architecture is shown in Figure 4.  
The input of the high-level control system is the joint velocity commands \(\mathbf{u}(t)\). At every sampling time instant, we need to solve a finite-horizon optimal control problem with prediction horizon \(T_{p}>0\), stage cost function \(\ell\), and terminal cost function \(\ell_{f}\), where \(\ell\) and \(\ell_{f}\) are given by  
\[\ell(\mathbf{\hat{x}},\mathbf{\hat{u}})=e(\hat{\mathbf{p}}_{\text{rh}},\mathbf{\hat{x}})^{ \intercal}\mathbf{R}_{p}e(\hat{\mathbf{p}}_{\text{rh}},\mathbf{\hat{x}})+\hat{\mathbf{u}}^{ \intercal}\mathbf{R}_{v}\mathbf{\hat{u}}, \tag{9}\]  
and  
\[\ell_{f}(\mathbf{\hat{x}})=\mathbf{e}(\hat{\mathbf{p}}_{\text{rh}}(T_{p}),\mathbf{\hat{x}}(T_ {p}))^{\intercal}\mathbf{Q}_{p}\mathbf{e}(\hat{\mathbf{p}}_{\text{rh}}(T_{p}),\mathbf{\hat{x}} (T_{p}))+\hat{\mathbf{u}}^{\intercal}(T_{p})\mathbf{Q}_{v}\mathbf{\hat{u}}(T_{p}), \tag{10}\]  
respectively. Here, \(\mathbf{e}(\hat{\mathbf{p}}_{\text{rh}}(T_{p}),\mathbf{x}(t))\) is an error metric between the given Cartesian goal \(\hat{\mathbf{p}}_{\text{rh}}(T_{p})\) and the current Cartesian state of the robot \(\mathbf{x}(t)\). The reference position \(\hat{\mathbf{p}}_{\text{rh}}(T_{p})\) is obtained by the motion predictor. \(\mathbf{Q}_{p}\) and \(\mathbf{Q}_{v}\) are constant weight matrices for terms in the  
Figure 3: Calculation of the distance, position, and orientation of the capsules for the representation of humans. terminal cost function, while \(\mathbf{R}_{p}\) and \(\mathbf{R}_{v}\) are the weight matrices for terms in the stage cost function. Following [39], the error function \(e(\mathbf{x}_{d},\hat{\mathbf{x}}(t))\) is defined as  
\[\mathbf{e}(\hat{\mathbf{p}}_{\text{rh}}(t),\mathbf{x}(t))= \begin{bmatrix}-\mathbf{I}_{3}&0\\ 0&\mathbf{\Pi}_{d}\end{bmatrix} \hat{\mathbf{x}}(t)+ \begin{bmatrix}\hat{\mathbf{p}}_{\text {rh}}(t)\\ 0\end{bmatrix} , \tag{11}\]  
where matrix \(\mathbf{\Pi}_{d}\) is built from the desired quaternion, which remains constant during the optimization. The initial and final configurations of the robot \(\mathbf{q}_{0}=[q_{01},\dots,q_{07}]\) and \(\mathbf{q}_{f}=[q_{f1},\dots,q_{f7}]\) are obtained using the inverse kinematics function \(\mathbf{f}_{\text{inv}}:\mathbb{R}^{6}\rightarrow\mathbb{R}^{7}\), which converts the Cartesian end effector poses (position and orientation) to the corresponding joint positions. Using the GJK algorithm, the minimum distance calculation at time \(t\) is performed based on the Cartesian coordinates of the robot, \(\mathbf{x}(t)\), and the predicted human pose, \(\hat{\mathbf{p}}_{o}(t)\). The distance between the end-effector and the predicted operator's position \(\lambda(\hat{\mathbf{x}}(t);\hat{\mathbf{p}}_{o}(t))\) must be greater than the minimum safe distance \(d_{\text{safe}}\). The value of \(d_{\text{safe}}\) is chosen to be greater than the maximum prediction error bound to guarantee the safety of humans. The constraints are described as  
\[\hat{\mathbf{q}}(t)=\hat{\mathbf{u}}(t),t\in[0,T_{p}], \tag{12a}\] \[\hat{\mathbf{q}}(0)=\mathbf{q}_{0},\] (12b) \[\hat{\mathbf{x}}(t)=f_{\text{twd}}(\hat{\mathbf{q}}(t)),t\in[0,T_{p}],\] (12c) \[\lambda(\hat{\mathbf{x}}(t);\hat{\mathbf{p}}_{o}(t))\geq d_{\text{safe}},t\in[0,T_{p}], \tag{12d}\]  
where \(\mathbf{q}=(q_{1},\dots,q_{7})\) denotes the state of the robotic arm. The control actions are constrained in the set \(\mathcal{U}\).