Diner Robot Inverse Kinematics calculations


```python
import sympy as sp
import numpy as np
import math


```

UR5 angles and link lengths:


```python
q1, q2, q3, q4, q5, q6, q7, = sp.symbols('q1 q2 q3 q4 q5 q6 q7')
d = [0.089159, 0, 0, 0.10915, 0.09465, 0.0823]
a = [0.0, -0.425, -0.39225, 0.0, 0.0, 0]



```

Transformation Matrix:


```python
T0 = sp.Matrix([[1, 0, 0, 0],
               [0, 1, 0, 0],
               [0, 0, 1, 0],
               [0, 0, 0, 1]])

T1 = sp.Matrix([[sp.cos(q1), 0, -sp.sin(q1), 0],
                [sp.sin(q1),     0, sp.cos(q1), 0],
                [0,             -1,   0,       d[0]],
                [0, 0, 0, 1]])

#T1 = T1.subs({q1: math.radians(90), q2: math.radians(0), q3: math.radians(0), q4: math.radians(-90), q5: math.radians(0), q6: math.radians(0), q7: math.radians(0)})

T2 = sp.Matrix([[sp.cos(q2), 0, sp.sin(q2), 0],
                [sp.sin(q2), 0, -sp.cos(q2), 0],
                [0, 1, 0, d[1]],
                [0, 0, 0, 1]])

#T2 = T2.subs({q1: math.radians(90), q2: math.radians(0), q3: math.radians(0), q4: math.radians(-90), q5: math.radians(0), q6: math.radians(0), q7: math.radians(0)})
q3 = 0
T3 = sp.Matrix([[sp.cos(q3), 0, sp.sin(q3), 0],
                [sp.sin(q3), 0, -sp.cos(q3), 0],
                [0, 1, 0, d[2]],
                [0, 0, 0, 1]])

#T3 = T3.subs({q1: math.radians(90), q2: math.radians(0), q3: math.radians(0), q4: math.radians(-90), q5: math.radians(0), q6: math.radians(0), q7: math.radians(0)})

T4 = sp.Matrix([[sp.cos(q4), 0, -sp.sin(q4), 0],
                [sp.sin(q4), 0, sp.cos(q4), 0],
                [0, -1, 0, d[3]],
                [0, 0, 0, 1]])
#T4 = T4.subs({q1: math.radians(90), q2: math.radians(0), q3: math.radians(0), q4: math.radians(-90), q5: math.radians(0), q6: math.radians(0), q7: math.radians(0)})

T5 = sp.Matrix([[sp.cos(q5), 0, -sp.sin(q5), 0],
                [sp.sin(q5), 0, sp.cos(q5), 0],
                [0, -1, 0, d[4]],
                [0, 0, 0, 1]])


#T5 = T5.subs({q1: math.radians(90), q2: math.radians(0), q3: math.radians(0), q4: math.radians(-90), q5: math.radians(0), q6: math.radians(0), q7: math.radians(0)})

T6 = sp.Matrix([[sp.cos(q6), 0, sp.sin(q6), 0],
                [sp.sin(q6), 0, -sp.cos(q6), 0],
                [0, 1, 0, d[5]],
                [0, 0, 0, 1]])
#T6= T6.subs({q1: math.radians(90), q2: math.radians(0), q3: math.radians(0), q4: math.radians(-90), q5: math.radians(0), q6: math.radians(0), q7: math.radians(0)})

```

Final Transformation Matrix:


```python
T = (T0*T1*T2*T3*T4*T5*T6)

T
```




