<aside> 🤪

Key words:

• Modeling and control,
• Compliance control,
• D-H table,
• Lagrangian modeling methods,
• dynamical model,
• simulation/animation . </aside>

<aside> 💻

More info:

GitHub link:

</aside>

<aside> 📌

Project overview :

</aside>

Brief introduction

✍️ Goal: Enable the robotic arm to adapt to my movements by sensing changes in force, achieving guide by touch.

    **The robotic arm uses [KINOVA-Gen3](<https://www.kinovarobotics.com/product/gen3-robots>), with 7-DoF and sufficient force sensors on each joint, capable of sensing external torque.** 

Figure 1: KINOVA-Gen3 Robotic Arm

Work Flow

1. Sense the torque ($\tau$) on the robotic arm;

2. Calculate the joint acceleration $\ddot \Theta$ based on the dynamic model like:

   $$ \mathbf{M}(\Theta)\ddot \Theta + \mathbf{C}(\Theta,\dot \Theta) + \mathbf{G}(\Theta) = \tau; $$

3. Integrate to obtain the joint angle $\Theta$;

4. Input the $\Theta$ command into the robotic arm;

5. Back to Step 1.

Results

    **The basic function allows the robotic arm to follow my drag (Figure 2), with added features for motion interruption and resumption (Figure 3), and actions for teaching and repetition (Figure 4). More details can be found in the GitHub project.**

Figure 2: Guide by drag

🙋Figure 2 : This example implements a basic algorithm that allows the robotic arm to follow my drag during movement.

Figure 3: Interruption and resumption

🙋‍♂️Figure 3: In this example, when an external torque is detected, the robotic arm adjusts to follow the drag. Once the external force disappears, it returns to its original motion path (moving around).

Figure 4: Teaching and repetition

🙋‍♀️ Figure 4: In this example, the user first drags the robotic arm to perform a desired action, and after the robotic arm resets, it repeats the same motion autonomously.

<aside> 📌

Complete code and video demonstration can be found in GitHub project :

</aside>

Theoretical Part: Modeling and Control

    **In this section, the 7-DoF dynamical system equations are established and the control law is obtained.** 

Main process

1. Derive transformation matrices $T$ by D-H table;

2. Get the Jacobian matrix of each joint, which is used to express the angular velocity, linear velocity and Lagrangian quantity $L$;

3. Use Euler-Lagrangian function to derive the model,

   $$ \frac{d}{dt} \left( \frac{\partial \mathbf{L}}{\partial \dot{\Theta}} \right) - \frac{\partial \mathbf{L}}{\partial \Theta} = \tau; $$

4. Derive the control law by obtained $\Theta, \dot \Theta, \ddot \Theta$.

Figure 5: D-H table of KINOVA-Gen3

Figure 6: Illustration of links, joints and frames of KINOVA-Gen3

Derive matrix $T$

    **Transformation matrix $^{i-1}T_i$ represents the transformation matrix from  frame $\\{i\\}$ to frame $\\{i-1\\}$, i.e., $^{i-1}P = ^{i-1}T_i ~\\cdot~ ^iP$  , where $^{i-1}P,~ ^{i}P$  are the vectors in frame $\\{i-1\\}$ and frame $\\{i\\},$ respectively.  The parameters $\\alpha_i, \\theta_i, a_i,d_i$ have been given in *Figure 5.*** 

$$ ^{i-1}T_i = \begin{bmatrix}\cos(\theta_i) & -\cos(\alpha_i)\sin(\theta_i) & \sin(\alpha_i)\sin(\theta_i) & a_i\cos(\theta_i) \\\sin(\theta_i) & \cos(\alpha_i)\cos(\theta_i) & -\sin(\alpha_i)\cos(\theta_i) & a_i\sin(\theta_i) \\0 & \sin(\alpha_i) & \cos(\alpha_i) & d_i \\0 & 0 & 0 & 1\end{bmatrix} $$

    **The first row of D-H table is unused because  transforming all coordinates to frame $\\{0\\}$ is enough. Hence, $^{0}T_{i=\\{1,2,...,7\\}}$ are obtained and will be used for the calculation of Jacobian matrix.** 

Derive Jacobian matrices and Lagrangian quantity $\mathbf{L}$

✍️Goal: To get the Lagrangian quantity $L$

$$ \mathbf{L} = \mathbf{K} -\mathbf{U}, $$

where $K$ is the kinematic energy of the robotic arm and $U$ is the potential energy.