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