$\displaystyle \left[\begin{matrix}\left(\left(\sin{\left(q_{2} \right)} \sin{\left(q_{4} \right)} \cos{\left(q_{1} \right)} + \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)} \cos{\left(q_{4} \right)}\right) \cos{\left(q_{5} \right)} - \sin{\left(q_{1} \right)} \sin{\left(q_{5} \right)}\right) \cos{\left(q_{6} \right)} + \left(- \sin{\left(q_{2} \right)} \cos{\left(q_{1} \right)} \cos{\left(q_{4} \right)} + \sin{\left(q_{4} \right)} \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)}\right) \sin{\left(q_{6} \right)} & - \left(\sin{\left(q_{2} \right)} \sin{\left(q_{4} \right)} \cos{\left(q_{1} \right)} + \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)} \cos{\left(q_{4} \right)}\right) \sin{\left(q_{5} \right)} - \sin{\left(q_{1} \right)} \cos{\left(q_{5} \right)} & \left(\left(\sin{\left(q_{2} \right)} \sin{\left(q_{4} \right)} \cos{\left(q_{1} \right)} + \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)} \cos{\left(q_{4} \right)}\right) \cos{\left(q_{5} \right)} - \sin{\left(q_{1} \right)} \sin{\left(q_{5} \right)}\right) \sin{\left(q_{6} \right)} - \left(- \sin{\left(q_{2} \right)} \cos{\left(q_{1} \right)} \cos{\left(q_{4} \right)} + \sin{\left(q_{4} \right)} \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)}\right) \cos{\left(q_{6} \right)} & - 0.0823 \left(\sin{\left(q_{2} \right)} \sin{\left(q_{4} \right)} \cos{\left(q_{1} \right)} + \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)} \cos{\left(q_{4} \right)}\right) \sin{\left(q_{5} \right)} - 0.0823 \sin{\left(q_{1} \right)} \cos{\left(q_{5} \right)} + 0.10915 \sin{\left(q_{1} \right)} + 0.09465 \sin{\left(q_{2} \right)} \cos{\left(q_{1} \right)} \cos{\left(q_{4} \right)} - 0.09465 \sin{\left(q_{4} \right)} \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)}\\\left(\left(\sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} \sin{\left(q_{4} \right)} + \sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)} \cos{\left(q_{4} \right)}\right) \cos{\left(q_{5} \right)} + \sin{\left(q_{5} \right)} \cos{\left(q_{1} \right)}\right) \cos{\left(q_{6} \right)} + \left(- \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} \cos{\left(q_{4} \right)} + \sin{\left(q_{1} \right)} \sin{\left(q_{4} \right)} \cos{\left(q_{2} \right)}\right) \sin{\left(q_{6} \right)} & - \left(\sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} \sin{\left(q_{4} \right)} + \sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)} \cos{\left(q_{4} \right)}\right) \sin{\left(q_{5} \right)} + \cos{\left(q_{1} \right)} \cos{\left(q_{5} \right)} & \left(\left(\sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} \sin{\left(q_{4} \right)} + \sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)} \cos{\left(q_{4} \right)}\right) \cos{\left(q_{5} \right)} + \sin{\left(q_{5} \right)} \cos{\left(q_{1} \right)}\right) \sin{\left(q_{6} \right)} - \left(- \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} \cos{\left(q_{4} \right)} + \sin{\left(q_{1} \right)} \sin{\left(q_{4} \right)} \cos{\left(q_{2} \right)}\right) \cos{\left(q_{6} \right)} & - 0.0823 \left(\sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} \sin{\left(q_{4} \right)} + \sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)} \cos{\left(q_{4} \right)}\right) \sin{\left(q_{5} \right)} + 0.09465 \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} \cos{\left(q_{4} \right)} - 0.09465 \sin{\left(q_{1} \right)} \sin{\left(q_{4} \right)} \cos{\left(q_{2} \right)} + 0.0823 \cos{\left(q_{1} \right)} \cos{\left(q_{5} \right)} - 0.10915 \cos{\left(q_{1} \right)}\\\left(- \sin{\left(q_{2} \right)} \sin{\left(q_{4} \right)} - \cos{\left(q_{2} \right)} \cos{\left(q_{4} \right)}\right) \sin{\left(q_{6} \right)} + \left(- \sin{\left(q_{2} \right)} \cos{\left(q_{4} \right)} + \sin{\left(q_{4} \right)} \cos{\left(q_{2} \right)}\right) \cos{\left(q_{5} \right)} \cos{\left(q_{6} \right)} & - \left(- \sin{\left(q_{2} \right)} \cos{\left(q_{4} \right)} + \sin{\left(q_{4} \right)} \cos{\left(q_{2} \right)}\right) \sin{\left(q_{5} \right)} & - \left(- \sin{\left(q_{2} \right)} \sin{\left(q_{4} \right)} - \cos{\left(q_{2} \right)} \cos{\left(q_{4} \right)}\right) \cos{\left(q_{6} \right)} + \left(- \sin{\left(q_{2} \right)} \cos{\left(q_{4} \right)} + \sin{\left(q_{4} \right)} \cos{\left(q_{2} \right)}\right) \sin{\left(q_{6} \right)} \cos{\left(q_{5} \right)} & - 0.0823 \left(- \sin{\left(q_{2} \right)} \cos{\left(q_{4} \right)} + \sin{\left(q_{4} \right)} \cos{\left(q_{2} \right)}\right) \sin{\left(q_{5} \right)} + 0.09465 \sin{\left(q_{2} \right)} \sin{\left(q_{4} \right)} + 0.09465 \cos{\left(q_{2} \right)} \cos{\left(q_{4} \right)} + 0.089159\\0 & 0 & 0 & 1\end{matrix}\right]$



