```python
from sympy import *
from mpmath import atan2, degrees
```


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


```python
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
```


```python
# Test cosine function 

g = get_cosine_law_angle(3, 4, 6)
print(g) # g = 2.05

g = get_cosine_law_angle(3, 4, 5)
print(g) # g = 1.57
```

    2.04691538770669
    1.5707963267949



```python
# Test hypotenuse function 

c = get_hypotenuse(3, 4)
print(c) # 5

c = get_hypotenuse(3, 5)
print(c) # sqrt(34) = 0.53

l = get_hypotenuse( 1.5, 0.054)
print(l)

gamma = atan2(1.5, 0.054)
print(gamma)
```

    5
    sqrt(34)
    1.50097168527591
    1.53481186671284



```python
def get_first_three_angles(wrist_center):
  # given the wrist center which a tuple of 3 numbers x, y, z
  # (x, y, z) is the wrist center point wrt base frame
  # return the angles q1, q2, q3 for each respective joint
  # given geometry of the kuka kr210
    
  x, y, z  = wrist_center
    
  a1, a2, a3 = 0.35, 1.25, -0.054
  d1, d4 = 0.75, 1.5
  l = 1.50097168527591 #get_hypotenuse(d4, -a3)
  phi = 1.53481186671284 # atan2(d4, -a3)
  
  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 
```


```python
# Test wrist center inverse kinematics
wrist_center = 1.79505, 1.84825, 0.3094

q1, q2, q3 = get_first_three_angles(wrist_center)
print("\n q1:", q1, "\n q2:", q2, "\n q3:", q3)# q1 = 0.8, q2 = 1.1, q3 = -0.4

print(q2.evalf())
```

    gamma 1.93479351084403
    phi 1.53481186671284
    
     q1: 0.799999272099671 
     q2: -0.470820255901082 + pi/2 
     q3: -0.399981644131188
    1.09997607089381



```python
print("q2:", -1.8470352502004 + 3.14 / 2 )
print("q3:", -3.25681192184063 + 3.14)

wrist_center = 0.750499, 1.201, 2.475

q1, q2, q3 = get_first_three_angles(wrist_center)
print("\n q1:", q1, "\n q2:", q2, "\n q3:", q3)
# q1 = 1.012, q2 = -0.2758003637377226 , q3 = -0.11568

q2.evalf()
```

    q2: -0.2770352502003999
    q3: -0.11681192184062983
    gamma 1.65003113496368
    phi 1.53481186671284
    
     q1: 1.01227245775763 
     q2: -1.8470352502004 + pi/2 
     q3: -0.115219268250835





    -0.276238923405500




```python

```


```python

```

2e32a6c1f3ab50f866c41b2bf2ffd282cf2351fe
ipynb
Jupyter Notebook
playground/wrist-center-ik.ipynb
mithi/arm-ik
e7e87ef0e43b04278d2300f67d863c3f7eafb77e
MIT
2017-07-29T11:40:03.000Z
2022-02-28T14:49:48.000Z
playground/wrist-center-ik.ipynb
mithi/arm-ik
e7e87ef0e43b04278d2300f67d863c3f7eafb77e
MIT
playground/wrist-center-ik.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