ur7e-sac-line-following / trajectory_generator.py
devkumarhf's picture
Release verified frozen UR7e policy and reproducible CPU benchmark
1ce77ac verified
Raw History Blame Contribute Delete
9.67 kB
"""
Générateur de trajectoires aléatoires 2D — UR7e
─────────────────────────────────────────────────
Produit des lignes / courbes dans le plan du "tableau"
(z constant), garanties dans l'espace atteignable du robot.
Types de trajectoires :
- segment droit
- courbe de Bézier (1 ou 2 points de contrôle)
- sinusoïde
- arc de cercle
Sortie : array (N, 3) de points 3D le long de la trajectoire.
Testable seul :
python3 trajectory_generator.py
"""
import numpy as np
# ─────────────────────────────────────────────
# Paramètres du "tableau" (plan vertical devant le robot)
# Le tableau est un plan à X fixe, on dessine dans (Y, Z)
# ─────────────────────────────────────────────
TABLEAU_X = 0.45 # distance du tableau devant la base (m)
TABLEAU_Y_MIN = -0.35 # bord gauche
TABLEAU_Y_MAX = 0.35 # bord droit
TABLEAU_Z_MIN = 0.25 # bas
TABLEAU_Z_MAX = 0.75 # haut
# Espace atteignable (cohérent avec ur7e_env_v4)
R_MIN = 0.20
R_MAX = 0.85
# Discrétisation par défaut
DEFAULT_N_POINTS = 60
POINT_SPACING = 0.025 # ~2.5 cm entre points (indicatif)
# ─────────────────────────────────────────────
# Vérification d'atteignabilité
# ─────────────────────────────────────────────
def _is_reachable(point):
"""Vérifie qu'un point est dans la sphère atteignable."""
r = np.linalg.norm(point)
return R_MIN <= r <= R_MAX
def _clip_to_tableau(y, z):
"""Contraint un point dans les bornes du tableau."""
y = np.clip(y, TABLEAU_Y_MIN, TABLEAU_Y_MAX)
z = np.clip(z, TABLEAU_Z_MIN, TABLEAU_Z_MAX)
return y, z
# ─────────────────────────────────────────────
# Générateurs de formes (dans le plan Y-Z)
# ─────────────────────────────────────────────
def _segment(rng, n):
"""Ligne droite entre deux points aléatoires du tableau."""
y0 = rng.uniform(TABLEAU_Y_MIN, TABLEAU_Y_MAX)
z0 = rng.uniform(TABLEAU_Z_MIN, TABLEAU_Z_MAX)
y1 = rng.uniform(TABLEAU_Y_MIN, TABLEAU_Y_MAX)
z1 = rng.uniform(TABLEAU_Z_MIN, TABLEAU_Z_MAX)
t = np.linspace(0, 1, n)
ys = y0 + t * (y1 - y0)
zs = z0 + t * (z1 - z0)
return ys, zs
def _bezier(rng, n):
"""Courbe de Bézier quadratique (3 points de contrôle)."""
p0 = np.array([rng.uniform(TABLEAU_Y_MIN, TABLEAU_Y_MAX),
rng.uniform(TABLEAU_Z_MIN, TABLEAU_Z_MAX)])
p1 = np.array([rng.uniform(TABLEAU_Y_MIN, TABLEAU_Y_MAX),
rng.uniform(TABLEAU_Z_MIN, TABLEAU_Z_MAX)])
p2 = np.array([rng.uniform(TABLEAU_Y_MIN, TABLEAU_Y_MAX),
rng.uniform(TABLEAU_Z_MIN, TABLEAU_Z_MAX)])
t = np.linspace(0, 1, n).reshape(-1, 1)
pts = (1-t)**2 * p0 + 2*(1-t)*t * p1 + t**2 * p2
return pts[:, 0], pts[:, 1]
def _bezier_cubic(rng, n):
"""Courbe de Bézier cubique (4 points de contrôle) — plus sinueuse."""
pts_ctrl = [
np.array([rng.uniform(TABLEAU_Y_MIN, TABLEAU_Y_MAX),
rng.uniform(TABLEAU_Z_MIN, TABLEAU_Z_MAX)])
for _ in range(4)
]
p0, p1, p2, p3 = pts_ctrl
t = np.linspace(0, 1, n).reshape(-1, 1)
pts = ((1-t)**3 * p0 + 3*(1-t)**2*t * p1
+ 3*(1-t)*t**2 * p2 + t**3 * p3)
return pts[:, 0], pts[:, 1]
def _sinusoide(rng, n):
"""Sinusoïde horizontale dans le tableau."""
y0 = TABLEAU_Y_MIN
y1 = TABLEAU_Y_MAX
ys = np.linspace(y0, y1, n)
z_center = rng.uniform(TABLEAU_Z_MIN + 0.1, TABLEAU_Z_MAX - 0.1)
amp = rng.uniform(0.05, 0.15)
freq = rng.uniform(1.0, 3.0)
phase = rng.uniform(0, 2*np.pi)
zs = z_center + amp * np.sin(freq * 2*np.pi * (ys - y0)/(y1 - y0) + phase)
return ys, zs
def _arc(rng, n):
"""Arc de cercle dans le tableau."""
cy = rng.uniform(TABLEAU_Y_MIN + 0.1, TABLEAU_Y_MAX - 0.1)
cz = rng.uniform(TABLEAU_Z_MIN + 0.1, TABLEAU_Z_MAX - 0.1)
radius = rng.uniform(0.08, 0.18)
a0 = rng.uniform(0, 2*np.pi)
a1 = a0 + rng.uniform(np.pi/2, 3*np.pi/2)
angles = np.linspace(a0, a1, n)
ys = cy + radius * np.cos(angles)
zs = cz + radius * np.sin(angles)
return ys, zs
SHAPE_GENERATORS = {
'segment' : _segment,
'bezier' : _bezier,
'bezier3' : _bezier_cubic,
'sinus' : _sinusoide,
'arc' : _arc,
}
# ─────────────────────────────────────────────
# Fonction principale
# ─────────────────────────────────────────────
def generate_trajectory(
shape=None,
n_points=DEFAULT_N_POINTS,
seed=None,
x_plane=TABLEAU_X,
):
"""
Génère une trajectoire 2D dans le plan du tableau (X fixe).
Args:
shape : type de courbe ('segment','bezier','bezier3','sinus','arc')
ou None pour un choix aléatoire
n_points : nombre de points de discrétisation
seed : graine aléatoire (reproductibilité)
x_plane : position X du tableau (profondeur)
Returns:
trajectory : array (n_points, 3) de points 3D [x, y, z]
shape_name : nom de la forme générée
"""
rng = np.random.default_rng(seed)
if shape is None:
shape = rng.choice(list(SHAPE_GENERATORS.keys()))
generator = SHAPE_GENERATORS[shape]
# Générer la forme 2D (dans le plan Y-Z)
ys, zs = generator(rng, n_points)
# Contraindre dans les bornes du tableau
ys = np.clip(ys, TABLEAU_Y_MIN, TABLEAU_Y_MAX)
zs = np.clip(zs, TABLEAU_Z_MIN, TABLEAU_Z_MAX)
# Construire les points 3D (X = profondeur du tableau)
xs = np.full(n_points, x_plane)
trajectory = np.column_stack([xs, ys, zs]).astype(np.float32)
# Vérifier l'atteignabilité de tous les points
reachable = np.array([_is_reachable(p) for p in trajectory])
if not reachable.all():
# Si certains points sont hors de portée, réduire l'amplitude
# en rapprochant du centre du tableau
center = np.array([x_plane,
(TABLEAU_Y_MIN + TABLEAU_Y_MAX)/2,
(TABLEAU_Z_MIN + TABLEAU_Z_MAX)/2])
for i in range(n_points):
if not _is_reachable(trajectory[i]):
# Rapprocher du centre jusqu'à atteignabilité
for alpha in np.linspace(0.1, 1.0, 10):
candidate = (1-alpha)*trajectory[i] + alpha*center
if _is_reachable(candidate):
trajectory[i] = candidate
break
return trajectory, shape
def trajectory_length(trajectory):
"""Longueur totale de la trajectoire (m)."""
diffs = np.diff(trajectory, axis=0)
return np.sum(np.linalg.norm(diffs, axis=1))
# ─────────────────────────────────────────────
# Test autonome
# ─────────────────────────────────────────────
if __name__ == '__main__':
print("Test du générateur de trajectoires\n")
for shape in SHAPE_GENERATORS.keys():
traj, name = generate_trajectory(shape=shape, seed=42)
length = trajectory_length(traj)
print(f" {name:10s} : {len(traj)} points | "
f"longueur {length*100:.1f} cm | "
f"X={traj[0,0]:.2f} | "
f"Y∈[{traj[:,1].min():.2f},{traj[:,1].max():.2f}] | "
f"Z∈[{traj[:,2].min():.2f},{traj[:,2].max():.2f}]")
# Test trajectoire aléatoire
print("\n5 trajectoires aléatoires :")
for i in range(5):
traj, name = generate_trajectory(seed=i)
print(f" {i}: {name:10s} | {trajectory_length(traj)*100:.1f} cm")
# Visualisation optionnelle
try:
import matplotlib.pyplot as plt
fig, axes = plt.subplots(1, len(SHAPE_GENERATORS),
figsize=(20, 4))
for ax, shape in zip(axes, SHAPE_GENERATORS.keys()):
traj, name = generate_trajectory(shape=shape, seed=7)
ax.plot(traj[:, 1], traj[:, 2], 'b-o', markersize=2)
ax.scatter(traj[0, 1], traj[0, 2], c='green', s=80,
zorder=5, label='début')
ax.scatter(traj[-1, 1], traj[-1, 2], c='red', s=80,
zorder=5, label='fin')
ax.set_title(name)
ax.set_xlabel('Y (m)')
ax.set_ylabel('Z (m)')
ax.set_xlim(TABLEAU_Y_MIN-0.05, TABLEAU_Y_MAX+0.05)
ax.set_ylim(TABLEAU_Z_MIN-0.05, TABLEAU_Z_MAX+0.05)
ax.set_aspect('equal')
ax.grid(True, alpha=0.3)
ax.legend(fontsize=8)
plt.tight_layout()
plt.savefig('trajectoires_exemples.png', dpi=120, bbox_inches='tight')
print("\nVisualisation sauvegardée : trajectoires_exemples.png")
except ImportError:
print("\n(matplotlib absent — pas de visualisation)")
print("\nTest terminé !")