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.Rectangle2d; 009import edu.wpi.first.math.geometry.Translation2d; 010 011/** Enforces a particular constraint only within a rectangular region. */ 012public class RectangularRegionConstraint implements TrajectoryConstraint { 013 private final Rectangle2d m_rectangle; 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 * @deprecated Use constructor taking Rectangle2d instead. 025 */ 026 @Deprecated(since = "2025", forRemoval = true) 027 public RectangularRegionConstraint( 028 Translation2d bottomLeftPoint, Translation2d topRightPoint, TrajectoryConstraint constraint) { 029 this(new Rectangle2d(bottomLeftPoint, topRightPoint), constraint); 030 } 031 032 /** 033 * Constructs a new RectangularRegionConstraint. 034 * 035 * @param rectangle The rectangular region in which to enforce the constraint. 036 * @param constraint The constraint to enforce when the robot is within the region. 037 */ 038 public RectangularRegionConstraint(Rectangle2d rectangle, TrajectoryConstraint constraint) { 039 m_rectangle = rectangle; 040 m_constraint = constraint; 041 } 042 043 @Override 044 public double getMaxVelocity(Pose2d pose, double curvature, double velocity) { 045 if (m_rectangle.contains(pose.getTranslation())) { 046 return m_constraint.getMaxVelocity(pose, curvature, velocity); 047 } else { 048 return Double.POSITIVE_INFINITY; 049 } 050 } 051 052 @Override 053 public MinMax getMinMaxAcceleration(Pose2d pose, double curvature, double velocity) { 054 if (m_rectangle.contains(pose.getTranslation())) { 055 return m_constraint.getMinMaxAcceleration(pose, curvature, velocity); 056 } else { 057 return new MinMax(); 058 } 059 } 060}