scurve.py

gistfile1.txt

import math
import matplotlib.pyplot as plt
from dataclasses import dataclass

# --------------------------- context + generators ---------------------------
@dataclass
class PositionGeneratorContext:
    velocity: float
    acceleration: float
    jerk_time: float
    terminal_velocity: float = 0.0


class PositionGeneratorSCurve:
    """Original Euler‑step integration (à l’origine de la dérive)."""
    def __init__(self, dt: float, ctx: PositionGeneratorContext):
        self.dt = dt
        self.ctx = ctx
        self.reset()

    # ---------------------------------------------------------------------
    def reset(self):
        self._t = self._seg_t = 0.0
        self._seg_idx = 0
        self._a = self._v = self._x = 0.0
        self._segments = []
        self._finished = True

    def move(self, distance: float) -> None:
        """
        Planifie un déplacement de `distance` [unités] en respectant :

            * v_max   = ctx.velocity
            * a_max   = ctx.acceleration
            * j_max   = a_max / Tj
            * v_f     = ctx.terminal_velocity  (vitesse d'arrivée)

        La trajectoire est composée de 7 segments (jerk ±j_max ou 0)
        et respecte toujours jerk, accélération et vitesse maximales.
        """
        # ----------- mise à zéro de l'état interne --------------------------
        self.reset()
        self._finished = False
        self.sign = 1.0 if distance >= 0 else -1.0
        D = abs(distance)

        # ----------- paramètres de profil -----------------------------------
        v_max = self.ctx.velocity
        a_max = self.ctx.acceleration
        Tj    = self.ctx.jerk_time
        v_f   = max(0.0, self.ctx.terminal_velocity)     # borne sécurité
        j_max = a_max / Tj

        # ----------- sous-profil ACCÉLÉRATION (0 → v_max) -------------------
        Ta1 = max((v_max - a_max*Tj) / a_max, 0.0)
        Sa  = a_max * (Tj**2 + 1.5*Tj*Ta1 + 0.5*Ta1**2)

        # ----------- sous-profil DÉCÉLÉRATION (v_max → v_f) -----------------
        Ta2 = max((v_max - v_f - a_max*Tj) / a_max, 0.0)
        Sd  = (a_max * (Tj**2 + 1.5*Tj*Ta2 + 0.5*Ta2**2) +
            v_f * 0.0)                                # Tc2 = 0 ici

        Sc = D - Sa - Sd        # distance disponible pour la croisière
        if Sc < 0.0:
            # --------- distance trop courte : trouver v_peak ≤ ctx.velocity -
            def total_dist(v_peak: float) -> float:
                Ta1_ = max((v_peak - a_max*Tj) / a_max, 0.0)
                Sa_  = a_max * (Tj**2 + 1.5*Tj*Ta1_ + 0.5*Ta1_**2)
                Ta2_ = max((v_peak - v_f - a_max*Tj) / a_max, 0.0)
                Sd_  = a_max * (Tj**2 + 1.5*Tj*Ta2_ + 0.5*Ta2_**2)
                return Sa_ + Sd_

            # recherche dichotomique sur v_peak ∈ [v_f, ctx.velocity]
            v_low, v_high = v_f, v_max
            for _ in range(40):                      # précision 1 ulp
                v_mid = 0.5*(v_low + v_high)
                if total_dist(v_mid) > D:
                    v_high = v_mid
                else:
                    v_low = v_mid
            v_max = v_mid                            # vitesse pic ajustée

            # recalcul des sous-profils avec le nouveau v_max
            Ta1 = max((v_max - a_max*Tj) / a_max, 0.0)
            Sa  = a_max * (Tj**2 + 1.5*Tj*Ta1 + 0.5*Ta1**2)
            Ta2 = max((v_max - v_f - a_max*Tj) / a_max, 0.0)
            Sd  = a_max * (Tj**2 + 1.5*Tj*Ta2 + 0.5*Ta2**2)
            Sc  = 0.0
        # ----------- temps de croisière éventuel ----------------------------
        Tc = Sc / v_max if v_max > 1e-12 else 0.0

        # ----------- table finale des segments ------------------------------
        self._segments = [
            (Tj,  +j_max),   # 0  jerk +        (accélération croissante)
            (Ta1,     0.0),  # 1  acc. constante
            (Tj,  -j_max),   # 2  jerk -
            (Tc,      0.0),  # 3  croisière v_max
            (Tj,  -j_max),   # 4  jerk -        (début décélération)
            (Ta2,     0.0),  # 5  acc. constante (décel)
            (Tj,  +j_max),   # 6  jerk +        (accélération → 0, v → v_f)
        ]

    def step(self) -> float:
        if self._finished:
            return 0.0
        # advance segments
        while self._seg_idx < len(self._segments):
            dur, j = self._segments[self._seg_idx]
            if self._seg_t < dur:
                break
            self._seg_t -= dur
            self._seg_idx += 1
        if self._seg_idx >= len(self._segments):
            self._finished = True
            return 0.0
        dur, j = self._segments[self._seg_idx]
        # Euler update
        self._a += j*self.dt
        self._v += self._a*self.dt
        dx = self._v*self.dt
        self._x += dx
        self._t += self.dt
        self._seg_t += self.dt
        return self.sign*dx

    @property
    def x(self): return self.sign*self._x


