Cinematica Inversa su braccio robotico

Mi sono messo a leggiucchiare il libro Robotica: Modellistica, Pianificazione e Controllo (Bruno Siciliano,Lorenzo Sciavicco,Luigi Villani, Giuseppe Oriolo McGraw-Hill Libri Italia srl) ...leggiucchiare perche' e' un libro di teoria di robot pieno di matematica

 

Partendo dalle misure e dalla forma di un robot reale (braccio robotico antropomorfo tinkerkit a 5 gradi di liberta' piu' presa) con l'idea di tracciare una traiettoria circolare sul pavimento

In questo tipo si esercizio si definisce l'obbiettivo e si lascia all'algoritmo il calcolo della posa del robot

 

import numpy as np

import matplotlib.pyplot as plt

from mpl_toolkits.mplot3d import Axes3D

import matplotlib.animation as animation

# --- 1. MOTORE CINEMATICO (IK) ---

def cinematica_inversa_3R_planare(r, z, phi_deg, L1, L2, L3):

phi_rad = np.deg2rad(phi_deg)

# Calcolo posizione polso

# Con phi = -90, cos(-90)=0 e sin(-90)=-1

# Quindi w_r = r e w_z = z + L3

w_r = r - L3 * np.cos(phi_rad)

w_z = z - L3 * np.sin(phi_rad)

dist_sq = w_r**2 + w_z**2

# Teorema Coseno per Gomito

cos_theta2 = (dist_sq - L1**2 - L2**2) / (2 * L1 * L2)

if abs(cos_theta2) > 1: return []

soluzioni = []

for sigma in [1, -1]:

sin_theta2 = sigma * np.sqrt(1 - cos_theta2**2)

theta2 = np.arctan2(sin_theta2, cos_theta2)

alpha = np.arctan2(w_z, w_r)

beta = np.arctan2(L2 * np.sin(theta2), L1 + L2 * np.cos(theta2))

theta1 = alpha - beta

theta3 = phi_rad - theta1 - theta2

soluzioni.append((theta1, theta2, theta3))

return soluzioni

def ik_braccio(x, y, z, phi_pitch_deg, L_base, L_omero, L_ulna, L_mano):

theta_base = np.arctan2(y, x)

r_planare = np.sqrt(x**2 + y**2)

z_relativa = z - L_base

sols_2d = cinematica_inversa_3R_planare(r_planare, z_relativa, phi_pitch_deg, L_omero, L_ulna, L_mano)

return [(theta_base, t1, t2, t3) for (t1, t2, t3) in sols_2d]

def calcola_punti_robot(angles, L0, L1, L2, L3):

th_base, th_spalla, th_gomito, th_polso = angles

p0 = np.array([0, 0, 0]); p1 = np.array([0, 0, L0])

cos_b, sin_b = np.cos(th_base), np.sin(th_base)

r2 = L1 * np.cos(th_spalla); z2 = L0 + L1 * np.sin(th_spalla)

p2 = np.array([r2 * cos_b, r2 * sin_b, z2])

angle_cum = th_spalla + th_gomito

r3 = r2 + L2 * np.cos(angle_cum); z3 = z2 + L2 * np.sin(angle_cum)

p3 = np.array([r3 * cos_b, r3 * sin_b, z3])

angle_cum_tot = angle_cum + th_polso

r4 = r3 + L3 * np.cos(angle_cum_tot); z4 = z3 + L3 * np.sin(angle_cum_tot)

p4 = np.array([r4 * cos_b, r4 * sin_b, z4])

return np.array([p0, p1, p2, p3, p4])

# --- 2. LOGICA DI SELEZIONE OTTIMIZZATA ---

def scegli_soluzione_migliore(soluzioni, angoli_precedenti):

if not soluzioni: return None

migliore_sol = None

miglior_punteggio = -np.inf

for sol in soluzioni:

deviazione_gomito_rad = abs(sol[2])

# Criterio: Angolo aperto (>90 gradi interni)

score = 0

if deviazione_gomito_rad < np.pi/2:

score += 1000

else:

score -= 1000

if angoli_precedenti is not None:

diff = np.linalg.norm(np.array(sol) - np.array(angoli_precedenti))

score -= diff * 100

if score > miglior_punteggio:

miglior_punteggio = score

migliore_sol = sol

return migliore_sol

# --- 3. CONFIGURAZIONE PARAMETRI ---

L_BASE, L_OMERO, L_ULNA, L_MANO = 0.0715, 0.125, 0.125, 0.060

# >>> MODIFICA CHIAVE QUI <<<

# Impostiamo -90 gradi. Il polso punta verso il basso (asse -Z)

# Ortogonale al piano XY.

PHI_PITCH = -90

CENTER = np.array([0.15, 0.15, 0.0]) # Spostato un po' più avanti per facilitare la presa verticale

RADIUS = 0.03

# Home Position

HOME_POS = np.array([0.0, 0.15, 0.25])

START_CIRCLE = np.array([CENTER[0] + RADIUS, CENTER[1], CENTER[2]])

# Generazione Traiettoria

frames_approach = 40

traj_approach_x = np.linspace(HOME_POS[0], START_CIRCLE[0], frames_approach)

traj_approach_y = np.linspace(HOME_POS[1], START_CIRCLE[1], frames_approach)

traj_approach_z = np.linspace(HOME_POS[2], START_CIRCLE[2], frames_approach)

frames_circle = 80

angles = np.linspace(0, 2*np.pi, frames_circle)

traj_circle_x = CENTER[0] + RADIUS * np.cos(angles)

traj_circle_y = CENTER[1] + RADIUS * np.sin(angles)

traj_circle_z = np.full_like(angles, CENTER[2])

full_traj_x = np.concatenate([traj_approach_x, traj_circle_x])

full_traj_y = np.concatenate([traj_approach_y, traj_circle_y])

full_traj_z = np.concatenate([traj_approach_z, traj_circle_z])

TOTAL_FRAMES = len(full_traj_x)

# --- 4. ANIMAZIONE ---

fig = plt.figure(figsize=(10, 8))

ax = fig.add_subplot(111, projection='3d')

ax.set_xlim(-0.1, 0.3); ax.set_ylim(-0.1, 0.3); ax.set_zlim(0, 0.4)

ax.set_xlabel('X'); ax.set_ylabel('Y'); ax.set_zlabel('Z')

ax.set_title(f'Tinkerkit Braccio - Polso Verticale ({PHI_PITCH}°)')

line_robot, = ax.plot([], [], [], 'o-', lw=4, color='tab:blue', label='Robot')

point_target, = ax.plot([], [], [], 'X', color='red', label='Target')

line_trace, = ax.plot([], [], [], '-', lw=1, color='red', alpha=0.5)

# Disegniamo un piccolo piano XY per riferimento visivo

xx, yy = np.meshgrid(np.linspace(-0.1, 0.3, 2), np.linspace(-0.1, 0.3, 2))

ax.plot_surface(xx, yy, np.zeros_like(xx), alpha=0.2, color='gray')

ax.legend()

prev_angles = None

history_x, history_y, history_z = [], [], []

def update(frame_idx):

global prev_angles

tx = full_traj_x[frame_idx]

ty = full_traj_y[frame_idx]

tz = full_traj_z[frame_idx]

sols = ik_braccio(tx, ty, tz, PHI_PITCH, L_BASE, L_OMERO, L_ULNA, L_MANO)

best_sol = scegli_soluzione_migliore(sols, prev_angles)

if best_sol:

prev_angles = best_sol

points = calcola_punti_robot(best_sol, L_BASE, L_OMERO, L_ULNA, L_MANO)

line_robot.set_data(points[:,0], points[:,1])

line_robot.set_3d_properties(points[:,2])

point_target.set_data([tx], [ty])

point_target.set_3d_properties([tz])

history_x.append(tx); history_y.append(ty); history_z.append(tz)

line_trace.set_data(history_x, history_y)

line_trace.set_3d_properties(history_z)

return line_robot, point_target, line_trace

ani = animation.FuncAnimation(fig, update, frames=TOTAL_FRAMES, interval=40, blit=False, repeat=True)

