La génération de trajectoire

WPILib contient des classes qui servent à générer des trajectoires. Une trajectoire est une courbe lisse, comportant des vitesses et des accélérations en chacun de ses points et qui relie deux extrémités sur le terrain. La génération et le suivi des trajectoires sont des opérations extrêment imporantes pour effectuer des tâches en mode autonome. Au lieu d’une simple routine autonome - qui consiste à avancer, à s’arrêter, à tourner de 90 degrés vers la droite, puis à avancer - l’utilisation de trajectoires permet de se déplacer le long d’une courbe lisse. Ce qui a pour avantage d’accélérer les routines autonomes et, donc, laisse plus de temps pour effectuer d’autres tâches; et lorsque l’utilisation de trajectoire est bien mise en œuvre, les déplacements en mode autonome plus beaucoup plus précis.

Cet article explique comment générer une trajectoire. Les prochains articles de cette série examineront comment suivre réellement la trajectoire générée. Afin de pouvoir suivre une trajectoire donnée, votre robot doit être capable de faire tout d’abord les mesures suivantes, en utilisant certains senseurs:

  • La mesure de la position et la vitesse de chaque côté du robot. Un encodeur est le meilleur moyen de le faire; cependant, d’autres options peuvent inclure des capteurs optiques de débit (optical flow sensors), etc.

  • Un moyen de mesurer l’angle ou la vitesse angulaire du châssis du robot. Un gyroscope est la meilleure façon de procéder. Bien que la vitesse angulaire puisse être calculée en utilisant les vitesses obtenues par les encodeurs, cette méthode n’est PAS recommandée en raison du glissement éventuel des roues lors d’un virage (Scrubbing).

Si vous cherchez un moyen plus simple d’effectuer une navigation autonome, voir la section relative au déplacement sur une distance donnée.

Courbes d’interpolation (Splines)

(N.D.T. Le mot anglais Spline sera utilisé dans cette section pour alléger le texte). Une Spline fait référence à un ensemble de courbes qui interpolent entre les points. Considérez-les comme des points de connexion, sauf avec des courbes. WPILib prend en charge deux types de Splines: « hermite cubic clamped » et « hermite quintic ».

  • « Hermite cubic clamped »: C’est l’option recommandée pour la plupart des utilisateurs. La génération de trajectoires à l’aide de ces Splines implique de spécifier les coordonnées (x, y) de tous les points et les caps (direction angulaire) pour les points de départ et de fin. Les changements de cap à l’intérieur de la Spline sont automatiquement calculés pour assurer une courbure continue (taux de changement du cap) sur tout le long du parcours.

  • Hermite quintic: Il s’agit d’une option plus avancée qui nécessite que l’utilisateur spécifie les coordonnées (x, y) et les caps (direction angulaire) pour tous les points internes au parcours. Cela doit être utilisé si vous n’êtes pas satisfait des trajectoires générées par les Splines cubiques décrites ci-dessus, ou si vous souhaitez un contrôle plus fin des caps sur points intérieurs au parcours.

Les splines sont utilisées comme un outil pour générer des trajectoires; cependant, la spline elle-même n’a aucune information sur les vitesses et les accélérations. Par conséquent, il n’est pas recommandé d’utiliser directement les classes de splines. Afin de générer un chemin lisse avec des vitesses et des accélérations, une trajectoire doit être générée.

La création de la configuration de trajectoire

Une configuration doit être créée afin de générer une trajectoire. La configuration contient des informations sur les contraintes spéciales, dont la vitesse maximale, l’accélération maximale, en plus de la vitesse de départ et de la vitesse de fin. La configuration contient également des informations sur l’opportunité d’inverser la trajectoire (le robot recule sur certains points internes). La classe TrajectoryConfig doit être utilisée pour construire une configuration. Le constructeur de cette classe prend deux arguments, la vitesse maximale et l’accélération maximale. Les autres champs (startVelocity, endVelocity, inversé, contraintes) ont par défaut des valeurs raisonnables (0, 0, false, {}) lorsque l’objet est créé. Si vous souhaitez modifier les valeurs de l’un de ces champs, vous pouvez appeler les méthodes suivantes:

  • setStartVelocity(double startVelocityMetersPerSecond) (Java/Python) / SetStartVelocity(units::meters_per_second_t startVelocity) (C++)

  • setEndVelocity(double endVelocityMetersPerSecond) (Java/Python) / SetEndVelocity(units::meters_per_second_t endVelocity) (C++)

  • setReversed(boolean reversed) (Java/Python) / SetReversed(bool reversed) (C++)

  • addConstraint(TrajectoryConstraint constraint) (Java/Python) / AddConstraint(TrajectoryConstraint constraint) (C++)

