```python
from sympy import symbols, cos, sin, pi, simplify, pprint, tan, expand_trig, sqrt, trigsimp, atan2
from sympy.matrices import Matrix
```


```python
q1, q2, q3, q4, q5, q6= symbols('q1:7')
alpha, beta, gamma = symbols('alpha beta gamma', real = True)
px, py, pz = symbols('px py pz', real = True)
```


```python
R03 = Matrix([
[sin(q2 + q3)*cos(q1), cos(q1)*cos(q2 + q3), -sin(q1)],
[sin(q1)*sin(q2 + q3), sin(q1)*cos(q2 + q3),  cos(q1)],
[        cos(q2 + q3),        -sin(q2 + q3),        0]])

R03T = Matrix([
[sin(q2 + q3)*cos(q1), sin(q1)*sin(q2 + q3),  cos(q2 + q3)],
[cos(q1)*cos(q2 + q3), sin(q1)*cos(q2 + q3), -sin(q2 + q3)],
[            -sin(q1),              cos(q1),             0]])
```


```python
R36 = Matrix([
[-sin(q4)*sin(q6) + cos(q4)*cos(q5)*cos(q6), -sin(q4)*cos(q6) - sin(q6)*cos(q4)*cos(q5), -sin(q5)*cos(q4)],
[                           sin(q5)*cos(q6),                           -sin(q5)*sin(q6),          cos(q5)],
[-sin(q4)*cos(q5)*cos(q6) - sin(q6)*cos(q4),  sin(q4)*sin(q6)*cos(q5) - cos(q4)*cos(q6),  sin(q4)*sin(q5)]])
```


```python
R0u = Matrix([
[1.0*cos(alpha)*cos(beta), -1.0*sin(alpha)*cos(gamma) + sin(beta)*sin(gamma)*cos(alpha), 1.0*sin(alpha)*sin(gamma) + sin(beta)*cos(alpha)*cos(gamma)],
[1.0*sin(alpha)*cos(beta),  sin(alpha)*sin(beta)*sin(gamma) + 1.0*cos(alpha)*cos(gamma), sin(alpha)*sin(beta)*cos(gamma) - 1.0*sin(gamma)*cos(alpha)],
[          -1.0*sin(beta),                                     1.0*sin(gamma)*cos(beta),                                    1.0*cos(beta)*cos(gamma)]])
```


```python
Rgu_eval = Matrix([[0, 0, 1], [0, -1.00000000000000, 0], [1.00000000000000, 0, 0]])
RguT_eval = Matrix([[0, 0, 1], [0, -1.00000000000000, 0], [1.00000000000000, 0, 0]])
```


```python
# Total transform wrt gripper given
# yaw (alpha), pitch (beta), roll (beta)
# position px, py, pz

T0g_b = Matrix([
[1.0*sin(alpha)*sin(gamma) + sin(beta)*cos(alpha)*cos(gamma),  1.0*sin(alpha)*cos(gamma) - 1.0*sin(beta)*sin(gamma)*cos(alpha), 1.0*cos(alpha)*cos(beta), px],
[sin(alpha)*sin(beta)*cos(gamma) - 1.0*sin(gamma)*cos(alpha), -1.0*sin(alpha)*sin(beta)*sin(gamma) - 1.0*cos(alpha)*cos(gamma), 1.0*sin(alpha)*cos(beta), py],
[                                   1.0*cos(beta)*cos(gamma),                                        -1.0*sin(gamma)*cos(beta),           -1.0*sin(beta), pz],
[                                                          0,                                                                0,                        0,  1]])
```


