Maratona di Firenze 2025

Ed anche quest'anno mi sono intrufolato nelle foto ufficiali e nel video (dove sono stato travolto dai partenti...sono uno dei volontari)

 

 

 

Cinematica Inversa SpiderCam

La spidercam e' la videocamera sospesa a fili che si vede spesso utilizzata negli eventi sportivi. Di fatto non e' trattabile come un corpo rigido perche' i vari tratti di cavo si allungano ed accorciano

Pensavo fosse piu' difficile dal punto di vista matematico. Basta calcolare la lunghezza del cavo usando la distanza cartesiana tra la coordinata della camera ed il vincolo del cavo...poi nella realta' i problemi di gestire la tensione dei cavi ed il corretto avvolgimento sono tutto un altro discorso 

  

 

import numpy as np

import matplotlib.pyplot as plt

from matplotlib.animation import FuncAnimation

# --- 1. Definizione della Scena e degli Ancoraggi ---

def setup_spidercam():

# Dimensione dello stadio

L_stadio = 100 # Lunghezza

W_stadio = 60 # Larghezza

H_massima = 30 # Altezza massima operativa (Z)

# Coordinate 3D dei 4 punti d'ancoraggio (i piloni)

Ancoraggi = np.array([

[0, 0, H_massima], # A1: Angolo basso-sinistra

[L_stadio, 0, H_massima], # A2: Angolo basso-destra

[L_stadio, W_stadio, H_massima], # A3: Angolo alto-destra

[0, W_stadio, H_massima] # A4: Angolo alto-sinistra

])

return Ancoraggi, L_stadio, W_stadio, H_massima

# --- 2. Funzione di Calcolo della Lunghezza dei Cavi ---

def calcola_lunghezze(P_carrello, Ancoraggi):

"""Calcola le lunghezze dei 4 cavi data la posizione del carrello."""

Lunghezze = np.linalg.norm(Ancoraggi - P_carrello, axis=1)

return Lunghezze

# --- 3. Funzione di Animazione (MODIFICATA) ---

def animate_spidercam(frame, Ancoraggi, L_stadio, W_stadio, ax, line_cavi, scat_carrello):

# Definiamo la traiettoria del carrello (una semplice sinusoide 3D)

t = frame * 0.1

# Movimento:

x_pos = 50 + 30 * np.sin(t / 5)

y_pos = 30 + 20 * np.cos(t / 3)

z_pos = 15 + 10 * np.sin(t / 7)

P_carrello = np.array([x_pos, y_pos, z_pos])

# Aggiorna la posizione del carrello

scat_carrello._offsets3d = ([P_carrello[0]], [P_carrello[1]], [P_carrello[2]])

# Aggiorna i cavi

for i in range(4):

x_dati = [P_carrello[0], Ancoraggi[i, 0]]

y_dati = [P_carrello[1], Ancoraggi[i, 1]]

z_dati = [P_carrello[2], Ancoraggi[i, 2]]

line_cavi[i].set_data(x_dati, y_dati)

line_cavi[i].set_3d_properties(z_dati)

# Calcola le lunghezze dei cavi

lunghezze = calcola_lunghezze(P_carrello, Ancoraggi)

# Formattazione delle lunghezze a 3 decimali

lunghezze_formattate = [f"{l:.3f}" for l in lunghezze]

# Aggiorna il titolo del grafico con le lunghezze formattate

title_text = (

f"Posizione: ({x_pos:.1f}, {y_pos:.1f}, {z_pos:.1f}) | "

f"Lunghezze Cavi: {lunghezze_formattate}"

)

ax.set_title(title_text)

return scat_carrello, *line_cavi

# --- 4. Configurazione della Visualizzazione Matplotlib ---

Ancoraggi, L_stadio, W_stadio, H_massima = setup_spidercam()

num_frames = 200

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

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

# Impostazioni iniziali degli assi

ax.set_xlim([0, L_stadio])

ax.set_ylim([0, W_stadio])

ax.set_zlim([0, H_massima + 10])

ax.set_xlabel("Asse X (Lunghezza)")

ax.set_ylabel("Asse Y (Larghezza)")

ax.set_zlabel("Asse Z (Altezza)")

# Disegna gli ancoraggi (Piloni)

ax.scatter(Ancoraggi[:, 0], Ancoraggi[:, 1], Ancoraggi[:, 2],

c='r', marker='o', s=100, label='Ancoraggi')

# Disegna il carrello iniziale a metà altezza

P_iniziale = np.array([L_stadio/2, W_stadio/2, H_massima/2])

