## 6 Controller Designs  
### Inner Joint Angle Control Loop  
As illustrated in Figure 8, the primary objective of the inner joint controller is to stabilize the manipulator dynamics, represented by the nonlinear Equation (29). This dynamic model can be simplified to the following form:  
\[M(q)\ddot{q}+h\big{(}q,\dot{q}\big{)}=u \tag{30}\]  
where:  
\[M(q)=D(q)+J \tag{31}\]  
\[h(q,\dot{q})=(C(q,\dot{q})+\frac{B}{r})\dot{q}+\text{g}(q) \tag{32}\]  
To facilitate control design, the input \(u\) is redefined as:  
\[u=M(q)v+h(q,\dot{q}) \tag{33}\]  
Here \(v\in\mathbb{R}^{6\times 1}\) is a virtual input. Substituting Equation (33) into Equation (30), and noting that \(M(q)e\in\mathbb{R}^{6\times 6}\) is invertible, the system dynamics simplify to:  
\[\ddot{q}=v \tag{34}\]  
This transformation technique is known as feedback linearization, resulting in a system of six decoupled double integrators, as described in Equation (34). The entire feedback linearization framework is illustrated in Figure 9. In this control architecture, the actual joint angles \(q\in\mathbb{R}^{6\times 1}\) are regulated to follow the desired joint angles \(q_{R}\in\mathbb{R}^{6\times 1}\). Figure 8: Inner joint angle control loop. A nonlinear interface block computes the actual control input \(u\in\mathbb{R}^{6\times 1}\) from the virtual control input \(v\)\(\epsilon\mathbb{R}^{6\times 1}\) using Equation (33). This block also calculates the matrices \(M(q)\in\mathbb{R}^{6\times 6}\), and \(h\left(q,\dot{q}\right)\)\(\epsilon\mathbb{R}^{6\times 1}\) based on the current joint angles \(q\) and their velocities \(\dot{q}\). The linear joint controller is designed using Youla parameterization technique [27] to control the nominally linear system in Equation (34). Since the six equations in (34) are decoupled and identical in structure, a Single-Input Single-Output (SISO) controller can be first designed and then extended to a Multiple-Input Multiple-Output (MIMO) controller by duplication. The SISO version is defined as:  
\[v_{SISO}=\ddot{q}_{SISO} \tag{35}\]  
Let the SISO controller be denoted \(G_{C_{MIMO}}^{Inner}\), then the MIMO controller is:  
\[G_{C_{MIMO}}^{Inner}=G_{C_{SISO}}^{Inner}\cdot I_{6\times 6} \tag{36}\]  
where \(I_{6\times 6}\) is a \(6\times 6\) identity matrix. The transfer function of the SISO plant, derived from Equation (34), is:  
\[G_{P_{SISO}}^{Inner}=\frac{1}{s^{2}} \tag{37}\]  
This system has two Bounded Input Bounded Output (BIBO) unstable poles at the origin. To ensure the internal stability of the closed-loop system, the transfer function \(T_{SISO}\), must satisfy the interpolation conditions [36]:  
\[M(q)=D(q)+J \tag{31}\]0  
\[M(q)=D(q)+J \tag{31}\]1  
Using the relationship:  
\[M(q)=D(q)+J \tag{31}\]2  
\[M(q)=D(q)+J \tag{31}\]3 Feedback linearization Youla control design for inner loop. the Youla parameter \(Y_{SISO}^{inner}\) can be derived from a properly chosen \(T_{SISO}^{inner}\) that satisfies Equations (38) and (39). Without delving into the full design details, a candidate closed-loop transfer function that meets these conditions is:  
\[M(q)=D(q)+J \tag{31}\]4  
where \(\tau_{in}\) determines the location of poles and zeros, and thus the system's bandwidth. Tuning \(\tau_{in}\) allows the controller to achieve a desired tradeoff between speed and overshoot. The next step is to derive \(G_{CISO}^{Inter}\) from relationships between the closed-loop transfer function, \(T_{SISO}^{inner}\), the sensitivity transfer function, \(S_{SISO}^{inner}\), and the Youla transfer function, \(Y_{SISO}^{inner}\), in Equations (42)-(44):  
\[M(q)=D(q)+J \tag{31}\]5  
\[M(q)=D(q)+J \tag{31}\]6  
\[M(q)=D(q)+J \tag{31}\]7  
Substituting back into Equation (36), the resulting MIMO controller is:  
\[M(q)=D(q)+J \tag{31}\]8  
The final expression for the closed-loop MIMO system is:  
\[M(q)=D(q)+J \tag{31}\]9