```python
# Total transform wrt gripper given 
# angles q1, q2, q3, q4, q5, q6

T0g_a = Matrix([
[((sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4))*cos(q5) + sin(q5)*cos(q1)*cos(q2 + q3))*cos(q6) - (-sin(q1)*cos(q4) + sin(q4)*sin(q2 + q3)*cos(q1))*sin(q6), -((sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4))*cos(q5) + sin(q5)*cos(q1)*cos(q2 + q3))*sin(q6) + (sin(q1)*cos(q4) - sin(q4)*sin(q2 + q3)*cos(q1))*cos(q6), -(sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4))*sin(q5) + cos(q1)*cos(q5)*cos(q2 + q3), -0.303*sin(q1)*sin(q4)*sin(q5) + 1.25*sin(q2)*cos(q1) - 0.303*sin(q5)*sin(q2 + q3)*cos(q1)*cos(q4) - 0.054*sin(q2 + q3)*cos(q1) + 0.303*cos(q1)*cos(q5)*cos(q2 + q3) + 1.5*cos(q1)*cos(q2 + q3) + 0.35*cos(q1)],
[ ((sin(q1)*sin(q2 + q3)*cos(q4) - sin(q4)*cos(q1))*cos(q5) + sin(q1)*sin(q5)*cos(q2 + q3))*cos(q6) - (sin(q1)*sin(q4)*sin(q2 + q3) + cos(q1)*cos(q4))*sin(q6), -((sin(q1)*sin(q2 + q3)*cos(q4) - sin(q4)*cos(q1))*cos(q5) + sin(q1)*sin(q5)*cos(q2 + q3))*sin(q6) - (sin(q1)*sin(q4)*sin(q2 + q3) + cos(q1)*cos(q4))*cos(q6), -(sin(q1)*sin(q2 + q3)*cos(q4) - sin(q4)*cos(q1))*sin(q5) + sin(q1)*cos(q5)*cos(q2 + q3),  1.25*sin(q1)*sin(q2) - 0.303*sin(q1)*sin(q5)*sin(q2 + q3)*cos(q4) - 0.054*sin(q1)*sin(q2 + q3) + 0.303*sin(q1)*cos(q5)*cos(q2 + q3) + 1.5*sin(q1)*cos(q2 + q3) + 0.35*sin(q1) + 0.303*sin(q4)*sin(q5)*cos(q1)],
[                                                                -(sin(q5)*sin(q2 + q3) - cos(q4)*cos(q5)*cos(q2 + q3))*cos(q6) - sin(q4)*sin(q6)*cos(q2 + q3),                                                                  (sin(q5)*sin(q2 + q3) - cos(q4)*cos(q5)*cos(q2 + q3))*sin(q6) - sin(q4)*cos(q6)*cos(q2 + q3),                                     -sin(q5)*cos(q4)*cos(q2 + q3) - sin(q2 + q3)*cos(q5),                                                                                 -0.303*sin(q5)*cos(q4)*cos(q2 + q3) - 0.303*sin(q2 + q3)*cos(q5) - 1.5*sin(q2 + q3) + 1.25*cos(q2) - 0.054*cos(q2 + q3) + 0.75],
[                                                                                                                                                            0,                                                                                                                                                             0,                                                                                        0,                                                                                                                                                                                                              1]])
```


```python
def get_hypotenuse(a, b):
  return sqrt(a*a + b*b)

def get_cosine_law_angle(a, b, c):    
  cos_gamma = (a*a + b*b - c*c) / (2*a*b)
  sin_gamma = sqrt(1 - cos_gamma * cos_gamma)
  gamma = atan2(sin_gamma, cos_gamma)

  return gamma

def get_wrist_center(gripper_point, R0g, dg = 0.303):

  xu, yu, zu = gripper_point 
    
  nx, ny, nz = R0g[0, 2], R0g[1, 2], R0g[2, 2]
  xw = xu - dg * nx
  yw = yu - dg * ny
  zw = zu - dg * nz 

  return xw, yw, zw
def get_first_three_angles(wrist_center):
    
  x, y, z  = wrist_center
    
  a1, a2, a3 = 0.35, 1.25, -0.054
  d1, d4 = 0.75, 1.5
  l = 1.50097168527591
  phi = 1.53481186671284
  
  x_prime = get_hypotenuse(x, y)
  mx = x_prime -  a1
  my = z - d1 
  m = get_hypotenuse(mx, my)
  alpha = atan2(my, mx)
  
  gamma = get_cosine_law_angle(l, a2, m)
  beta = get_cosine_law_angle(m, a2, l)
  
  q1 = atan2(y, x)
  q2 = pi/2 - beta - alpha 
  q3 = -(gamma - phi)
    
  return q1, q2, q3 

def get_last_three_angles(R):
    
  sin_q4 = R[2, 2]
  cos_q4 =  -R[0, 2]
    
  sin_q5 = sqrt(R[0, 2]**2 + R[2, 2]**2) 
  cos_q5 = R[1, 2]
    
  sin_q6 = -R[1, 1]
  cos_q6 = R[1, 0] 
  
  q4 = atan2(sin_q4, cos_q4)
  q5 = atan2(sin_q5, cos_q5)
  q6 = atan2(sin_q6, cos_q6)
    
  return q4, q5, q6
```


