Interactional Modeling and Optimized PD Impedance Control Design for Robust Safe Fingertip Grasping

Abstract

Dynamic and robust control of fingertip grasping is essential in robotic hand manipulation. This study introduces detailed kinematic and dynamic mathematical modeling of a two-fingered robotic hand, which can easily be extended to a multi-fingered robotic hand and its control. The Lagrangian technique is applied as a common procedure to obtain the complete nonlinear dynamic model. Fingertip grasping is considered in developing the detailed model. The computed torque controller of six proportional derivative (PD) controllers, which use the rotation angle of the joint variables, is proposed to linearize the hand model and to establish trajectory tracking. An impedance controller of optimized gains is suggested in the control loop to regulate the impedance of the robotic hand during the interaction with the grasped object in order to provide safe grasping. In this impedance controller, the gains are designed by applying a genetic algorithm to reach minimum contact position and velocity errors. The robustness against disturbances is achieved within the overall control loop. A computer program using MATLAB is used to simulate, monitor, and test the interactional model and the designed controllers.

1. Introduction

A recent research on service robotics has shown that there is an environmental reaction on major applications [1]. The robotic hand is the main interactive part of the environment with the service robotics [2, 3]. Grasping operation [4] has been used explicitly in robotic hand during the interactive period which represents the process of holding objects via two stages: free motion and constrained motion. Free motion is the moving of fingers before detecting the grasped object, and the constrained motion is the state of fingers when the object is detected. The common purpose in grasping is to handle unknown objects. Thus, finding both a model system and a control algorithm to deal with non-predefined objects is essential in a robotic hand. Regarding modeling, interactional modeling is the demand to perform a specific control algorithm. With interactional modeling, the contact force between the robotic hand and the grasped objects has obtained without needing their properties. In control point of view, the robotic hand holds an object by its fingers and moves the object to another place. In this case, there are different modes of operation which can be listed as before holding the grasped object, holding the grasped object, and moving the robotic hand while holding the grasped object. The aim at each mode is distinct to the others and has to respond to the operation assigned. Thus, the multitude of various goals which have to be achieved with regard to the various operations presents an interesting scenario in the control of a robotic hand.

Generally, it is essential to obtain the modeling of a multi-fingered robotic hand before implementing a specific model-based control algorithm. Arimoto et al. [5] presented a two-fingertip grasping model to control the rolling contact between the fingertips and the grasped object. Arimoto et al. derived the model through the use of the Euler-Lagrange equation with respect to the geometry of the fingertips which is represented by the arc length of a fingertip. Corrales et al. [6] developed a model for a three-fingered robotic hand; in their study the transmission of fingertip contact force from the hand to the grasped object is considered. Boughdiri et al. [7] discussed the issue of modeling a multi-fingered robotic hand as a first step, before designing controllers. They considered free motion only, without the constraints of grasped objects. In a proceeding work, Boughdiri et al. later [8] considered the constraints of grasped objects and the coupling between fingers in grasping tasks.

In the control scenario of a robotic-environment interaction, there is generally also a similar case of a robotic hand-grasped object interaction; force tracking is necessary to obtain appropriate and safe contacts. In force tracking, three main position-force controllers are introduced: stiffness control, hybrid position/force control, and impedance control [9]. Among the above three controllers, impedance control presents an integrated process to constrained manipulators [10]. The impedance controller is introduced by Hogan in 1985 to set the dynamic conduct between the motion of the robot end-effector and the contact force with the environment, instead of regarding the control of motion and force individually [11, 12]. Chen et al. [13] derived impedance controller for an elastic joint-type robotic hand in Cartesian and joint spaces. Chen et al. focused on the compensation of gravity and managing the parameter of uncertainties. Huang et al. [14] proposed the impedance controller for grasping by two fingers to deal with the problem of dropping objects when grasping by one finger. Zhang et al. [15] proposed an adaptive impedance controller with friction compensation to track forces of the multi-fingered robotic hand.

However, the synthesis of impedance control design continues to exist as a major challenge in the performance advancement of robotic hand in order to provide safe and robust grasping. Besides this, the greatest problems in robotic hand-grasped object interaction are the existence of nonlinearity in the dynamic system, dealing with different grasped objects, disturbances, and interference. In the previous studies, the aforementioned problems have not taken in consideration. The aim of this paper is to treat these problems using interactional modeling and the design gains of impedance control. The action of the suggested control expression is to provide a fingertip interactional robust control algorithm that takes into account the dynamics of the robotic hand under unknown contact conditions. To accomplish this, we propose first to linearize the dynamics on the level of position-tracking controller through six PD controllers. When executed, the calculated gains of the designed PD controllers result in zero steady errors without overshoot. Second, through an impedance controller, the interactional fingertips’ force is regulated by the robotic hand’s impedance. The gains of the impedance are designed based on genetic algorithm. As a result, the introduced control expression enables the position and velocity of fingertips to be controlled simultaneously without changing over control subspace. This control expression avoids selecting between different controllers which causes indirect control and consumption of high energy. The operation and the robustness of the presented control expression are approved by MATLAB/Simulink in several tests.

The rest of the chapter is organized as follows. In Section 2, the mathematical representation of kinematics and dynamics of the robotic hand with environment interaction is derived and explained. In Section 3, the overall controllers of position tracking and impedance control are designed. In addition, the suggested procedure of obtaining optimum values of impedance gains using genetic algorithm is explained. In Section 4, following the joint path and fingertip position is tested. Then, the robustness of the system is examined. Finally, the conclusions of this study are presented in Section 5.

2. Mathematical model

The models of a robotic hand system are the mathematical equations that describe all the contact forces and motion of the fingers at any time [16]. These models are essential for grasping and manipulation operations which include kinematics, dynamics, and contacts between the fingers and the environment. A multi-fingered robotic hand with an existing grasped object has a complex structure. Thus, deriving the mathematical representation is not a simple task. In this section, the mathematical representation for a two-fingered robotic hand with an environment is developed. Changing the orientation of the robotic hand with respect to the inertial (earth) frame during manipulation operation is considered in this analysis.

2.1 Reference frames

The system is a two-fingered robotic hand that can adjust fingertip grasp forces by the torque of actuators in the joints. Each finger includes three joints (MCP, PIP, and DIP) and three rigid phalanges (proximal, intermediate, and distal). The motion of the finger actuates through the torque in the joints. The variables $i=1,2$ and $j=1,2,3$ represent the index of the finger and the joint, respectively. The analysis is based on the four main reference frames: finger frames $O_{\mathit{ij}}$ , hand base frame $O_{\mathit{hb}}$ , fingertip contact point frames ${O_{\mathit{tip}}}_i$ , and inertia frame $O_e$ as shown in Figure 1. The joints MCP, PIP, and DIP are located at $O_{i1}$ , $O_{i2}$ , and $O_{i3}$ , respectively. The length $L_{\mathit{ij}}$ represents the distance from $O_{\mathit{ij}}$ to $O_{i\left(j+1\right)}$ . The angle of rotation ${\theta }_{\mathit{ij}}$ around $z_{j-1}$ associates with the $i^{\mathit{th}}$ phalanx and $j^{\mathit{th}}$ joint. The coordinate frame $O_{\mathit{ij}}$ is attached in order to express the coordinates of any point on the phalanges as a result of the motion of ${\theta }_{\mathit{ij}}$ . In our case, expressing the motion of the fingertip points $({x_{\mathit{tip}}}_1,{y_{\mathit{tip}}}_1,{z_{\mathit{tip}}}_1)$ and $({x_{\mathit{tip}}}_2,{y_{\mathit{tip}}}_2,{z_{\mathit{tip}}}_2)$ is the primary interest. The frame $O_{\mathit{hb}}$ is defined to switch between the motions of the fingertips with respect to the base of the hand through matrices. The frames ${O_{\mathit{tip}}}_i$ are defined to represent the contact points between the fingertips and the environment. Finally, the inertial frame $O_e$ which is assumed at $O_{\mathit{hb}}$ is assigned to represent the orientation of the robotic hand with respect to the earth.

Figure 1.

Coordinate frames of the robotic hand.

The result of orienting the robotic hand with respect to the earth coordinates is expressed by defining the three rotation angles roll, pitch, and yaw (RPY). In which, roll is ( $\alpha$ : around $x_{\mathit{hb}}$ -axis), pitch is ( $\gamma$ : around $y_{\mathit{hb}}$ -axis), and yaw is ( $\phi$ : around $z_{\mathit{hb}}$ -axis). Mathematical representation of rotation about a fixed axis (earth frame) can be expressed by a specified sequence of multiplying the free rotation matrices $\mathrm{Rx}\left(\mathrm{\alpha }\right)$ , $\mathrm{Ry}\left(\mathrm{\gamma }\right)$ , and $\mathrm{Rz}\left(\mathrm{\phi }\right)$ [17]. Thus, the final rotation matrix $R_e^{\mathit{hb}}$ which switches from the earth frame $O_e\left(x_e,y_e,z_e\right)$ to the robotic hand base frame is obtained as

$$R_e^{\mathit{hb}}=R\left(z_e{y}_e{x}_e,\mathit{yaw},\mathit{\text{pitch}},\mathit{\text{roll}}\right)$$

$$=R\left(z_e,\phi \right)R\left(y_e,\gamma \right)R\left(x_e,\alpha \right)$$

$$=\left[\begin{array}{ccc}{c}_{\phi }{c}_{\gamma }& c_{\phi }{s}_{\gamma }{s}_{\alpha }-s_{\phi }{c}_{\alpha }& c_{\phi }{s}_{\gamma }{c}_{\alpha }+s_{\phi }{s}_{\alpha }\\ s_{\phi }{c}_{\gamma }& s_{\phi }{s}_{\gamma }{s}_{\alpha }+c_{\phi }{c}_{\alpha }& s_{\phi }{s}_{\gamma }{c}_{\alpha }-c_{\phi }{s}_{\alpha }\\ -s_{\gamma }& c_{\gamma }{s}_{\alpha }& c_{\gamma }{c}_{\alpha }\end{array}\right].$$ E1

where $s$ and $c$ indicate the symbols of the sine and cosine functions, respectively.

2.2 Kinematic analysis