Transformation at start position:


```python
q = sp.Matrix([ [math.radians(90.001)], [math.radians(0.001)], [math.radians(-90.001)], [math.radians(0.001)], [math.radians(0.001)],  [math.radians(0.001)]])
Tstart = T.subs({q1: q[0], q2: q[1], q4: q[2], q5: q[3], q6: q[4], q7: q[5] })
Tstart
```




$\displaystyle \left[\begin{matrix}-1.74523786614818 \cdot 10^{-5} & -0.999999999695393 & -1.74535971126676 \cdot 10^{-5} & 0.0268483480543087\\-5.23601821401489 \cdot 10^{-5} & -1.74526832816749 \cdot 10^{-5} & 0.999999998476908 & 0.0946504685989643\\-0.999999998476913 & 1.7453292508424 \cdot 10^{-5} & -5.23598775333623 \cdot 10^{-5} & 0.0891571324977001\\0 & 0 & 0 & 1\end{matrix}\right]$



Jacobian calculation:


```python
J = sp.Matrix([[-a[1]*sp.sin(q1)*sp.sin(q2) - d[5]*sp.sin(q1)*sp.sin(q5)*sp.cos(q2 + q3 + q4) - a[2]*sp.sin(q1)*sp.sin(q2 + q3) + d[4]*sp.sin(q1)*sp.sin(q2 + q3 + q4) + d[5]*sp.cos(q1)*sp.cos(q5) + d[3]*sp.cos(q1),
                -(d[5]*sp.sin(q5)*sp.sin(q2 + q3 + q4) - a[1]*sp.cos(q2) -
                  a[2]*sp.cos(q2 + q3) + d[4]*sp.cos(q2 + q3 + q4))*sp.cos(q1),
                -(d[5]*sp.sin(q5)*sp.sin(q2 + q3 + q4) - a[2] *
                  sp.cos(q2 + q3) + d[4]*sp.cos(q2 + q3 + q4))*sp.cos(q1),
                -(d[5]*sp.sin(q5)*sp.sin(q2 + q3 + q4) +
                  d[4]*sp.cos(q2 + q3 + q4))*sp.cos(q1),
                - d[5]*sp.sin(q1)*sp.sin(q5) + d[5]*sp.cos(q1) *
                sp.cos(q5)*sp.cos(q2 + q3 + q4),
                0],
               [d[5]*sp.sin(q1)*sp.cos(q5) + d[3]*sp.sin(q1) + a[1]*sp.sin(q2)*sp.cos(q1) + d[5]*sp.sin(q5)*sp.cos(q1)*sp.cos(q2 + q3 + q4) + a[2]*sp.sin(q2 + q3)*sp.cos(q1) - d[4]*sp.sin(q2 + q3 + q4)*sp.cos(q1),
                -(d[5]*sp.sin(q5)*sp.sin(q2 + q3 + q4) - a[1]*sp.cos(q2) -
                   a[2]*sp.cos(q2 + q3) + d[4]*sp.cos(q2 + q3 + q4))*sp.sin(q1),
                -(d[5]*sp.sin(q5)*sp.sin(q2 + q3 + q4) - a[2] *
                   sp.cos(q2 + q3) + d[4]*sp.cos(q2 + q3 + q4))*sp.sin(q1),
                -(d[5]*sp.sin(q5)*sp.sin(q2 + q3 + q4) +
                   d[4]*sp.cos(q2 + q3 + q4))*sp.sin(q1),
                d[5]*sp.sin(q1)*sp.cos(q5)*sp.cos(q2 + q3 + q4) +
                d[5]*sp.sin(q5)*sp.cos(q1),
                0],
               [0,
                a[1]*sp.sin(q2) + d[5]*sp.sin(q5)*sp.cos(q2 + q3 + q4) +
                a[2]*sp.sin(q2 + q3) - d[4]*sp.sin(q2 + q3 + q4),
                d[5]*sp.sin(q5)*sp.cos(q2 + q3 + q4) + a[2] *
                sp.sin(q2 + q3) - d[4]*sp.sin(q2 + q3 + q4),
                d[5]*sp.sin(q5)*sp.cos(q2 + q3 + q4) -
                d[4]*sp.sin(q2 + q3 + q4),
                d[5]*sp.sin(q2 + q3 + q4)*sp.cos(q5),
                0],
               [0, sp.sin(q1), sp.sin(q1), sp.sin(q1), -sp.sin(q2 + q3 + q4)*sp.cos(q1),
                sp.sin(q1)*sp.cos(q5) + sp.sin(q5)*sp.cos(q1)*sp.cos(q2 + q3 + q4)],
               [0, -sp.cos(q1), -sp.cos(q1), -sp.cos(q1), -sp.sin(q1)*sp.sin(q2 + q3 + q4),
                sp.sin(q1)*sp.sin(q5)*sp.cos(q2 + q3 + q4) - sp.cos(q1)*sp.cos(q5)],
               [1, 0, 0, 0, sp.cos(q2 + q3 + q4), sp.sin(q5)*sp.sin(q2 + q3 + q4)]])
J

```




