Added Collision Avoidance.
Prioritizes the intake and ensures no collisions.
Change-Id: I4d70097e5fe3a075e847b5c7481848b551e3c9ec
diff --git a/y2019/control_loops/superstructure/BUILD b/y2019/control_loops/superstructure/BUILD
index 5d07e17..453c680 100644
--- a/y2019/control_loops/superstructure/BUILD
+++ b/y2019/control_loops/superstructure/BUILD
@@ -39,3 +39,31 @@
"//aos/events:shm-event-loop",
],
)
+
+cc_library(
+ name = "collision_avoidance",
+ srcs = [
+ "collision_avoidance.cc",
+ ],
+ hdrs = [
+ "collision_avoidance.h",
+ ],
+ deps = [
+ ":superstructure_queue",
+ "//aos/controls:control_loop_queues",
+ "//frc971:constants",
+ ],
+)
+
+cc_test(
+ name = "collision_avoidance_tests",
+ srcs = [
+ "collision_avoidance_tests.cc",
+ ],
+ deps = [
+ ":collision_avoidance",
+ ":superstructure_queue",
+ "//aos:math",
+ "//aos/testing:googletest",
+ ],
+)
diff --git a/y2019/control_loops/superstructure/collision_avoidance.cc b/y2019/control_loops/superstructure/collision_avoidance.cc
new file mode 100644
index 0000000..7a26b90
--- /dev/null
+++ b/y2019/control_loops/superstructure/collision_avoidance.cc
@@ -0,0 +1,162 @@
+#include "y2019/control_loops/superstructure/collision_avoidance.h"
+
+#include <cmath>
+#include "y2019/control_loops/superstructure/superstructure.q.h"
+
+namespace y2019 {
+namespace control_loops {
+namespace superstructure {
+
+constexpr double CollisionAvoidance::kElevatorClearHeight;
+constexpr double CollisionAvoidance::kElevatorClearWristDownHeight;
+constexpr double CollisionAvoidance::kElevatorClearIntakeHeight;
+constexpr double CollisionAvoidance::kWristMaxAngle;
+constexpr double CollisionAvoidance::kWristMinAngle;
+constexpr double CollisionAvoidance::kIntakeOutAngle;
+constexpr double CollisionAvoidance::kIntakeInAngle;
+constexpr double CollisionAvoidance::kWristElevatorCollisionMinAngle;
+constexpr double CollisionAvoidance::kWristElevatorCollisionMaxAngle;
+constexpr double CollisionAvoidance::kEps;
+constexpr double CollisionAvoidance::kEpsIntake;
+constexpr double CollisionAvoidance::kEpsWrist;
+
+CollisionAvoidance::CollisionAvoidance() {
+ clear_min_wrist_goal();
+ clear_max_wrist_goal();
+ clear_min_elevator_goal();
+ clear_min_intake_goal();
+ clear_max_intake_goal();
+}
+
+bool CollisionAvoidance::IsCollided(const SuperstructureQueue::Status *status) {
+ const double wrist_position = status->wrist.position;
+ const double elevator_position = status->elevator.position;
+ const double intake_position = status->intake.position;
+
+ // Elevator is down, so the wrist can't be close to vertical.
+ if (elevator_position < kElevatorClearHeight) {
+ if (wrist_position < kWristElevatorCollisionMaxAngle &&
+ wrist_position > kWristElevatorCollisionMinAngle) {
+ return true;
+ }
+ }
+
+ // Elevator is down so wrist can't go below horizontal in either direction.
+ if (elevator_position < kElevatorClearWristDownHeight) {
+ if (wrist_position > kWristMaxAngle) {
+ return true;
+ }
+ if (wrist_position < kWristMinAngle) {
+ return true;
+ }
+ }
+
+ // Elevator is down so the intake has to be at either extreme.
+ if (elevator_position < kElevatorClearIntakeHeight) {
+ if (intake_position < kIntakeOutAngle && intake_position > kIntakeInAngle) {
+ return true;
+ }
+ }
+
+ // Nothing is hitting, we must be good.
+ return false;
+}
+
+void CollisionAvoidance::UpdateGoal(
+ const SuperstructureQueue::Status *status,
+ const SuperstructureQueue::Goal *unsafe_goal) {
+ const double wrist_position = status->wrist.position;
+ const double elevator_position = status->elevator.position;
+ const double intake_position = status->intake.position;
+
+ // Start with our constraints being wide open.
+ clear_max_wrist_goal();
+ clear_min_wrist_goal();
+ clear_max_intake_goal();
+ clear_min_intake_goal();
+
+ // 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) {
+ update_max_wrist_goal(kWristElevatorCollisionMinAngle - kEpsWrist);
+ } else {
+ update_min_wrist_goal(kWristElevatorCollisionMaxAngle + kEpsWrist);
+ }
+ }
+
+ // If the elevator is too low, the wrist needs to be above the clearance
+ // angles to avoid crashing the frame.
+ if (elevator_position < kElevatorClearWristDownHeight) {
+ update_min_wrist_goal(kWristMinAngle + kEpsWrist);
+ update_max_wrist_goal(kWristMaxAngle - kEpsWrist);
+ }
+
+ constexpr double kIntakeMiddleAngle =
+ (kIntakeOutAngle + kIntakeInAngle) / 2.0;
+
+ // If the elevator is too low, the intake can't transition from in to out or
+ // back.
+ if (elevator_position < kElevatorClearIntakeHeight) {
+ // Figure out if the intake is in our out and keep it there.
+ if (intake_position < kIntakeMiddleAngle) {
+ update_max_intake_goal(kIntakeInAngle - kEpsIntake);
+ } else {
+ update_min_intake_goal(kIntakeOutAngle + kEpsIntake);
+ }
+ }
+
+ // Start with an unconstrained elevator.
+ 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) {
+ update_min_elevator_goal(kElevatorClearIntakeHeight + kEps);
+ }
+
+ // If the wrist is within the elevator collision range, don't let the elevator
+ // go down.
+ if (wrist_position > kWristElevatorCollisionMinAngle &&
+ wrist_position < kWristElevatorCollisionMaxAngle) {
+ update_min_elevator_goal(kElevatorClearHeight + kEps);
+ }
+
+ // If the wrist is far enough down that we are going to hit the frame, don't
+ // let the elevator go too far down.
+ if (wrist_position > kWristMaxAngle || wrist_position < kWristMinAngle) {
+ update_min_elevator_goal(kElevatorClearWristDownHeight + kEps);
+ }
+
+ if (unsafe_goal) {
+ const double wrist_goal = unsafe_goal->wrist.angle;
+ const double intake_goal = unsafe_goal->intake.joint_angle;
+
+ // Compute if we need to move the intake.
+ const bool intake_needs_to_move = (intake_position < kIntakeMiddleAngle) ^
+ (intake_goal < kIntakeMiddleAngle);
+
+ // Compute if we need to move the wrist across 0.
+ const bool wrist_needs_to_move =
+ (wrist_position < 0.0) ^ (wrist_goal < 0.0);
+
+ // 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) {
+ update_min_elevator_goal(kElevatorClearIntakeHeight + kEps);
+ }
+ // If we need to move the wrist, we've got to shove the elevator up too. The
+ // wrist is already constrained so it can't hit anything until it's clear.
+ // If both the intake and wrist need to move, figure out which one will
+ // require the higher motion and move that.
+ if (wrist_needs_to_move) {
+ update_min_elevator_goal(kElevatorClearHeight + kEps);
+ }
+
+ // TODO(austin): We won't shove the elevator up if the wrist is asked to go
+ // down below horizontal. I think that's fine.
+ }
+}
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2019
diff --git a/y2019/control_loops/superstructure/collision_avoidance.h b/y2019/control_loops/superstructure/collision_avoidance.h
new file mode 100644
index 0000000..1ad9582
--- /dev/null
+++ b/y2019/control_loops/superstructure/collision_avoidance.h
@@ -0,0 +1,111 @@
+#ifndef Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_COLLISION_AVOIDANCE_H_
+#define Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_COLLISION_AVOIDANCE_H_
+
+#include <cmath>
+#include "aos/controls/control_loops.q.h"
+#include "frc971/constants.h"
+#include "y2019/control_loops/superstructure/superstructure.q.h"
+
+namespace y2019 {
+namespace control_loops {
+namespace superstructure {
+
+// CollisionAvoidance computes the min and max allowable ranges for various
+// subsystems to avoid collisions. It also shoves the elevator up to let the
+// intake go in and out, and to let the wrist switch sides.
+class CollisionAvoidance {
+ public:
+ CollisionAvoidance();
+
+ // Reports if the superstructure is collided.
+ bool IsCollided(const SuperstructureQueue::Status *status);
+
+ // Checks and alters goals to make sure they're safe.
+ // TODO(austin): Either we will have a unit delay because this has to happen
+ // after the controls, or we need to be more clever about restructuring.
+ void UpdateGoal(const SuperstructureQueue::Status *status,
+ const SuperstructureQueue::Goal *unsafe_goal);
+
+ // Returns the goals to give to the respective control loops in
+ // superstructure.
+ double min_wrist_goal() const { return min_wrist_goal_; }
+ double max_wrist_goal() const { return max_wrist_goal_; }
+ double min_elevator_goal() const { return min_elevator_goal_; }
+ double min_intake_goal() const { return min_intake_goal_; }
+ double max_intake_goal() const { return max_intake_goal_; }
+
+ void update_max_wrist_goal(double max_wrist_goal) {
+ max_wrist_goal_ = ::std::min(max_wrist_goal, max_wrist_goal_);
+ }
+ void update_min_wrist_goal(double min_wrist_goal) {
+ min_wrist_goal_ = ::std::max(min_wrist_goal, min_wrist_goal_);
+ }
+ void update_max_intake_goal(double max_intake_goal) {
+ max_intake_goal_ = ::std::min(max_intake_goal, max_intake_goal_);
+ }
+ void update_min_intake_goal(double min_intake_goal) {
+ min_intake_goal_ = ::std::max(min_intake_goal, min_intake_goal_);
+ }
+ void update_min_elevator_goal(double min_elevator_goal) {
+ 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.5;
+
+ // Height above which we can move the wrist down.
+ static constexpr double kElevatorClearWristDownHeight = 0.3;
+ // Height the carriage needs to be above to move the intake.
+ 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;
+
+ // Angles outside of which the intake is fully clear of the wrist.
+ static constexpr double kIntakeOutAngle = M_PI / 6.0;
+ static constexpr double kIntakeInAngle = -M_PI / 3.0;
+
+ // Angles within which we will crash the wrist into the elevator if the
+ // elevator is below kElevatorClearHeight.
+ static constexpr double kWristElevatorCollisionMinAngle = -M_PI / 4.0;
+ static constexpr double kWristElevatorCollisionMaxAngle = M_PI / 4.0;
+
+ // Tolerance for the elevator.
+ static constexpr double kEps = 0.02;
+ // Tolerance for the intake.
+ static constexpr double kEpsIntake = 0.05;
+ // Tolerance for the wrist.
+ static constexpr double kEpsWrist = 0.05;
+
+ private:
+ void clear_min_wrist_goal() {
+ min_wrist_goal_ = -::std::numeric_limits<double>::infinity();
+ }
+ void clear_max_wrist_goal() {
+ max_wrist_goal_ = ::std::numeric_limits<double>::infinity();
+ }
+ void clear_min_elevator_goal() {
+ min_elevator_goal_ = -::std::numeric_limits<double>::infinity();
+ }
+ void clear_min_intake_goal() {
+ min_intake_goal_ = -::std::numeric_limits<double>::infinity();
+ }
+ void clear_max_intake_goal() {
+ max_intake_goal_ = ::std::numeric_limits<double>::infinity();
+ }
+
+ double min_wrist_goal_;
+ double max_wrist_goal_;
+ double min_elevator_goal_;
+ double min_intake_goal_;
+ double max_intake_goal_;
+};
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2019
+
+#endif // Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_COLLISION_AVOIDANCE_H_
diff --git a/y2019/control_loops/superstructure/collision_avoidance_tests.cc b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
new file mode 100644
index 0000000..834a251
--- /dev/null
+++ b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
@@ -0,0 +1,281 @@
+#include "y2019/control_loops/superstructure/collision_avoidance.h"
+
+#include "aos/commonmath.h"
+#include "gtest/gtest.h"
+#include "y2019/control_loops/superstructure/superstructure.q.h"
+
+namespace y2019 {
+namespace control_loops {
+namespace superstructure {
+namespace testing {
+
+/*
+Test List:
+ FullClockwiseRotationFromBottomBackIntakeIn
+ QuarterClockwiseRotationFromMiddleFrontIntakeOut
+ QuarterClockwiseRotationFromMiddleFrontIntakeMiddle
+ QuarterClockwiseRotationFromMiddleFrontIntakeMoving
+
+ FullCounterClockwiseRotationFromBottomFrontIntakeIn
+ QuarterCounterClockwiseRotationFromBottomFrontIntakeOut
+ QuarterCounterClockwiseRotationFromBottomFrontIntakeMiddle
+ QuarterCounterClockwiseRotationFromBottomFrontIntakeMoving
+*/
+
+class CollisionAvoidanceTests : public ::testing::Test {
+ public:
+ void Iterate() {
+ SuperstructureQueue::Goal safe_goal;
+ while (true) {
+ avoidance.UpdateGoal(&status, &unsafe_goal);
+
+ EXPECT_FALSE(avoidance.IsCollided(&status));
+ safe_goal.wrist.angle =
+ ::aos::Clip(unsafe_goal.wrist.angle, avoidance.min_wrist_goal(),
+ avoidance.max_wrist_goal());
+
+ safe_goal.elevator.height = ::std::max(unsafe_goal.elevator.height,
+ avoidance.min_elevator_goal());
+
+ safe_goal.intake.joint_angle =
+ ::aos::Clip(unsafe_goal.intake.joint_angle,
+ avoidance.min_intake_goal(), avoidance.max_intake_goal());
+
+ LimitedMove(&status.wrist.position, safe_goal.wrist.angle);
+ LimitedMove(&status.elevator.position, safe_goal.elevator.height);
+ LimitedMove(&status.intake.position, safe_goal.intake.joint_angle);
+ if (IsMoving()) {
+ break;
+ }
+ past_status = status;
+ }
+ }
+
+ bool IsMoving() {
+ if ((past_status.wrist.position == status.wrist.position) &&
+ (past_status.elevator.position == status.elevator.position) &&
+ (past_status.intake.position == status.intake.position)) {
+ return true;
+ } else {
+ return false;
+ }
+ }
+
+ // provide goals and status messages
+ SuperstructureQueue::Goal unsafe_goal;
+ SuperstructureQueue::Status status;
+ SuperstructureQueue::Status past_status;
+
+ protected:
+ // setup for all tests
+ CollisionAvoidance avoidance;
+
+ void CheckGoals() {
+ // check to see if we reached the goals
+ ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.elevator.height, status.elevator.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+ }
+
+ private:
+ void LimitedMove(float *position, double goal) {
+ if (*position + kIterationMove < goal) {
+ *position += kIterationMove;
+ } else if (*position - kIterationMove > goal) {
+ *position -= kIterationMove;
+ } else {
+ *position = goal;
+ }
+ }
+
+ static constexpr double kIterationMove = 0.001;
+};
+
+// It is trying to rotate from far back to front low.
+TEST_F(CollisionAvoidanceTests, FullClockwiseRotationFromBottomBackIntakeIn) {
+ // 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
+ // in.
+ unsafe_goal.wrist.angle = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+
+ // sets the status position messgaes to be have the elevator at the bottom
+ // with the intake in and the wrist low back
+ status.wrist.position = avoidance.kWristMinAngle + avoidance.kEpsWrist;
+ status.elevator.position = 0.0;
+ status.intake.position = avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+
+ Iterate();
+
+ CheckGoals();
+}
+
+// It is trying to rotate from the front middle to front bottom.
+TEST_F(CollisionAvoidanceTests,
+ QuarterClockwiseRotationFromMiddleFrontIntakeOut) {
+ // 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.angle = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ // 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 * 2.0);
+ status.elevator.position = 0.0;
+ status.intake.position = avoidance.kIntakeOutAngle;
+
+ Iterate();
+
+ CheckGoals();
+}
+// It is trying to rotate from the front middle to front bottom.
+TEST_F(CollisionAvoidanceTests,
+ QuarterClockwiseRotationFromMiddleFrontIntakeMiddle) {
+ // 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
+ // in.
+ status.wrist.position = avoidance.kWristMaxAngle / 2.0;
+ status.elevator.position = 0.5;
+ status.intake.position =
+ (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
+ unsafe_goal.wrist.angle = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ Iterate();
+
+ CheckGoals();
+}
+
+// It is trying to rotate from front low to far back.
+TEST_F(CollisionAvoidanceTests,
+ FullCounterClockwiseRotationFromBottomBackIntakeIn) {
+ // sets the status position messgaes to be have the elevator at the bottom
+ // with the intake in and the wrist low back
+ status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
+ status.elevator.position = 0.0;
+ status.intake.position = avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+
+ // 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
+ // in.
+ unsafe_goal.wrist.angle = avoidance.kWristMinAngle + avoidance.kEpsWrist;
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+
+ Iterate();
+
+ CheckGoals();
+}
+
+// It is trying to rotate from the front bottom to front middle.
+TEST_F(CollisionAvoidanceTests,
+ QuarterCounterClockwiseRotationFromMiddleFrontIntakeOut) {
+ // 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.angle =
+ avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0);
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ // 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.elevator.position = 0.0;
+ status.intake.position = avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ Iterate();
+
+ CheckGoals();
+}
+
+// It is trying to rotate from the front bottom to front middle.
+TEST_F(CollisionAvoidanceTests,
+ QuarterCounterClockwiseRotationFromBottomFrontIntakeMiddle) {
+ // 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.angle =
+ avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0);
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ // 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.elevator.position = 0.5;
+ status.intake.position =
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+
+ Iterate();
+
+ CheckGoals();
+}
+
+// Unreasonable Elevator Goal
+TEST_F(CollisionAvoidanceTests, UnreasonableElevatorGoal) {
+ // 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.angle = 4.0;
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ // 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.elevator.position = 0.45;
+ status.intake.position = avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+
+ Iterate();
+
+ ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+ ASSERT_NEAR((avoidance.kElevatorClearWristDownHeight + avoidance.kEps),
+ status.elevator.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+}
+
+// Unreasonable Wrist Goal
+TEST_F(CollisionAvoidanceTests, UnreasonableWristGoal) {
+ // 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.angle = avoidance.kWristMinAngle;
+ unsafe_goal.elevator.height = 0.0;
+ unsafe_goal.intake.joint_angle =
+ (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.elevator.position = 0.45;
+ status.intake.position =
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+
+ Iterate();
+
+ ASSERT_NEAR(unsafe_goal.wrist.angle, status.wrist.position, 0.001);
+ ASSERT_NEAR((avoidance.kElevatorClearIntakeHeight + avoidance.kEps),
+ status.elevator.position, 0.001);
+ ASSERT_NEAR(unsafe_goal.intake.joint_angle, status.intake.position, 0.001);
+}
+
+} // namespace testing
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2019