Forward kinematics is implemented here to express the relationship between the Cartesian spaces of the fingertip contact points, which are represented by coordinates ( ${x_{\mathit{tip}}}_i,{y_{\mathit{tip}}}_i,{z_{\mathit{tip}}}_i$ ) relative to the joint position ${\theta }_{\mathit{ij}}$ . The positions of fingertip 1 and fingertip 2 change by rotating the joints in clockwise and counterclockwise directions, respectively. According to the coordinate frames and the dimension parameters of Figure 1, the forward kinematic of finger 1 is expressed as

$$T_{{\mathit{tip}}_1}^{\mathit{hb}}=\left[\begin{array}{cccc}{c}_{123}& -s_{123}& 0& \frac{L_0}{2}+L_1{c}_1+L_2{c}_{12}+L_3{c}_{123}\\ s_{123}& c_{123}& 0& h+L_1{s}_1+L_2{s}_{12}+L_3{s}_{123}\\ 0& 0& 1& -\frac{b}{2}\\ 0& 0& 0& 1\end{array}\right],$$ E2

and the forward kinematic of finger 2 is

$$T_{{\mathit{tip}}_2}^{\mathit{hb}}=\left[\begin{array}{cccc}{c}_{123}& -s_{123}& 0& -\frac{L_0}{2}-L_1{c}_1-L_2{c}_{12}-L_3{c}_{123}\\ s_{123}& c_{123}& 0& h+L_1{s}_1+L_2{s}_{12}+L_3{s}_{123}\\ 0& 0& 1& -\frac{b}{2}\\ 0& 0& 0& 1\end{array}\right].$$ E3

where $L_0$ and $b$ are the length and the width of the palm shown in Figure 1, respectively. In Eqs. (2) and (3) and the rest of equations in this study, the denotation $c_1$ , $s_1$ , $c_{12}$ , $s_{12}$ , ${\mathrm{c}}_{123}$ , and ${\mathrm{s}}_{123}$ represents $\text{cos}{\mathrm{\theta }}_1,\text{sin}{\mathrm{\theta }}_1,\text{cos}\left({\mathrm{\theta }}_1+{\mathrm{\theta }}_2\right),\text{sin}\left({\mathrm{\theta }}_1+{\mathrm{\theta }}_2\right),\text{cos}\left({\mathrm{\theta }}_1+{\mathrm{\theta }}_2+{\mathrm{\theta }}_3\right),\text{and} \text{sin}\left({\mathrm{\theta }}_1+{\mathrm{\theta }}_2+{\mathrm{\theta }}_2\right)$ , respectively. To control the position of the three links, the joint variables ${\theta }_{i1},{\theta }_{i2},{\theta }_{i3}$ are calculated using inverse kinematic as

$${\theta }_{i2}=\pi -{\text{cos}}^{-1}\frac{{L_1}^2+{L_2}^2-{x_{{\mathit{tip}}_i}}^2-{y_{{\mathit{tip}}_i}}^2}{2L_1{L}_2},$$ E4

$${\theta }_{i1}={\text{tan}}^{-1}\frac{y_{{\mathit{tip}}_i}}{x_{{\mathit{tip}}_i}}-{\text{cos}}^{-1}\frac{{x_{{\mathit{tip}}_i}}^2+{y_{{\mathit{tip}}_i}}^2+{L_1}^2-{L_2}^2}{2l_1\sqrt{{x_{{\mathit{tip}}_i}}^2+{y_{{\mathit{tip}}_i}}^2}},$$ E5

$${\theta }_{i3}={\phi }_{\mathit{ie}}-{\theta }_1-{\theta }_2.$$ E6

where ${\phi }_{\mathit{ie}}$ is the angle of distal phalanx with respect to the axis $y_{i2}$ . Next, the Jacobian matrix for each finger is obtained based on the forward kinematic equations, which were derived from Eqs. (2) and (3) as

$$J_{{\mathit{\text{finger}}}_1}=\left[\begin{array}{ccc}-L_1{s}_1-L_2{s}_{12}-L_3{s}_{123}& -L_2{s}_{12}-L_3{s}_{123}& -L_3{s}_{123}\\ L_1{c}_1+L_2{c}_{12}+L_3{c}_{123}& L_2{c}_{12}+L_3{c}_{123}& L_3{c}_{123}\\ 0& 0& 0\end{array}\right],$$ E7

and

$$J_{{\mathit{\text{finger}}}_2}=\left[\begin{array}{ccc}{L}_1{s}_1+L_2{s}_{12}+L_3{s}_{123}& L_2{s}_{12}+L_3{s}_{123}& L_3{s}_{123}\\ L_1{c}_1+L_2{c}_{12}+L_3{c}_{123}& L_2{c}_{12}+L_3{c}_{123}& L_3{c}_{123}\\ 0& 0& 0\end{array}\right],$$ E8

The robotic hand transforms the generated torque by each joint to the wrenches exerted on the grasped object through the following Jacobian matrix:

$$J_{\mathit{\text{hand}}}=\left[\begin{array}{cc}{J}_{{\mathit{\text{finger}}}_1}& 0\\ 0& J_{{\mathit{\text{finger}}}_2}\end{array}\right].$$ E9

2.3 Contact interaction

A contact interaction, which uses contact behavior between rigid fingertips and a flexible grasped object, is proposed in this study (see Figure 2). The grasped object is assumed as a linear spring of stiffness $k_{\mathit{\text{object}}}$ . Hence, each fingertip exerts a force on the grasped object as

$$f_{{\mathit{tip}}_{x,i}}=k_{\mathit{\text{object}}}\left(x_{{\mathit{tip}}_{x,i}}-x_{{\mathit{\text{object}}}_{x,i}}\right),$$ E10

$$f_{{\mathit{tip}}_{y,i}}=k_{\mathit{\text{object}}}\left(y_{{\mathit{tip}}_{y,i}}-y_{{\mathit{\text{object}}}_{y,i}}\right),$$ E11

$$f_{{\mathit{tip}}_{z,i}}=k_{\mathit{\text{object}}}\left(z_{{\mathit{tip}}_{z,i}}-z_{{\mathit{\text{object}}}_{z,i}}\right).$$ E12

where $x_{{\mathit{\text{object}}}_i}$ , $y_{{\mathit{\text{object}}}_i}$ , and $z_{{\mathit{\text{object}}}_i}$ are the coordinate points of the grasped object. The current and the desired location of the fingertips are denoted by ( $x_{{\mathit{tip}}_i},y_{{\mathit{tip}}_i},z_{{\mathit{tip}}_i}$ ) and ( $x_{d_i}$ , $y_{d_i}$ , $z_{d_i}$ ), respectively.

Figure 2.

Side view of fingertip contact interaction system on the x-axis.

2.4 Dynamics of robotic hand

The motion equations are derived to describe the dynamics of the robotic hand when interacting with the grasped object; the explained fingertip forces in Eqs. (10)–(12) are involved in the dynamic equation. When ignoring friction, each finger in free motion can be described by the following general motion equation:

$$M_{{\mathit{\text{finger}}}_1}\left(q_1\right){\ddot{q}}_1+D_{{\mathit{\text{finger}}}_1}\left(q_1,{\dot{q}}_1\right){\dot{q}}_1+G_{{\mathit{\text{finger}}}_1}\left(q_1\right)={\tau }_{{\mathit{\text{finger}}}_1},$$ E13

$$M_{{\mathit{\text{finger}}}_2}\left(q_2\right){\ddot{q}}_2+D_{{\mathit{\text{finger}}}_2}\left(q_2,{\dot{q}}_2\right){\dot{q}}_2+G_{{\mathit{\text{finger}}}_2}\left(q_2\right)={\tau }_{{\mathit{\text{finger}}}_2},$$ E14

where $q_i\in R^3$ is the general coordinate vector, $M_{{\mathit{\text{finger}}}_i}\left(q_i\right)$ is the $3\times 3$ inertia matrix, $D_{{\mathit{\text{finger}}}_i}\left(q_i,{\dot{q}}_i\right)$ is the 3-vector which involves forces of centrifugal and Coriolis, $G_{{\mathit{\text{finger}}}_i}\left(q_i\right)$ is the vector of the gravitational force, and ${\tau }_{{\mathit{\text{finger}}}_i}$ is the vector of the input torque at joints. When the fingertips are interacting with the grasped object (see Figure 2), then Eqs. (13) and (14) become

$$M_{{\mathit{\text{finger}}}_1}\left(q_1\right){\ddot{q}}_1+D_{{\mathit{\text{finger}}}_1}\left(q_1,{\dot{q}}_1\right){\dot{q}}_1+G_{{\mathit{\text{finger}}}_1}\left(q_1\right)={\tau }_{{\mathit{\text{finger}}}_1}-J_{{\mathit{\text{finger}}}_1}^T{F}_{{\mathit{tip}}_1},$$ E15

$$M_{{\mathit{\text{finger}}}_2}\left(q_2\right){\ddot{q}}_2+D_{{\mathit{\text{finger}}}_2}\left(q_2,{\dot{q}}_2\right){\dot{q}}_2+G_{{\mathit{\text{finger}}}_2}\left(q_2\right)={\tau }_{{\mathit{\text{finger}}}_2}-J_{{\mathit{\text{finger}}}_2}^T{F}_{{\mathit{tip}}_2},$$ E16

where $J_{{\mathit{\text{finger}}}_i}$ is the Jacobian matrix (see Subsection 2.2) and $F_{{\mathit{tip}}_i}$ is the external force 3-vector with expression value:

$F_{{\mathit{tip}}_1}={\left[f_{{\mathit{tip}}_{x,1}}{f}_{{\mathit{tip}}_{y,1}}{f}_{{\mathit{tip}}_{z,1}}\right]}^T$ and $F_{{\mathit{tip}}_2}={\left[\begin{array}{ccc}{f}_{{\mathit{tip}}_{x,2}}& f_{{\mathit{tip}}_{y,2}}& f_{{\mathit{tip}}_{z,2}}\end{array}\right]}^T$ ;

to derive Eqs. (15) and (16), the Lagrange equations of motion are used. For each finger manipulator, the equation of motion is given by [18]

$$\frac{d}{\mathit{dt}}\frac{\partial T}{\partial {\dot{\theta }}_j}-\frac{\partial T}{\partial {\theta }_j}=Q_j+T_j.$$ E17

