Disegnatore robot a pantografo

 Questo post deriva dalla curiosita' suscitata dal giocattolo Lisciani Step

 

 

 

 

 

import numpy as np

import matplotlib.pyplot as plt

from matplotlib.animation import FuncAnimation

# ---------------------------------------------------------

# PARAMETRI DEL PANTOGRAFO

# ---------------------------------------------------------

L = 1.0          # lunghezza dei bracci

cx, cy = 0.5, 0.5    # centro del cerchio

R = 0.3          # raggio del cerchio (deve essere <= 2L)

omega = 1.0      # velocità angolare

# ---------------------------------------------------------

# TRAIETTORIA DEL PENNINO

# ---------------------------------------------------------

t = np.linspace(0, 6*np.pi, 600)

Px = cx + R * np.cos(omega * t)

Py = cy + R * np.sin(omega * t)

# ---------------------------------------------------------

# Cinematica inversa (soluzione simmetrica)

# ---------------------------------------------------------

def inv_kin(px, py):

    p = np.array([px, py]) / L

    rho = np.linalg.norm(p)

    if rho > 2:

        raise ValueError("Punto fuori dal workspace del pantografo: ", (px,py))

    phi = np.arctan2(p[1], p[0])

    alpha = np.arccos(rho / 2)

    theta1 = phi + alpha

    theta2 = phi - alpha

    return theta1, theta2

# ---------------------------------------------------------

# Calcolo posizioni dei giunti A, B e del punto P

# ---------------------------------------------------------

A, B, P = [], [], []

for px, py in zip(Px, Py):

    th1, th2 = inv_kin(px, py)

    a = np.array([L*np.cos(th1), L*np.sin(th1)])

    b = np.array([L*np.cos(th2), L*np.sin(th2)])

    p = a + b

    A.append(a)

    B.append(b)

    P.append(p)

A = np.array(A)

B = np.array(B)

P = np.array(P)

# ---------------------------------------------------------

# ANIMAZIONE

# ---------------------------------------------------------

fig, ax = plt.subplots(figsize=(6,6))

ax.set_xlim(-1.5, 1.5)

ax.set_ylim(-1.5, 1.5)

ax.set_aspect('equal')

ax.set_title("Pantografo – cerchio centrato in (0.5, 0.5) r=0.3")

line_OA, = ax.plot([], [], lw=2)

line_OB, = ax.plot([], [], lw=2)

line_AP, = ax.plot([], [], lw=1, ls='--')

line_BP, = ax.plot([], [], lw=1, ls='--')

pen,      = ax.plot([], [], 'o')

traj,     = ax.plot(P[:,0], P[:,1], alpha=0.2)

def init():

    line_OA.set_data([], [])

    line_OB.set_data([], [])

    line_AP.set_data([], [])

    line_BP.set_data([], [])

    pen.set_data([], [])

    return line_OA, line_OB, line_AP, line_BP, pen

def update(i):

    O = np.array([0,0])

    a = A[i]

    b = B[i]

    p = P[i]

    line_OA.set_data([O[0], a[0]], [O[1], a[1]])

    line_OB.set_data([O[0], b[0]], [O[1], b[1]])

    line_AP.set_data([a[0], p[0]], [a[1], p[1]])

    line_BP.set_data([b[0], p[0]], [b[1], p[1]])

    # FIX richiesto da Matplotlib: sequenze, non scalari

    pen.set_data([p[0]], [p[1]])

    return line_OA, line_OB, line_AP, line_BP, pen

ani = FuncAnimation(fig, update, frames=len(t), init_func=init, blit=True)

plt.show()