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()
