## 2 Background Theory
### Kinematics of Manipulators  
The configuration of a robotic manipulator system is described in terms of a vector of generalized coordinates \(q\in\mathbb{R}^{n_{e}}\), which are a complete and independent set of scalar coordinates. The degrees of freedom are the minimal set of coordinates needed to describe the system configuration, which in our case correspond to the number of actuated joints in the manipulator \(n_{e}\), and each coordinate in \(q\) is a joint angle.  
The forward kinematic equations \(\chi_{e}(q)\) are a geometric mapping from the manipulator configuration in generalized coordinates \(q\) to the desired end-effector pose task \(p_{e}\) in Cartesian space: \(p_{e}=\chi_{e}(q)\). In our case, we desire a target end-effector position in the 2D plane, and hence \(p_{e}\in\mathbb{R}^{2}\), consisting of the \(x_{I}\) and \(z_{I}\) Cartesian coordinates at a given time \(t\).  
For kinematic control of the end-effector, we set the corresponding target velocity \(v_{e}\in\mathbb{R}^{2}\) of the end-effector based on a generated trajectory velocity profile that follows the desired trajectory positions. The relationship between the forward kinematics in position space and the forward differential kinematics in velocity space is described through the Jacobian matrix \(J(q)\in\mathbb{R}^{2\times n_{e}}\):  
\[\dot{p}_{e}=v_{e}=J(q)\dot{q} \tag{3}\]  
where \(J(q)=\frac{\partial\chi_{g}(q)}{\partial q}\), and \(\dot{q}\) is the vector of joint velocities at time \(t\). For brevity's sake, all Jacobians \(J(q)\) will be hereafter denoted as \(J\), with the implicit relationship to the joint state \(q(t)\).  
We invert this relationship to obtain the inverse differential kinematic relation which defines the required joint velocities to achieve our desired end-effector velocity at time \(t\):  
\[\dot{q}=J^{-1}v_{e} \tag{4}\]  
The inverse \(J^{-1}\) exists as long as the matrix \(J\) is square and invertible, i.e., the manipulator is not in a singular configuration where \(Det(J)\approx 0\). These are typically configurations where the links are aligned in a straight line, and we initialize the manipulator configuration to avoid such configurations. In the case of a redundant manipulator (\(n_{e}>2\)), the matrix \(J\) is not square and, for a desired Cartesian velocity task, infinite solutions of \(\dot{q}\) that achieve the desired \(v_{e}\) are possible. To find the optimal joint velocities, we minimize the objective function \(g(\dot{q})\) for desired joint velocities \(\dot{q}_{0}\):  
\[g(\dot{q})=\tfrac{1}{2}\big{(}\dot{q}-\dot{q}_{0}\big{)}^{T}W\big{(}\dot{q}-\dot {q}_{0}\big{)} \tag{5}\]  
where \(W\in\mathbb{R}^{2\times 2}\) is a symmetric positive definite weighting matrix. This objective function is minimized subject to the forward differential kinematic constraint (3) and formulated as a Lagrange multiplier problem:  
\[g^{\prime}(q,\lambda)=\frac{1}{2}\big{(}\dot{q}-\dot{q}_{0}\big{)}^{T}W\big{(} \dot{q}-\dot{q}_{0}\big{)}-\lambda^{T}\big{(}v_{e}-J(q)\dot{q}\big{)} \tag{6}\]  
where \(\lambda\in R^{n_{e}}\) is the vector of Lagrange multipliers. We find the minimum of \(g^{\prime}\big{(}\dot{q},\lambda\big{)}\) by solving \(\
abla_{(\dot{q},\lambda)}g^{\prime}\big{(}\dot{q},\lambda\big{)}=0\). This is carried out through setting \(\frac{\partial g^{\prime}\big{(}\dot{q},\lambda\big{)}}{\partial\dot{q}}=0\) and \(\frac{\partial g^{\prime}\big{(}\dot{q},\lambda\big{)}}{\partial\lambda}=0\), which results in the following solution:  
\[\dot{q}=W^{-1}J^{T}(JW^{-1}J^{T})^{-1}v_{e}+(I-W^{-1}J^{T}(JW^{-1}J^{T})^{-1}) \dot{q}_{0} \tag{7}\]  
If we desire the solution which minimizes joint velocities, it is sufficient to set \(\dot{q}_{0}=0\), and if the weighting of all joints is equal (i.e., \(W=I\)), then the solution to the redundant manipulator kinematic control equation is:  
\[\dot{q}=J^{T}(JJ^{T})^{-1}v_{e}=J^{+}v_{e} \tag{8}\]  
with \(J^{+}=J^{T}(JJ^{T})^{-1}\) as the Moore-Penrose right pseudo-inverse.  
Detailed derivation of these inverse kinematics control equations can be found in [21].