plt.show()

 

 il braccio segue la traiettoria circolare ma muovendosi in uno spazio reale sbatterebbe il gomito sul pavimento...Mettiamo la condizione che i giunti debbano avere coordinata Z sempre positiva

import numpy as np

import matplotlib.pyplot as plt

from mpl_toolkits.mplot3d import Axes3D

import matplotlib.animation as animation

# --- 1. PARAMETRI E COSTANTI GLOBALI ---

L_BASE, L_OMERO, L_ULNA, L_MANO = 0.0715, 0.125, 0.125, 0.060

PHI_PITCH = -90 # Polso verticale

CENTER = np.array([0.15, 0.0, 0.0]) # Z target = 0.0, come richiesto

RADIUS = 0.03

# --- 2. FUNZIONI CINEMATICHE (Invariate) ---

def cinematica_inversa_3R_planare(r, z, phi_deg, L1, L2, L3):

phi_rad = np.deg2rad(phi_deg)

w_r = r - L3 * np.cos(phi_rad)

w_z = z - L3 * np.sin(phi_rad)

dist_sq = w_r**2 + w_z**2

cos_theta2 = (dist_sq - L1**2 - L2**2) / (2 * L1 * L2)

if abs(cos_theta2) > 1: return []

soluzioni = []

for sigma in [1, -1]:

sin_theta2 = sigma * np.sqrt(1 - cos_theta2**2)

theta2 = np.arctan2(sin_theta2, cos_theta2)

alpha = np.arctan2(w_z, w_r)

beta = np.arctan2(L2 * np.sin(theta2), L1 + L2 * np.cos(theta2))

theta1 = alpha - beta

theta3 = phi_rad - theta1 - theta2

soluzioni.append((theta1, theta2, theta3))

return soluzioni

def ik_braccio(x, y, z, phi_pitch_deg, L_base, L_omero, L_ulna, L_mano):

theta_base = np.arctan2(y, x)

r_planare = np.sqrt(x**2 + y**2)

z_relativa = z - L_base

sols_2d = cinematica_inversa_3R_planare(r_planare, z_relativa, phi_pitch_deg, L_omero, L_ulna, L_mano)

return [(theta_base, t1, t2, t3) for (t1, t2, t3) in sols_2d]

def calcola_punti_robot(angles):

# Uso le costanti globali L_BASE, L_OMERO, etc.

th_base, th_spalla, th_gomito, th_polso = angles

# P0 e P1 (Base e Spalla)

p0 = np.array([0, 0, 0]); p1 = np.array([0, 0, L_BASE])

# P2 (Gomito)

cos_b, sin_b = np.cos(th_base), np.sin(th_base)

r2 = L_OMERO * np.cos(th_spalla); z2 = L_BASE + L_OMERO * np.sin(th_spalla)

p2 = np.array([r2 * cos_b, r2 * sin_b, z2])

# P3 (Polso)

angle_cum = th_spalla + th_gomito

r3 = r2 + L_ULNA * np.cos(angle_cum); z3 = z2 + L_ULNA * np.sin(angle_cum)

p3 = np.array([r3 * cos_b, r3 * sin_b, z3])

# P4 (Pinza)

angle_cum_tot = angle_cum + th_polso

r4 = r3 + L_MANO * np.cos(angle_cum_tot); z4 = z3 + L_MANO * np.sin(angle_cum_tot)

p4 = np.array([r4 * cos_b, r4 * sin_b, z4])

return np.array([p0, p1, p2, p3, p4])

# --- 3. LOGICA DI SELEZIONE MODIFICATA ---

def scegli_soluzione_migliore(soluzioni, angoli_precedenti):

if not soluzioni: return None

migliore_sol = None

miglior_punteggio = -np.inf

for sol in soluzioni:

th_spalla = sol[1] # Angolo M2 (Spalla)

deviazione_gomito_rad = abs(sol[2])

# >>> NUOVO VINCOLO FORTE: Z DEL GOMITO POSITIVA <<<

# Z_gomito = L_BASE + L_OMERO * sin(theta_spalla)

