轨迹约束

在 :ref:`previous article <docs/software/advanced-controls/trajectories/trajectory-generation:Generating the trajectory>`中，您可能已经注意到在生成轨迹时没有添加自定义约束。自定义约束允许用户基于位置和曲率对轨迹上点的速度和加速度施加更多的限制。

例如，自定义约束可以将轨迹的速度保持在某一区域的某一阈值以下，或者在转弯附近使机器人减速以达到稳定的目的。

WPILib-Provided约束

WPILib包括一组预定义的约束，用户在生成轨迹时可以利用这些约束。 WPILib提供的约束列表如下：

  • ``CentripetalAccelerationConstraint’’：限制机器人沿轨迹行进时的向心加速度。这可以帮助机器人在急转弯时减速。

  • ``DifferentialDriveKinematicsConstraint’’：限制转弯时机器人的速度，以使差速驱动机器人的车轮都不会超过指定的最大速度。

  • ``DifferentialDriveVoltageConstraint’’：限制差动驱动机器人的加速度，以使命令电压不超过指定的最大值。

  • ``EllipticalRegionConstraint’’：仅在赛场上的椭圆区域中施加约束。

  • ``MaxVelocityConstraint’’：施加最大速度约束。这可以与``EllipticalRegionConstraint’’或``RectangularRegionConstraint’’组成，以仅在特定区域限制机器人的速度。

  • ``MecanumDriveKinematicsConstraint’’：限制转弯时机器人的速度，以使麦克纳姆驱动机器人的车轮都不会超过指定的最大速度。

  • ``RectangularRegionConstraint’’：仅在字段的矩形区域中施加约束。

  • ``SwerveDriveKinematicsConstraint’’：限制转弯时机器人的速度，以使转弯驱动机器人的车轮都不会超过指定的最大速度。

备注

``DifferentialDriveVoltageConstraint’’只确保使用:ref:`feedforward model <docs/software/advanced-controls/controllers/feedforward:SimpleMotorFeedforward>`的理论电压命令不会超过指定的最大值。如果机器人在跟踪过程中偏离基准，指令电压可能会高于指定的最大电压。

创建自定义约束

用户可以通过实现``TrajectoryConstraint’’接口来创建自己的约束。

@Override
public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
                                            double velocityMetersPerSecond) {
  // code here
}

@Override
public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
                                                     double curvatureRadPerMeter,
                                                     double velocityMetersPerSecond) {
  // code here
}

units::meters_per_second_t MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) override {
  // code here
}

MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
                      units::meters_per_second_t speed) override {
  // code here
}

from wpimath import units
from wpimath.geometry import Pose2d
from wpimath.trajectory.constraint import TrajectoryConstraint


class MyConstraint(TrajectoryConstraint):
    def maxVelocity(
        self,
        pose: Pose2d,
        curvature: units.radians_per_meter,
        velocity: units.meters_per_second,
    ) -> units.meters_per_second:
        ...

    def minMaxAcceleration(
        self,
        pose: Pose2d,
        curvature: units.radians_per_meter,
        speed: units.meters_per_second,
    ) -> TrajectoryConstraint.MinMax:
        ...

MaxVelocity方法应返回给定姿势，曲率和轨迹的原始速度的最大允许速度，且无任何限制。MinMaxAcceleration``方法应返回给定姿势，曲率和约束速度的最小和最大允许加速度。

See the source code (Java, C++) for the WPILib-provided constraints for more examples on how to write your own custom trajectory constraints.