$\displaystyle \left[\begin{matrix}0.81725 \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} - 0.0823 \sin{\left(q_{1} \right)} \sin{\left(q_{5} \right)} \cos{\left(q_{2} + q_{4} \right)} + 0.09465 \sin{\left(q_{1} \right)} \sin{\left(q_{2} + q_{4} \right)} + 0.0823 \cos{\left(q_{1} \right)} \cos{\left(q_{5} \right)} + 0.10915 \cos{\left(q_{1} \right)} & \left(- 0.0823 \sin{\left(q_{5} \right)} \sin{\left(q_{2} + q_{4} \right)} - 0.81725 \cos{\left(q_{2} \right)} - 0.09465 \cos{\left(q_{2} + q_{4} \right)}\right) \cos{\left(q_{1} \right)} & \left(- 0.0823 \sin{\left(q_{5} \right)} \sin{\left(q_{2} + q_{4} \right)} - 0.39225 \cos{\left(q_{2} \right)} - 0.09465 \cos{\left(q_{2} + q_{4} \right)}\right) \cos{\left(q_{1} \right)} & \left(- 0.0823 \sin{\left(q_{5} \right)} \sin{\left(q_{2} + q_{4} \right)} - 0.09465 \cos{\left(q_{2} + q_{4} \right)}\right) \cos{\left(q_{1} \right)} & - 0.0823 \sin{\left(q_{1} \right)} \sin{\left(q_{5} \right)} + 0.0823 \cos{\left(q_{1} \right)} \cos{\left(q_{5} \right)} \cos{\left(q_{2} + q_{4} \right)} & 0\\0.0823 \sin{\left(q_{1} \right)} \cos{\left(q_{5} \right)} + 0.10915 \sin{\left(q_{1} \right)} - 0.81725 \sin{\left(q_{2} \right)} \cos{\left(q_{1} \right)} + 0.0823 \sin{\left(q_{5} \right)} \cos{\left(q_{1} \right)} \cos{\left(q_{2} + q_{4} \right)} - 0.09465 \sin{\left(q_{2} + q_{4} \right)} \cos{\left(q_{1} \right)} & \left(- 0.0823 \sin{\left(q_{5} \right)} \sin{\left(q_{2} + q_{4} \right)} - 0.81725 \cos{\left(q_{2} \right)} - 0.09465 \cos{\left(q_{2} + q_{4} \right)}\right) \sin{\left(q_{1} \right)} & \left(- 0.0823 \sin{\left(q_{5} \right)} \sin{\left(q_{2} + q_{4} \right)} - 0.39225 \cos{\left(q_{2} \right)} - 0.09465 \cos{\left(q_{2} + q_{4} \right)}\right) \sin{\left(q_{1} \right)} & \left(- 0.0823 \sin{\left(q_{5} \right)} \sin{\left(q_{2} + q_{4} \right)} - 0.09465 \cos{\left(q_{2} + q_{4} \right)}\right) \sin{\left(q_{1} \right)} & 0.0823 \sin{\left(q_{1} \right)} \cos{\left(q_{5} \right)} \cos{\left(q_{2} + q_{4} \right)} + 0.0823 \sin{\left(q_{5} \right)} \cos{\left(q_{1} \right)} & 0\\0 & - 0.81725 \sin{\left(q_{2} \right)} + 0.0823 \sin{\left(q_{5} \right)} \cos{\left(q_{2} + q_{4} \right)} - 0.09465 \sin{\left(q_{2} + q_{4} \right)} & - 0.39225 \sin{\left(q_{2} \right)} + 0.0823 \sin{\left(q_{5} \right)} \cos{\left(q_{2} + q_{4} \right)} - 0.09465 \sin{\left(q_{2} + q_{4} \right)} & 0.0823 \sin{\left(q_{5} \right)} \cos{\left(q_{2} + q_{4} \right)} - 0.09465 \sin{\left(q_{2} + q_{4} \right)} & 0.0823 \sin{\left(q_{2} + q_{4} \right)} \cos{\left(q_{5} \right)} & 0\\0 & \sin{\left(q_{1} \right)} & \sin{\left(q_{1} \right)} & \sin{\left(q_{1} \right)} & - \sin{\left(q_{2} + q_{4} \right)} \cos{\left(q_{1} \right)} & \sin{\left(q_{1} \right)} \cos{\left(q_{5} \right)} + \sin{\left(q_{5} \right)} \cos{\left(q_{1} \right)} \cos{\left(q_{2} + q_{4} \right)}\\0 & - \cos{\left(q_{1} \right)} & - \cos{\left(q_{1} \right)} & - \cos{\left(q_{1} \right)} & - \sin{\left(q_{1} \right)} \sin{\left(q_{2} + q_{4} \right)} & \sin{\left(q_{1} \right)} \sin{\left(q_{5} \right)} \cos{\left(q_{2} + q_{4} \right)} - \cos{\left(q_{1} \right)} \cos{\left(q_{5} \right)}\\1 & 0 & 0 & 0 & \cos{\left(q_{2} + q_{4} \right)} & \sin{\left(q_{5} \right)} \sin{\left(q_{2} + q_{4} \right)}\end{matrix}\right]$




