# EMG-Driven Shared Control Architecture for Human-Robot Co-Manipulation Tasks +
## 2 Materials and Methods
### Low-Level Layer
#### 2.2.1 Inverse Kinematics and Joints Controller  
Considering a serial manipulator with \(n\) Degrees of Freedom (DOFs), the state of the system is described by the vector \(\mathbf{q}=\left[q_{1},q_{2},\cdots,q_{n}\right]^{T}\in\mathbb{R}^{n}\), which contains the joint positions. Define the vector that gathers the end-effector configuration as \(\mathbf{x}=\left[\mathbf{p}^{T}\ \mathbf{o}^{T}\right]^{T}\in\mathbb{R}^{7}\), where \(\mathbf{p}=\left[p_{x},p_{y},p_{z}\right]^{T}\in\mathbb{R}^{3}\) represents the coordinates of the end-effector expressed in the arm base frame, while \(\mathbf{o}=\left[q_{x},q_{y},\mathbf{o}_{z},\mathbf{o}_{w}\right]^{T}\in\mathbb{R}^{4}\) is the quaternion that expresses the orientation of the end-effector with respect to the arm base frame. The velocity of the end-effector can be described by the vector \(\mathbf{v}=\left[\dot{\mathbf{p}}^{T}\ \mathbf{\omega}^{T}\right]^{T}\in\mathbb{R}^{6}\), where \(\dot{\mathbf{p}}=\left[\dot{p}_{x},\dot{p}_{y},\dot{p}_{z}\right]^{T}\in\mathbb{R }^{3}\) and \(\mathbf{\omega}=\left[\omega_{x},\omega_{y},\omega_{z}\right]^{T}\in\mathbb{R}^{3}\) represent the end-effector linear and angular velocities, respectively. The differential relationship between the end-effector velocity and the joint velocity vector \(\dot{\mathbf{q}}=\left[\dot{q}_{1},\dot{q}_{2},\cdots,\dot{q}_{n}\right]^{T}\) can be expressed as \(\mathbf{v}=\mathbf{J}\mathbf{q}\), where \(\mathbf{J}\in\mathbb{R}^{6\times n}\) is the robot Jacobian matrix. Assuming a redundant manipulator, i.e., \(n>6\), and the availability of a _desired_ end-effector trajectory expressed as:  
\[\mathbf{x}_{d}= \begin{bmatrix}\mathbf{p}_{d}\\ \mathbf{o}_{d}\end{bmatrix} \in\mathbb{R}^{7}\quad\mathbf{v}_{d}= \begin{bmatrix}\dot{ \mathbf{p}}_{d}\\ \mathbf{\omega}_{d}\end{bmatrix} \in\mathbb{R}^{6}\quad\mathbf{a}_{d}= \begin{bmatrix} \ddot{\mathbf{p}}_{d}\\ \mathbf{\alpha}_{d}\end{bmatrix} \in\mathbb{R}^{6}\, \tag{1}\]  
where \(\mathbf{x}_{d}\) represents the desired position and quaternion, \(\mathbf{v}_{d}\) gathers the desired linear and angular velocities, and \(\mathbf{a}_{d}\) represents the desired linear and angular accelerations. The desired joint velocity that makes the end-effector track the desired trajectory can be computed by resorting to the Closed-Loop Inverse Kinematics (CLIK) algorithm:  
\[\dot{\mathbf{q}}_{d}=\mathbf{J}^{\dagger}(\mathbf{v}_{d}+\mathbf{K}_{\rm ik}\bar{\mathbf{x}})\, \tag{2}\]  
Figure 1: Proposed shared control architecture. The High-Level layer is responsible for the EMG signal acquisition and classification. Then, the recognized class is used into a finite-state machine algorithm that outputs a set of admittance gains. The Low-Level layer is responsible for the motion control of the manipulator and employs a variable admittance controller, an inverse kinematics algorithm and a joints controller. where \(\mathbf{J}^{\dagger}\) is the Moore-Penrose pseudoinverse of the Jacobian matrix, \(\mathbf{K}_{\text{ik}}\in\mathbb{R}^{6\times 6}\) is a positive-definite matrix of gains, and \(\tilde{\mathbf{x}}\) is the error vector, defined as:  
\[\tilde{\mathbf{x}}= \begin{bmatrix}\tilde{\mathbf{p}}\\ \tilde{\mathbf{o}}\end{bmatrix} = \begin{bmatrix}\mathbf{p}_{d}-\mathbf{p}(\mathbf{q}_{d})\\ \mathbf{o}_{d}^{-1}\star\mathbf{o}(\mathbf{q}_{d})\end{bmatrix} \in\mathbb{R}^{6}\, \tag{3}\]  
where \(\tilde{\mathbf{p}}\) and \(\tilde{\mathbf{o}}\) are the position and quaternion errors, respectively, and \"\(\star\)\" is the quaternion product operator; \(\mathbf{q}_{d}\) is the vector of the desired joint position obtained by numerically integrating the desired joint velocity vector \(\dot{q}_{d}\); \(\mathbf{p}(\mathbf{q}_{d})\) and \(\mathbf{o}(\mathbf{q}_{d})\) are the position and orientation of the end-effector obtained by considering \(\mathbf{q}_{d}\) as joint positions in the direct kinematics computation.