Generación de Trayectoria
WPILib contains classes that help generating trajectories. A trajectory is a smooth curve, with velocities and accelerations at each point along the curve, connecting two endpoints on the field. Generation and following of trajectories is incredibly useful for performing autonomous tasks. Instead of a simple autonomous routine – which involves moving forward, stopping, turning 90 degrees to the right, then moving forward – using trajectories allows for motion along a smooth curve. This has the advantage of speeding up autonomous routines, creating more time for other tasks; and when implemented well, makes autonomous navigation more accurate and precise.
Este artículo trata sobre cómo generar una trayectoria. Los próximos artículos de esta serie repasarán cómo seguir realmente la trayectoria generada. Hay algunas cosas que tu robot debe tener antes de sumergirse en el mundo de las trayectorias:
Una forma de medir la posición y la velocidad de cada lado del robot. Un codificador es la mejor manera de hacerlo; sin embargo, otras opciones pueden incluir sensores ópticos de flujo, etc.
Una forma de medir el ángulo o la velocidad angular del chasis del robot. Un giroscopio es la mejor manera de hacerlo. Aunque la tasa angular puede ser calculada usando las velocidades del codificador, este método NO es recomendado debido al lavado de las ruedas.
If you are looking for a simpler way to perform autonomous navigation, see the section on driving to a distance.
Splines
Un spline se refiere a un conjunto de curvas que se interpolan entre puntos. Piense en esto como conectar puntos, excepto con las curvas. WPILib apoya dos tipos de splines: cúbico sujetado por la hérmite y quístico sujetado por la hérmite.
Hermite clamped cubic: Esta es la opción recomendada para la mayoría de los usuarios. La generación de trayectorias utilizando estas splines implica especificar las coordenadas (x, y) de todos los puntos, y los encabezamientos en los puntos de inicio y final. Los encabezamientos en los waypoints interiores se determinan automáticamente para asegurar una curvatura continua (tasa de cambio de rumbo) en todo momento.
Hermite quintic: Esta es una opción más avanzada que requiere que el usuario especifique coordenadas (x, y) y encabezamientos para todos los waypoints. Esto debería utilizarse si no se está satisfecho con las trayectorias que están siendo generadas por las clamped cubic splines o si se desea un control más fino de los encabezamientos en los puntos interiores.
Los splines son usados como una herramienta para generar trayectorias; sin embargo, el propio spline no tiene ninguna información sobre las velocidades y aceleraciones. Por lo tanto, no se recomienda utilizar las clases de spline directamente. Para generar una trayectoria suave con velocidades y aceleraciones, se debe generar una trayectoria.
Creando la configuración de la trayectoria
Se debe crear una configuración para generar una trayectoria. La configuración contiene información sobre las restricciones especiales, la velocidad máxima, la aceleración máxima además de la velocidad inicial y la velocidad final. La configuración también contiene información sobre si la trayectoria debe ser invertida (el robot viaja hacia atrás a lo largo de los puntos de ruta). La clase TrajectoryConfig
debe ser usada para construir una configuración. El constructor de esta clase toma dos argumentos, la velocidad máxima y la aceleración máxima. Los otros campos (startVelocity
, endVelocity
, reversed
, constraints
) son predeterminados a valores razonables (0
, 0
, false
, {}
) cuando se crea el objeto. Si desea modificar los valores de cualquiera de estos campos, puede llamar a los siguientes métodos:
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++)
Nota
La propiedad «invertida» simplemente representa si el robot está viajando hacia atrás. Si especificas cuatro puntos, a, b, c y d, el robot seguirá viajando en el mismo orden a través de los puntos cuando la bandera «invertida» se ponga en «verdadero». Esto también significa que debe tener en cuenta la dirección del robot cuando proporcione los puntos de ruta. Por ejemplo, si su robot está de cara a la pared de la estación de la alianza y viaja hacia atrás a algún elemento del campo, el waypoint inicial debe tener una rotación de 180 grados.
Generando la trayectoria
El método usado para generar una trayectoria es generateTrajectory(...)
. Hay cuatro sobrecargas para este método. Dos que usan clamped cubic splines y otras dos que usan quintic splines. Por cada tipo de spline, hay dos formas para construir una trayectoria. Los métodos más sencillos son las sobrecargas que aceptan objetos Pose2d
.
Para los clamped cubic splines, este método acepta dos objetos Pose2d
, uno para el punto de inicio y otro para el punto final. El método toma un vector de objetos «Translation2d» que representan los puntos de ruta interiores. Los encabezamientos de estos waypoints interiores se determinan automáticamente para asegurar una curvatura continua. Para las curvas quínticas, el método simplemente toma una lista de objetos Pose2d
, con cada Pose2d
representando un punto y un encabezamiento en la cancha.
La sobrecarga más compleja acepta «vectores de control» para los splines. Este método se utiliza cuando se generan trayectorias con Pathweaver, en las que se puede controlar la magnitud del vector tangente en cada punto. La clase ControlVector
consiste de dos arreglos «dobles». Cada arreglo representa una dimensión (x o y), y sus elementos representan las derivadas en ese punto. Por ejemplo, el valor en el elemento 0 de la matriz x
representa la coordenada x (0ª derivada), el valor en el elemento 1 representa la 1ª derivada en la dimensión x y así sucesivamente.
Al usarse clamped cubic splines, la longitud de la matriz debe ser 2 (0ª y 1ª derivadas), mientras que cuando se utilizan quintic splines, la longitud del conjunto debe ser 3 (0ª, 1ª y 2ª derivadas). A menos que sepas exactamente lo que estás haciendo, el primer y más simple método es ALTAMENTE recomendado para generar trayectorias manualmente. (es decir, cuando no se utilizan los archivos JSON de Pathweaver).
Aquí hay un ejemplo de cómo generar una trayectoria usando clamped cubic splines para el juego de 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
)
Nota
The Java code utilizes the Units utility, for easy unit conversions.
Nota
Generating a typical trajectory takes about 10 ms to 25 ms. This isn’t long, but it’s still highly recommended to generate all trajectories on startup (robotInit
).
Concatenating Trajectories
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.
Advertencia
It is up to the user to ensure that the end of the initial and start of the appended trajectory match. It is also the user’s responsibility to ensure that the start and end velocities of their trajectories match.
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