where $T$ is the total kinetic energy of the finger manipulator, $Q_j$ is the potential energy, and $T_j$ is the external torques. The x-y coordinates are represented according to the assigned frames of Figure 1. The velocities of the mass centers for the phalanges are important to find the kinetic energy. For proximal phalanx, the position of the mass center with respect to MP joint is represented by

$${\overline{x}}_1=d_1{c}_1,$$

$${\overline{y}}_1=d_1{s}_1\mathrm{;}$$

the position of the mass center of intermediate phalanx with respect to MP joint is represented by

$${\overline{x}}_2=L_1{c}_1+d_2{c}_{12}$$

$${\overline{y}}_2=L_1{s}_1+d_2{s}_{12}\mathrm{;}$$

and the position of the mass center of distal phalanx with respect to MP joint is represented by

$${\overline{\mathrm{x}}}_3={\mathrm{L}}_1{c}_1+{\mathrm{L}}_2{c}_{12}+{\mathrm{d}}_3{\mathrm{c}}_{123},$$

$${\overline{\mathrm{y}}}_3={\mathrm{L}}_1{s}_1+{\mathrm{l}}_2{s}_{12}+{\mathrm{d}}_3{\mathrm{s}}_{123},$$

where $d_i$ is the distance to the center of mass which is assumed in the middle of the phalanx. The total kinetic energy of the finger in Eq. (17) is expressed by the following equation:

$$\begin{array}{cc}T=& \frac{1}{2}{m}_1\left({{\dot{\overline{x}}}_1}^2+{{\dot{\overline{y}}}_1}^2\right)+\frac{1}{2}{I}_1{{\dot{\theta }}_1}^2+\frac{1}{2}{m}_2\left({{\dot{\overline{x}}}_2}^2+{{\dot{\overline{y}}}_2}^2\right)+\frac{1}{2}{I}_2{\left({\dot{\theta }}_1+{\dot{\theta }}_2\right)}^2\\ & +\frac{1}{2}{m}_3\left({{\dot{\overline{x}}}_3}^2+{{\dot{\overline{y}}}_3}^2\right)+\frac{1}{2}{I}_3{\left({\dot{\theta }}_1+{\dot{\theta }}_2+{\dot{\theta }}_3\right)}^2,\end{array}$$ E18

and to obtain the kinetic energy, the above formulas ( ${\overline{x}}_1$ , ${\overline{y}}_1$ , ${\overline{x}}_2$ , ${\overline{y}}_2$ , ${\overline{\mathrm{x}}}_3$ , ${\overline{\mathrm{y}}}_3$ ) are differentiated with time. Lagrangian equations according to Eq. (17) are

$$\frac{d}{\mathit{dt}}\frac{\partial T}{\partial {\dot{\theta }}_1}-\frac{\partial T}{\partial {\theta }_1}=Q_1+T_1,$$ E19

$$\frac{d}{\mathit{dt}}\frac{\partial T}{\partial {\dot{\theta }}_2}-\frac{\partial T}{\partial {\theta }_2}=Q_2+T_2,$$ E20

$$\frac{d}{\mathit{dt}}\frac{\partial T}{\partial {\dot{\theta }}_3}-\frac{\partial T}{\partial {\theta }_3}=Q_3+T_3,$$ E21

where

$$Q_1=F_1\frac{\partial {{\displaystyle \overrightarrow{r}}}_1}{\partial {\theta }_1}+F_2\frac{\partial {{\displaystyle \overrightarrow{r}}}_2}{\partial {\theta }_1}+F_3\frac{\partial {{\displaystyle \overrightarrow{r}}}_3}{\partial {\theta }_1},$$ E22

$$Q_2=F_1\frac{\partial {{\displaystyle \overrightarrow{r}}}_1}{\partial {\theta }_2}+F_2\frac{\partial {{\displaystyle \overrightarrow{r}}}_2}{\partial {\theta }_2}+F_3\frac{\partial {{\displaystyle \overrightarrow{r}}}_3}{\partial {\theta }_2},$$ E23

$$Q_3=F_1\frac{\partial {{\displaystyle \overrightarrow{r}}}_1}{\partial {\theta }_3}+F_2\frac{\partial {{\displaystyle \overrightarrow{r}}}_2}{\partial {\theta }_3}+F_3\frac{\partial {{\displaystyle \overrightarrow{r}}}_3}{\partial {\theta }_3}.$$ E24

The procedure for calculating $Q_i$ is shown in Appendix A. Thus, Eqs. (22)–(24) become

$$\begin{array}{c}{Q}_1=-m_{1}g{d}_1\text{cos}{\theta }_1-m_{2}g{l}_1\text{cos}{\theta }_1-m_{2}g{d}_2\text{cos}\left({\theta }_1+{\theta }_2\right)-m_{3}g{l}_1\text{cos}{\theta }_1\\ -m_{3}g{l}_2\text{cos}\left({\theta }_1+{\theta }_2\right)-m_{3}g{d}_3\text{cos}\left({\theta }_1+{\theta }_2+{\theta }_3\right),\end{array}$$ E25

$$Q_2=-m_{2}g{d}_2\text{cos}\left({\theta }_1+{\theta }_2\right)-m_{3}g{l}_2\text{cos}\left({\theta }_1+{\theta }_2\right)m_{3}g{d}_3\text{cos}\left({\theta }_1+{\theta }_2+{\theta }_3\right),$$ E26

$$Q_3=-m_{3}g{d}_3\text{cos}\left({\theta }_1+{\theta }_2+{\theta }_3\right)\mathrm{;}$$ E27

by using the kinetic energy in Eq. (18) and the potential energy in Eqs. (25)–(27), it results that Eqs. (19)–(21) after being rearranged in a matrix form result in

$$\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]\left[\begin{array}{c}{\ddot{\theta }}_{i1}\\ {\ddot{\theta }}_{i2}\\ {\ddot{\theta }}_{i3}\end{array}\right]+\left[\begin{array}{ccc}{d}_{11}& d_{12}& d_{13}\\ d_{21}& d_{22}& d_{23}\\ d_{31}& d_{32}& d_{33}\end{array}\right]\left[\begin{array}{c}{\dot{\theta }}_{i1}\\ {\dot{\theta }}_{i2}\\ {\dot{\theta }}_{i3}\end{array}\right]+\left[\begin{array}{c}{g}_{i1}\\ g_{i2}\\ g_{i3}\end{array}\right]={\tau }_{f{\mathit{\text{inger}}}_i}-J_{{\mathit{\text{finger}}}_i}^T{F}_{{\mathit{tip}}_i}.$$ E28

The formula of matrices’ elements in Eq. (28) is listed in Appendix A. From Eqs. (15) and (16), the overall dynamics of robotic hand can be constituted as

$$M_{\mathit{\text{hand}}}\left(q_1,q_2\right)\left[\begin{array}{c}\ddot{q_1}\\ \ddot{q_2}\end{array}\right]+D_{\mathit{\text{hand}}}\left(q_1,{\dot{q}}_1,q_2,{\dot{q}}_2\right)\left[\begin{array}{c}{\dot{q}}_1\\ {\dot{q}}_2\end{array}\right]+G_{\mathit{\text{hand}}}\left(q_1,q_2\right)={\tau }_{\mathit{\text{hand}}}-F_{\mathit{\text{tips}}},$$ E29

with the following matrix forms;

$$M_{\mathit{\text{hand}}}=\left[\begin{array}{cc}{M}_{{\mathit{\text{finger}}}_1}\left(q_1\right)& 0\\ 0& M_{{\mathit{\text{finger}}}_2}\left(q_2\right)\end{array}\right],$$

$$D_{\mathit{\text{hand}}}=\left[\begin{array}{cc}{D}_{{\mathit{\text{finger}}}_1}\left(q_1,{\dot{q}}_1\right)& 0\\ 0& D_{{\mathit{\text{finger}}}_2}\left(q_2,{\dot{q}}_2\right)\end{array}\right],$$

$$G_{\mathit{\text{hand}}}=\left[\begin{array}{c}{G}_{{\mathit{\text{finger}}}_1}\left(q_1\right)\\ G_{{\mathit{\text{finger}}}_2}\left(q_2\right)\end{array}\right],$$

$${\tau }_{\mathit{\text{hand}}}=\left[\begin{array}{c}{\tau }_{{\mathit{\text{finger}}}_1}\\ {\tau }_{{\mathit{\text{finger}}}_2}\end{array}\right],$$

$$F_{\mathit{\text{tips}}}=J_{\mathit{\text{hand}}}^T\left[\begin{array}{c}{F}_{{\mathit{tip}}_1}\\ F_{{\mathit{tip}}_2}\end{array}\right].$$

Thus, the six motion equations are presented by rearranging Eq. (29) to result in

$$\left[\begin{array}{c}\ddot{q_1}\\ \ddot{q_2}\end{array}\right]=M_{\mathit{\text{hand}}}{\left(q_1,q_2\right)}^{-1}\left(-D_{\mathit{\text{hand}}}\left(q_1,{\dot{q}}_1,q_2,{\dot{q}}_2\right)\left[\begin{array}{c}{\dot{q}}_1\\ {\dot{q}}_2\end{array}\right]-G_{\mathit{\text{hand}}}\left(q_1,q_2\right)+{\tau }_{\mathit{\text{hand}}}-F_{\mathit{\text{tips}}}\right).$$ E30

The desired finger configuration in joint space can be obtained by solving the above equation, which will be explained later in Section 3.1 for designing the position-tracking controller.

3. Controller design

The derived mathematical representation is applied using MATLAB/Simulink to implement the proposed controllers and the tests of this study. The parameters of the model, which are used in Simulink, are shown in Table 1. The overall controllers of the robotic hand are depicted as a complete block diagram in Figure 3. In the following sections, the design of the controllers is described in detailed.

Parameter	Value	Parameter	Value
$L_1$	$50$ mm	$L_3$	$20$ mm
$d_1$	$25$ mm	$d_3$	10 mm
${\mathrm{m}}_1$	$34$ g	$m_3$	$12$ g
$I_1$	7083 g.mm^2	$I_3$	400 g.mm^2
$L_2$	$30$ mm	$h$	100 mm
$d_2$	$15$ mm	$b$	50 mm
$m_2$	$15$ g	$L_0$	70 mm
$I_2$	1125 g.mm^2