Note

La propriété reversed représente simplement si le robot recule durant son parcours. Si vous spécifiez quatre points de cheminement, a, b, c et d, le robot continuera de voyager dans le même ordre à travers les points de cheminement lorsque le drapeau reversed est réglé sur true (vrai). Cela signifie également que vous devez tenir compte de la direction du robot lorsque vous fournissez les points de cheminement. Par exemple, si votre robot fait face au mur de votre station d’alliance et se déplace vers l’arrière vers un élément situé sur le terrain, le point de cheminement de départ doit avoir une rotation de 180 degrés.

La génération de la trajectoire

La méthode utilisée pour générer une trajectoire est generateTrajectory(...). Il existe quatre « surcharges » (Overloads) pour cette méthode. Deux qui utilisent des Splines cubiques et les deux autres qui utilisent des splines quintiques. Pour chaque type de Spline, il existe deux façons de construire une trajectoire. Les méthodes les plus simples utilisent des surcharges qui acceptent les objets Pose2d.

Pour les Splines cubiques, cette méthode accepte deux objets Pose2d, un pour le point de cheminement de départ et un pour le point de cheminement de fin. La méthode utilise un vecteur d’objets Translation2d qui représentent les points de cheminement intérieurs. Les caps à ces points intérieurs sont déterminés automatiquement pour assurer une courbure continue. Pour les splines quintiques, la méthode prend simplement une liste d’objets Pose2d, chaque Pose2d représentant un point et un cap sur le champ.

La surcharge qui est plus complexe accepte les « vecteurs de contrôle » pour les Splines. Cette méthode est utilisée lors de la génération de trajectoires avec « Pathweaver », où vous pouvez contrôler l’amplitude du vecteur tangent à chaque point. La classe ControlVector se compose de deux tableaux double. Chaque tableau représente une dimension (x ou y) et ses éléments représentent les dérivées à ce point. Par exemple, la valeur à l’élément 0 du tableau x représente la coordonnée x (dérivée 0), la valeur à l’élément 1 représente la dérivée 1 dans la dimension x et ainsi de suite.

Lorsque vous utilisez des Splines cubiques, la taille du tableau doit permettre 2 éléments (dérivées 0 et 1), tandis que lorsque vous utilisez des Splines quintiques, la longueur du tableau doit être 3 (dérivée 0, 1 et 2). À moins que vous ne sachiez exactement ce que vous faites, la première méthode (et la plus simple) est FORTEMENT recommandée pour générer manuellement des trajectoires. (c’est-à-dire lorsque vous n’utilisez pas de fichiers JSON Pathweaver).

Voici un exemple de génération d’une trajectoire à l’aide de splines cubiques serrées pour le jeu 2018, FIRST Power Up:

class ExampleTrajectory {
  public void generateTrajectory() {

    // 2018 cross scale auto waypoints.
    var sideStart = new Pose2d(Units.feetToMeters(1.54), Units.feetToMeters(23.23),
        Rotation2d.fromDegrees(-180));
    var crossScale = new Pose2d(Units.feetToMeters(23.7), Units.feetToMeters(6.8),
        Rotation2d.fromDegrees(-160));

    var interiorWaypoints = new ArrayList<Translation2d>();
    interiorWaypoints.add(new Translation2d(Units.feetToMeters(14.54), Units.feetToMeters(23.23)));
    interiorWaypoints.add(new Translation2d(Units.feetToMeters(21.04), Units.feetToMeters(18.23)));

    TrajectoryConfig config = new TrajectoryConfig(Units.feetToMeters(12), Units.feetToMeters(12));
    config.setReversed(true);

    var trajectory = TrajectoryGenerator.generateTrajectory(
        sideStart,
        interiorWaypoints,
        crossScale,
        config);
  }
}