scat_carrello = ax.scatter([P_iniziale[0]], [P_iniziale[1]], [P_iniziale[2]],

c='b', marker='^', s=200, label='Carrello/Telecamera')

# Linee dei cavi iniziali

line_cavi = []

for i in range(4):

line, = ax.plot([P_iniziale[0], Ancoraggi[i, 0]],

[P_iniziale[1], Ancoraggi[i, 1]],

[P_iniziale[2], Ancoraggi[i, 2]],

'k--', alpha=0.7, linewidth=1)

line_cavi.append(line)

ax.legend()

ax.view_init(elev=20, azim=135)

# --- 5. Esegui l'Animazione ---

anim = FuncAnimation(fig, animate_spidercam, frames=num_frames,

fargs=(Ancoraggi, L_stadio, W_stadio, ax, line_cavi, scat_carrello),

interval=50, blit=False)

plt.show()

Cinematica inversa delta robot

Ho trovato gratuito su Internet  "Modern Robotics: Mechanics, Planning, and Control," Kevin M. Lynch and Frank C. Park, Cambridge University Press, 2017, ISBN 9781107156302. (link) https://hades.mech.northwestern.edu/images/2/2e/MR-largefont-v2.pdf

 

 Interessante e' il delta robot, un robot parallelo parente stretto della piattaforma Stewart–Gough (il primo ha l'attuatore che trasla parallelo al piano superiore, utilizzato anche per classe di stampanti 3D

 

 

import numpy as np

import matplotlib.pyplot as plt

from matplotlib.animation import FuncAnimation

from mpl_toolkits.mplot3d import Axes3D

# --- Delta Robot Geometric Parameters (Units in mm) ---

# Fixed Base Triangle side length (equilateral)

E = 400.0

# Moving Platform Triangle side length (equilateral)

F = 150.0

# Base and Platform Radii

e = E / (2 * np.sqrt(3))

f = F / (2 * np.sqrt(3))

# Arm Lengths

L = 200.0  # Upper arm (Motor arm)

l = 400.0  # Forearm (Rod)

# Fixed Motor Arm Angles (for the base)

alpha = np.radians(30)

beta = np.radians(150)

gamma = np.radians(270)

motor_angles = [alpha, beta, gamma]

# --- Kinematics Functions ---

def delta_calcInverse(x0, y0, z0):

    """

    Calculates the motor angles (theta1, theta2, theta3) for a given

    end-effector position (x0, y0, z0).

    Returns an array of angles in radians, or None if the position is unreachable.

    """

   

    theta = np.zeros(3)

    for i, angle in enumerate(motor_angles):

        x = x0 - f * np.cos(angle)

        y = y0 - f * np.sin(angle)

       

        y1 = -e + x * np.cos(angle) + y * np.sin(angle)

       

        a = x**2 + y**2 + z0**2 + L**2 - l**2 + e**2 + 2 * e * y1

        b = -2 * z0 * L

        c = 2 * y1 * L

       

        try:

            D = (a**2 - (b**2 + c**2))

           

            if D < 0:

                return None

           

            D_sqrt = np.sqrt(D)

            theta[i] = np.arctan2(c * a - b * D_sqrt, b * a + c * D_sqrt)

           

        except ZeroDivisionError:

            return None

    return theta

# --- Animation Setup ---

z_fixed = -150.0  # Safe operating height

radius = 40.0     # Adjusted Radius of the drawn circle

center_x = 0.0

center_y = 0.0

# Time steps for the animation

t = np.linspace(0, 2 * np.pi, 100)

x_path = center_x + radius * np.cos(t)

y_path = center_y + radius * np.sin(t)

z_path = np.full_like(t, z_fixed)

# Store the calculated end-effector points for the trail

effector_x, effector_y, effector_z = [], [], []

# --- Plotting Helper Functions ---

def get_motor_coords(theta):

    """Returns the base pivot coordinates B_i"""

    x_b = e * np.cos(theta)

    y_b = e * np.sin(theta)

    z_b = 0

    return np.array([x_b, y_b, z_b])

def get_elbow_coords(theta, angle):

    """Returns the elbow joint coordinates C_i"""

    x_c = (e - L * np.cos(theta)) * np.cos(angle)

    y_c = (e - L * np.cos(theta)) * np.sin(angle)

    z_c = -L * np.sin(theta)

    return np.array([x_c, y_c, z_c])

# --- Main Plotting Setup ---

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

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

# Set limits and labels

plot_lim = max(E/2, l+L) * 1.1

ax.set_xlim([-plot_lim, plot_lim])

ax.set_ylim([-plot_lim, plot_lim])

ax.set_zlim([-plot_lim, 0])

ax.set_xlabel('X-axis (mm)')

