Fix collision avoidance min turret angle
We wrap from 0 to 2 pi, but this was a negative angle...
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I97a00201fd68b572d4fbdb77440f5598f7a6f720
diff --git a/y2022/control_loops/superstructure/collision_avoidance.cc b/y2022/control_loops/superstructure/collision_avoidance.cc
index 01b0463..d87054a 100644
--- a/y2022/control_loops/superstructure/collision_avoidance.cc
+++ b/y2022/control_loops/superstructure/collision_avoidance.cc
@@ -47,6 +47,12 @@
return wrapped + 2.0 * M_PI * wraps;
}
+bool AngleInRange(double theta, double theta_min, double theta_max) {
+ return (
+ (theta >= theta_min && theta <= theta_max) ||
+ (theta_min > theta_max && (theta >= theta_min || theta <= theta_max)));
+}
+
bool CollisionAvoidance::TurretCollided(double intake_position,
double turret_position,
double min_turret_collision_position,
@@ -55,8 +61,8 @@
const double turret_position_wrapped = turret_position_wrapped_pair.first;
// Checks if turret is in the collision area.
- if (turret_position_wrapped >= min_turret_collision_position &&
- turret_position_wrapped <= max_turret_collision_position) {
+ if (AngleInRange(turret_position_wrapped, min_turret_collision_position,
+ max_turret_collision_position)) {
// Reterns true if the intake is raised.
if (intake_position > kCollisionZoneIntake) {
return true;
@@ -110,8 +116,8 @@
// If the turret goal is in a collison zone or moving through one, limit
// intake.
const bool turret_pos_unsafe =
- (turret_position_wrapped >= min_turret_collision_goal &&
- turret_position_wrapped <= max_turret_collision_goal);
+ AngleInRange(turret_position_wrapped, min_turret_collision_goal,
+ max_turret_collision_goal);
const bool turret_moving_forward = (turret_goal > turret_position);
@@ -129,8 +135,11 @@
}
min_turret_collision_goal_unwrapped =
UnwrapTurretAngle(min_turret_collision_goal, bounds_wraps);
+ // If we are checking the back intake, the max turret angle is on the wrap
+ // after the min, so add 1 to the number of wraps for it
const double max_turret_collision_goal_unwrapped =
- UnwrapTurretAngle(max_turret_collision_goal, bounds_wraps);
+ UnwrapTurretAngle(max_turret_collision_goal,
+ intake_front ? bounds_wraps : bounds_wraps + 1);
// Check if the closest unwrapped angles are going to be passed
const bool turret_moving_past_intake =
diff --git a/y2022/control_loops/superstructure/collision_avoidance.h b/y2022/control_loops/superstructure/collision_avoidance.h
index a64bbd3..b407409 100644
--- a/y2022/control_loops/superstructure/collision_avoidance.h
+++ b/y2022/control_loops/superstructure/collision_avoidance.h
@@ -19,6 +19,10 @@
// Returns the absolute angle given the wrapped angle and number of wraps.
double UnwrapTurretAngle(double wrapped, int wraps);
+// Checks if theta is between theta_min and theta_max. Expects all angles to be
+// wrapped from 0 to 2pi
+bool AngleInRange(double theta, double theta_min, double theta_max);
+
// 1. Prevent the turret from moving if the intake is up
// 2. If the intake is up, drop it so it is not in the way
// 3. Move the turret to the desired position.
@@ -49,7 +53,8 @@
static constexpr double kMaxCollisionZoneFrontTurret =
M_PI + kCollisionZoneTurret;
- static constexpr double kMinCollisionZoneBackTurret = -kCollisionZoneTurret;
+ static constexpr double kMinCollisionZoneBackTurret =
+ (2.0 * M_PI) - kCollisionZoneTurret;
static constexpr double kMaxCollisionZoneBackTurret = kCollisionZoneTurret;
// Maximum position of the intake to avoid collisions
diff --git a/y2022/control_loops/superstructure/collision_avoidance_test.cc b/y2022/control_loops/superstructure/collision_avoidance_test.cc
index adf672b..44dee44 100644
--- a/y2022/control_loops/superstructure/collision_avoidance_test.cc
+++ b/y2022/control_loops/superstructure/collision_avoidance_test.cc
@@ -416,4 +416,12 @@
}
}
+// Test that AngleInRange works correctly for wrapped angles
+TEST(AngleTest, AngleInRange) {
+ EXPECT_TRUE(AngleInRange(0.5, 0.4, 0.6));
+ EXPECT_TRUE(AngleInRange(0, (2.0 * M_PI) - 0.2, 0.2));
+ EXPECT_FALSE(AngleInRange(0, (2.0 * M_PI) - 0.2, (2.0 * M_PI) - 0.1));
+ EXPECT_TRUE(AngleInRange(0.5, (2.0 * M_PI) - 0.1, 0.6));
+}
+
} // namespace y2022::control_loops::superstructure::testing
diff --git a/y2022/vision/geometry.h b/y2022/vision/geometry.h
index 75a6496..31e8629 100644
--- a/y2022/vision/geometry.h
+++ b/y2022/vision/geometry.h
@@ -106,16 +106,18 @@
return std::atan2(p_prime.y, p_prime.x);
}
+ // Expects all angles to be from 0 to 2pi
+ // TODO(milind): handle wrapping
+ static inline bool AngleInRange(double theta, double theta_min,
+ double theta_max) {
+ return (
+ (theta >= theta_min && theta <= theta_max) ||
+ (theta_min > theta_max && (theta >= theta_min || theta <= theta_max)));
+ }
+
inline bool InAngleRange(cv::Point2d p, double theta_min,
double theta_max) const {
- const double theta = AngleOf(p);
-
- // Handle the case if the bounds wrap around 2pi
- const double max_diff = aos::math::NormalizeAngle(theta_max - theta);
- const double min_diff = aos::math::NormalizeAngle(theta - theta_min);
-
- return ((theta == theta_max) || (theta == theta_min) ||
- (std::signbit(min_diff) == std::signbit(max_diff)));
+ return AngleInRange(AngleOf(p), theta_min, theta_max);
}
private:
diff --git a/y2022/vision/geometry_test.cc b/y2022/vision/geometry_test.cc
index 9d2159b..0a5a759 100644
--- a/y2022/vision/geometry_test.cc
+++ b/y2022/vision/geometry_test.cc
@@ -81,10 +81,7 @@
const cv::Point2d kZeroPoint = {c->center.x + c->radius, c->center.y};
EXPECT_NEAR(c->AngleOf(kZeroPoint), 0.0, 1e-5);
- EXPECT_TRUE(
- c->InAngleRange(kZeroPoint, -2.1 * 2.0 * M_PI, -1.9 * 2.0 * M_PI));
- EXPECT_TRUE(
- c->InAngleRange(kZeroPoint, 1.9 * 2.0 * M_PI, 2.1 * 2.0 * M_PI));
+ EXPECT_TRUE(c->InAngleRange(kZeroPoint, (2.0 * M_PI) - 0.1, 0.1));
EXPECT_EQ(c->DistanceTo(kZeroPoint), 0.0);
// Test the distance to another point
@@ -101,6 +98,14 @@
auto c = Circle::Fit({{-6.0, 3.2}, {-3.0, 2.0}, {-6.0, 3.2}});
EXPECT_FALSE(c.has_value());
}
+ // Test if angles are in ranges
+ {
+ EXPECT_TRUE(Circle::AngleInRange(0.5, 0.4, 0.6));
+ EXPECT_TRUE(Circle::AngleInRange(0, (2.0 * M_PI) - 0.2, 0.2));
+ EXPECT_FALSE(
+ Circle::AngleInRange(0, (2.0 * M_PI) - 0.2, (2.0 * M_PI) - 0.1));
+ EXPECT_TRUE(Circle::AngleInRange(0.5, (2.0 * M_PI) - 0.1, 0.6));
+ }
}
} // namespace y2022::vision::testing