Add collision avoidance
Change-Id: I2f8d3fcd544eedba568d86fdbc8a317f1fde5f46
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index c7127cc..a2b7a9e 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -24,10 +24,103 @@
constexpr double kZeroingVoltage = 4.0;
} // namespace
+// ///// CollisionAvoidance /////
+
+void CollisionAvoidance::UpdateGoal(double shoulder_angle_goal,
+ double wrist_angle_goal,
+ double intake_angle_goal) {
+ double shoulder_angle = arm_->shoulder_angle();
+ double wrist_angle = arm_->wrist_angle();
+ double intake_angle = intake_->angle();
+
+ // TODO(phil): This may need tuning to account for bounciness in the limbs or
+ // some other thing that I haven't thought of. At the very least,
+ // incorporating a small safety margin makes writing test cases much easier
+ // since you can directly compare statuses against the constants in the
+ // CollisionAvoidance class.
+ constexpr double kSafetyMargin = 0.01; // radians
+
+ // Avoid colliding the shooter with the frame.
+ // If the shoulder is below a certain angle or we want to move it below
+ // that angle, then the shooter has to stay level to the ground. Otherwise,
+ // it will crash into the frame.
+ if (shoulder_angle < kMinShoulderAngleForHorizontalShooter ||
+ shoulder_angle_goal < kMinShoulderAngleForHorizontalShooter) {
+ wrist_angle_goal = 0.0;
+
+ // Make sure that we don't move the shoulder below a certain angle until
+ // the wrist is level with the ground.
+ if (::std::abs(wrist_angle) > kMaxWristAngleForSafeArmStowing) {
+ shoulder_angle_goal =
+ ::std::max(shoulder_angle_goal,
+ kMinShoulderAngleForHorizontalShooter + kSafetyMargin);
+ }
+ }
+
+ // Is the arm where it could interfere with the intake right now?
+ bool shoulder_is_in_danger =
+ (shoulder_angle < kMinShoulderAngleForIntakeInterference &&
+ shoulder_angle > kMaxShoulderAngleUntilSafeIntakeStowing);
+
+ // Is the arm moving into collision zone from above?
+ bool shoulder_moving_into_danger_from_above =
+ (shoulder_angle >= kMinShoulderAngleForIntakeInterference &&
+ shoulder_angle_goal <= kMinShoulderAngleForIntakeInterference);
+
+ // Is the arm moving into collision zone from below?
+ bool shoulder_moving_into_danger_from_below =
+ (shoulder_angle <= kMaxShoulderAngleUntilSafeIntakeStowing &&
+ shoulder_angle_goal >= kMaxShoulderAngleUntilSafeIntakeStowing);
+
+ // Avoid colliding the arm with the intake.
+ if (shoulder_is_in_danger || shoulder_moving_into_danger_from_above ||
+ shoulder_moving_into_danger_from_below) {
+ // If the arm could collide with the intake, we make sure to move the
+ // intake out of the way. The arm has priority.
+ intake_angle_goal =
+ ::std::min(intake_angle_goal,
+ kMaxIntakeAngleBeforeArmInterference - kSafetyMargin);
+
+ // Don't let the shoulder move into the collision area until the intake is
+ // out of the way.
+ if (intake_angle > kMaxIntakeAngleBeforeArmInterference) {
+ const double kHalfwayPointBetweenSafeZones =
+ (kMinShoulderAngleForIntakeInterference +
+ kMaxShoulderAngleUntilSafeIntakeStowing) /
+ 2.0;
+
+ if (shoulder_angle >= kHalfwayPointBetweenSafeZones) {
+ // The shoulder is closer to being above the collision area. Move it up
+ // there.
+ shoulder_angle_goal =
+ ::std::max(shoulder_angle_goal,
+ kMinShoulderAngleForIntakeInterference + kSafetyMargin);
+ } else {
+ // The shoulder is closer to being below the collision zone (i.e. in
+ // stowing/intake position), keep it there for now.
+ shoulder_angle_goal =
+ ::std::min(shoulder_angle_goal,
+ kMaxShoulderAngleUntilSafeIntakeStowing - kSafetyMargin);
+ }
+ }
+ }
+
+ // Send the possibly adjusted goals to the components.
+ arm_->set_unprofiled_goal(shoulder_angle_goal, wrist_angle_goal);
+ intake_->set_unprofiled_goal(intake_angle_goal);
+}
+
+constexpr double CollisionAvoidance::kMinShoulderAngleForHorizontalShooter;
+constexpr double CollisionAvoidance::kMinShoulderAngleForIntakeInterference;
+constexpr double CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference;
+constexpr double CollisionAvoidance::kMaxWristAngleForSafeArmStowing;
+constexpr double CollisionAvoidance::kMaxShoulderAngleUntilSafeIntakeStowing;
+
Superstructure::Superstructure(
control_loops::SuperstructureQueue *superstructure_queue)
: aos::controls::ControlLoop<control_loops::SuperstructureQueue>(
- superstructure_queue) {}
+ superstructure_queue),
+ collision_avoidance_(&intake_, &arm_) {}
bool Superstructure::IsArmNear(double shoulder_tolerance,
double wrist_tolerance) {
@@ -81,16 +174,6 @@
}
}
-constexpr double Superstructure::kShoulderMiddleAngle;
-constexpr double Superstructure::kLooseTolerance;
-constexpr double Superstructure::kIntakeUpperClear;
-constexpr double Superstructure::kIntakeLowerClear;
-constexpr double Superstructure::kShoulderUpAngle;
-constexpr double Superstructure::kShoulderLanded;
-constexpr double Superstructure::kTightTolerance;
-constexpr double Superstructure::kWristAlmostLevel;
-constexpr double Superstructure::kShoulderWristClearAngle;
-
void Superstructure::RunIteration(
const control_loops::SuperstructureQueue::Goal *unsafe_goal,
const control_loops::SuperstructureQueue::Position *position,
@@ -330,9 +413,15 @@
intake_.AdjustProfile(unsafe_goal->max_angular_velocity_intake,
unsafe_goal->max_angular_acceleration_intake);
- arm_.set_unprofiled_goal(unsafe_goal->angle_shoulder,
- unsafe_goal->angle_wrist);
- intake_.set_unprofiled_goal(unsafe_goal->angle_intake);
+ if (collision_avoidance_enabled()) {
+ collision_avoidance_.UpdateGoal(unsafe_goal->angle_shoulder,
+ unsafe_goal->angle_wrist,
+ unsafe_goal->angle_intake);
+ } else {
+ arm_.set_unprofiled_goal(unsafe_goal->angle_shoulder,
+ unsafe_goal->angle_wrist);
+ intake_.set_unprofiled_goal(unsafe_goal->angle_intake);
+ }
}
// ESTOP if we hit any of the limits. It is safe(ish) to hit the limits
@@ -404,6 +493,16 @@
last_state_ = state_before_switch;
}
+constexpr double Superstructure::kShoulderMiddleAngle;
+constexpr double Superstructure::kLooseTolerance;
+constexpr double Superstructure::kIntakeUpperClear;
+constexpr double Superstructure::kIntakeLowerClear;
+constexpr double Superstructure::kShoulderUpAngle;
+constexpr double Superstructure::kShoulderLanded;
+constexpr double Superstructure::kTightTolerance;
+constexpr double Superstructure::kWristAlmostLevel;
+constexpr double Superstructure::kShoulderWristClearAngle;
+
} // namespace superstructure
} // namespace control_loops
} // namespace y2016
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index a860f09..346a33b 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -15,11 +15,72 @@
namespace control_loops {
namespace superstructure {
namespace testing {
+class SuperstructureTest_RespectsRange_Test;
class SuperstructureTest_DisabledGoalTest_Test;
class SuperstructureTest_ArmZeroingErrorTest_Test;
class SuperstructureTest_IntakeZeroingErrorTest_Test;
+class SuperstructureTest_UpperHardstopStartup_Test;
} // namespace testing
+// Helper class to prevent parts from crashing into each other. The parts in
+// question here are: the frame, the arm (plus shooter), and the intake.
+// Assumptions:
+// - The shoulder, the wrist, and intake are horizontal when at angle 0.
+// - The arm (i.e. shoulder) and shooter (i.e. wrist) are stored when they are
+// both at zero degrees.
+// - The intake at angle 0 is in a position to help get a ball in the robot.
+// - The intake at angle PI is in a "stowed" position. In other words, it is
+// folded over the arm and shooter when they are also in a stowed position.
+// - The shooter must remain horizontal when the arm is folding into the robot.
+// Otherwise, the shooter will collide with the frame.
+// - The arm has priority over the intake. If the arm wants to move in such a
+// way that interferes with the intake's movement then the intake must move
+// out of the way.
+class CollisionAvoidance {
+ public:
+ // Set up collision avoidance for an arm and intake.
+ CollisionAvoidance(Intake *intake, Arm *arm) : intake_(intake), arm_(arm) {}
+
+ // This function accepts goals for the intake and the arm and modifies them
+ // in such a way that collisions between all the different parts of the robot
+ // are avoided. The modified goals are then sent to the arm and intake as
+ // unprofiled goals.
+ void UpdateGoal(double shoulder_angle_goal, double wrist_angle_goal,
+ double intake_angle_goal);
+
+ // TODO(phil): Verify that these numbers actually make sense. Someone needs
+ // to verify these either on the CAD or the real robot.
+
+ // The shoulder angle (in radians) below which the shooter must be in a
+ // stowing position. In other words the wrist must be at angle zero if the
+ // shoulder is below this angle.
+ static constexpr double kMinShoulderAngleForHorizontalShooter = 0.1;
+
+ // The shoulder angle (in radians) below which the arm as a whole has the
+ // potential to interfere with the intake.
+ static constexpr double kMinShoulderAngleForIntakeInterference = M_PI / 3.0;
+
+ // The intake angle (in radians) above which the intake can interfere (i.e.
+ // collide) with the arm and/or shooter.
+ static constexpr double kMaxIntakeAngleBeforeArmInterference = M_PI / 2.0;
+
+ // The maximum absolute angle (in radians) that the wrist must be below in
+ // order for the shouler to be allowed to move below
+ // kMinShoulderAngleForHorizontalShooter. In other words, only allow the arm
+ // to move down into the belly pan if the shooter is horizontal, ready to
+ // also be placed into the belly pan.
+ static constexpr double kMaxWristAngleForSafeArmStowing = 0.01;
+
+ // The shoulder angle (in radians) below which the intake can safely move
+ // into the collision zone. This is necessary when the robot wants to fold up
+ // completely (i.e. stow the arm, shooter, and intake).
+ static constexpr double kMaxShoulderAngleUntilSafeIntakeStowing = 0.01;
+
+ private:
+ Intake *intake_;
+ Arm *arm_;
+};
+
class Superstructure
: public ::aos::controls::ControlLoop<control_loops::SuperstructureQueue> {
public:
@@ -100,6 +161,9 @@
static double MoveButKeepAbove(double reference_angle, double current_angle,
double move_distance);
+ // Returns true if collision avoidance is turned on. False if not.
+ bool collision_avoidance_enabled() { return collision_avoidance_enabled_; }
+
protected:
virtual void RunIteration(
const control_loops::SuperstructureQueue::Goal *unsafe_goal,
@@ -111,9 +175,16 @@
friend class testing::SuperstructureTest_DisabledGoalTest_Test;
friend class testing::SuperstructureTest_ArmZeroingErrorTest_Test;
friend class testing::SuperstructureTest_IntakeZeroingErrorTest_Test;
+ friend class testing::SuperstructureTest_RespectsRange_Test;
+ friend class testing::SuperstructureTest_UpperHardstopStartup_Test;
Intake intake_;
Arm arm_;
+ CollisionAvoidance collision_avoidance_;
+
+ // NOTE: Only touch this if you absolutely know what you're doing!
+ bool collision_avoidance_enabled_ = true;
+
State state_ = UNINITIALIZED;
State last_state_ = UNINITIALIZED;
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index 14dc0e1..0ab4eb7 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -117,6 +117,11 @@
pot_encoder_wrist_.Initialize(start_pos, kNoiseScalar);
}
+ // Must be called after any changes to InitializeShoulderPosition.
+ void InitializeAbsoluteWristPosition(double start_pos) {
+ InitializeRelativeWristPosition(start_pos - arm_plant_->X(0, 0));
+ }
+
// Sends a queue message with the position.
void SendPositionMessage() {
::aos::ScopedMessagePtr<control_loops::SuperstructureQueue::Position>
@@ -237,6 +242,31 @@
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();
@@ -281,6 +311,10 @@
superstructure_plant_.wrist_angular_velocity());
EXPECT_LE(-peak_wrist_velocity_,
superstructure_plant_.wrist_angular_velocity());
+
+ if (check_for_collisions_) {
+ VerifyAbsenceOfCollisions();
+ }
}
}
@@ -301,6 +335,8 @@
}
void set_peak_wrist_velocity(double value) { peak_wrist_velocity_ = value; }
+ bool check_for_collisions_ = true;
+
// Create a new instance of the test queue so that it invalidates the queue
// that it points to. Otherwise, we will have a pointed to
// shared memory that is no longer valid.
@@ -364,6 +400,10 @@
// Tests that the loop doesn't try and go beyond the physical range of the
// mechanisms.
TEST_F(SuperstructureTest, RespectsRange) {
+ // Turn off collision avoidance for this test.
+ superstructure_.collision_avoidance_enabled_ = false;
+ check_for_collisions_ = false;
+
// Set some ridiculous goals to test upper limits.
ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(M_PI * 10)
@@ -376,7 +416,6 @@
.max_angular_velocity_wrist(20)
.max_angular_acceleration_wrist(20)
.Send());
-
RunForTime(Time::InSeconds(10));
// Check that we are near our soft limit.
@@ -443,6 +482,9 @@
// Tests that starting at the lower hardstops doesn't cause an abort.
TEST_F(SuperstructureTest, LowerHardstopStartup) {
+ // Don't check for collisions for this test.
+ check_for_collisions_ = false;
+
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange.lower);
superstructure_plant_.InitializeShoulderPosition(
@@ -462,6 +504,10 @@
// Tests that starting at the upper hardstops doesn't cause an abort.
TEST_F(SuperstructureTest, UpperHardstopStartup) {
+ // Turn off collision avoidance for this test.
+ superstructure_.collision_avoidance_enabled_ = false;
+ check_for_collisions_ = false;
+
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange.upper);
superstructure_plant_.InitializeShoulderPosition(
@@ -487,6 +533,7 @@
constants::Values::kShoulderRange.upper);
superstructure_plant_.InitializeRelativeWristPosition(
constants::Values::kWristRange.upper);
+
ASSERT_TRUE(
superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(constants::Values::kIntakeRange.lower + 0.3)
@@ -507,6 +554,9 @@
// Tests that the internal goals don't change while disabled.
TEST_F(SuperstructureTest, DisabledGoalTest) {
+ // Don't check for collisions for this test.
+ check_for_collisions_ = false;
+
ASSERT_TRUE(
superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(constants::Values::kIntakeRange.lower + 0.03)
@@ -536,6 +586,9 @@
// Tests that the integrators works.
TEST_F(SuperstructureTest, IntegratorTest) {
+ // Don't check for collisions for this test.
+ check_for_collisions_ = false;
+
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange.lower);
superstructure_plant_.InitializeShoulderPosition(
@@ -843,6 +896,65 @@
VerifyNearGoal();
}
+// Make sure that the intake moves out of the way when the arm wants to move
+// into a shooting position.
+TEST_F(SuperstructureTest, AvoidCollisionWhenMovingArmFromStart) {
+ superstructure_plant_.InitializeIntakePosition(
+ constants::Values::kIntakeRange.upper);
+ superstructure_plant_.InitializeShoulderPosition(0.0);
+ superstructure_plant_.InitializeAbsoluteWristPosition(0.0);
+
+ ASSERT_TRUE(
+ superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(constants::Values::kIntakeRange.upper) // stowed
+ .angle_shoulder(M_PI / 4.0) // in the collision area
+ .angle_wrist(M_PI / 2.0) // down
+ .Send());
+
+ RunForTime(Time::InSeconds(10));
+
+ superstructure_queue_.status.FetchLatest();
+ ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
+
+ // The intake should be out of the way despite being told to move to stowing.
+ EXPECT_LT(superstructure_queue_.status->intake.angle, M_PI);
+ EXPECT_LT(superstructure_queue_.status->intake.angle,
+ constants::Values::kIntakeRange.upper);
+ EXPECT_LT(superstructure_queue_.status->intake.angle,
+ CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference);
+
+ // The arm should have reached its goal.
+ EXPECT_NEAR(M_PI / 4.0, superstructure_queue_.status->shoulder.angle, 0.001);
+ EXPECT_NEAR(M_PI / 2.0, superstructure_queue_.status->wrist.angle, 0.001);
+}
+
+// Make sure that the shooter holds itself level when the arm comes down
+// into a stowing/intaking position.
+TEST_F(SuperstructureTest, AvoidCollisionWhenStowingArm) {
+ superstructure_plant_.InitializeIntakePosition(0.0); // forward
+ superstructure_plant_.InitializeShoulderPosition(M_PI / 2.0); // up
+ superstructure_plant_.InitializeAbsoluteWristPosition(M_PI); // forward
+
+ ASSERT_TRUE(superstructure_queue_.goal.MakeWithBuilder()
+ .angle_intake(0.0)
+ .angle_shoulder(0.0)
+ .angle_wrist(M_PI) // intentionally asking for forward
+ .Send());
+
+ RunForTime(Time::InSeconds(10));
+
+ superstructure_queue_.status.FetchLatest();
+ ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
+
+ // The intake should be in intaking position, as asked.
+ EXPECT_NEAR(0.0, superstructure_queue_.status->intake.angle, 0.001);
+
+ // The shoulder and wrist should both be at zero degrees (i.e.
+ // stowed/intaking position).
+ EXPECT_NEAR(0.0, superstructure_queue_.status->shoulder.angle, 0.001);
+ EXPECT_NEAR(0.0, superstructure_queue_.status->wrist.angle, 0.001);
+}
+
} // namespace testing
} // namespace superstructure
} // namespace control_loops