001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.math.trajectory.constraint;
006
007import edu.wpi.first.math.geometry.Pose2d;
008import edu.wpi.first.math.geometry.Translation2d;
009
010/** Enforces a particular constraint only within a rectangular region. */
011public class RectangularRegionConstraint implements TrajectoryConstraint {
012  private final Translation2d m_bottomLeftPoint;
013  private final Translation2d m_topRightPoint;
014  private final TrajectoryConstraint m_constraint;
015
016  /**
017   * Constructs a new RectangularRegionConstraint.
018   *
019   * @param bottomLeftPoint The bottom left point of the rectangular region in which to enforce the
020   *     constraint.
021   * @param topRightPoint The top right point of the rectangular region in which to enforce the
022   *     constraint.
023   * @param constraint The constraint to enforce when the robot is within the region.
024   */
025  public RectangularRegionConstraint(
026      Translation2d bottomLeftPoint, Translation2d topRightPoint, TrajectoryConstraint constraint) {
027    m_bottomLeftPoint = bottomLeftPoint;
028    m_topRightPoint = topRightPoint;
029    m_constraint = constraint;
030  }
031
032  @Override
033  public double getMaxVelocityMetersPerSecond(
034      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
035    if (isPoseInRegion(poseMeters)) {
036      return m_constraint.getMaxVelocityMetersPerSecond(
037          poseMeters, curvatureRadPerMeter, velocityMetersPerSecond);
038    } else {
039      return Double.POSITIVE_INFINITY;
040    }
041  }
042
043  @Override
044  public MinMax getMinMaxAccelerationMetersPerSecondSq(
045      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
046    if (isPoseInRegion(poseMeters)) {
047      return m_constraint.getMinMaxAccelerationMetersPerSecondSq(
048          poseMeters, curvatureRadPerMeter, velocityMetersPerSecond);
049    } else {
050      return new MinMax();
051    }
052  }
053
054  /**
055   * Returns whether the specified robot pose is within the region that the constraint is enforced
056   * in.
057   *
058   * @param robotPose The robot pose.
059   * @return Whether the robot pose is within the constraint region.
060   */
061  public boolean isPoseInRegion(Pose2d robotPose) {
062    return robotPose.getX() >= m_bottomLeftPoint.getX()
063        && robotPose.getX() <= m_topRightPoint.getX()
064        && robotPose.getY() >= m_bottomLeftPoint.getY()
065        && robotPose.getY() <= m_topRightPoint.getY();
066  }
067}