```python
def ik(x, y, z, roll, pitch, yaw, debug = False):
    
  # input: given position and orientation of the gripper wrt to URDFrame
  # output: angles q1, q2, q3, q4, q5, q6
    
  gripper_point = x, y, z

  R0u_eval = R0u.evalf(subs = {alpha: yaw, beta: pitch, gamma: roll})
  R0g_eval = R0u_eval * RguT_eval

  wrist_center = get_wrist_center(gripper_point, R0g_eval, dg = 0.303)

  j1, j2, j3 = get_first_three_angles(wrist_center)

  R03T_eval = R03T.evalf(subs = {q1: j1.evalf(), q2: j2.evalf(), q3: j3.evalf()})
  R36_eval = R03T_eval * R0g_eval

  j4, j5, j6 = get_last_three_angles(R36_eval)

  j1 = j1.evalf()
  j2 = j2.evalf()
  j3 = j3.evalf()
  j4 = j4.evalf()
  j5 = j5.evalf()
  j6 = j6.evalf()

  if debug:
  
    print()
    print("\n x:", x, "\n y:", y, "\n z:", z)
    print("\n roll:", roll, "\n pitch:", pitch, "\n yaw:", yaw)
        
    print()
    print(" j1:", j1, "\n j2:", j2, "\n j3:", j3)
    print(" j4:", j4, "\n j5:", j5, "\n j6:", j6)
    
    print()
    print("wrist_center", wrist_center)

    print()
    print("evaluated R0g:")
    pprint(R0g_eval)

    print()
    print("Total transform wrt gripper: given yaw (alpha), pitch (beta), roll (beta), px, py, pz")
    pprint(T0g_b.evalf(subs = {
      gamma: roll, beta: pitch, alpha: yaw, px: x, py: y, pz: z
    }))

    print()
    print("Total transform wrt gripper: given angles q1, q2, q3, q4, q5, q6")
    pprint(T0g_a.evalf(subs = {
      q1: j1, q2: j2, q3: j3, q4: j4, q5: j5, q6: j6    
    }))
    
  return j1, j2, j3, j4, j5, j6
```


```python
qs = ik(x = 0.49792, y = 1.3673, z = 2.4988,
        roll = 0.366, pitch = -0.078, yaw = 2.561, debug = True)
```

    
    
     x: 0.49792 
     y: 1.3673 
     z: 2.4988
    
     roll: 0.366 
     pitch: -0.078 
     yaw: 2.561
    
     j1: 1.01249809363771 
     j2: -0.275800363737724 
     j3: -0.115686651053751
     j4: 1.63446527240323 
     j5: 1.52050002599430 
     j6: -0.815781306199679
    
    wrist_center (0.750499428337951, 1.20160389781975, 2.47518995758694)
    
    evaluated R0g:
    ⎡0.257143295038827   0.48887208255965   -0.833595473062543⎤
    ⎢                                                         ⎥
    ⎢0.259329420712765  0.796053601157403    0.54685182237706 ⎥
    ⎢                                                         ⎥
    ⎣0.93092726749696   -0.356795110642117  0.0779209320563015⎦
    
    Total transform wrt gripper: given yaw (alpha), pitch (beta), roll (beta), px, py, pz
    ⎡0.257143295038827   0.48887208255965   -0.833595473062543  0.49792⎤
    ⎢                                                                  ⎥
    ⎢0.259329420712765  0.796053601157403    0.54685182237706   1.3673 ⎥
    ⎢                                                                  ⎥
    ⎢0.93092726749696   -0.356795110642117  0.0779209320563015  2.4988 ⎥
    ⎢                                                                  ⎥
    ⎣        0                  0                   0             1.0  ⎦
    
    Total transform wrt gripper: given angles q1, q2, q3, q4, q5, q6
    ⎡0.257143295038827   0.48887208255965   -0.833595473062543  0.497919999999998⎤
    ⎢                                                                            ⎥
    ⎢0.259329420712765  0.796053601157403    0.54685182237706        1.3673      ⎥
    ⎢                                                                            ⎥
    ⎢0.93092726749696   -0.356795110642117  0.0779209320563016  2.49880000000001 ⎥
    ⎢                                                                            ⎥
    ⎣        0                  0                   0                  1.0       ⎦



