```python
import modern_robotics as mr
import numpy as np
import sympy as sp
from sympy.physics.mechanics import dynamicsymbols, mechanics_printing
mechanics_printing()
from Utilities.symbolicFunctions import *
from Utilities.kukaKinematics import Slist, Mlist
```

# Task 3
## 3.2 
### Develop and implement a solution for the analytic inverse kinematics for the Agilus robot.


```python
np.set_printoptions(precision=5)
np.set_printoptions(suppress=True)

#The zero config body frame used in the analytical solution. This is defined as the frame Me found in kukaKinematics.py.
M = np.array([[1, 0, 0, 980], #End effector
             [0, 1, 0, 0],
             [0, 0, 1, 435],
             [0, 0, 0, 1]])
print("M: \n",M)

#Thetas_gen can be modified to generate a valid desired position T_sd
thetas_gen = np.array([0,0,0,0,0,0])
T_sd = mr.FKinSpace(M,Slist,thetas_gen)


# Finding the thetas. agilus_analytical_IK is defined in symbolicFunctions.py
thetas_up, thetas_down = agilus_analytical_IK(Slist, M, T_sd)

# Resulting end effector pose and Ps for elbow up:
T_up = mr.FKinSpace(M,Slist,thetas_up)
P_up = ps_from_Tsd(T_up)

# And for elbow down:
T_down = mr.FKinSpace(M,Slist,thetas_down)
P_down = ps_from_Tsd(T_down)

print("The points P reached by both solutions: ", P_down, P_up)



```

    M: 
     [[  1   0   0 980]
     [  0   1   0   0]
     [  0   0   1 435]
     [  0   0   0   1]]
    The points P reached by both solutions:  [900.   0. 435.   1.] [900.   0. 435.   1.]


## Task 3.3
### Confirm that the solution of the analytical inverse kinematics from the previous point agrees with the solution from a numerical inverse kinematics solver.



```python
#Numerical solution
thetas_num, found = mr.IKinSpace(Slist,M,T_sd,[0,0,1,0,0.5,0],0.001,0.01)
thetas_num_p = [0,0,0,0,0,0] #the post processed solution

#Post process numerical angles to be [-pi, pi]
for i in range(0,6):
    thetas_num_p[i] = thetas_num[i]%(2*np.pi)
    if thetas_num_p[i]>np.pi:
        thetas_num_p[i] = thetas_num_p[i]-2*np.pi

T_num = mr.FKinSpace(M,Slist,thetas_num_p)

print("T_sd: \n",T_sd,'\n\n', "T_up: \n",T_up, '\n\n',"T_down: \n",T_down, "\n\nT_num: \n", T_num)

print('\nGenerating thetas:', thetas_gen, '\nElbow up: ', np.round(thetas_up,3), '\nElbow down: ', np.round(thetas_down,3), '\nNumerical: ', np.round(thetas_num_p,3), found, '\n')
print("\nElbow down: \n")

#apply_joint_lim is a function that finds out if a joint angle is outside the joints limits. Defined in symbolicFunctions.py:
print("Elbow down solution viable: ", apply_joint_lim(jointLimits, thetas_down), "\n")
print("\nElbow up: \n")
print("Elbow up solution viable: ", apply_joint_lim(jointLimits, thetas_up), "\n")

```

    T_sd: 
     [[  1.   0.   0. 980.]
     [  0.   1.   0.   0.]
     [  0.   0.   1. 435.]
     [  0.   0.   0.   1.]] 
    
     T_up: 
     [[  1.  -0.   0. 980.]
     [  0.   1.   0.   0.]
     [ -0.  -0.   1. 435.]
     [  0.   0.   0.   1.]] 
    
     T_down: 
     [[  1.   0.   0. 980.]
     [  0.   1.   0.   0.]
     [  0.   0.   1. 435.]
     [  0.   0.   0.   1.]] 
    
    T_num: 
     [[  1.        0.        0.      979.99945]
     [  0.        1.        0.       -0.     ]
     [ -0.        0.        1.      434.99998]
     [  0.        0.        0.        1.     ]]
    
    Generating thetas: [0 0 0 0 0 0] 
    Elbow up:  [ 0.    -0.08   0.166 -3.142  0.086 -3.142] 
    Elbow down:  [ 0. -0.  0.  0.  0.  0.] 
    Numerical:  [-0.  0. -0.  0.  0. -0.] True 
    
    
    Elbow down: 
    
    Elbow down solution viable:  True 
    
    
    Elbow up: 
    
    Elbow up solution viable:  True 
    


## Task 3.4
### Using the developed analytic inverse kinematics formulation, visualize the Agilus robot in both elbow-up and elbow-down configurations for the same end-effector pose.


```python
from Utilities.RobotClass import * 
```


```python
Kuka = Robot(Mlist, ['z', '-z', 'x', 'x', '-z','x']) #Initializes kuka-robot object in zero-configuration
Kuka.draw_robot()
```


    WebVisualizer(window_uid='window_0')



```python
Kuka.transform(Slist, thetas_down_num ) # Elbow DOWN
pUp = np.array(Kuka.joints[5].coord.get_center()) #Get coordinates of {6} in elbow down 
Kuka.draw_robot()

Kuka.transform(Slist, thetas_up_num ) # Elbow UP
pDown = np.array(Kuka.joints[5].coord.get_center(),dtype=float)  #Get coordinates of {6} in elbow up
Kuka.draw_robot()

print("Both elbow up and elbow down config yields the same end-effector pose: ",pUp.round(4) == pDown.round(4))
```


    WebVisualizer(window_uid='window_1')



    WebVisualizer(window_uid='window_2')


    Both elbow up and elbow down config yields the same end-effector pose:  [ True  True  True]



```python

```

9ee28400b01c5d641c06abcde49fe4b286109314
ipynb
Jupyter Notebook
Task_3.ipynb
BirkHveding/RobotTek
37f4ab0de6de9131239ff5d97e4b68a7091f291b
Apache-2.0
Task_3.ipynb
BirkHveding/RobotTek
37f4ab0de6de9131239ff5d97e4b68a7091f291b
Apache-2.0
Task_3.ipynb
BirkHveding/RobotTek
37f4ab0de6de9131239ff5d97e4b68a7091f291b
Apache-2.0
Qwen/Qwen-72B
1. YES
2. YES