IK_functions.py

import math 
import numpy as np

"""
    # {Alessandro Bassi}
    # {alebas@kth.se}
"""
import math
import numpy as np 
print(f"Il file è stato caricato")


def scara_IK(point):
    x = point[0]
    y = point[1]
    z = point[2]
    q = [0.0, 0.0, 0.0]
    l1 = 0.3
    l2 = 0.35
    l0 = 0.07

    # define the x axe with respect the robot frame 
    x = x - l0

    #q2
    cos_theta_2 = (x**2 + y**2 - l1**2 - l2**2) / (2 * l1 * l2)
    theta_2 = math.acos(cos_theta_2)

    #q1
    sin_theta_1 = y * (l1 + l2 * math.cos(theta_2)) - x * l2 * math.sin(theta_2)
    cos_theta_1 = x * (l1 + l2 * math.cos(theta_2)) + y * l2 * math.sin(theta_2)
    theta_1 = math.atan2(sin_theta_1, cos_theta_1)

    #q3
    d3 = z

    q[0] = theta_1
    q[1] = theta_2
    q[2] = d3

    return q

# prende in ingresso Ts=[T_01, T_12, T_23, T_34, T_45, T_56, T_67] e restituisce v = [x, y, z]
def Direct_Kinematics(Ts):

    T_07 = Ts[0] @ Ts[1] @ Ts[2] @ Ts[3] @ Ts[4] @ Ts[5] @ Ts[6] 
    pose = T_07[:3, 3]

    return pose

# Prende in ingresso i parametri di D-H e restituisce la matrice di trasformazione 
def Transfer_Matrix(alpha, d, a, theta):
    T = np.array([
        [math.cos(theta), - math.sin(theta) * math.cos(alpha), math.sin(theta) * math.sin(alpha), a * math.cos(theta)], 
        [math.sin(theta), math.cos(theta) * math.cos(alpha), - math.cos(theta) * math.sin(alpha), a * math.sin(theta)], 
        [0, math.sin(alpha), math.cos(alpha), d],
        [0, 0, 0, 1]
    ])
    return T



# prende in ingresso Ts=[T_01, T_12, T_23, T_34, T_45, T_56, T_67] e restituisce lo Jacobiano
def Jacobian(Ts):
    #L'end-effector e' traslato rispetto al giunto 7
    T_07 = Ts[0] @ Ts[1] @ Ts[2] @ Ts[3] @ Ts[4] @ Ts[5] @ Ts[6]
    T_0ee = T_07 @ np.array([0, 0, 0.078, 1])
    p_ee = T_0ee[0:3]

    #L'asse 0 e' fissato 
    z_0 = np.array([0, 0, 1])

    T_01 = Ts[0]
    T_02 = T_01 @ Ts[1]
    T_03 = T_02 @ Ts[2]
    T_04 = T_03 @ Ts[3]
    T_05 = T_04 @ Ts[4]
    T_06 = T_05 @ Ts[5]


    z_1 = T_01[:3, 2]
    z_2 = T_02[:3, 2]
    z_3 = T_03[:3, 2]
    z_4 = T_04[:3, 2]
    z_5 = T_05[:3, 2]
    z_6 = T_06[:3, 2]

    z = []
    z.append(z_0)
    z.append(z_1)
    z.append(z_2)
    z.append(z_3)
    z.append(z_4)
    z.append(z_5)
    z.append(z_6)


    p_1 = np.array([0, 0, 0])
    p_2 = T_01[:3, 3]
    p_3 = T_02[:3, 3]
    p_4 = T_03[:3, 3]
    p_5 = T_04[:3, 3]
    p_6 = T_05[:3, 3]
    p_7 = T_06[:3, 3]

    p = []
    p.append(p_1)
    p.append(p_2)
    p.append(p_3)
    p.append(p_4)
    p.append(p_5)
    p.append(p_6)
    p.append(p_7)

    Jv = []
    Jw = []

    #Calcoliamo le componenti dello Jacobiano
    for i in range(len(p)):
        v = np.cross(z[i], p_ee - p[i]) #v_i = z_{i-1} X (p_ee - p_i)
        Jv.append(v)
        Jw.append(z[i]) #per giunti rotazionali
    
    Jv = np.column_stack(Jv)
    Jw = np.column_stack(Jw)
    Jacobian = np.vstack((Jv, Jw))
    print(Jacobian)
    return Jacobian



def kuka_IK(point, R, joint_positions):
    x = point[0]
    y = point[1]
    z = point[2]
    q = joint_positions #it must contain 7 elements
    L = 0.4 #[m]
    M = 0.39 #[m]

    #trasformo tutti gli array in NumPy array
    point = np.array(point)
    R = np.array(R)
    q = np.array(q)

    tollerance = 0.01

    # Calcliamo la cinematica inversa 

    for i in range(100):

        #1. CALCOLO DELLA CINEMATICA DIRETTA 

        Ts = []
        T_01 = Transfer_Matrix(math.pi/2, 0.311, 0, q[0])
        T_02 = Transfer_Matrix(-math.pi/2, 0, 0, q[1])
        T_03 = Transfer_Matrix(-math.pi/2, L, 0, q[2])
        T_04 = Transfer_Matrix(math.pi/2, 0, 0, q[3])
        T_05 = Transfer_Matrix(math.pi/2, M, 0, q[4])
        T_06 = Transfer_Matrix(-math.pi/2, 0, 0, q[5])
        T_07 = Transfer_Matrix(0, 0, 0, q[6])

        Ts.append(T_01)
        Ts.append(T_02)
        Ts.append(T_03)
        Ts.append(T_04)
        Ts.append(T_05)
        Ts.append(T_06)
        Ts.append(T_07)

        T_07 = Ts[0] @ Ts[1] @ Ts[2] @ Ts[3] @ Ts[4] @ Ts[5] @ Ts[6] 
        T_0ee = T_07 @ np.array([0, 0, 0.078, 1])

        p_ee = T_0ee[0:3]
        R_e = T_07[:3, :3]

        #2. CALCOLO DELL'ERRORE 

        n_e = R_e[:3, 0]
        s_e = R_e[:3, 1]
        a_e = R_e[:3, 2]

        n = R[:3, 0]
        s = R[:3, 1]
        a = R[:3, 2]

            #2.1 ERRORE SULL'ORIENTAMENTO 

        e_o = 0.5 * (np.cross(n_e, n) + np.cross(s_e, s) + np.cross(a_e, a))

            #2.2 ERRORE SULLA POSIZIONE
        
        e_p = p_ee - point

        error = np.concatenate((e_p, e_o))
        print(f"Iter {i}: Error norm = {np.linalg.norm(error)}")


        if np.linalg.norm(error)< tollerance:
                print("CONVERGENZA")
                break

        J = Jacobian(Ts)
        Inv_Jacobian = np.linalg.pinv(J)
        
        eps_theta = Inv_Jacobian @ error

        q = q - eps_theta

    if (i == 99):
         print("Nessuna Convergenza")

    return q