Table 1.

Robotic hand model parameters.

Figure 3.

Block diagram of the complete robotic hand controllers.

3.1 Position tracking

Recently, a variant of the PD controller explicitly has been considered in the control of nonlinear system where the aim is to make the system follow a specific trajectory [19, 20, 21, 22]. The joint angle controller shown in Figure 3 is the inner feedforward loop which presents the effect of the input torque on the error in Eq. (30). The detailed block diagram of this controller is shown in Figure 4. The tracking error is formed by subtracting the joint angle vector ( $q_i$ ) from the desired joint angle vector ( $q_{d_i}$ ) as below:

$$e_i\left(t\right)=q_{d_i}\left(t\right)-q_i\left(t\right).$$ E31

Figure 4.

PD joint angle controller for each finger.

The objective is to minimize this error at any time, by differentiating Eq. (31) twice, solving for $\left(\ddot{q_1},\ddot{q_2}\right)$ in Eq. (30), and rearranging the results in terms of the input torque that yields the following computed torque control equation:

$${\tau }_{\mathit{\text{hand}}}=M_{\mathit{\text{hand}}}\left(\left[\begin{array}{c}{\ddot{q}}_{d_1}\\ {\ddot{q}}_{d_2}\end{array}\right]-\left[\begin{array}{c}\ddot{e_1}\\ \ddot{e_2}\end{array}\right]\right)+D_{\mathit{\text{hand}}}+G_{\mathit{\text{hand}}}+F_{\mathit{\text{tips}}},$$ E32

where ${\ddot{e}}_i$ is the control input which is selected as six PD controllers with the following feedback:

$${\ddot{e}}_i=-K_{v_i}{\dot{e}}_i-K_{p_i}{e}_i,$$ E33

where $K_{v_i}=\mathit{diag}\left\{k_{v_i}\right\}$ and $K_{p_i}=\mathit{diag}\left\{k_{p_i}\right\}$ ; hence, the robotic hand input becomes

$${\tau }_{\mathit{\text{hand}}}=M_{\mathit{\text{hand}}}\left(\left[\begin{array}{c}{\ddot{q}}_{d_1}\\ {\ddot{q}}_{d_2}\end{array}\right]+\left[\begin{array}{c}{K}_{v_1}\dot{e_1}+K_{p_1}{e}_1\\ K_{v_2}{\dot{e}}_2+K_{p_2}{e}_2\end{array}\right]\right)+D_{\mathit{\text{hand}}}+G_{\mathit{\text{hand}}}+F_{\mathit{\text{tips}}}.$$ E34

The system can be stabilized in Eq. (34) by a proper design of PD controller to track the error $\mathit{\text{error}}\left(t\right)$ to zero. In which, the input torque at joints results in the trajectory of reaching the grasped object. According to Eq. (33), the typical transfer function of the error dynamic in closed-loop approach is

$$\frac{\mathit{\text{error}}\left(s\right)}{w\left(s\right)}=\frac{1}{s^2+k_{v}s+k_p},$$ E35

where $w$ is the disturbance. Thus, the system is stable for all positive values of $k_v$ and $k_p$ . Comparing Eq. (35) with the standard second-order polynomial term ( $s^2+2\lambda {\omega }_{n}s+{\omega }_n^2$ ), we get

$$k_{p_i}={\omega }_n^2,$$

$$k_{v_i}=2\lambda {\omega }_n.$$

The design of gains is selected for the closed-loop time constant ( $0.1s$ ); thus

$${\omega }_n=\frac{1}{0.1}=10,$$

$$k_p={{\omega }_n}^2=100,$$

$$k_v=2{\omega }_n=20$$

where ${\omega }_n$ and $\lambda$ are the natural frequency and the damping coefficient, respectively.

The stability of robustness analysis by Lyapunov’s direct method is implemented here to verify the ability of applying the proposed PD controller in real situations where unmodeled disturbances are included in the actual robotic hand. The effect of unmodeled disturbances in robotic manipulator can be represented by replacing the PD controller gain matrices with matrices of nonlinear gains [23]. Considering the nonlinear matrices gains, the robotic hand input in Eq. (34) becomes

$${\tau }_{\mathit{\text{hand}}}=M_{\mathit{\text{hand}}}\left(\left[\begin{array}{c}{\ddot{q}}_{d_1}\\ {\ddot{q}}_{d_2}\end{array}\right]+\left[\begin{array}{c}{K}_{v_1}\left(e_1\right)\dot{e_1}+K_{p_1}\left(e_1\right)e_1\\ K_{v_2}\left(e_2\right){\dot{e}}_2+K_{p_2}\left(e_2\right)e_2\end{array}\right]\right)+D_{\mathit{\text{hand}}}+G_{\mathit{\text{hand}}}+F_{\mathit{\text{tips}}},$$ E36

where $K_{p_i}\left(e_i\right)=\mathit{diag}\left\{k_{p_i}\left(e_i\right)\right\}$ and $K_{v_i}\left(e_i\right)=\mathit{diag}\left\{k_{v_i}\left(e_i\right)\right\}$ are the nonlinear proportional gain matrix and the nonlinear derivative gain matrix, respectively. Combining Eq. (32) with Eq. (36), the equation of the closed loop becomes

$$\frac{d}{\mathit{dt}}\left[\begin{array}{c}\begin{array}{c}{e}_1\\ e_2\end{array}\\ \dot{e_1}\\ {\dot{e}}_2\end{array}\right]=\left[\begin{array}{c}\begin{array}{c}\dot{e_1}\\ {\dot{e}}_2\end{array}\\ -K_{v_1}\left(e_1\right)\dot{e_1}-K_{p_1}\left(e_1\right)e_1\\ -K_{v_2}\left(e_2\right){\dot{e}}_2-K_{p_2}\left(e_2\right)e_2\end{array}\right].$$ E37

The stability of Eq. (32) together with Eq. (36) is assumed if there exists $\rho >0$ with the following inequality sentences:

$$k_{p_i}\left(e_i\right)\ge \rho ,$$

$$k_{v_i}\left(e_i\right)\ge \rho .$$

The stability analysis is implemented by proposing the following Lyapunov function:

$$\begin{array}{cc}V=& \frac{1}{2}{{\dot{e}}_1}^T{\dot{e}}_1+\frac{1}{2}{{\dot{e}}_2}^T{\dot{e}}_2+{\int }_0^{e_1}{{\mathit{\epsilon }}_{\mathbf{1}}}^T\left[K_{p_1}\left({\mathit{\epsilon }}_{\mathbf{1}}\right)+{\beta }_1{K}_{v_1}\left({\mathit{\epsilon }}_{\mathbf{1}}\right)\right]d{\mathit{\epsilon }}_{\mathbf{1}}+{\int }_0^{e_2}{{\mathit{\epsilon }}_{\mathbf{2}}}^T\left[K_{p_2}\left({\mathit{\epsilon }}_{\mathbf{2}}\right)+{\beta }_2{K}_{v_2}\left({\mathit{\epsilon }}_{\mathbf{2}}\right)\right]d{\mathit{\epsilon }}_{\mathbf{2}}\\ & +{\beta }_1{e_1}^T{\dot{e}}_1+{\beta }_2{e_2}^T{\dot{e}}_2.\end{array}$$ E38

Considering the smallest eigenvalues ${{\lambda }_s}_i$ , the limit of ${\beta }_i$ satisfies

$${{\lambda }_s}_1\left\{K_{v_1}\left({\mathit{\epsilon }}_{\mathbf{1}}\right)\right\}>{\beta }_1>0,$$

$${{\lambda }_s}_2\left\{K_{v_2}\left({\mathit{\epsilon }}_{\mathbf{2}}\right)\right\}>{\beta }_2>0\mathrm{;}$$

thus

$${k_{v_j}}_1\left({{\epsilon }_j}_1\right)-\frac{{\beta }_1}{2}>0,$$

$${k_{v_j}}_2\left({{\epsilon }_j}_2\right)-\frac{{\beta }_2}{2}>0,$$

where $j=1,\dots ,3$ . Since ${\beta }_1,{\beta }_2>0$ and ${k_{p_j}}_1\left({{\epsilon }_j}_1\right),{k_{p_j}}_2\left({{\epsilon }_j}_2\right)\ge \rho$ ; thus

$${k_{p_j}}_1\left({{\epsilon }_j}_1\right)+{\beta }_1{k_{v_j}}_1\left({{\epsilon }_j}_1\right)-\frac{{{\beta }_1}^2}{2}\ge \rho ,$$ E39

$${k_{p_j}}_2\left({{\epsilon }_j}_2\right)+{\beta }_2{k_{v_j}}_2\left({{\epsilon }_j}_2\right)-\frac{{{\beta }_2}^2}{2}\ge \rho .$$ E40

Then, Eq. (38) becomes

$$\begin{array}{cc}V=& \frac{1}{2}{\left({\dot{e}}_1+{\beta }_1{e}_1\right)}^T\left({\dot{e}}_1+{\beta }_1{e}_1\right)+\frac{1}{2}{\left({\dot{e}}_2+{\beta }_2{e}_2\right)}^T\left({\dot{e}}_2+{\beta }_2{e}_2\right)\\ & +{\int }_0^{e_1}{{\mathit{\epsilon }}_{\mathbf{1}}}^T\left[K_{p_1}\left({\mathit{\epsilon }}_{\mathbf{1}}\right)+{\beta }_1{K}_{v_1}\left({\mathit{\epsilon }}_{\mathbf{1}}\right)-\frac{{{\beta }_1}^2}{2}I\right]d{\mathit{\epsilon }}_{\mathbf{1}}\mathbf{+}{\int }_0^{e_2}{{\mathit{\epsilon }}_{\mathbf{2}}}^T\left[K_{p_2}\left({\mathit{\epsilon }}_{\mathbf{2}}\right)+{\beta }_2{K}_{v_2}\left({\mathit{\epsilon }}_{\mathbf{2}}\right)-\frac{{{\beta }_2}^2}{2}I\right]d{\mathit{\epsilon }}_{\mathbf{2}}\mathbf{.}\end{array}$$ E41