class PositionGeneratorAccurate(PositionGeneratorSCurve):
    """Intégration exacte sur les sous‑pas – aucune dérive."""
    def step(self) -> float:
        if self._finished:
            return 0.0
        dx_total = 0.0
        dt_remain = self.dt
        while dt_remain > 0 and not self._finished:
            while self._seg_idx < len(self._segments):
                dur, j = self._segments[self._seg_idx]
                if self._seg_t < dur:
                    break
                self._seg_t -= dur
                self._seg_idx += 1
            if self._seg_idx >= len(self._segments):
                self._finished = True
                break
            dur, j = self._segments[self._seg_idx]
            dt_step = min(dt_remain, dur - self._seg_t)
            a0, v0 = self._a, self._v
            self._a = a0 + j*dt_step
            self._v = v0 + a0*dt_step + 0.5*j*dt_step**2
            dx = v0*dt_step + 0.5*a0*dt_step**2 + (1/6)*j*dt_step**3
            self._x += dx
            dx_total += dx
            self._t += dt_step
            self._seg_t += dt_step
            dt_remain -= dt_step
        return self.sign*dx_total


# --------------------------- analytic reference -----------------------------
def analytic_position(t: float, ctx: PositionGeneratorContext,
                      distance: float,
                      segments: list[tuple[float, float]]) -> float:
    """Position exacte en fonction du temps, pour comparaison."""
    D = abs(distance)
    sign = 1 if distance >= 0 else -1
    x0 = v0 = a0 = 0.0
    t0 = 0.0
    for dur, j in segments:
        if t <= t0 + dur:
            tau = t - t0
            if j == 0.0:
                a = a0
                v = v0 + a*tau
                x = x0 + v0*tau + 0.5*a*tau**2
            else:
                a = a0 + j*tau
                v = v0 + a0*tau + 0.5*j*tau**2
                x = (x0 + v0*tau + 0.5*a0*tau**2 +
                     (1/6)*j*tau**3)
            return sign*x
        # update init for next seg
        if j == 0.0:
            a_end = a0
            v_end = v0 + a0*dur
            x_end = x0 + v0*dur + 0.5*a0*dur**2
        else:
            a_end = a0 + j*dur
            v_end = v0 + a0*dur + 0.5*j*dur**2
            x_end = (x0 + v0*dur + 0.5*a0*dur**2 +
                     (1/6)*j*dur**3)
        a0, v0, x0 = a_end, v_end, x_end
        t0 += dur
    return sign*distance


# ------------------------- simulation parameters ----------------------------
ctx = PositionGeneratorContext(
    velocity=10.0, 
    acceleration=2.0, 
    jerk_time=0.1)
dt = 200e-6
D  = 1.0                 # target distance

# --- simulate both generators ----------------------------------------------
gen_euler = PositionGeneratorSCurve(dt, ctx)
gen_euler.move(D)

gen_exact = PositionGeneratorAccurate(dt, ctx)
gen_exact.move(D)

times, err_euler, err_exact = [], [], []
t = 0.0

X = [0]
T = [0]
V = [0]
A = [0]
while not gen_exact._finished:
    if not gen_euler._finished:
        v = gen_euler.step()
        X.append(X[-1] + v)
        V.append(v)
        A.append((V[-1] - V[-2]) / dt)
        T.append(t + dt)

    gen_exact.step()
    t += dt
    times.append(t)
    x_ref = analytic_position(t, ctx, D, gen_exact._segments)
    err_euler.append(gen_euler.x - x_ref)
    err_exact.append(gen_exact.x - x_ref)

fig, ax = plt.subplots(3, 1, figsize=(8, 6), sharex=True)
ax[0].plot(T, X, label="x(t)")
ax[1].plot(T, V, label="v(t)")
ax[2].plot(T, A, label="a(t)")
ax[0].set_title("Position, velocity and acceleration")
ax[0].set_ylabel("Position")
ax[1].set_ylabel("Velocity")
ax[2].set_ylabel("Acceleration")
ax[2].set_xlabel("Time [s]")
ax[0].legend()
ax[1].legend()
ax[2].legend()
plt.tight_layout()
plt.show()

# ------------------------------ plotting ------------------------------------
plt.figure()
plt.plot(times, err_euler)
plt.title("Erreur cumulée – intégration Euler naïve")
plt.xlabel("Temps [s]")
plt.ylabel("Erreur [u]")

plt.figure()
plt.plot(times, err_exact)
plt.title("Erreur cumulée – intégration exacte par segments")
plt.xlabel("Temps [s]")
plt.ylabel("Erreur [u]")

plt.show()

print(f"Erreur finale Euler : {err_euler[-1]:.6f} u")
print(f"Erreur finale exacte : {err_exact[-1]:.6e} u")