```python
qs = ik(x = 2.3537, y = -0.1255546, z = 2.841452,
        roll = 0.131008, pitch = -0.10541, yaw = 0.0491503, debug = True)

# -0.07, 0.41, -1.07, 0.32, 0.46, 0 
```

    
    
     x: 2.3537 
     y: -0.1255546 
     z: 2.841452
    
     roll: 0.131008 
     pitch: -0.10541 
     yaw: 0.0491503
    
     j1: -0.0682697289101386 
     j2: 0.434273483083027 
     j3: -1.13476160607020
     j4: 0.206486955261342 
     j5: 0.604353673052791 
     j6: -0.0272724984420472
    
    wrist_center (2.05274568076731, -0.140358517861560, 2.80957188470638)
    
    evaluated R0g:
    ⎡-0.0977692193362551  0.0624374999845176  0.993248578325719 ⎤
    ⎢                                                           ⎥
    ⎢-0.135600778861087   -0.989558155295888  0.0488578147246194⎥
    ⎢                                                           ⎥
    ⎣ 0.985927790724374   -0.129908490419534  0.105214901959143 ⎦
    
    Total transform wrt gripper: given yaw (alpha), pitch (beta), roll (beta), px, py, pz
    ⎡-0.0977692193362551  0.0624374999845176  0.993248578325719     2.3537  ⎤
    ⎢                                                                       ⎥
    ⎢-0.135600778861087   -0.989558155295888  0.0488578147246194  -0.1255546⎥
    ⎢                                                                       ⎥
    ⎢ 0.985927790724374   -0.129908490419534  0.105214901959143    2.841452 ⎥
    ⎢                                                                       ⎥
    ⎣         0                   0                   0              1.0    ⎦
    
    Total transform wrt gripper: given angles q1, q2, q3, q4, q5, q6
    ⎡-0.0977692193362551  0.0624374999845176  0.993248578325719   2.35369999999999
    ⎢                                                                             
    ⎢-0.135600778861087   -0.989558155295888  0.0488578147246194     -0.1255546   
    ⎢                                                                             
    ⎢ 0.985927790724374   -0.129908490419534  0.105214901959143       2.841452    
    ⎢                                                                             
    ⎣         0                   0                   0                 1.0       
    
    ⎤
    ⎥
    ⎥
    ⎥
    ⎥
    ⎥
    ⎦



```python
qs = ik(x = 2.3537, y = -0.1255546, z = 2.841452,
        yaw = 0.131008, pitch = -0.10541, roll = 0.0491503, debug = True)

# -0.07, 0.41, -1.07, 0.32, 0.46, 0 
```

    
    
     x: 2.3537 
     y: -0.1255546 
     z: 2.841452
    
     roll: 0.0491503 
     pitch: -0.10541 
     yaw: 0.131008
    
     j1: -0.0800813023075095 
     j2: 0.439897727669121 
     j3: -1.14290690404898
     j4: 0.363106480125535 
     j5: 0.626901765463759 
     j6: -0.226818468786107
    
    wrist_center (2.05496387941051, -0.164916872597119, 2.80957188470638)
    
    evaluated R0g:
    ⎡-0.0977692193362551   0.135600778861087   0.985927790724374⎤
    ⎢                                                           ⎥
    ⎢-0.0624374999845176  -0.989558155295888   0.129908490419534⎥
    ⎢                                                           ⎥
    ⎣ 0.993248578325719   -0.0488578147246194  0.105214901959143⎦
    
    Total transform wrt gripper: given yaw (alpha), pitch (beta), roll (beta), px, py, pz
    ⎡-0.0977692193362551   0.135600778861087   0.985927790724374    2.3537  ⎤
    ⎢                                                                       ⎥
    ⎢-0.0624374999845176  -0.989558155295888   0.129908490419534  -0.1255546⎥
    ⎢                                                                       ⎥
    ⎢ 0.993248578325719   -0.0488578147246194  0.105214901959143   2.841452 ⎥
    ⎢                                                                       ⎥
    ⎣         0                    0                   0             1.0    ⎦
    
    Total transform wrt gripper: given angles q1, q2, q3, q4, q5, q6
    ⎡-0.0977692193362552   0.135600778861087   0.985927790724374   2.3536999999999
    ⎢                                                                             
    ⎢-0.0624374999845176  -0.989558155295888   0.129908490419534  -0.1255545999999
    ⎢                                                                             
    ⎢ 0.993248578325719   -0.0488578147246194  0.105214901959143   2.8414520000000
    ⎢                                                                             
    ⎣         0                    0                   0                 1.0      
    
    9 ⎤
      ⎥
    99⎥
      ⎥
    1 ⎥
      ⎥
      ⎦



```python

```

725f15ba5198348403be819ff74cf76573674377
ipynb
Jupyter Notebook
notebooks/joint_angles_ik_optimized.ipynb
mithi/arm-ik
e7e87ef0e43b04278d2300f67d863c3f7eafb77e
MIT
2017-07-29T11:40:03.000Z
2022-02-28T14:49:48.000Z
notebooks/joint_angles_ik_optimized.ipynb
mithi/arm-ik
e7e87ef0e43b04278d2300f67d863c3f7eafb77e
MIT
notebooks/joint_angles_ik_optimized.ipynb
mithi/arm-ik
e7e87ef0e43b04278d2300f67d863c3f7eafb77e
MIT
2017-10-27T13:30:21.000Z
2022-02-10T10:08:42.000Z
Qwen/Qwen-72B
1. YES
2. YES