```python
q = sp.Matrix([ [math.radians(90.001)], [math.radians(0.001)], [math.radians(-90.001)], [math.radians(0.001)], [math.radians(0.001)],  [math.radians(0.001)]])
Jstart = J.subs({q1: q[0], q2: q[1], q4: q[2], q5: q[3], q6: q[4], q7: q[5] })
Jstart
```




$\displaystyle \left[\begin{matrix}-0.0946390777151275 & 1.42636782391076 \cdot 10^{-5} & 6.84602891958906 \cdot 10^{-6} & -2.50700136463318 \cdot 10^{-11} & -1.43640597409963 \cdot 10^{-6} & 0\\0.191448348253117 & -0.817248563345077 & -0.39224856347454 & 1.43640597411485 \cdot 10^{-6} & -2.50700268809156 \cdot 10^{-11} & 0\\0 & 0.0946357362966888 & 0.0946431539460094 & 0.09465 & -0.082299999987465 & 0\\0 & 0.999999999847691 & 0.999999999847691 & 0.999999999847691 & -1.74532925191725 \cdot 10^{-5} & 0.999999999695383\\0 & 1.74532925191725 \cdot 10^{-5} & 1.74532925191725 \cdot 10^{-5} & 1.74532925191725 \cdot 10^{-5} & 0.999999999847691 & 1.74532925165142 \cdot 10^{-5}\\1 & 0 & 0 & 0 & -1.60812264967664 \cdot 10^{-16} & -1.74532925190572 \cdot 10^{-5}\end{matrix}\right]$



Waypoint Generation:


