## 5 NMPC Motion Planning Scheme  
### System Description  
To model the \(n\)-DOF robotic manipulator (chosen as the right arm of our Baxter robot) with joint positions \(\mathbf{q}\in\mathbb{R}^{n}\), we consider an input-affine nonlinear system of the form  
\[\mathbf{\dot{\chi}}=\mathbf{f}(\mathbf{\chi})+\mathbf{g}(\mathbf{\chi})\mathbf{\tau}, \tag{2}\]  
where \(\mathbf{\chi}=[\mathbf{q},\mathbf{\dot{q}}]\in\mathbb{R}^{2n}\) is the the state of the system and \(\mathbf{\tau}\in\mathbb{R}^{n}\) is the control input. Functions \(\mathbf{f}:\mathbb{R}^{2n}\rightarrow\mathbb{R}^{2n}\) and \(\mathbf{g}:\mathbb{R}^{2n}\rightarrow\mathbb{R}^{2n\times n}\) are continuously differentiable in \(\mathbf{\chi}\).  
Consider an \(n\)-degrees-of-freedom manipulator with equations of motion defined as  
\[\mathbf{M}(\mathbf{q})\mathbf{\ddot{q}}+\mathbf{C}(\mathbf{q},\mathbf{\dot{q}})\mathbf{\dot{q} }+\mathbf{g}(\mathbf{q})=\mathbf{\tau}, \tag{3}\] \[\mathbf{x}=f_{\text{fwd}}(\mathbf{q}), \tag{4}\]  
where \(\mathbf{M}(\mathbf{q})\in\mathbb{R}^{n\times n}\) is a symmetric positive definite matrix, \(\mathbf{C}(\mathbf{q},\mathbf{\dot{q}})\in\mathbb{R}^{n\times n}\) is a matrix of Coriolis and centrifugal forces terms, \(\mathbf{g}(\mathbf{q})\in\mathbb{R}^{n}\) is a vector of gravitational forces, \(\mathbf{q}\in\mathbb{R}^{7}\) are the joint positions, and \(f_{\text{fwd}}:\mathbb{R}^{7}\rightarrow\mathbb{R}^{3}\) is the kinematic transformation from the joint angle positions \(\mathbf{q}\) to the Cartesian position of the end-effector \(\mathbf{x}\in\mathbb{R}^{3}\). We consider the joint torque \(\mathbf{\tau}\in\mathbb{R}^{7}\) as the system input and \(\mathbf{x}\) as the system output.  
Defining \(\mathbf{J}=\mathbf{J}(\mathbf{q})=\frac{\partial f_{\text{fwd}}}{\partial\mathbf{q}}\) as the Jacobian matrix, it follows that  
\[\mathbf{\dot{x}} =\mathbf{J}(\mathbf{q})\mathbf{\dot{q}}, \tag{5a}\] \[\mathbf{\ddot{x}} =\mathbf{J}(\mathbf{q})\mathbf{\ddot{q}}+\mathbf{\dot{J}}(\mathbf{q})\mathbf{\dot{q}}. \tag{5b}\]  
By substituting (5a) into (3), the Cartesian robotic system dynamics can be expressed as  
\[\mathbf{M}_{x}(\mathbf{q})\mathbf{\ddot{x}}+\mathbf{C}_{x}(\mathbf{q},\mathbf{\dot{q}})\mathbf{\dot{x}}+ \mathbf{g}_{x}(\mathbf{q})=f_{\mathbf{h}}, \tag{6}\]  
where \(\mathbf{M}_{x}(\mathbf{q})=\mathbf{J}^{\dagger\intercal}\mathbf{M}(\mathbf{q})\mathbf{J}^{\dagger}\), \(\mathbf{C}_{x}(\mathbf{q},\mathbf{\dot{q}})=\mathbf{J}^{\dagger\intercal}(\mathbf{C}(\mathbf{q},\mathbf{ \dot{q}})-\mathbf{M}(\mathbf{q})\mathbf{I}^{\dagger}\mathbf{\dot{j}})\mathbf{J}^{\dagger}\), \(\mathbf{g}_{x}(\mathbf{q})=\mathbf{J}^{\dagger\intercal}\mathbf{g}(\mathbf{q})\) and \(\mathbf{f}_{\mathbf{h}}=\mathbf{J}^{\dagger}\mathbf{\tau}\) is the vector of generated control forces. \(\mathbf{J}^{\dagger}=\mathbf{J}^{\dagger}(\mathbf{q})\) denotes the pseudo-inverse of \(\mathbf{J}(\mathbf{q})\), that is:  
\[\mathbf{J}^{\dagger}(\mathbf{q})=(\mathbf{J}^{\intercal}(\mathbf{q})\mathbf{J}(\mathbf{q}))^{-1}\mathbf{ J}^{\intercal}(\mathbf{q}). \tag{7}\]  
For notational simplicity, \(\mathbf{M}_{\mathbf{x}}\), \(\mathbf{C}_{\mathbf{x}}\), \(\mathbf{g}_{\mathbf{x}}\) will be used to denote \(\mathbf{M}_{x}(\mathbf{q})\), \(\mathbf{C}_{x}(\mathbf{q},\mathbf{\dot{q}})\), \(\mathbf{g}_{x}(\mathbf{q})\), respectively. The system (2) is converted from joint space to task space, which can be expressed as  
\[\begin{bmatrix}\mathbf{\ddot{x}}\\ \mathbf{\dot{x}}\end{bmatrix}=\underbrace{\begin{bmatrix}\mathbf{\dot{x}}\\ -\mathbf{M}_{x}^{-1}(\mathbf{C}_{x}\mathbf{\dot{x}}+\mathbf{g}_{x})\end{bmatrix}}_{f(\mathbf{x}, \mathbf{\dot{x}})}+\underbrace{\begin{bmatrix}0\\ -\mathbf{M}_{x}^{-1}\end{bmatrix}}_{g(\mathbf{x})}f_{\mathbf{h}}. \tag{8}\]  
Thus, the robotic system (8) is equivalent to the nonlinear affine system (2) with respect to the input \(\mathbf{f}_{\mathbf{h}}\). Therefore, the ECBF can be applied to the robotic system (8) to deal with the time-varying output constraints.