ax.set_ylabel('Y-axis (mm)')

ax.set_zlabel('Z-axis (mm)')

ax.set_title(f'Delta Robot Simulating a Circle (R={radius:.0f}mm, Z={z_fixed:.0f}mm)')

# 1. Initial setup: Draw the fixed base triangle

B1 = get_motor_coords(alpha)

B2 = get_motor_coords(beta)

B3 = get_motor_coords(gamma)

base_x = [B1[0], B2[0], B3[0], B1[0]]

base_y = [B1[1], B2[1], B3[1], B1[1]]

base_z = [0, 0, 0, 0]

ax.plot(base_x, base_y, base_z, 'k-', linewidth=3, label='Base E')

# 2. Draw the ENTIRE target circle path (as a dashed line) for reference

ax.plot(x_path, y_path, z_path, 'c--', linewidth=1, label='Target Path')

# 3. Initialize dynamic artists (the robot arms and platform)

motor_arms = [] # L arms

rods = []       # l rods

for i in range(3):

    # Initial state (set to base coords as dummy data)

    motor_arm_line, = ax.plot([B1[0], B1[0]], [B1[1], B1[1]], [B1[2], B1[2]], 'r-', linewidth=3)

    rod_line, = ax.plot([B1[0], B1[0]], [B1[1], B1[1]], [B1[2], B1[2]], 'b-', linewidth=2)

    motor_arms.append(motor_arm_line)

    rods.append(rod_line)

# The moving platform triangle

platform_line, = ax.plot([], [], [], 'g--', linewidth=1)

# The traced path (trail) and end-effector point

path_line, = ax.plot([], [], [], 'y-', linewidth=1.5, label='Traced Path')

end_effector_point, = ax.plot([], [], [], 'ko', markersize=5, label='End Effector')

# Collect all dynamic artists for return in update

dynamic_artists = motor_arms + rods + [platform_line, path_line, end_effector_point]

# --- Update Function ---

def update(frame):

    """Update function for the animation."""

   

    # 1. Get the target position for the current frame

    x0, y0, z0 = x_path[frame], y_path[frame], z_path[frame]

   

    # 2. Calculate the required motor angles

    angles_rad = delta_calcInverse(x0, y0, z0)

   

    if angles_rad is None:

        print(f"Error: Unreachable position at frame {frame} ({x0:.1f}, {y0:.1f}, {z0:.1f}). Stopping.")

        # This returns the current artists without updating them, effectively stopping the movement

        return dynamic_artists

    # Store the end-effector position for the trail

    effector_x.append(x0)

    effector_y.append(y0)

    effector_z.append(z0)

    # Update the traced path (trail)

    path_line.set_data(effector_x, effector_y)

    path_line.set_3d_properties(effector_z)

   

    # Update the end-effector tip marker

    end_effector_point.set_data([x0], [y0])

    end_effector_point.set_3d_properties([z0])

    # 3. Update the robot linkage for the current frame

    for i in range(3):

        # Coordinates of the platform pivot point P_i relative to the base center

        P_i = np.array([x0, y0, z0])

       

        # Calculate motor and elbow coordinates

        B_i = get_motor_coords(motor_angles[i])

        C_i = get_elbow_coords(angles_rad[i], motor_angles[i])

       

        # Update the Motor arm (L): B_i -> C_i

        motor_arms[i].set_data([B_i[0], C_i[0]], [B_i[1], C_i[1]])

        motor_arms[i].set_3d_properties([B_i[2], C_i[2]])

       

        # Update the Rod (l): C_i -> P_i

        rods[i].set_data([C_i[0], P_i[0]], [C_i[1], P_i[1]])

        rods[i].set_3d_properties([C_i[2], P_i[2]])

   

    # Update the moving platform

    P1 = np.array([x0, y0, z0]) + f * np.array([np.cos(alpha), np.sin(alpha), 0])

    P2 = np.array([x0, y0, z0]) + f * np.array([np.cos(beta), np.sin(beta), 0])

    P3 = np.array([x0, y0, z0]) + f * np.array([np.cos(gamma), np.sin(gamma), 0])

    platform_x = [P1[0], P2[0], P3[0], P1[0]]

    platform_y = [P1[1], P2[1], P3[1], P1[1]]

    platform_z = [P1[2], P2[2], P3[2], P1[2]]

   

    platform_line.set_data(platform_x, platform_y)

    platform_line.set_3d_properties(platform_z)

    # Return all updated artists

    return dynamic_artists

# Create the animation

ani = FuncAnimation(fig, update, frames=len(t), interval=50, blit=False, repeat=True)

plt.legend()

plt.show()