```python
qdash = sp.Matrix([ [math.radians(0)], [math.radians(0)], [math.radians(0)], [math.radians(0)], [math.radians(0)], [math.radians(0)]])

theta = np.pi/2 

while(theta< ((2*np.pi) + np.pi/2)):
    Js = J.subs({q1: q[0], q2: q[1], q4: q[2], q5: q[3], q6: q[4], q7: q[5]})
    try:
        Jp = Js.inv()
    except:
        print('Singularity occured')
        q = (q + qdash).evalf()
    radius = 100
    Vx = -radius*np.sin(theta)*((2*np.pi) / 40)
    Vz = radius*np.cos(theta)*((2*np.pi) / 40)
    theta += ((2*np.pi) / 40)
    V = sp.Matrix([ [Vx], [0], [Vz], [0], [0], [0]])
    qdash=Jp*V
    #q_current= q_pervious + 𝑞̇_current . ∆t
    q = (q + qdash).evalf()
     
    Tdash = T.subs({q1: q[0], q2: q[1], q4: q[2], q5: q[3], q6: q[4], q7: q[5] })
    x = Tdash[0,3]
    y = Tdash[1,3]
    z = Tdash[2,3]

    print('x: ',x)
    print('y: ',y)
    print('z: ',z)


```

    x:  -0.103148396181722
    y:  0.0438657283681490
    z:  -0.0355430285281103
    x:  -0.147273159898797
    y:  -0.125202044444935
    z:  0.124921686787584
    x:  -0.00937470520408906
    y:  -0.176525091404733
    z:  0.173067649289900
    x:  0.0427741715230540
    y:  0.0457480001443346
    z:  0.174259377166235
    x:  0.200543121647522
    y:  -0.0465204867667080
    z:  0.0942175473649006
    x:  0.169376367062934
    y:  -0.0628805749009145
    z:  0.147405750141163
    x:  -0.134108848018656
    y:  0.0477309380107116
    z:  0.0328496368472096
    x:  -0.0350128115402672
    y:  0.0515125302498191
    z:  0.00857264081972968
    x:  -0.169606940063976
    y:  0.0988007497385818
    z:  0.151133368225458
    x:  -0.184433570610797
    y:  -0.0372919938820578
    z:  0.186481385745066
    x:  -0.00990999729662447
    y:  0.0445627947231336
    z:  0.177971844273570
    x:  -0.0576029004372159
    y:  0.0709841622164546
    z:  -0.0310210247623603
    Singularity occured
    x:  0.203005119143047
    y:  0.0201969079554846
    z:  0.152017052475800
    x:  0.160648802671862
    y:  -0.0530220562046413
    z:  0.0914412222132482
    x:  0.0709230323404108
    y:  0.0714487921069014
    z:  0.0752101830647717
    x:  0.113419298694141
    y:  -0.138450091962684
    z:  -0.0138227579807484
    x:  -0.0662967586403884
    y:  -0.0668554221950254
    z:  0.00915276648810031
    x:  -0.199541201193674
    y:  -0.0354925315615094
    z:  0.0787001938399681
    x:  -0.123104025540629
    y:  0.0115586624282927
    z:  0.173154659364518
    x:  -0.0814403328514375
    y:  0.162513388345019
    z:  0.185558652419942
    x:  0.0380996583327256
    y:  0.0679197985395505
    z:  0.149309033042676
    x:  -0.0174403172925617
    y:  0.0869443544770498
    z:  -0.0319437852086548
    x:  -0.100645009355833
    y:  0.0332332432424140
    z:  -0.0324276269241449
    x:  -0.0969571742203594
    y:  0.0103086517259070
    z:  0.200678604211980
    x:  0.0590625064303628
    y:  -0.0451298281083800
    z:  -0.00742153409214925
    x:  -0.0407356020896451
    y:  0.0883687577773600
    z:  0.126692415459500
    x:  0.0703878408935651
    y:  0.0663175685738039
    z:  -0.00434934606126991
    x:  0.0158311061762567
    y:  0.0459512835742991
    z:  0.192073463732889
    Singularity occured
    x:  -0.147150931015434
    y:  0.139781190636149
    z:  0.0372244168588145
    x:  -0.0466785404697651
    y:  0.200460967122675
    z:  0.145376416304960
    x:  -0.150464368091746
    y:  0.0192014133460974
    z:  0.0338198542996504
    x:  0.0143818348683777
    y:  -0.0770862493933570
    z:  -0.0300539329563644
    Singularity occured
    x:  0.174641032076904
    y:  0.0894126858472976
    z:  0.00479471558306991
    x:  -0.103326895483028
    y:  -0.160937358613615
    z:  0.0211206718056824
    x:  0.0885162493401619
    y:  0.180639467472392
    z:  0.134417659979678
    x:  -0.0165738006154281
    y:  0.0488425167188685
    z:  0.180814713126384
    x:  0.126874745384624
    y:  -0.0159131656002903
    z:  0.0325312652304365
    x:  -0.156766615937550
    y:  -0.0590423776609432
    z:  0.192240684272611
    x:  0.0513729551442988
    y:  -0.0448545243770389
    z:  -0.00971169175926069
    x:  0.0503098007065950
    y:  -0.101772739030657
    z:  -0.0299103754424827


dec7b2057f26a389c7ba90b9042d8e27fba6be35
ipynb
Jupyter Notebook
UR5Transformation.ipynb
kavyadevd/Diner_Robot
dc09def4407e687fe0eb430b784a068e703f5d4c
BSD-3-Clause
UR5Transformation.ipynb
kavyadevd/Diner_Robot
dc09def4407e687fe0eb430b784a068e703f5d4c
BSD-3-Clause
UR5Transformation.ipynb
kavyadevd/Diner_Robot
dc09def4407e687fe0eb430b784a068e703f5d4c
BSD-3-Clause
Qwen/Qwen-72B
1. YES
2. YES