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