void GenerateTrajectory() {
  // 2018 cross scale auto waypoints
  const frc::Pose2d sideStart{1.54_ft, 23.23_ft, frc::Rotation2d(180_deg)};
  const frc::Pose2d crossScale{23.7_ft, 6.8_ft, frc::Rotation2d(-160_deg)};

  std::vector<frc::Translation2d> interiorWaypoints{
      frc::Translation2d{14.54_ft, 23.23_ft},
      frc::Translation2d{21.04_ft, 18.23_ft}};

  frc::TrajectoryConfig config{12_fps, 12_fps_sq};
  config.SetReversed(true);

  auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
      sideStart, interiorWaypoints, crossScale, config);
}

def generateTrajectory():
    # 2018 cross scale auto waypoints.
    sideStart = Pose2d.fromFeet(1.54, 23.23, Rotation2d.fromDegrees(-180))
    crossScale = Pose2d.fromFeet(23.7, 6.8, Rotation2d.fromDegrees(-160))

    interiorWaypoints = [
        Translation2d.fromFeet(14.54, 23.23),
        Translation2d.fromFeet(21.04, 18.23),
    ]

    config = TrajectoryConfig.fromFps(12, 12)
    config.setReversed(True)

    trajectory = TrajectoryGenerator.generateTrajectory(
        sideStart, interiorWaypoints, crossScale, config
    )

Note

The Java code utilizes the Units utility, for easy unit conversions.

Note

La génération d’une trajectoire typique prend environ 10 ms à 25 ms. Ce n’est pas long, mais il est tout de même fortement recommandé de générer toutes les trajectoires au démarrage (robotInit).

Concaténation des trajectoires

Trajectories in Java can be combined into a single trajectory using the concatenate(trajectory) function. C++/Python users can simply add (+) the two trajectories together.

Avertissement

Il appartient à l’utilisateur de s’assurer que la fin de la trajectoire initiale et le début de la trajectoire ajoutée correspondent. Il est également de la responsabilité de l’utilisateur de s’assurer que les vitesses de début et de fin de ses trajectoires correspondent.

var trajectoryOne =
TrajectoryGenerator.generateTrajectory(
   new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
   List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
   new Pose2d(3, 0, Rotation2d.fromDegrees(0)),
   new TrajectoryConfig(Units.feetToMeters(3.0), Units.feetToMeters(3.0)));

var trajectoryTwo =
TrajectoryGenerator.generateTrajectory(
   new Pose2d(3, 0, Rotation2d.fromDegrees(0)),
   List.of(new Translation2d(4, 4), new Translation2d(6, 3)),
   new Pose2d(6, 0, Rotation2d.fromDegrees(0)),
   new TrajectoryConfig(Units.feetToMeters(3.0), Units.feetToMeters(3.0)));

var concatTraj = trajectoryOne.concatenate(trajectoryTwo);

auto trajectoryOne = frc::TrajectoryGenerator::GenerateTrajectory(
   frc::Pose2d(0_m, 0_m, 0_rad),
   {frc::Translation2d(1_m, 1_m), frc::Translation2d(2_m, -1_m)},
   frc::Pose2d(3_m, 0_m, 0_rad), frc::TrajectoryConfig(3_fps, 3_fps_sq));

auto trajectoryTwo = frc::TrajectoryGenerator::GenerateTrajectory(
   frc::Pose2d(3_m, 0_m, 0_rad),
   {frc::Translation2d(4_m, 4_m), frc::Translation2d(5_m, 3_m)},
   frc::Pose2d(6_m, 0_m, 0_rad), frc::TrajectoryConfig(3_fps, 3_fps_sq));

auto concatTraj = m_trajectoryOne + m_trajectoryTwo;

from wpimath.geometry import Pose2d, Rotation2d, Translation2d
from wpimath.trajectory import TrajectoryGenerator, TrajectoryConfig


trajectoryOne = TrajectoryGenerator.generateTrajectory(
   Pose2d(0, 0, Rotation2d.fromDegrees(0)),
   [Translation2d(1, 1), Translation2d(2, -1)],
   Pose2d(3, 0, Rotation2d.fromDegrees(0)),
   TrajectoryConfig.fromFps(3.0, 3.0),
)

trajectoryTwo = TrajectoryGenerator.generateTrajectory(
   Pose2d(3, 0, Rotation2d.fromDegrees(0)),
   [Translation2d(4, 4), Translation2d(6, 3)],
   Pose2d(6, 0, Rotation2d.fromDegrees(0)),
   TrajectoryConfig.fromFps(3.0, 3.0),
)

concatTraj = trajectoryOne + trajectoryTwo