The first two terms of Lyapunov function are positive. Regarding the third and the fourth terms, according to Eqs. (39) and (40), we have

$${\int }_0^{{e_j}_1}{\epsilon }_1\left[{k_{p_j}}_1\left({{\epsilon }_j}_1\right)+{\beta }_1{k_{v_j}}_1\left({{\epsilon }_j}_1\right)-\frac{{{\beta }_1}^2}{2}\right]d{\epsilon }_1\ge \frac{1}{2}\rho {\left|{e_j}_1\right|}^2,$$ E42

$${\int }_0^{{e_j}_2}{\epsilon }_2\left[{k_{p_j}}_2\left({{\epsilon }_j}_2\right)+{\beta }_2{k_{v_j}}_2\left({{\epsilon }_j}_2\right)-\frac{{{\beta }_2}^2}{2}\right]d{\epsilon }_2\ge \frac{1}{2}\rho {\left|{e_j}_2\right|}^2\mathrm{;}$$ E43

thus

$${\int }_0^{{e_j}_1}{\epsilon }_1\left[{k_{p_j}}_1\left({{\epsilon }_j}_1\right)+{\beta }_1{k_{v_j}}_1\left({{\epsilon }_j}_1\right)-\frac{{{\beta }_1}^2}{2}\right]d{\epsilon }_1\to \infty \mathit{\text{when}}\left|{e_j}_1\right|\to \infty ,$$ E44

$${\int }_0^{{e_j}_2}{\epsilon }_2\left[{k_{p_j}}_2\left({{\epsilon }_j}_2\right)+{\beta }_2{k_{v_j}}_2\left({{\epsilon }_j}_2\right)-\frac{{{\beta }_2}^2}{2}\right]d{\epsilon }_2\to \infty \mathit{\text{when}}\left|{e_j}_2\right|\to \infty .$$ E45

Equations (44) and (45) result in

$${\int }_0^{e_1}{{\mathit{\epsilon }}_{\mathbf{1}}}^T\left[{K_p}_1\left({\mathit{\epsilon }}_{\mathbf{1}}\right)+{\beta }_1{K_v}_1\left({\mathit{\epsilon }}_{\mathbf{1}}\right)-\frac{{{\beta }_1}^2}{2}\right]d{\mathit{\epsilon }}_{\mathbf{1}}\ge \frac{1}{2}\rho {‖e_1‖}^2,$$ E46

$${\int }_0^{e_2}{{\mathit{\epsilon }}_{\mathbf{2}}}^T\left[{K_p}_2\left({\mathit{\epsilon }}_{\mathbf{2}}\right)+{\beta }_2{K_v}_2\left({\mathit{\epsilon }}_{\mathbf{2}}\right)-\frac{{{\beta }_2}^2}{2}\right]d{\mathit{\epsilon }}_{\mathbf{2}}\ge \frac{1}{2}\rho {‖e_2‖}^2,$$ E47

and

$${\int }_0^{e_1}{{\mathit{\epsilon }}_{\mathbf{1}}}^T\left[{K_p}_1\left({\mathit{\epsilon }}_{\mathbf{1}}\right)+{\beta }_1{K_v}_1\left({\mathit{\epsilon }}_{\mathbf{1}}\right)-\frac{{{\beta }_1}^2}{2}\right]d{\mathit{\epsilon }}_{\mathbf{1}}\to \infty \mathit{\text{when}}‖e_1‖\to \infty ,$$ E48

$${\int }_0^{e_2}{{\mathit{\epsilon }}_{\mathbf{2}}}^T\left[{K_p}_2\left({\mathit{\epsilon }}_{\mathbf{2}}\right)+{\beta }_2{K_v}_2\left({\mathit{\epsilon }}_{\mathbf{2}}\right)-\frac{{{\beta }_2}^2}{2}\right]d{\mathit{\epsilon }}_{\mathbf{2}}\to \infty \mathit{\text{when}}‖e_2‖\to \infty .$$ E49

Hence, the Lyapunov function presented in Eq. (38) is globally positive definitive function.

Taking the derivative of Eq. (38) with respect to time, we get

$$\begin{array}{cc}\dot{V}=& {{\dot{e}}_1}^T{\ddot{e}}_1+{{\dot{e}}_2}^T{\ddot{e}}_2+{e_1}^T\left[{K_p}_1\left(e_1\right)+{\beta }_1{K_v}_1\left(e_1\right)\right]{\dot{e}}_1+{e_2}^T\left[{K_p}_2\left(e_2\right)+{\beta }_2{K_v}_2\left(e_2\right)\right]{\dot{e}}_2+{\beta }_1{{\dot{e}}_1}^T{\dot{e}}_1\\ & + {\beta }_1{e_1}^T{\ddot{e}}_1+{\beta }_2{{\dot{e}}_2}^T{\dot{e}}_2+{\beta }_2{e_2}^T{\ddot{e}}_2\end{array}$$ E50

Finally, using the rule of Leibnitz for representation, the integrals in differentiation form and Substituting Eq. (37) in (50) yield

$$\dot{V}=-{{\dot{e}}_1}^T\left[{K_v}_1\left(e_1\right)-{\beta }_{1}I\right]{\dot{e}}_1-{{\dot{e}}_2}^T\left[{K_v}_2\left(e_2\right)-{\beta }_{2}I\right]{\dot{e}}_2-{\beta }_1{e_1}^T{K_p}_1\left(e_1\right)e_1-{\beta }_2{e_2}^T{K_p}_2\left(e_2\right)e_2,$$ E51

for all positive values of gain matrices ${K_v}_1\left(e_1\right)$ , ${K_v}_2\left(e_2\right)$ , ${K_p}_1\left(e_1\right)$ ,and ${K_p}_2\left(e_2\right)$ ; Eq. (51) is globally negative function. Then

$$\dot{V}<0.$$

In this way, the system is globally exponential stabile.

3.2 Impedance controller

The PD impedance controller has been widely applied in modern researches for controlling the interaction with the environment [24, 25]. The fundamental principle of the implemented impedance control in this study is that the fingers should track a motion trajectory, and adjust the mechanical impedance of the robotic hand. The mechanical impedance of the robotic hand is defined in terms of velocity and position as

$$Z_{\mathit{\text{hand}}}=\frac{F_{{\mathit{tip}}_i}\left(s\right)}{\dot{X}\left(s\right)},$$ E52

$$s{Z}_{\mathit{\text{hand}}}=\frac{F_{{\mathit{tip}}_i}\left(s\right)}{X\left(s\right)}.$$ E53

By virtue of the above two equations, the fingertip contact force $F_{{\mathit{tip}}_i}$ gives the essential property of regulating the position and velocity of the contact points between the fingertips and the grasped object. This regulating behavior is obtained by two PD controllers as shown in Figure 3. In the first PD controller, the position is regulated by

$${∆X}_{\mathit{\text{stiffness}}}=K_{F1}{F}_{{\mathit{tip}}_i},$$ E54

where $K_{F1}$ is the matrix of the proportional gain of the controller and the velocity is regulated by

$$∆{\dot{X}}_{\mathit{\text{damping}}}=K_{F2}{F}_{{\mathit{tip}}_i},$$ E55

where $K_{F2}$ is the matrix of the controller’s derivative gain. The regulated position and velocity of Eqs. (54) and (55) are integrated in the impedance control loop with another PD controller of proportional gain matrix $K_p^{\prime }$ and derivative gain matrix $K_v^{\prime }$ in order to adjust the damping of the robotic hand during grasping operation according to the following equation:

$${\tau }_{\mathit{imp}}=J^T\left(K_p^{\prime }{∆X}_D+K_v^{\prime }∆{\dot{X}}_D\right),$$ E56

where

$${∆X}_D=X_D-X-{∆X}_{\mathit{\text{stiffness}}},$$

and

$$∆{\dot{X}}_D={\dot{X}}_D-\dot{X}-∆{\dot{X}}_{\mathit{\text{damping}}}.$$

Hence, the contact forces are regulated in joint space, and the robotic hand can adapt the collision of the fingers with the grasped objects.

The application of Lyapunov function technique determines the conditions of stability of impedance controller [26]. In the forward kinematic model, the equations of fingers are

$$X_i=f\left(q_i\right),$$ E57

$${\dot{X}}_i={J_{\mathit{\text{finger}}}}_i{\dot{q}}_i.$$ E58

On the other hand, the errors are given as

$${\mathrm{\beth }}_i=q_i-{q_i}_d,$$ E59

$${\mathrm{\aleph }}_i\left({\mathrm{\beth }}_i\right)=X_i\left(q_i\right)-{X_i}_d,$$ E60

where ${\mathrm{\beth }}_i$ , ${\mathrm{\aleph }}_i\left({\mathrm{\beth }}_i\right)$ , and $i$ are the error of joint angle, the error in task space, and the index of finger ( $i=1,2$ ), respectively. Consider the constrained dynamics of the two fingers in Eqs. (15) and (16) for the control torque in Eq. (56). Applying Eqs. (59) and (60), the error dynamic equation of each finger becomes

$$\begin{array}{cc}{M}_{{\mathit{\text{finger}}}_1}\left({\mathrm{\beth }}_1\right){\ddot{\mathrm{\beth }}}_1& +D_{{\mathit{\text{finger}}}_1}\left({\mathrm{\beth }}_1,{\dot{\mathrm{\beth }}}_1\right){\dot{\mathrm{\beth }}}_1+G_{{\mathit{\text{finger}}}_1}\left({\mathrm{\beth }}_1\right)+J_{{\mathit{\text{finger}}}_1}^T\left[K_p^{\prime }+K_p^{\prime }{K}_{F1}{K}_e+K_v^{\prime }{K}_{F2}{K}_e+K_e\right]{\mathrm{\aleph }}_1\left({\mathrm{\beth }}_1\right)\\ & +J_{{\mathit{\text{finger}}}_1}^T{K}_v^{\prime }{\dot{\mathrm{\aleph }}}_1\left({\mathrm{\beth }}_1\right)=0,\end{array}$$ E61