z_gomito = L_BASE + L_OMERO * np.sin(th_spalla)

if z_gomito <= 0.01: # Uso 0.01 come margine di sicurezza

# Scarta la soluzione: il gomito è sotto il tavolo o troppo vicino

continue

# Vincolo di flessibilità (Angolo > 90 gradi interni)

score = 0

if deviazione_gomito_rad < np.pi/2:

score += 1000

else:

score -= 1000

# Vincolo di continuità

if angoli_precedenti is not None:

diff = np.linalg.norm(np.array(sol) - np.array(angoli_precedenti))

score -= diff * 100

if score > miglior_punteggio:

miglior_punteggio = score

migliore_sol = sol

return migliore_sol

# --- 4. CONFIGURAZIONE TRAIETTORIA ---

HOME_POS = np.array([0.0, 0.15, 0.25])

START_CIRCLE = np.array([CENTER[0] + RADIUS, CENTER[1], CENTER[2]])

# Generazione Traiettoria (Avvicinamento + Cerchio)

frames_approach = 40

traj_approach_x = np.linspace(HOME_POS[0], START_CIRCLE[0], frames_approach)

traj_approach_y = np.linspace(HOME_POS[1], START_CIRCLE[1], frames_approach)

traj_approach_z = np.linspace(HOME_POS[2], START_CIRCLE[2], frames_approach)

frames_circle = 80

angles = np.linspace(0, 2*np.pi, frames_circle)

traj_circle_x = CENTER[0] + RADIUS * np.cos(angles)

traj_circle_y = CENTER[1] + RADIUS * np.sin(angles)

traj_circle_z = np.full_like(angles, CENTER[2])

full_traj_x = np.concatenate([traj_approach_x, traj_circle_x])

full_traj_y = np.concatenate([traj_approach_y, traj_circle_y])

full_traj_z = np.concatenate([traj_approach_z, traj_circle_z])

TOTAL_FRAMES = len(full_traj_x)

# --- 5. ANIMAZIONE ---

fig = plt.figure(figsize=(10, 8))

ax = fig.add_subplot(111, projection='3d')

ax.set_xlim(-0.1, 0.3); ax.set_ylim(-0.2, 0.2); ax.set_zlim(0, 0.4) # Aggiornato Y per vedere meglio

ax.set_xlabel('X'); ax.set_ylabel('Y'); ax.set_zlabel('Z')

ax.set_title(f'Tinkerkit Braccio - Z Target={CENTER[2]:.2f}m con Z Gomito > 0')

line_robot, = ax.plot([], [], [], 'o-', lw=4, color='tab:blue', label='Robot')

point_target, = ax.plot([], [], [], 'X', color='red', label='Target')

line_trace, = ax.plot([], [], [], '-', lw=1, color='red', alpha=0.5)

# Aggiunge il piano Z=0

xx, yy = np.meshgrid(np.linspace(-0.1, 0.3, 2), np.linspace(-0.2, 0.2, 2))

ax.plot_surface(xx, yy, np.zeros_like(xx), alpha=0.2, color='gray')

ax.legend()

prev_angles = None

history_x, history_y, history_z = [], [], []

def update(frame_idx):

global prev_angles

tx = full_traj_x[frame_idx]

ty = full_traj_y[frame_idx]

tz = full_traj_z[frame_idx]

sols = ik_braccio(tx, ty, tz, PHI_PITCH, L_BASE, L_OMERO, L_ULNA, L_MANO)

best_sol = scegli_soluzione_migliore(sols, prev_angles)

if best_sol:

prev_angles = best_sol

points = calcola_punti_robot(best_sol)

line_robot.set_data(points[:,0], points[:,1])

line_robot.set_3d_properties(points[:,2])

point_target.set_data([tx], [ty])

point_target.set_3d_properties([tz])

history_x.append(tx); history_y.append(ty); history_z.append(tz)

line_trace.set_data(history_x, history_y)

line_trace.set_3d_properties(history_z)

return line_robot, point_target, line_trace

ani = animation.FuncAnimation(fig, update, frames=TOTAL_FRAMES, interval=40, blit=False, repeat=True)

plt.show()