Yörünge Üretimi

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.

Bu makale, bir yörüngenin nasıl oluşturulacağını anlatıyor. Bu dizideki sonraki birkaç makale, oluşturulan yörüngenin gerçekte nasıl izleneceğini ele alacaktır. Yörüngeler dünyasına dalmadan önce robotunuzun sahip olması gereken birkaç şey var:

  • Robotun her iki tarafının konumunu ve hızını ölçmenin bir yolu. Bunu yapmanın en iyi yolu kodlayıcıdır; ancak diğer seçenekler optik akış sensörleri vb. içerebilir.

  • Robot kasasının açısını veya açısal oranını ölçmenin bir yolu. Bunu yapmanın en iyi yolu jiroskoptur. Açısal hız enkoder hızları kullanılarak hesaplanabilmesine rağmen, bu yöntem tekerlek dönme kaybı nedeniyle ÖNERİLMEZ.

If you are looking for a simpler way to perform autonomous navigation, see the section on driving to a distance.

Spline’lar - Eğri setleri

Spline, noktalar arasında enterpolasyon yapan bir dizi eğriyi ifade eder. Eğriler dışında, noktaları birleştiren noktalar olarak düşünün. WPILib iki tür spline’ı destekler: hermit kenetli kübik ve hermit beşli.

  • Hermite sıkıştırılmış kübik: Bu, çoğu kullanıcı için önerilen seçenektir. Bu eğri çizgiler kullanılarak yörüngelerin oluşturulması, tüm noktaların (x, y) koordinatlarının ve başlangıç ve bitiş yol noktalarındaki başlıkların belirlenmesini içerir. İç yol noktalarındaki başlıklar, sürekli eğriliği (yön değiştirme hızı) sağlamak için otomatik olarak belirlenir.

  • Hermite quintic: Bu, kullanıcının * tüm * yol noktaları için (x, y) koordinatlarını ve başlıklarını belirtmesini gerektiren daha gelişmiş bir seçenektir. Bu, kenetlenmiş kübik yivlerin oluşturduğu yörüngelerden memnun değilseniz veya iç noktalardaki başlıkların daha iyi kontrolünü istiyorsanız kullanılmalıdır.

Spline’lar yörüngeler oluşturmak için bir araç olarak kullanılır; ancak, spline’ın kendisi hızlar ve ivmeler hakkında herhangi bir bilgiye sahip değildir. Bu nedenle, spline sınıflarını doğrudan kullanmanız önerilmez. Hızları ve ivmeleri olan düzgün bir yol oluşturmak için, bir * yörünge * oluşturulmalıdır.

Yörünge yapılandırmasını oluşturma

Bir yörünge oluşturmak için bir konfigürasyon oluşturulmalıdır. Yapılandırma, başlangıç hızı ve bitiş hızına ek olarak özel kısıtlamalar, maksimum hız, maksimum ivme hakkında bilgi içerir. Yapılandırma ayrıca yörüngenin tersine çevrilmesi gerekip gerekmediği hakkında bilgi içerir (robot yol noktaları boyunca geriye doğru gider). Bir yapılandırma oluşturmak için `` TrajectoryConfig ‘’ sınıfı kullanılmalıdır. Bu sınıfın yapıcısı iki argüman alır: maksimum hız ve maksimum ivme. Diğer alanlar (`` startVelocity ‘’, `` endVelocity ‘’, `` reversed ‘’, `` kısıtlamalar ‘’) varsayılan değerlere (`` 0 ‘’, `` 0 ‘’, `` false` Nesne oluşturulduğunda , {} ``). Bu alanlardan herhangi birinin değerlerini değiştirmek isterseniz, aşağıdaki yöntemleri çağırabilirsiniz:

  • 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++)

Not

`` Ters-reversed ‘’ özelliği, basitçe robotun geriye doğru hareket edip etmediğini gösterir. Dört yol noktası, a, b, c ve d belirtirseniz, robot, `` ters-reversed`` bayrağı `` doğru-true`` olarak ayarlandığında, yol noktaları boyunca aynı sırayla seyahat edecektir. Bu aynı zamanda, yol noktalarını sağlarken robotun yönünü hesaba katmanız gerektiği anlamına gelir. Örneğin, robotunuz ittifak istasyonu duvarınıza bakıyorsa ve bazı alan unsurlarına geri giderse, başlangıç yol noktasının 180 derece dönüşü olmalıdır.

Yörünge oluşturmak

Bir yörünge oluşturmak için kullanılan yöntem generateTrajectory (...) şeklindedir. Bu yöntem için dört aşırı yük vardır. Kenetli kübik spline kullanan iki tanesi ve beşli spline kullanan diğer ikisi. Her bir spline türü için bir yörünge oluşturmanın iki yolu vardır. En kolay yöntemler, Pose2d nesnelerini kabul eden aşırı yüklemelerdir.

Kelepçeli kübik eğriler için bu yöntem, biri başlangıç yol noktası ve diğeri bitiş yol noktası için olmak üzere iki Pose2d nesnesini kabul eder. Yöntem, iç yol noktalarını temsil eden `` Translation2d`` nesnelerinin bir vektörünü alır. Bu iç yol noktalarındaki başlıklar, sürekli eğriliği sağlamak için otomatik olarak belirlenir. Beşli spline’lar için, yöntem basitçe `` Pose2d`` nesnelerinin bir listesini alır ve her bir `` Pose2d`` , bir noktayı ve alandaki başlığı temsil eder.

Daha karmaşık aşırı yük, spline’lar için “kontrol vektörlerini-control vectors” kabul eder. Bu yöntem, her noktada teğet vektörün büyüklüğünü kontrol edebileceğiniz Pathweaver ile yörüngeler oluştururken kullanılır. `` ControlVector`` sınıfı iki `` çift`-double`` diziden oluşur. Her dizi bir boyutu (x veya y) temsil eder ve onun elemanları bu noktadaki türevleri temsil eder. Örneğin, `` x`` dizisinin 0 öğesindeki değer x koordinatını (0. türev), 1. öğedeki değer x boyutundaki 1. türevi temsil eder ve bu böyle devam eder.

Sabitlenmiş kübik spline’lar kullanılırken, dizinin uzunluğu 2 (0. ve 1. türevler) olmalıdır, oysa beşli spline’lar kullanılırken, dizinin uzunluğu 3 (0., 1. ve 2. türev) olmalıdır. Tam olarak ne yaptığınızı bilmiyorsanız, yörüngeleri manuel olarak oluşturmak için ilk ve daha basit yöntem ŞİDDETLE önerilir. (yani Pathweaver JSON dosyalarını kullanmadığınızda).

2018 oyunu FIRST Power Up için sabitlenmiş kübik eğriler kullanarak bir yörünge oluşturmanın bir örneği:

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
    )

Not

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

Not

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.

Uyarı

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