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


def rotx(q):

  sq, cq = sin(q), cos(q)

  r = Matrix([
    [1., 0., 0.],
    [0., cq,-sq],
    [0., sq, cq]
  ])
    
  return r


def roty(q):

  sq, cq = sin(q), cos(q)

  r = Matrix([
    [ cq, 0., sq],
    [ 0., 1., 0.],
    [-sq, 0., cq]
  ])
    
  return r


def rotz(q):

  sq, cq = sin(q), cos(q)

  r = Matrix([
    [cq,-sq, 0.],
    [sq, cq, 0.],
    [0., 0., 1.]
  ])
    
  return r
```


```python
# the yaw, pitch roll is given wrt to the URDF frame 
# We must convert this to gripper frame by performing
# a rotation of 180 degrees ccw about the z axis and then 
# a rotation of 90 degrees cw about the new y axis

r = rotz(pi) * roty(-pi/2) 

pprint(r)
pprint(r.T)

result1 = r * Matrix([0,0,1])
result2 = r.T * Matrix([0,0,1])
print(result1 == result2)
print(r.T == r)
```

    ⎡ 0    0    1⎤
    ⎢            ⎥
    ⎢ 0   -1.0  0⎥
    ⎢            ⎥
    ⎣1.0   0    0⎦
    ⎡0   0    1.0⎤
    ⎢            ⎥
    ⎢0  -1.0   0 ⎥
    ⎢            ⎥
    ⎣1   0     0 ⎦
    True
    True



```python
RguT = Matrix([[0,0,1],[0, -1, 0],[1,0,0]]) #RguT = (rotz(pi) * roty(-pi/2)).T

def get_wrist_center(xu, yu, zu, R0u, dg = 0.303):
  # get the coordinates of the wrist center wrt to the base frame (xw, yw, zw)
  # given the following info:
  # the coordinates of the gripper (end effector) (x, y, z)
  # the rotation of the gripper in URDF frame wrt to the base frame (R0u)
  # the distance between gripper and wrist center dg
  # which is along common z axis
    
  nx, ny, nz = R0u[0, 2], R0u[1, 2], R0u[2, 2]
  xw = xu - dg * nx
  yw = yu - dg * ny
  zw = zu - dg * nz 

  return xw, yw, zw
```


```python
px, py, pz = 0.49792, 1.3673, 2.4988
roll, pitch, yaw = 0.366, -0.078, 2.561

R0u = rotz(yaw) * roty(pitch) * rotx(roll) * RguT

print("wrist center:")
print(get_wrist_center(px, py, pz, R0u, dg = 0.303))
```

    wrist center:
    (0.750499428337951, 1.20160389781975, 2.47518995758694)



```python
px, py, pz = 2.153, 0, 1.946
roll, pitch, yaw = 0, 0, 0

R0u = rotz(yaw) * roty(pitch) * rotx(roll) * RguT

print("wrist center:")
print(get_wrist_center(px, py, pz, R0u, dg = 0.303))
```

    wrist center:
    (1.85000000000000, 0, 1.94600000000000)


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