```python
from sympy import *
from IPython.display import Math
import matplotlib.pyplot as plt
```

# Modified Denavit Parameters of Kuka Robot


```python
alpha = [pi, pi/2, 0, -pi/2, pi/2, -pi/2]

q = [symbols('q1'), symbols('q2'), symbols('q3'), symbols('q4'), symbols('q5'), symbols('q6')]

a2 = symbols('a2')
a3 = symbols('a3')
a4 = symbols('a4')
r1 = symbols('r1')
r4 = symbols('r4')
wte = symbols('wte')

robot_size_sym = [a2, a3, a4, r1, r4, wte]
robot_size_val = [0.26, 0.68, 0.035, 0.675, 0.67, 0.158]

mdh_table = {
            'joint1' : [pi, 0, q[0], -r1],
            'joint2' : [pi/2, a2, q[1], 0],
            'joint3' : [0, a3, pi/2 + q[2], 0],
            'joint4' : [-pi/2, a4, q[3], -r4],
            'joint5' : [pi/2, 0, q[4], 0],
            'joint6' : [-pi/2, 0, q[5], 0]}
```

# Homogeneous transform from Fi-1 to Fi


```python
def poseFi(mdh_row):
    # unpack input
    alpha = mdh_row[0]
    a = mdh_row[1]
    q = mdh_row[2]
    r = mdh_row[3]
    
    trans_about_x = Matrix([[1, 0, 0, a],
                         [0, cos(alpha), -sin(alpha), 0],
                         [0, sin(alpha), cos(alpha), 0],
                         [0, 0, 0, 1]])
    
    trans_about_z = Matrix([[cos(q), -sin(q), 0, 0],
                         [sin(q), cos(q), 0, 0],
                         [0, 0, 1, r],
                         [0, 0, 0, 1]])
    return trans_about_x * trans_about_z
    
```

# Forward Geometric 


```python
M0_1 = poseFi(mdh_table['joint1'])
M1_2 = poseFi(mdh_table['joint2'])
M2_3 = poseFi(mdh_table['joint3'])
M3_4 = poseFi(mdh_table['joint4'])
M4_5 = poseFi(mdh_table['joint5'])
M5_6 = poseFi(mdh_table['joint6'])

Mw_E = poseFi([pi, 0, 0, wte])

# pose of E-E relative to frame 0
M0_E = M0_1 * M1_2 * M2_3 * M3_4 * M4_5 * M5_6 * Mw_E

# coordinate of E-E check
val_q = zeros(6)
val_M0_E = M0_E.subs(zip(q, val_q))
val_M0_E = val_M0_E.subs(zip(robot_size_sym, robot_size_val))

print('Pose of E-E when all joints at neutral position:')
pprint(val_M0_E)

```

    Pose of E-E when all joints at neutral position:
    ⎡0   0  1  1.768⎤
    ⎢               ⎥
    ⎢0   1  0    0  ⎥
    ⎢               ⎥
    ⎢-1  0  0  0.64 ⎥
    ⎢               ⎥
    ⎣0   0  0    1  ⎦


# Inverse Geometric

## Wrist Positioning

Formula for the position of the Wrist in the fixed frame (frame 0)


```python
M0_6 = M0_1 * M1_2 * M2_3 * M3_4 * M4_5 * M5_6
pprint(simplify(M0_6[:-1, -1]))
```

    ⎡(a₂ + a₃⋅cos(q₂) - a₄⋅sin(q₂ + q₃) + r₄⋅cos(q₂ + q₃))⋅cos(q₁) ⎤
    ⎢                                                              ⎥
    ⎢(-a₂ - a₃⋅cos(q₂) + a₄⋅sin(q₂ + q₃) - r₄⋅cos(q₂ + q₃))⋅sin(q₁)⎥
    ⎢                                                              ⎥
    ⎣     -a₃⋅sin(q₂) - a₄⋅cos(q₂ + q₃) + r₁ - r₄⋅sin(q₂ + q₃)     ⎦


