Add basic shooting-on-the-fly implementation
This makes a bunch of simplistic assumptions about how the dynamics of
the shot work, but it'll probably get us pretty close initially and is
easy to implement and understand as-is.
Change-Id: I2ee8fa9d2681e4dec0e490395027d8860f58e521
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index 8fb2ca1..d5468e9 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -43,11 +43,12 @@
if (joystick_state_fetcher_.Fetch()) {
alliance = joystick_state_fetcher_->alliance();
}
- const turret::Aimer::Mode mode =
+ const turret::Aimer::WrapMode mode =
(unsafe_goal != nullptr && unsafe_goal->shooting())
- ? turret::Aimer::Mode::kAvoidWrapping
- : turret::Aimer::Mode::kAvoidEdges;
- aimer_.Update(drivetrain_status_fetcher_.get(), alliance, mode);
+ ? turret::Aimer::WrapMode::kAvoidWrapping
+ : turret::Aimer::WrapMode::kAvoidEdges;
+ aimer_.Update(drivetrain_status_fetcher_.get(), alliance, mode,
+ turret::Aimer::ShotMode::kShootOnTheFly);
}
const flatbuffers::Offset<AimerStatus> aimer_status_offset =
diff --git a/y2020/control_loops/superstructure/turret/aiming.cc b/y2020/control_loops/superstructure/turret/aiming.cc
index 97331f3..7aaefac 100644
--- a/y2020/control_loops/superstructure/turret/aiming.cc
+++ b/y2020/control_loops/superstructure/turret/aiming.cc
@@ -10,6 +10,32 @@
using frc971::control_loops::Pose;
+// Shooting-on-the-fly concept:
+// The current way that we manage shooting-on-the fly endeavors to be reasonably
+// simple, until we get a chance to see how the actual dynamics play out.
+// Essentially, we assume that the robot's velocity will represent a constant
+// offset to the ball's velocity over the entire trajectory to the goal and
+// then offset the target that we are pointing at based on that.
+// Let us assume that, if the robot shoots while not moving, regardless of shot
+// distance, the ball's average speed-over-ground to the target will be a
+// constant s_shot (this implies that if the robot is driving straight towards
+// the target, the actual ball speed-over-ground will be greater than s_shot).
+// We will define things in the robot's coordinate frame. We will be shooting
+// at a target that is at position (target_x, target_y) in the robot frame. The
+// robot is travelling at (v_robot_x, v_robot_y). In order to shoot the ball,
+// we need to generate some virtual target (virtual_x, virtual_y) that we will
+// shoot at as if we were standing still. The total time-of-flight to that
+// target will be t_shot = norm2(virtual_x, virtual_y) / s_shot.
+// we will have virtual_x + v_robot_x * t_shot = target_x, and the same
+// for y. This gives us three equations and three unknowns (virtual_x,
+// virtual_y, and t_shot), and given appropriate assumptions, can be solved
+// analytically. However, doing so is obnoxious and given appropriate functions
+// for t_shot may not be feasible. As such, instead of actually solving the
+// equation analytically, we will use an iterative solution where we maintain
+// a current virtual target estimate. We start with this estimate as if the
+// robot is stationary. We then use this estimate to calculate t_shot, and
+// calculate the next value for the virtual target.
+
namespace {
// The overall length and width of the field, in meters.
constexpr double kFieldLength = 15.983;
@@ -30,6 +56,16 @@
// The amount (in meters) that the inner port is set back from the outer port.
constexpr double kInnerPortBackset = 0.743;
+// Average speed-over-ground of the ball on its way to the target. Our current
+// model assumes constant ball velocity regardless of shot distance.
+// TODO(james): Is this an appropriate model? For the outer port it should be
+// good enough that it doesn't really matter, but for the inner port it may be
+// more appropriate to do something more dynamic--however, it is not yet clear
+// how we would best estimate speed-over-ground given a hood angle + shooter
+// speed. Assuming a constant average speed over the course of the trajectory
+// should be reasonable, since all we are trying to do here is calculate an
+// overall time-of-flight (we don't actually care about the ball speed itself).
+constexpr double kBallSpeedOverGround = 15.0; // m/s
// Minimum distance that we must be from the inner port in order to attempt the
// shot--this is to account for the fact that if we are too close to the target,
@@ -62,6 +98,22 @@
fbb.Finish(builder.Finish());
return fbb.Release();
}
+
+// This implements the iteration in the described shooting-on-the-fly algorithm.
+// robot_pose: Current robot pose.
+// robot_velocity: Current robot velocity, in the absolute field frame.
+// target_pose: Absolute goal Pose.
+// current_virtual_pose: Current estimate of where we want to shoot at.
+Pose IterateVirtualGoal(const Pose &robot_pose,
+ const Eigen::Vector3d &robot_velocity,
+ const Pose &target_pose,
+ const Pose ¤t_virtual_pose) {
+ const double air_time =
+ current_virtual_pose.Rebase(&robot_pose).xy_norm() / kBallSpeedOverGround;
+ const Eigen::Vector3d virtual_target =
+ target_pose.abs_pos() - air_time * robot_velocity;
+ return Pose(virtual_target, target_pose.abs_theta());
+}
} // namespace
Pose InnerPortPose(aos::Alliance alliance) {
@@ -86,21 +138,13 @@
Aimer::Aimer() : goal_(MakePrefilledGoal()) {}
-void Aimer::Update(const Status *status, aos::Alliance alliance, Mode mode) {
+void Aimer::Update(const Status *status, aos::Alliance alliance,
+ WrapMode wrap_mode, ShotMode shot_mode) {
const Pose robot_pose({status->x(), status->y(), 0}, status->theta());
const Pose inner_port = InnerPortPose(alliance);
const Pose outer_port = OuterPortPose(alliance);
const Pose robot_pose_from_inner_port = robot_pose.Rebase(&inner_port);
- const double inner_port_angle = robot_pose_from_inner_port.heading();
- const double inner_port_distance = robot_pose_from_inner_port.xy_norm();
- aiming_for_inner_port_ =
- (std::abs(inner_port_angle) < kMaxInnerPortAngle) &&
- (inner_port_distance > kMinimumInnerPortShotDistance);
- const Pose goal =
- (aiming_for_inner_port_ ? inner_port : outer_port).Rebase(&robot_pose);
- const double heading_to_goal = goal.heading();
- CHECK(status->has_localizer());
- distance_ = goal.xy_norm();
+
// TODO(james): This code should probably just be in the localizer and have
// xdot/ydot get populated in the status message directly... that way we don't
// keep duplicating this math.
@@ -111,25 +155,64 @@
drivetrain::GetDrivetrainConfig().Tlr_to_la() *
Eigen::Vector2d(status->localizer()->left_velocity(),
status->localizer()->right_velocity());
- // X and Y dot are negated because we are interested in the derivative of
- // (target_pos - robot_pos).
- const double xdot = -linear_angular(0) * std::cos(status->theta());
- const double ydot = -linear_angular(0) * std::sin(status->theta());
- const double rel_x = goal.rel_pos().x();
- const double rel_y = goal.rel_pos().y();
+ const double xdot = linear_angular(0) * std::cos(status->theta());
+ const double ydot = linear_angular(0) * std::sin(status->theta());
+
+ const double inner_port_angle = robot_pose_from_inner_port.heading();
+ const double inner_port_distance = robot_pose_from_inner_port.xy_norm();
+ aiming_for_inner_port_ =
+ (std::abs(inner_port_angle) < kMaxInnerPortAngle) &&
+ (inner_port_distance > kMinimumInnerPortShotDistance);
+
+ // This code manages compensating the goal turret heading for the robot's
+ // current velocity, to allow for shooting on-the-fly.
+ // This works by solving for the correct turret angle numerically, since while
+ // we technically could do it analytically, doing so would both make it hard
+ // to make small changes (since it would force us to redo the math) and be
+ // error-prone since it'd be easy to make typos or other minor math errors.
+ Pose virtual_goal;
+ {
+ const Pose goal = aiming_for_inner_port_ ? inner_port : outer_port;
+ virtual_goal = goal;
+ if (shot_mode == ShotMode::kShootOnTheFly) {
+ for (int ii = 0; ii < 3; ++ii) {
+ virtual_goal =
+ IterateVirtualGoal(robot_pose, {xdot, ydot, 0}, goal, virtual_goal);
+ }
+ VLOG(1) << "Shooting-on-the-fly target position: "
+ << virtual_goal.abs_pos().transpose();
+ }
+ virtual_goal = virtual_goal.Rebase(&robot_pose);
+ }
+
+ const double heading_to_goal = virtual_goal.heading();
+ CHECK(status->has_localizer());
+ distance_ = virtual_goal.xy_norm();
+
+ // The following code all works to calculate what the rate of turn of the
+ // turret should be. The code only accounts for the rate of turn if we are
+ // aiming at a static target, which should be close enough to correct that it
+ // doesn't matter that it fails to account for the
+ // shooting-on-the-fly compensation.
+ const double rel_x = virtual_goal.rel_pos().x();
+ const double rel_y = virtual_goal.rel_pos().y();
const double squared_norm = rel_x * rel_x + rel_y * rel_y;
+
// If squared_norm gets to be too close to zero, just zero out the relevant
// term to prevent NaNs. Note that this doesn't address the chattering that
// would likely occur if we were to get excessively close to the target.
+ // Note that x and y terms are swapped relative to what you would normally see
+ // in the derivative of atan because xdot and ydot are the derivatives of
+ // robot_pos and we are working with the atan of (target_pos - robot_pos).
const double atan_diff = (squared_norm < 1e-3)
? 0.0
- : (rel_x * ydot - rel_y * xdot) / squared_norm;
+ : (rel_y * xdot - rel_x * ydot) / squared_norm;
// heading = atan2(relative_y, relative_x) - robot_theta
// dheading / dt = (rel_x * rel_y' - rel_y * rel_x') / (rel_x^2 + rel_y^2) - dtheta / dt
const double dheading_dt = atan_diff - linear_angular(1);
double range = kTurretRange;
- if (mode == Mode::kAvoidEdges) {
+ if (wrap_mode == WrapMode::kAvoidEdges) {
range -= 2.0 * kAntiWrapBuffer;
}
// Calculate a goal turret heading such that it is within +/- pi of the
diff --git a/y2020/control_loops/superstructure/turret/aiming.h b/y2020/control_loops/superstructure/turret/aiming.h
index 5fbbdc6..3b3071e 100644
--- a/y2020/control_loops/superstructure/turret/aiming.h
+++ b/y2020/control_loops/superstructure/turret/aiming.h
@@ -28,7 +28,7 @@
typedef frc971::control_loops::drivetrain::Status Status;
// Mode to run the aimer in, to control how we manage wrapping the turret
// angle.
- enum class Mode {
+ enum class WrapMode {
// Keep the turret as far away from the edges of the range of motion as
// reasonable, to minimize the odds that we will hit the hardstops once we
// start shooting.
@@ -38,9 +38,23 @@
// shooting.
kAvoidWrapping,
};
+
+ // Control modes for managing how we manage shooting on the fly.
+ enum class ShotMode {
+ // Don't do any shooting-on-the-fly compensation--just point straight at the
+ // target. Primarily used in tests.
+ kStatic,
+ // Do do shooting-on-the-fly compensation.
+ kShootOnTheFly,
+ };
+
Aimer();
- void Update(const Status *status, aos::Alliance alliance, Mode mode);
+
+ void Update(const Status *status, aos::Alliance alliance, WrapMode wrap_mode,
+ ShotMode shot_mode);
+
const Goal *TurretGoal() const { return &goal_.message(); }
+
// Returns the distance to the goal, in meters.
double DistanceToGoal() const { return distance_; }
diff --git a/y2020/control_loops/superstructure/turret/aiming_test.cc b/y2020/control_loops/superstructure/turret/aiming_test.cc
index 9bccfdd..ef81252 100644
--- a/y2020/control_loops/superstructure/turret/aiming_test.cc
+++ b/y2020/control_loops/superstructure/turret/aiming_test.cc
@@ -44,11 +44,12 @@
return fbb.Release();
}
- const Goal *Update(const StatusData &data,
- aos::Alliance alliance = aos::Alliance::kBlue,
- Aimer::Mode mode = Aimer::Mode::kAvoidEdges) {
+ const Goal *Update(
+ const StatusData &data, aos::Alliance alliance = aos::Alliance::kBlue,
+ Aimer::WrapMode wrap_mode = Aimer::WrapMode::kAvoidEdges,
+ Aimer::ShotMode shot_mode = Aimer::ShotMode::kShootOnTheFly) {
const auto buffer = MakeStatus(data);
- aimer_.Update(&buffer.message(), alliance, mode);
+ aimer_.Update(&buffer.message(), alliance, wrap_mode, shot_mode);
const Goal *goal = aimer_.TurretGoal();
EXPECT_TRUE(goal->ignore_profile());
return goal;
@@ -67,6 +68,7 @@
.angular = 0.0});
EXPECT_EQ(M_PI, goal->unsafe_goal());
EXPECT_EQ(0.0, goal->goal_velocity());
+ EXPECT_EQ(1.0, aimer_.DistanceToGoal());
goal = Update({.x = target.abs_pos().x() + 1.0,
.y = target.abs_pos().y() + 0.0,
.theta = 1.0,
@@ -81,6 +83,7 @@
.angular = 0.0});
EXPECT_EQ(-M_PI + 1.0, aos::math::NormalizeAngle(goal->unsafe_goal()));
EXPECT_EQ(0.0, goal->goal_velocity());
+ EXPECT_EQ(1.0, aimer_.DistanceToGoal());
// Test that we handle the case that where we are right on top of the target.
goal = Update({.x = target.abs_pos().x() + 0.0,
.y = target.abs_pos().y() + 0.0,
@@ -89,6 +92,7 @@
.angular = 0.0});
EXPECT_EQ(0.0, goal->unsafe_goal());
EXPECT_EQ(0.0, goal->goal_velocity());
+ EXPECT_EQ(0.0, aimer_.DistanceToGoal());
}
TEST_F(AimerTest, SpinningRobot) {
@@ -106,25 +110,68 @@
// the turret.
TEST_F(AimerTest, DrivingAwayFromTarget) {
const Pose target = OuterPortPose(aos::Alliance::kBlue);
+ // To keep the test simple, disable shooting on the fly so that the
+ // goal distance comes out in an easy to calculate number.
const Goal *goal = Update({.x = target.abs_pos().x() + 1.0,
.y = target.abs_pos().y() + 0.0,
.theta = 0.0,
.linear = 1.0,
- .angular = 0.0});
+ .angular = 0.0},
+ aos::Alliance::kBlue, Aimer::WrapMode::kAvoidEdges,
+ Aimer::ShotMode::kStatic);
EXPECT_EQ(M_PI, goal->unsafe_goal());
EXPECT_FLOAT_EQ(0.0, goal->goal_velocity());
+ EXPECT_EQ(1.0, aimer_.DistanceToGoal());
+ // Next, try with shooting-on-the-fly enabled--because we are driving straight
+ // towards the target, only the goal distance should be impacted.
+ goal = Update({.x = target.abs_pos().x() + 1.0,
+ .y = target.abs_pos().y() + 0.0,
+ .theta = 0.0,
+ .linear = 1.0,
+ .angular = 0.0},
+ aos::Alliance::kBlue, Aimer::WrapMode::kAvoidEdges,
+ Aimer::ShotMode::kShootOnTheFly);
+ EXPECT_EQ(M_PI, goal->unsafe_goal());
+ EXPECT_FLOAT_EQ(0.0, goal->goal_velocity());
+ EXPECT_LT(1.0001, aimer_.DistanceToGoal());
+ EXPECT_GT(1.1, aimer_.DistanceToGoal());
}
// Tests that when we drive perpendicular to the target, we do have to spin.
TEST_F(AimerTest, DrivingLateralToTarget) {
const Pose target = OuterPortPose(aos::Alliance::kBlue);
+ // To keep the test simple, disable shooting on the fly so that the
+ // goal_velocity comes out in an easy to calculate number.
const Goal *goal = Update({.x = target.abs_pos().x() + 0.0,
.y = target.abs_pos().y() + 1.0,
.theta = 0.0,
.linear = 1.0,
- .angular = 0.0});
+ .angular = 0.0},
+ aos::Alliance::kBlue, Aimer::WrapMode::kAvoidEdges,
+ Aimer::ShotMode::kStatic);
EXPECT_EQ(-M_PI_2, goal->unsafe_goal());
EXPECT_FLOAT_EQ(-1.0, goal->goal_velocity());
+ EXPECT_EQ(1.0, aimer_.DistanceToGoal());
+ // Next, test with shooting-on-the-fly enabled, The goal numbers should all be
+ // slightly offset due to the robot velocity.
+ goal = Update({.x = target.abs_pos().x() + 0.0,
+ .y = target.abs_pos().y() + 1.0,
+ .theta = 0.0,
+ .linear = 1.0,
+ .angular = 0.0},
+ aos::Alliance::kBlue, Aimer::WrapMode::kAvoidEdges,
+ Aimer::ShotMode::kShootOnTheFly);
+ // Confirm that the turret heading goal is less then -pi / 2, but not by too
+ // much.
+ EXPECT_GT(-M_PI_2 - 0.001, goal->unsafe_goal());
+ EXPECT_LT(-M_PI_2 - 0.1, goal->unsafe_goal());
+ // Similarly, the turret velocity goal should be a bit greater than -1.0,
+ // since the turret is no longer at exactly a right angle.
+ EXPECT_LT(-1.0, goal->goal_velocity());
+ EXPECT_GT(-0.95, goal->goal_velocity());
+ // And the distance to the goal should be a bit greater than 1.0.
+ EXPECT_LT(1.0001, aimer_.DistanceToGoal());
+ EXPECT_GT(1.1, aimer_.DistanceToGoal());
}
// Confirms that we will indeed shoot at the inner port when we have a good shot
@@ -177,18 +224,18 @@
.linear = 0.0,
.angular = 0.0};
const Goal *goal =
- Update(status, aos::Alliance::kBlue, Aimer::Mode::kAvoidWrapping);
+ Update(status, aos::Alliance::kBlue, Aimer::WrapMode::kAvoidWrapping);
EXPECT_EQ(M_PI, goal->unsafe_goal());
EXPECT_EQ(0.0, goal->goal_velocity());
constexpr double kUpperLimit = constants::Values::kTurretRange().upper;
// Move the robot to the upper limit with AvoidWrapping set--we should be at
// the upper limit and not wrapped.
status.theta = goal->unsafe_goal() - kUpperLimit;
- goal = Update(status, aos::Alliance::kBlue, Aimer::Mode::kAvoidWrapping);
+ goal = Update(status, aos::Alliance::kBlue, Aimer::WrapMode::kAvoidWrapping);
EXPECT_FLOAT_EQ(kUpperLimit, goal->unsafe_goal());
EXPECT_EQ(0.0, goal->goal_velocity());
// Enter kAvoidEdges mode--we should wrap around.
- goal = Update(status, aos::Alliance::kBlue, Aimer::Mode::kAvoidEdges);
+ goal = Update(status, aos::Alliance::kBlue, Aimer::WrapMode::kAvoidEdges);
// confirm that this test is actually testing something...
ASSERT_NE(aos::math::NormalizeAngle(kUpperLimit), kUpperLimit);
EXPECT_FLOAT_EQ(aos::math::NormalizeAngle(kUpperLimit), goal->unsafe_goal());