$$\begin{array}{cc}{M}_{{\mathit{\text{finger}}}_2}\left({\mathrm{\beth }}_2\right){\ddot{\mathrm{\beth }}}_2& +D_{{\mathit{\text{finger}}}_2}\left({\mathrm{\beth }}_2,{\dot{\mathrm{\beth }}}_2\right){\dot{\mathrm{\beth }}}_2+G_{{\mathit{\text{finger}}}_2}\left({\mathrm{\beth }}_2\right)+J_{{\mathit{\text{finger}}}_2}^T\left[K_p^{\prime }+K_p^{\prime }{K}_{F1}{K}_e+K_v^{\prime }{K}_{F2}{K}_e+K_e\right]{\mathrm{\aleph }}_2\left({\mathrm{\beth }}_2\right)\\ & +J_{{\mathit{\text{finger}}}_2}^T{K}_v^{\prime }{\dot{\mathrm{\aleph }}}_2\left({\mathrm{\beth }}_2\right)=0,\end{array}$$ E62

where $K_e$ is the stiffness matrix of the fingertip/grasped object. In order to find the potential energy $P\left({\mathrm{\beth }}_i\right)$ and the dissipation function $D\left({\mathrm{\beth }}_i\right)$ , compare Eqs. (61) and (62) with the following Lagrangian’s equations of total kinetic energy T:

$$\frac{d}{\mathit{dt}}\left(\frac{\partial T}{\partial {\dot{\mathrm{\beth }}}_i}\right)-\frac{\partial T}{\partial {\mathrm{\beth }}_i}+\frac{\partial P}{\partial {\mathrm{\beth }}_i}+\frac{\partial D}{\partial {\dot{\mathrm{\beth }}}_i}=0\mathrm{;}$$ E63

then, we get

$$\frac{\partial P}{\partial {\mathrm{\beth }}_1}=\left[K_p^{\prime }+K_p^{\prime }{K}_{F1}{K}_e+K_v^{\prime }{K}_{F2}{K}_e+K_e\right]{\mathrm{\aleph }}_1\left({\mathrm{\beth }}_1\right),$$ E64

$$\frac{\partial D}{\partial {\dot{\mathrm{\beth }}}_1}=K_v^{\prime }{\dot{\mathrm{\aleph }}}_1\left({\mathrm{\beth }}_1\right),$$ E65

$$\frac{\partial P}{\partial {\mathrm{\beth }}_2}=\left[K_p^{\prime }+K_p^{\prime }{K}_{F1}{K}_e+K_v^{\prime }{K}_{F2}{K}_e+K_e\right]{\mathrm{\aleph }}_2\left({\mathrm{\beth }}_2\right),$$ E66

$$\frac{\partial D}{\partial {\dot{\mathrm{\beth }}}_2}=K_v^{\prime }{\dot{\mathrm{\aleph }}}_2\left({\mathrm{\beth }}_2\right),$$ E67

$$T\left({\mathrm{\beth }}_1,{\dot{\mathrm{\beth }}}_1\right)=\frac{1}{2}{{\dot{\mathrm{\beth }}}_1}^T{M}_{{\mathit{\text{finger}}}_1}\left({\mathrm{\beth }}_1\right){\dot{\mathrm{\beth }}}_1,$$ E68

$$T\left({\mathrm{\beth }}_2,{\dot{\mathrm{\beth }}}_2\right)=\frac{1}{2}{{\dot{\mathrm{\beth }}}_2}^T{M}_{{\mathit{\text{finger}}}_2}\left({\mathrm{\beth }}_2\right){\dot{\mathrm{\beth }}}_2.$$ E69

The error dynamics in Eqs. (61) and (62) is asymptotically stable if the following candidate Lyapunov function

$$V\left(\mathrm{\beth },\dot{\mathrm{\beth }}\right)=T\left({\mathrm{\beth }}_1,{\dot{\mathrm{\beth }}}_1\right)+T\left({\mathrm{\beth }}_2,{\dot{\mathrm{\beth }}}_2\right)+P\left({\mathrm{\beth }}_1\right)+P\left({\mathrm{\beth }}_2\right)$$ E70

satisfies the following conditions:

$$V\left(\mathrm{\beth },\dot{\mathrm{\beth }}\right)\mathit{\text{is positive}},$$

$$\dot{V}\left(\mathrm{\beth },\dot{\mathrm{\beth }}\right)\mathit{\text{is negative}}.$$

The derivative of Eq. (70) is

$$\frac{d}{\mathit{dt}}V\left(\mathrm{\beth },\dot{\mathrm{\beth }}\right)=\frac{\mathit{dT}\left({\mathrm{\beth }}_1\right)}{\mathit{dt}}+\frac{\mathit{dT}\left({\mathrm{\beth }}_2\right)}{\mathit{dt}}+\frac{\mathit{dP}\left({\mathrm{\beth }}_1\right)}{\mathit{dt}}+\frac{\mathit{dP}\left({\mathrm{\beth }}_2\right)}{\mathit{dt}},$$

$$\begin{array}{cc}\dot{V}\left(\mathrm{\beth },\dot{\mathrm{\beth }}\right)=& {{\dot{\mathrm{\beth }}}_1}^T{M}_{{\mathit{\text{finger}}}_1}{\ddot{\mathrm{\beth }}}_1+{{\dot{\mathrm{\beth }}}_1}^T\frac{d{M}_{{\mathit{\text{finger}}}_1}}{\mathit{dt}}\frac{{\dot{\mathrm{\beth }}}_1}{2}+{{\dot{\mathrm{\beth }}}_2}^T{M}_{{\mathit{\text{finger}}}_2}{\ddot{\mathrm{\beth }}}_2+{{\dot{\mathrm{\beth }}}_2}^T\frac{d{M}_{{\mathit{\text{finger}}}_2}}{\mathit{dt}}\frac{{\dot{\mathrm{\beth }}}_2}{2}\\ & +{{\dot{\mathrm{\beth }}}_1}^T\left(\left[K_p^{\prime }+K_p^{\prime }{K}_{F1}{K}_e+K_v^{\prime }{K}_{F2}{K}_e+K_e\right]{\mathrm{\aleph }}_1\left({\mathrm{\beth }}_1\right)\right)\\ & +{{\dot{\mathrm{\beth }}}_2}^T\left(\left[K_p^{\prime }+K_p^{\prime }{K}_{F1}{K}_e+K_v^{\prime }{K}_{F2}{K}_e+K_e\right]{\mathrm{\aleph }}_2\left({\mathrm{\beth }}_2\right)\right).\end{array}$$ E71

Equations (61) and (62) can be rearranged as below

$$\begin{array}{cc}{M}_{{\mathit{\text{finger}}}_1}\left({\mathrm{\beth }}_1\right){\ddot{\mathrm{\beth }}}_1& +D_{{\mathit{\text{finger}}}_1}\left({\mathrm{\beth }}_1,{\dot{\mathrm{\beth }}}_1\right){\dot{\mathrm{\beth }}}_1+G_{{\mathit{\text{finger}}}_1}\left({\mathrm{\beth }}_1\right)\\ & +J_{{\mathit{\text{finger}}}_1}^T\left[K_p^{\prime }+K_p^{\prime }{K}_{F1}{K}_e+K_v^{\prime }{K}_{F2}{K}_e+K_e\right]{\mathrm{\aleph }}_1\left({\mathrm{\beth }}_1\right)=-J_{{\mathit{\text{finger}}}_1}^T{K}_v^{\prime }{\dot{\mathrm{\aleph }}}_1\left({\mathrm{\beth }}_1\right),\end{array}$$ E72

$$\begin{array}{cc}{M}_{{\mathit{\text{finger}}}_2}\left({\mathrm{\beth }}_2\right){\ddot{\mathrm{\beth }}}_2& +D_{{\mathit{\text{finger}}}_2}\left({\mathrm{\beth }}_2,{\dot{\mathrm{\beth }}}_2\right){\dot{\mathrm{\beth }}}_2+G_{{\mathit{\text{finger}}}_2}\left({\mathrm{\beth }}_2\right)\\ & +J_{{\mathit{\text{finger}}}_2}^T\left[K_p^{\prime }+K_p^{\prime }{K}_{F1}{K}_e+K_v^{\prime }{K}_{F2}{K}_e+K_e\right]{\mathrm{\aleph }}_2\left({\mathrm{\beth }}_2\right)=-J_{{\mathit{\text{finger}}}_2}^T{K}_v^{\prime }{\dot{\mathrm{\aleph }}}_2\left({\mathrm{\beth }}_2\right),\end{array}$$ E73

By substituting Eqs. (72) and (73) in Eq. (71) and applying the relations in Eqs. (57)–(60), it yields

$$\dot{V}\left(\mathrm{\beth },\dot{\mathrm{\beth }}\right)=-{{\dot{\mathrm{\aleph }}}_1}^T\left({\mathrm{\beth }}_1\right)K_v^{\prime }{\dot{\mathrm{\aleph }}}_1\left({\mathrm{\beth }}_1\right)-{{\dot{\mathrm{\aleph }}}_2}^T\left({\mathrm{\beth }}_2\right)K_v^{\prime }{\dot{\mathrm{\aleph }}}_2\left({\mathrm{\beth }}_2\right).$$ E74

Hence, the robotic hand system is asymptotically stable under the following boundaries:

$$\left[2K_p^{\prime }+2K_p^{\prime }{K}_{F1}{k}_e+2K_v^{\prime }{K}_{F2}{k}_e+2k_e\right]>0,$$

$$K_v^{\prime }>0,$$

or

$$K_v^{\prime }>0.$$

3.3 Gains design using genetic algorithm

Genetic algorithm is a well-known optimization procedure for complex problems which ascertains optimum values for different systems [27]. The genetic algorithm is written to find the optimum impedance gains. The code takes the calculated data from the robotic hand Simulink model to compute impedance’s gains. However, the detailed genetic algorithm that is applied in the design of the gains can be defined by the following proceedings:

(1) Generate a number of solutions. This represents the random number of population which includes the gains $K_{F1}$ , $K_{F2}$ , $K_p^{\prime }$ , and $K_v^{\prime }$ of the impedance controller of Figure 3. These gains are the individual of population.

(2) Fitness function. During the running of the program, the fitness is evaluated for each individual in the population. In this study, the objective of the fitness function is to minimize the errors $∆{\mathrm{X}}_{\mathrm{D}}$ and $∆{\dot{\mathrm{X}}}_{\mathrm{D}}$ of Eq. (56) as below:

$$\begin{array}{c}\text{Erro}{\mathrm{r}}_{\text{tracking}}=0.05∆{\mathrm{X}}_{\mathrm{D}}+0.05∆{\dot{\mathrm{X}}}_{\mathrm{D}}+0.1{\mathrm{t}}_{{\mathrm{r}}_1}+0.1{\mathrm{t}}_{{\mathrm{s}}_1}+0.1{\mathrm{d}}_{{\mathrm{o}}_1}+0.1{\mathrm{t}}_{{\mathrm{r}}_2}+0.1{\mathrm{t}}_{{\mathrm{s}}_2}\\ +0.1{\mathrm{d}}_{{\mathrm{o}}_2}+0.1{\mathrm{t}}_{{\mathrm{r}}_3}+0.1{\mathrm{t}}_{{\mathrm{s}}_3}+0.1{\mathrm{d}}_{{\mathrm{o}}_3},\end{array}$$ E75

where ${\mathrm{t}}_{{\mathrm{r}}_{\mathrm{j}}}$ , ${\mathrm{t}}_{{\mathrm{s}}_{\mathrm{j}}}$ , and ${\mathrm{d}}_{{\mathrm{o}}_{\mathrm{j}}}$ are the rising time, the settling time, and the overshoot, respectively, for each error signal ( $j=1,2,3$ ).

(3) Create the next generation [28]. At each step, genetic algorithm implements the following three rules to create a new generation of the recent population:

• Individuals called parents are selected by the selection rules, and then the population of the next generation is obtained by contributing the parents.

• Two parents are combined to generate children by crossover rules for the next generation.

• Children are formed by mutation rules. The process of forming children is dependent on applying random changes on the individual parents.

(4) Setting the genetic algorithm. The following parameters are used for setting the proposed genetic algorithm in this study: the number of generations = 80, the size of population = 30, and the range of gains are ${K_{F1}}_{\mathit{min}}=0$ , ${K_{F1}}_{\mathit{max}}=50$ , ${K_{F2}}_{\mathit{min}}=0$ , ${K_{F2}}_{\mathit{max}}=50$ , ${K_p}_{\mathit{min}}=0$ , ${K_p}_{\mathit{max}}=50$ , ${K_V}_{\mathit{min}}=0$ , ${K_V}_{\mathit{max}}=50$ .

4. Simulation results

The obtained interactional model was applied in the simulation to verify the designed controllers through three tests as follows:

• Test 1: Following a specific joint path.

• Test 2: Following fingertip position.

• Test 3: Robustness of the controllers.

The Simulink model is implemented using MATLAB as shown in Figure 5.

Figure 5.

The Simulink model of the robot hand.

4.1 Following a specific joint path

This test is focused on position-tracking performance of joints. We assumed the two fingers are in initial position ( ${\theta }_{\mathit{ij}}=0$ ) and the joints should follow the following path:

$${\theta }_{{\mathit{ij}}_d}=0.2\text{sin}\mathit{\pi t}.$$

The simulation result explained in Figure 6 shows that the designed position-tracking controller can follow a specific path with zero steady error and without overshoot. A unit step input function of amplitude ${20}^{°}$ is implemented to check this tracking error, and the response of the controllers is more detailed as shown in Figure 7.

Figure 6.

Joint tracking of sine wave path.

Figure 7.

Tracking errors of finger 1 joints.

4.2 Following fingertip position

The second test is implemented to monitor the operation of the designed impedance controller in task space. We assumed the initial position of the fingertip 1 is (135, 100, −25 mm) and it has to reach the position (115, 153.7, −25 mm). The position of the grasped object is assumed at $x_{{\mathit{\text{object}}}_1}=118$ mm, $y_{{\mathit{\text{object}}}_1}=152.7$ mm, $z_{{\mathit{\text{object}}}_1}=-25$ . The result of the optimization process of the impedance controller is obtained as $K_{F1}=1.8089$ , $K_{F2}=1$ , $K_P=1$ , $K_V=1$ . At these gains, as shown in Figure 8, the percentage error of position and velocity are 0.6 and 0%, respectively. Regarding the fingertip contact force represented in Figure 9, the overall designed controllers have given the essential property of contact without overshoot.

Figure 8.

Motion of fingertip 1 with impedance controller.

Figure 9.

Fingertip contact force with the grasped object.

For comparison purposes, another study is implemented without considering the impedance controller in the overall control loop, i.e., $K_{F1}=0$ , $K_{F2}=0$ , $K_P=0$ , $K_V=0$ . The results showed that the fingertip position error reaches 0.69%, but the main side effect was of the fingertip velocity error which reached 100% (see Figure 10). In turn, the fingertip contacts the grasped object with high value of error in velocity; this situation causes unsafe grasping. As a result, the proposed overall controller has minimized the velocity error to 0% thanks to the proposed impedance controller with genetic algorithm.

Figure 10.

Motion of fingertip 1 without impedance controller.

4.3 System robustness

The robustness of the designed controllers is checked in this section. This test is implemented within the same parameters of the desired position and object location in the test of Section 4.2. The disturbances on the robotic hand cause change of the fingertip contact forces. Here, we assumed that the disturbances result in a contact force with a white Gaussian noise [29, 30, 31, 32] shown in Figure 11. The Gaussian noise model is generated using block AWGN channel in Simulink. The position of the AWGN channel is shown in Figure 5. The parameters of white Gaussian noise are set as shown in Figure 12. The designed controllers have achieved the functionality of rejecting the disturbances and kept the motion of the finger as shown in Figure 13.

Figure 11.

Contact force signal with a white Gaussian noise.

Figure 12.

Parameters of the applied white Gaussian noise.

Figure 13.

Response of the designed controllers to the disturbances.

5. Conclusion

Grasping by two fingertips plays a distinctive role in its performance related to robotic hand-environment interaction. A mathematical model including the dynamic equation has been derived based on the Lagrange technique. The coupling between two fingers and the fingertip contacts with unknown grasped objects has been presented in this model. The performance of the derived model is confirmed when a designed based model controller is implemented in Simulink. A computed torque controller of six PD controllers has been implemented to control the rotation of joints. The impedance controller is applied to operate as the outer loop of the overall control system. This controller allowed the regulation of the fingertip contact forces without overshoot response, which is essential for safe grasping, especially in holding fragile objects. Besides this, the impedance gains have been obtained using a genetic algorithm with a new behavior of improving the response of the position and velocity errors. The applied genetic algorithm method has minimized the position error and the velocity error while keeping the operation of the overall control system. The robot finger has been ordered to follow a specific joint path and fingertip position grasping; the results of simulations have achieved a typical accomplishment of trajectory tracking. Finally, the overall controllers have shown a perfect rejection of disturbances in terms of fingertip contact force. As a future work, it is recommended to apply the proposed controllers of this study to mobile manipulator for grasping and manipulation objects.

Appendix A.

Let

${\overrightarrow{F}}_1=-m_{1}g{\displaystyle \overrightarrow{j}},{\overrightarrow{F}}_2=-m_{2}g{\displaystyle \overrightarrow{j}}$ , ${\overrightarrow{F}}_3=-m_{3}g{\displaystyle \overrightarrow{j}},$

and

$${{\displaystyle \overrightarrow{r}}}_1=x_1{\displaystyle \overrightarrow{i}}+ y_1{\displaystyle \overrightarrow{j}},{{\displaystyle \overrightarrow{r}}}_2=x_2{\displaystyle \overrightarrow{i}}+ y_2{\displaystyle \overrightarrow{j}},{{\displaystyle \overrightarrow{r}}}_3=x_3{\displaystyle \overrightarrow{i}}+ y_3{\displaystyle \overrightarrow{j}}.$$

Then

$${{\displaystyle \overrightarrow{r}}}_1=d_1\text{cos}{\theta }_1{\displaystyle \overrightarrow{i}}+d_1\text{sin}{\theta }_1{\displaystyle \overrightarrow{j}},$$

$${\overrightarrow{r}}_2=\left(L_1\text{cos}{\theta }_1+d_2\text{cos}\left({\theta }_1+{\theta }_2\right)\right){\displaystyle \overrightarrow{i}}+\left(L_1\text{sin}{\theta }_1+d_2\text{sin}\left({\theta }_1+{\theta }_2\right)\right){\displaystyle \overrightarrow{j}},$$

$$\begin{array}{c}{{\displaystyle \overrightarrow{r}}}_3=\left(L_1\text{cos}{\theta }_1+L_2\text{cos}\left({\theta }_1+{\theta }_2\right)+d_3\text{cos}\left({\theta }_1+{\theta }_2+{\theta }_3\right)\right){\displaystyle \overrightarrow{i}}\\ +\left(L_1\text{sin}{\theta }_1+L_2\text{sin}\left({\theta }_1+{\theta }_2\right)+d_3\text{sin}\left({\theta }_1+{\theta }_2+{\theta }_3\right)\right){\displaystyle \overrightarrow{j}}.\end{array}$$

The partial derivatives of ${\overrightarrow{r}}_i$ with respect to ${\theta }_i$ are

$$\frac{\partial {{\displaystyle \overrightarrow{r}}}_1}{\partial {\theta }_1}=-d_1\text{sin}{\theta }_1{\displaystyle \overrightarrow{i}}+d_1\text{cos}{\theta }_1{\displaystyle \overrightarrow{j}},$$

$$\frac{\partial {{\displaystyle \overrightarrow{r}}}_1}{\partial {\theta }_2}=0,$$

$$\frac{\partial {{\displaystyle \overrightarrow{r}}}_1}{\partial {\theta }_3}=0,$$

$$\frac{\partial {{\displaystyle \overrightarrow{r}}}_2}{\partial {\theta }_1}=\left[-L_1\text{sin}{\theta }_1-d_2\text{sin}\left({\theta }_1+{\theta }_2\right)\right]{\displaystyle \overrightarrow{i}}+\left[L_1\text{cos}{\theta }_1+d_2\text{cos}\left({\theta }_1+{\theta }_2\right)\right]{\displaystyle \overrightarrow{j}},$$

