Tweak collision avoidance
We want to prioritize moving the wrist where the big suction cup is
clear as opposed to randomly when near 0.
Also, we don't need to lift the elevator if the wrist is pretty far up
to pull the intake in.
Change-Id: Iba34152a69e563894f96439908a2fea71aab6d8f
diff --git a/y2019/control_loops/superstructure/collision_avoidance.cc b/y2019/control_loops/superstructure/collision_avoidance.cc
index ed6787f..c999cac 100644
--- a/y2019/control_loops/superstructure/collision_avoidance.cc
+++ b/y2019/control_loops/superstructure/collision_avoidance.cc
@@ -52,7 +52,8 @@
}
// Elevator is down so the intake has to be at either extreme.
- if (elevator_position < kElevatorClearIntakeHeight) {
+ if (elevator_position < kElevatorClearIntakeHeight &&
+ wrist_position > kWristMaxAngle) {
if (intake_position < kIntakeOutAngle && intake_position > kIntakeInAngle) {
return true;
}
@@ -78,7 +79,9 @@
// If the elevator is low enough, we also can't transition the wrist.
if (elevator_position < kElevatorClearHeight) {
// Figure out which side the wrist is on and stay there.
- if (wrist_position < 0.0) {
+ if (wrist_position < (2.0 * kWristElevatorCollisionMinAngle +
+ kWristElevatorCollisionMaxAngle) /
+ 3.0) {
update_max_wrist_goal(kWristElevatorCollisionMinAngle - kEpsWrist);
} else {
update_min_wrist_goal(kWristElevatorCollisionMaxAngle + kEpsWrist);
@@ -97,7 +100,8 @@
// If the elevator is too low, the intake can't transition from in to out or
// back.
- if (elevator_position < kElevatorClearIntakeHeight) {
+ if (elevator_position < kElevatorClearIntakeHeight &&
+ wrist_position > kWristElevatorCollisionMaxAngle) {
// Figure out if the intake is in our out and keep it there.
if (intake_position < kIntakeMiddleAngle) {
update_max_intake_goal(kIntakeInAngle - kEpsIntake);
@@ -110,10 +114,18 @@
clear_min_elevator_goal();
// If the intake is within the collision range, don't let the elevator down.
- if (intake_position > kIntakeInAngle && intake_position < kIntakeOutAngle) {
+ if (intake_position > kIntakeInAngle && intake_position < kIntakeOutAngle &&
+ wrist_position > kWristElevatorCollisionMaxAngle) {
update_min_elevator_goal(kElevatorClearIntakeHeight + kEps);
}
+ // If the intake is in the collision range and the elevator is down, don't let
+ // the wrist go far down.
+ if (intake_position > kIntakeInAngle && intake_position < kIntakeOutAngle &&
+ elevator_position < kElevatorClearIntakeHeight) {
+ update_max_wrist_goal(kWristMaxAngle - kEpsWrist);
+ }
+
// If the wrist is within the elevator collision range, don't let the elevator
// go down.
if (wrist_position > kWristElevatorCollisionMinAngle &&
@@ -141,7 +153,8 @@
// If we need to move the intake, we've got to shove the elevator up. The
// intake is already constrained so it can't hit anything until it's clear.
- if (intake_needs_to_move && wrist_position > 0) {
+ if (intake_needs_to_move &&
+ wrist_position > kWristElevatorCollisionMaxAngle) {
update_min_elevator_goal(kElevatorClearIntakeHeight + kEps);
}
// If we need to move the wrist, we've got to shove the elevator up too. The
diff --git a/y2019/control_loops/superstructure/collision_avoidance.h b/y2019/control_loops/superstructure/collision_avoidance.h
index 5a8a772..ca29e7c 100644
--- a/y2019/control_loops/superstructure/collision_avoidance.h
+++ b/y2019/control_loops/superstructure/collision_avoidance.h
@@ -50,8 +50,6 @@
min_elevator_goal_ = ::std::max(min_elevator_goal, min_elevator_goal_);
}
- // TODO(sabina): set all the numbers to correctly match the robot.
-
// Height above which we can move the wrist freely.
static constexpr double kElevatorClearHeight = 0.35;
@@ -61,8 +59,8 @@
static constexpr double kElevatorClearIntakeHeight = 0.4;
// Angle constraints for the wrist when below kElevatorClearDownHeight
- static constexpr double kWristMaxAngle = M_PI / 2.0;
- static constexpr double kWristMinAngle = -M_PI / 2.0;
+ static constexpr double kWristMaxAngle = M_PI / 2.0 + 0.05;
+ static constexpr double kWristMinAngle = -M_PI / 2.0 - 0.05;
// Angles outside of which the intake is fully clear of the wrist.
static constexpr double kIntakeOutAngle = 0.3;
diff --git a/y2019/control_loops/superstructure/collision_avoidance_tests.cc b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
index 17f9fdc..918bc5e 100644
--- a/y2019/control_loops/superstructure/collision_avoidance_tests.cc
+++ b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
@@ -260,24 +260,25 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// out.
- unsafe_goal.wrist.unsafe_goal = avoidance.kWristMinAngle;
+ unsafe_goal.wrist.unsafe_goal =
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist;
unsafe_goal.elevator.unsafe_goal = 0.0;
unsafe_goal.intake.unsafe_goal =
(avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
// sets the status position messgaes to be have the elevator at the half way
// with the intake in and the wrist middle front
- status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ status.wrist.position = avoidance.kWristMinAngle + avoidance.kEpsWrist;
status.elevator.position = 0.45;
status.intake.position =
(avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
Iterate();
- ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
- ASSERT_NEAR((avoidance.kElevatorClearIntakeHeight + avoidance.kEps),
+ EXPECT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
+ EXPECT_NEAR((avoidance.kElevatorClearIntakeHeight + avoidance.kEps),
status.elevator.position, 0.001);
- ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
+ EXPECT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
}
// Fix Collision Wrist in Elevator
@@ -304,13 +305,13 @@
// Fix Collision Wrist in Intake
TEST_F(CollisionAvoidanceTests, FixWristCollision) {
// changes the goals
- unsafe_goal.wrist.unsafe_goal = avoidance.kWristMinAngle + avoidance.kEpsWrist;
+ unsafe_goal.wrist.unsafe_goal = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
unsafe_goal.elevator.unsafe_goal = 0.0;
unsafe_goal.intake.unsafe_goal =
(avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
// sets the status position messgaes
- status.wrist.position = avoidance.kWristMinAngle + avoidance.kEpsWrist;
+ status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
status.elevator.position = 0.0;
status.intake.position =
(avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;