Break collision detection into CollisionAvoidance
This lets us re-use the code for when we want to write the "slow
running" state in the superstructure.
Change-Id: Ie61437293ff2f27453623b4d63f3d63067fb1d24
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index a2b7a9e..d4b5578 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -110,6 +110,34 @@
intake_->set_unprofiled_goal(intake_angle_goal);
}
+bool CollisionAvoidance::collided() const {
+ return collided_with_given_angles(arm_->shoulder_angle(), arm_->wrist_angle(),
+ intake_->angle());
+}
+
+bool CollisionAvoidance::collided_with_given_angles(double shoulder_angle,
+ double wrist_angle,
+ double intake_angle) {
+ // The arm and the intake must not hit.
+ if (shoulder_angle >=
+ CollisionAvoidance::kMaxShoulderAngleUntilSafeIntakeStowing &&
+ shoulder_angle <=
+ CollisionAvoidance::kMinShoulderAngleForIntakeInterference &&
+ intake_angle > CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference) {
+ return true;
+ }
+
+ // The wrist must go back to zero when the shoulder is moving the arm into
+ // a stowed/intaking position.
+ if (shoulder_angle <
+ CollisionAvoidance::kMinShoulderAngleForHorizontalShooter &&
+ ::std::abs(wrist_angle) > kMaxWristAngleForSafeArmStowing) {
+ return true;
+ }
+
+ return false;
+}
+
constexpr double CollisionAvoidance::kMinShoulderAngleForHorizontalShooter;
constexpr double CollisionAvoidance::kMinShoulderAngleForIntakeInterference;
constexpr double CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference;
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index 346a33b..25b7939 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -48,6 +48,17 @@
void UpdateGoal(double shoulder_angle_goal, double wrist_angle_goal,
double intake_angle_goal);
+ // Returns true if any of the limbs and frame are somehow currently
+ // interfering with one another. This is based purely on the angles that the
+ // limbs are reporting.
+ bool collided() const;
+
+ // Detects collision with the specified angles. This is especially useful for
+ // unit testing where we have proper ground truth for all the angles.
+ static bool collided_with_given_angles(double shoulder_angle,
+ double wrist_angle,
+ double intake_angle);
+
// TODO(phil): Verify that these numbers actually make sense. Someone needs
// to verify these either on the CAD or the real robot.
@@ -162,7 +173,10 @@
double move_distance);
// Returns true if collision avoidance is turned on. False if not.
- bool collision_avoidance_enabled() { return collision_avoidance_enabled_; }
+ bool collision_avoidance_enabled() const { return collision_avoidance_enabled_; }
+
+ // Returns true if anything is currently considered "collided".
+ bool collided() const { return collision_avoidance_.collided(); }
protected:
virtual void RunIteration(
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index 0ab4eb7..ea666de 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -242,31 +242,6 @@
TickTime();
}
- void VerifyAbsenceOfCollisions() {
- superstructure_queue_.status.FetchLatest();
- ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
-
- double intake_angle = superstructure_queue_.status->intake.angle;
- double shoulder_angle = superstructure_queue_.status->shoulder.angle;
- double wrist_angle = superstructure_queue_.status->wrist.angle;
-
- // The arm and the intake must not hit.
- if (shoulder_angle >=
- CollisionAvoidance::kMaxShoulderAngleUntilSafeIntakeStowing &&
- shoulder_angle <=
- CollisionAvoidance::kMinShoulderAngleForIntakeInterference) {
- EXPECT_LT(intake_angle,
- CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference);
- }
-
- // The wrist must go back to zero when the shoulder is moving the arm into
- // a stowed/intaking position.
- if (shoulder_angle <
- CollisionAvoidance::kMaxShoulderAngleUntilSafeIntakeStowing) {
- EXPECT_NEAR(0.0, wrist_angle, 0.01);
- }
- }
-
// Runs iterations until the specified amount of simulated time has elapsed.
void RunForTime(const Time &run_for, bool enabled = true) {
const auto start_time = Time::Now();
@@ -313,11 +288,22 @@
superstructure_plant_.wrist_angular_velocity());
if (check_for_collisions_) {
- VerifyAbsenceOfCollisions();
+ ASSERT_TRUE(superstructure_.collision_avoidance_enabled());
+ ASSERT_FALSE(collided());
}
}
}
+ // Helper function to quickly check if either the estimation detected a
+ // collision or if there's a collision using ground-truth plant values.
+ bool collided() const {
+ return superstructure_.collided() ||
+ CollisionAvoidance::collided_with_given_angles(
+ superstructure_plant_.shoulder_angle(),
+ superstructure_plant_.wrist_angle(),
+ superstructure_plant_.intake_angle());
+ }
+
// Runs iterations while watching the average acceleration per cycle and
// making sure it doesn't exceed the provided bounds.
void set_peak_intake_acceleration(double value) {
@@ -534,12 +520,11 @@
superstructure_plant_.InitializeRelativeWristPosition(
constants::Values::kWristRange.upper);
- ASSERT_TRUE(
- superstructure_queue_.goal.MakeWithBuilder()
- .angle_intake(constants::Values::kIntakeRange.lower + 0.3)
- .angle_shoulder(constants::Values::kShoulderRange.lower + 0.3)
- .angle_wrist(constants::Values::kWristRange.lower + 0.3)
- .Send());
+ ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(constants::Values::kIntakeRange.lower + 0.3)
+ .angle_shoulder(constants::Values::kShoulderRange.lower + 0.3)
+ .angle_wrist(constants::Values::kWristRange.lower + 0.3)
+ .Send());
RunForTime(Time::InSeconds(15));
EXPECT_EQ(Superstructure::RUNNING, superstructure_.state());
@@ -955,6 +940,26 @@
EXPECT_NEAR(0.0, superstructure_queue_.status->wrist.angle, 0.001);
}
+// Make sure that we can properly detect a collision.
+TEST_F(SuperstructureTest, DetectAndFixCollisionBetweenArmAndIntake) {
+ superstructure_plant_.InitializeIntakePosition(
+ constants::Values::kIntakeRange.upper); // upper limit
+ superstructure_plant_.InitializeShoulderPosition(M_PI * 0.25); // 45° up
+ superstructure_plant_.InitializeAbsoluteWristPosition(0); // level
+
+ // Since we're explicitly checking for collisions, we don't want to fail the
+ // test because of collisions.
+ check_for_collisions_ = false;
+
+ RunForTime(Time::InSeconds(1));
+ ASSERT_TRUE(collided());
+
+ // Make sure that the collision avoidance will properly move the limbs out of
+ // the collision area.
+ RunForTime(Time::InSeconds(10));
+ ASSERT_FALSE(collided());
+}
+
} // namespace testing
} // namespace superstructure
} // namespace control_loops