$$\frac{\partial {{\displaystyle \overrightarrow{r}}}_2}{\partial {\theta }_2}=-d_2\text{sin}\left({\theta }_1+{\theta }_2\right){\displaystyle \overrightarrow{i}}+d_2\text{cos}\left({\theta }_1+{\theta }_2\right){\displaystyle \overrightarrow{j}},$$

$$\frac{\partial {{\displaystyle \overrightarrow{r}}}_2}{\partial {\theta }_3}=0,$$

$$\begin{array}{c}\frac{\partial {{\displaystyle \overrightarrow{r}}}_3}{\partial {\theta }_1}=\left(-L_1\text{sin}{\theta }_1-L_2\text{sin}\left({\theta }_1+{\theta }_2\right)-d_3\text{sin}\left({\theta }_1+{\theta }_2+{\theta }_3\right)\right){\displaystyle \overrightarrow{i}}\\ + \left(L_1\text{cos}{\theta }_1+L_2\text{cos}\left({\theta }_1+{\theta }_2\right)+d_3\text{cos}\left({\theta }_1+{\theta }_2+{\theta }_3\right)\right){\displaystyle \overrightarrow{j}},\end{array}$$

$$\frac{\partial {{\displaystyle \overrightarrow{r}}}_3}{\partial {\theta }_2}=\left(-L_2\text{sin}\left({\theta }_1+{\theta }_2\right)-d_3\text{sin}\left({\theta }_1+{\theta }_2+{\theta }_3\right)\right){\displaystyle \overrightarrow{i}}+\left(L_2\text{cos}\left({\theta }_1+{\theta }_2\right)+d_3\text{cos}\left({\theta }_1+{\theta }_2+{\theta }_3\right)\right){\displaystyle \overrightarrow{j}},$$

$$\frac{\partial {{\displaystyle \overrightarrow{r}}}_3}{\partial {\theta }_3}=-d_3\text{sin}\left({\theta }_1+{\theta }_2+{\theta }_3\right){\displaystyle \overrightarrow{i}}+d_3\text{cos}\left({\theta }_1+{\theta }_2+{\theta }_3\right)){\displaystyle \overrightarrow{j}},$$

The matrices’ elements in Eq. (28) are

$$\begin{array}{c}{m}_{11}=m_1{d_1}^2+I_1+m_2{L_1}^2+m_2{d_2}^2+2m_2{L}_1{d}_2\text{cos}{\theta }_2+I_2+m_3{L_1}^2+m_3{L_2}^2\\ + 2m_3{L}_1{L}_2\text{cos}{\theta }_2+2m_3{L}_1{d}_3\text{cos}\left({\theta }_2+{\theta }_3\right)+2m_3{L}_2{d}_3\text{cos}{\theta }_3+m_3{d_3}^2+I_3,\end{array}$$

$$\begin{array}{c}{m}_{12}=m_2{d_2}^2+m_2{L}_1{d}_2\text{cos}{\theta }_2+I_2+m_3{L}_1{L}_2\text{cos}{\theta }_2+m_3{L}_1{d}_3\text{cos}\left({\theta }_2+{\theta }_3\right)+2m_3{L_2}^2\\ + 2m_3{L}_2{d}_3\text{cos}{\theta }_3+m_3{d_3}^2+I_3,\end{array}$$

$$m_{13}=m_3{L}_1{d}_3\text{cos}\left({\theta }_2+{\theta }_3\right)+m_3{L}_2{d}_3\text{cos}{\theta }_3+m_3{d_3}^2+I_3,$$

$$\begin{array}{c}{m}_{21}=m_2{d_2}^2+m_2{L}_1{d}_2\text{cos}{\theta }_2+I_2+m_3{L}_1{L}_2\text{cos}{\theta }_2+m_3{L}_1{d}_3\text{cos}\left({\theta }_2+{\theta }_3\right)\\ +2m_3{L}_2{d}_3\text{cos}{\theta }_3+2m_3{L_2}^2+m_3{d_3}^2+I_3,\end{array}$$

$$m_{22}={m_2{d}_2}^2+I_2+m_3{L_2}^2+2m_3{L}_2{d}_3\text{cos}{\theta }_3+m_3{d_3}^2+I_3,$$

$$m_{23}=m_3{L}_2{d}_3\text{cos}{\theta }_3+m_3{d_3}^2+I_3,$$

$$m_{31}=m_3{L}_1{d}_3\text{cos}\left({\theta }_2+{\theta }_3\right)+m_3{L}_2{d}_3\text{cos}{\theta }_3+m_3{d_3}^2+I_3,$$

$$m_{32}=m_3{L}_2{d}_3\text{cos}{\theta }_3+m_3{d_3}^2+I_3,$$

$$m_{33}=m_3{d_3}^2+I_3,$$

$$\begin{array}{c}{d}_{11}=-2m_2{L}_1{d}_2\text{sin}{\theta }_2{\dot{\theta }}_2-2m_3{L}_1{L}_2\text{sin}{\theta }_2{\dot{\theta }}_2-2m_3{L}_1{d}_3\text{sin}\left({\theta }_2+{\theta }_3\right){\dot{\theta }}_2\\ -2m_3{L}_1{d}_3\text{sin}\left({\theta }_2+{\theta }_3\right){\dot{\theta }}_3-2m_3{L}_2{d}_3\text{sin}{\theta }_3{\dot{\theta }}_3,\end{array}$$

$$\begin{array}{c}{d}_{12}=-m_2{L}_1{d}_2\text{sin}{\theta }_2{\dot{\theta }}_2-m_3{L}_1{L}_2\text{sin}{\theta }_2{\dot{\theta }}_2-m_3{L}_1{d}_3\text{sin}\left({\theta }_2+{\theta }_3\right){\dot{\theta }}_2\\ -m_3{L}_1{d}_3\text{sin}\left({\theta }_2+{\theta }_3\right){\dot{\theta }}_3-2m_3{L}_2{d}_3\text{sin}{\theta }_3{\dot{\theta }}_3-m_3{L}_1{d}_3\text{sin}\left({\theta }_2+{\theta }_3\right){\dot{\theta }}_3,\end{array}$$

$$d_{13}=-m_3{L}_1{d}_3\text{sin}\left({\theta }_2+{\theta }_3\right){\dot{\theta }}_3-m_3{L}_2{d}_3\text{sin}{\theta }_3{\dot{\theta }}_3,$$

$$\begin{array}{c}{d}_{21}=-m_2{L}_1{d}_2\text{sin}{\theta }_2{\dot{\theta }}_2-m_3{L}_1{L}_2\text{sin}{\theta }_2{\dot{\theta }}_2-m_3{L}_1{d}_3\text{sin}\left({\theta }_2+{\theta }_3\right)\left[{\dot{\theta }}_2+{\dot{\theta }}_3\right]\\ -2m_3{L}_2{d}_3\text{sin}{\theta }_3{\dot{\theta }}_3+m_2{L}_1{d}_2\text{sin}{\theta }_2{\dot{\theta }}_1+m_2{L}_1{d}_2\text{sin}{\theta }_2{\dot{\theta }}_2+m_3{L}_1{L}_2\text{sin}{\theta }_2{\dot{\theta }}_1\\ + m_3{L}_1{d}_3\text{sin}\left({\theta }_2+{\theta }_3\right){\dot{\theta }}_1+m_3{L}_1{L}_2\text{sin}{\theta }_2{\dot{\theta }}_2+m_3{L}_1{d}_3\text{sin}\left({\theta }_2+{\theta }_3\right){\dot{\theta }}_2+m_3{L}_1{d}_3\text{sin}\left({\theta }_2+{\theta }_3\right){\dot{\theta }}_3,\end{array}$$

$$d_{22}=-2m_3{L}_2{d}_3\text{sin}{\theta }_3{\dot{\theta }}_3,$$

$$d_{23}=-m_3{L}_2{d}_3\text{sin}{\theta }_3{\dot{\theta }}_3,$$

$$\begin{array}{c}{d}_{31}=-m_3{L}_1{d}_3\text{sin}\left({\theta }_2+{\theta }_3\right)\left[{\dot{\theta }}_2+{\dot{\theta }}_3\right]-m_3{L}_2{d}_3\text{sin}{\theta }_3{\dot{\theta }}_3+m_3{L}_1{d}_3\text{sin}\left({\theta }_2+{\theta }_3\right){\dot{\theta }}_1\\ +m_3{L}_2{d}_3\text{sin}{\theta }_3{\dot{\theta }}_1+m_3{L}_1{d}_3\text{sin}\left({\theta }_2+{\theta }_3\right){\dot{\theta }}_2+2m_3{L}_2{d}_3\text{sin}{\theta }_3{\dot{\theta }}_2\\ +m_3{L}_1{d}_3\text{sin}\left({\theta }_2+{\theta }_3\right){\dot{\theta }}_3+m_3{L}_2{d}_3\text{sin}{\theta }_3{\dot{\theta }}_3,\end{array}$$

$$d_{32}=-m_3{L}_2{d}_3\text{sin}{\theta }_3{\dot{\theta }}_3+m_3{L}_2{d}_3\text{sin}{\theta }_3{\dot{\theta }}_2+m_3{L}_2{d}_3\text{sin}{\theta }_3{\dot{\theta }}_3,$$

$$d_{33}=0,$$

$$\begin{array}{c}{g}_1=m_{1}g{d}_1\text{cos}{\theta }_1+m_{2}g{L}_1\text{cos}{\theta }_1+m_{2}g{d}_2\text{cos}\left({\theta }_1+{\theta }_2\right)+m_{3}g{L}_1\text{cos}{\theta }_1\\ +m_{3}g{L}_2\text{cos}\left({\theta }_1+{\theta }_2\right)+m_{3}g{d}_3\text{cos}\left({\theta }_1+{\theta }_2+{\theta }_3\right),\end{array}$$

$$g_2=m_{2}g{d}_2\text{cos}\left({\theta }_1+{\theta }_2\right)+m_{3}g{L}_2\text{cos}\left({\theta }_1+{\theta }_2\right)+m_{3}g{d}_3\text{cos}\left({\theta }_1+{\theta }_2+{\theta }_3\right),$$

$$g_3=m_{3}g{d}_3\text{cos}\left({\theta }_1+{\theta }_2+{\theta }_3\right),$$