Assume the numerical value of the Wrist position in the fixed frame is denoted by $\left[w^{0}_{x}, w^{0}_{y}, w^{0}_{z}\right]^T$. The system of equations describing the Wrist Positioning problem is 
$$
\left\{
    \begin{array}{lll}
        \left(a_2 + a_3c_2 - a_4s_{23} + r_4c_{23}\right)c_1  & = ^{0}w_{x} & (1) \\
        \left(-a_2 - a_3c_2 + a_4s_{23} - r_4c_{23}\right)s_1 & = ^{0}w_{y} & (2) \\
        -a_3s_2 - a_4c_{23} + r_1 - r_4s_{23} & = ^{0}w_{z} & (3)
    \end{array}
\right. 
$$

Add the square of (1) and (2),

$$
\left(-a_2 - a_3c_2 + a_4s_{23} - r_4c_{23}\right)^2 = ^{0}w^{2}_{x} + ^{0}w^{2}_{y}
$$

Take the square root of the equation above 
$$
-a_2 - a_3c_2 + a_4s_{23} - r_4c_{23} = +/- \left(^{0}w^{2}_{x} + ^{0}w^{2}_{y}\right)^{\frac{1}{2}}
$$

For shake of notation simplicity, asign the right hand side of the equation above to $\gamma$. This equation together with (3) form a Type 7 trigonometric equation
$$
\left\{
    \begin{array}{ll}
        a_4c_{23} + r_4s_{23}  & = -a_3s_2 + r_1 - ^{0}w_{z} \\
        a_4s_{23} - r_4c_{23} & = -(-a_3)c_2 + a_2 +/- \gamma \\
    \end{array}
\right. 
$$

with $W_1 = a_4, W_2 = r_4, X = 0, Y = -a_3, z_1 = r_1 - ^0w_z,z_2 = a_2 +/- \gamma$ and $q_i = q_2, q_j = q_2 + q_3$.

Let $\kappa = -a_2 - a_3c_2 + a_4s_{23} - r_4c_{23}$. With a pair of $\left(q_2,q_3\right)$, the value of $q_1$ is
$$
q_1 = atan2(^{0}w_{y}/\kappa, -^{0}w_{x}/\kappa)
$$


## Wrist Orienting

### Orientation of frame 3 relative to fixed frame (frame 0)
Given the desired orientation of the wrist frame (frame 6), the orientation of frame 6 relative to frame 3 is computed by
$$ ^{3}R_{6} = ^{3}R_{0} \times ^{0}R_{6}$$
Therefore, to get the orientation of the wrist relative to frame 3, the orientation of frame 3 relative to fixed frame need to be computed first


```python
M0_3 = M0_1 * M1_2 * M2_3
print('Orientationg of frame 3 relative to fixed frame')
print('Column 1')
M0_3 = simplify(M0_3)
pprint(M0_3[:-1, 0])
print('-------')
print('Column 2')
pprint(M0_3[:-1, 1])
print('-------')
print('Column 3')
pprint(M0_3[:-1, 2])
print('-------')
```

    Orientationg of frame 3 relative to fixed frame
    Column 1
    ⎡-sin(q₂ + q₃)⋅cos(q₁)⎤
    ⎢                     ⎥
    ⎢sin(q₁)⋅sin(q₂ + q₃) ⎥
    ⎢                     ⎥
    ⎣    -cos(q₂ + q₃)    ⎦
    -------
    Column 2
    ⎡-cos(q₁)⋅cos(q₂ + q₃)⎤
    ⎢                     ⎥
    ⎢sin(q₁)⋅cos(q₂ + q₃) ⎥
    ⎢                     ⎥
    ⎣    sin(q₂ + q₃)     ⎦
    -------
    Column 3
    ⎡sin(q₁)⎤
    ⎢       ⎥
    ⎢cos(q₁)⎥
    ⎢       ⎥
    ⎣   0   ⎦
    -------


With the numreical value of the orientation of the Wrist relative to frame 3, the value of $q_4, q_5$ and $q_6$ are computed by comparing the symbolic expression of $^3R_6$ to its numerical value


```python
M3_6 = M3_4 * M4_5 * M5_6
print('Orientation of Frame 6 relative to Frame 3')
print('Column 0')
pprint(M3_6[:-1, 0])
print('Column 1')
pprint(M3_6[:-1, 1])
print('Column 2')
pprint(M3_6[:-1, 2])
```

    Orientation of Frame 6 relative to Frame 3
    Column 0
    ⎡-sin(q₄)⋅sin(q₆) + cos(q₄)⋅cos(q₅)⋅cos(q₆)⎤
    ⎢                                          ⎥
    ⎢             sin(q₅)⋅cos(q₆)              ⎥
    ⎢                                          ⎥
    ⎣-sin(q₄)⋅cos(q₅)⋅cos(q₆) - sin(q₆)⋅cos(q₄)⎦
    Column 1
    ⎡-sin(q₄)⋅cos(q₆) - sin(q₆)⋅cos(q₄)⋅cos(q₅)⎤
    ⎢                                          ⎥
    ⎢             -sin(q₅)⋅sin(q₆)             ⎥
    ⎢                                          ⎥
    ⎣sin(q₄)⋅sin(q₆)⋅cos(q₅) - cos(q₄)⋅cos(q₆) ⎦
    Column 2
    ⎡-sin(q₅)⋅cos(q₄)⎤
    ⎢                ⎥
    ⎢    cos(q₅)     ⎥
    ⎢                ⎥
    ⎣sin(q₄)⋅sin(q₅) ⎦


According to the matrix above, the value of $q_4, q_5,$ and $q_6$ can be derived using the second row and the third column of $^{3}R_{6}$
$$
q_5 = +/- acos\left(^3R_6[1,2]\right)
$$
With a value of $q_5$
$$
q_4 = atan2\left(^3R_6[2,2]/s_5, -^3R_6[0,2]/s_5\right)
$$
$$
q_6 = atan2\left(-^3R_6[1,1]/s_5, ^3R_6[1,0]/s_5\right)
$$

## Note on organizing the IG solutions
The solution to Wrist Orienting problem shows that a solution to Wrist Positioning problem results in two soluntions to Wrist Orienting problem. So, the total number of solutions to IGM is 2 times the number of solutions to Wrist Positioning problem. 

Using the relation between the number of solutions to Wrist Positioning and Wrist Orienting, all the solutions to IGM are stored in a matrix of size 6 by 2 times number of solutions to Wrist Position ( = number of solutions to type 7 trigonometric model). 

A pair of $\left(q_2, q_3\right)$ is used to update 2 columns of the IG solutions matrix. These columns are differed by the solutions to Wrist Orienting problem.


```python

```

4e9318491738227771451f1ada94a6e6acd73339
ipynb
Jupyter Notebook
kuka_geometric_model.ipynb
quan-dao/ECN-MANIP
4b455cb5e0db06d9e24b1a70dd7289a68f29067f
MIT
kuka_geometric_model.ipynb
quan-dao/ECN-MANIP
4b455cb5e0db06d9e24b1a70dd7289a68f29067f
MIT
kuka_geometric_model.ipynb
quan-dao/ECN-MANIP
4b455cb5e0db06d9e24b1a70dd7289a68f29067f
MIT
Qwen/Qwen-72B
1. YES
2. YES