Merge changes Ie9558d75,I9c32f14b,I8f40462d,I61a8d277,Ibbf4b7e2, ...
* changes:
Recalibrate comp bot after SFR 2019
Make stilts come back down fast.
Tune elevator setpoints for Comp
Add 4 localizer reset buttons for each orientation in each HP station
Always re-encode the image in image_streamer
Calibrated bot a couple of times
Add right autonomous mode
Fixed control loop driving in drivetrain_input
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 1b37612..58ce2a1 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -257,7 +257,7 @@
dt_closedloop_.SetGoal(*goal);
dt_openloop_.SetGoal(*goal);
dt_spline_.SetGoal(*goal);
- dt_line_follow_.SetGoal(*goal);
+ dt_line_follow_.SetGoal(monotonic_now, *goal);
}
dt_openloop_.Update(robot_state().voltage_battery);
@@ -272,7 +272,7 @@
dt_spline_.Update(output != NULL && controller_type == 2, trajectory_state);
- dt_line_follow_.Update(trajectory_state);
+ dt_line_follow_.Update(monotonic_now, trajectory_state);
switch (controller_type) {
case 0:
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 74be414..7f778f7 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -67,6 +67,14 @@
float x;
float y;
float theta;
+ // Current lateral offset from line pointing straight out of the target.
+ float offset;
+ // Current distance from the plane of the target, in meters.
+ float distance_to_target;
+ // Current goal heading.
+ float goal_theta;
+ // Current relative heading.
+ float rel_theta;
};
queue_group DrivetrainQueue {
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index b00ab3f..839f1b8 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -744,7 +744,7 @@
// Should have run off the end of the target, running along the y=x line.
EXPECT_LT(1.0, drivetrain_motor_plant_.GetPosition().x());
EXPECT_NEAR(drivetrain_motor_plant_.GetPosition().x(),
- drivetrain_motor_plant_.GetPosition().y(), 1e-4);
+ drivetrain_motor_plant_.GetPosition().y(), 0.1);
}
// Tests that the line follower will not run and defer to regular open-loop
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
index b5417af..b6e4947 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
@@ -11,7 +11,6 @@
namespace drivetrain {
constexpr double LineFollowDrivetrain::kMaxVoltage;
-constexpr double LineFollowDrivetrain::kPolyN;
namespace {
::Eigen::Matrix<double, 3, 3> AContinuous(
@@ -99,71 +98,34 @@
target_selector_(target_selector),
U_(0, 0) {}
-double LineFollowDrivetrain::GoalTheta(
- const ::Eigen::Matrix<double, 5, 1> &state) const {
- // TODO(james): Consider latching the sign of the goal so that the driver
- // can back up without the robot trying to turn around...
- // On the other hand, we may just want to force the driver to take over
- // entirely if they need to backup.
- int sign = ::aos::sign(goal_velocity_);
- // Given a Nth degree polynomial with just a single term that
- // has its minimum (with a slope of zero) at (0, 0) and passing
- // through (x0, y0), we will have:
- // y = a * x^N
- // a = y0 / x0^N
- // And the slope of the tangent line at (x0, y0) will be
- // N * a * x0^(N-1)
- // = N * y0 / x0^N * x0^(N-1)
- // = N * y0 / x0
- // Giving a heading of
- // atan2(-N * y0, -x0)
- // where we add the negative signs to make things work out properly when we
- // are trying to drive forwards towards zero. We reverse the sign of both
- // terms if we want to drive backwards.
- return ::std::atan2(-sign * kPolyN * state.y(), -sign * state.x());
-}
-
-double LineFollowDrivetrain::GoalThetaDot(
- const ::Eigen::Matrix<double, 5, 1> &state) const {
- // theta = atan2(-N * y0, -x0)
- // Note: d(atan2(p(t), q(t)))/dt
- // = (p(t) * q'(t) - q(t) * p'(t)) / (p(t)^2 + q(t)^2)
- // Note: x0' = cos(theta) * v, y0' = sin(theta) * v
- // Note that for the sin/cos calculations we discard the
- // negatives in the arctangent to make the signs work out (the
- // dtheta/dt needs to be correct as we travel along the path). This
- // also corresponds with the fact that thetadot is agnostic towards
- // whether the robot is driving forwards or backwards, so long as it is
- // trying to drive towards the target.
- // Note: sin(theta) = sin(atan2(N * y0, x0))
- // = N * y0 / sqrt(N^2 * y0^2 + x0^2)
- // Note: cos(theta) = cos(atan2(N * y0, x0))
- // = x0 / sqrt(N^2 * y0^2 + x0^2)
- // dtheta/dt
- // = (-x0 * (-N * y0') - -N * y0 * (-x0')) / (N^2 * y0^2 + x0^2)
- // = N * (x0 * v * sin(theta) - y0 * v * cos(theta)) / (N^2 * y0^2 + x0^2)
- // = N * v * (x0 * (N * y0) - y0 * (x0)) / (N^2 * y0^2 + x0^2)^1.5
- // = N * v * (N - 1) * x0 * y0 / (N^2 * y0^2 + x0^2)^1.5
- const double linear_vel = (state(3, 0) + state(4, 0)) / 2.0;
- const double x2 = ::std::pow(state.x(), 2);
- const double y2 = ::std::pow(state.y(), 2);
- const double norm = y2 * kPolyN * kPolyN + x2;
- // When we get too near the goal, avoid singularity in a sane manner.
- if (norm < 1e-3) {
- return 0.0;
- }
- return kPolyN * (kPolyN - 1) * linear_vel * state.x() * state.y() /
- (norm * ::std::sqrt(norm));
-}
-
void LineFollowDrivetrain::SetGoal(
+ ::aos::monotonic_clock::time_point now,
const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
// TODO(james): More properly calculate goal velocity from throttle.
- goal_velocity_ = goal.throttle;
+ goal_velocity_ = 4.0 * goal.throttle;
+ // The amount of time to freeze the target choice after the driver releases
+ // the button. Depending on the current state, we vary how long this timeout
+ // is so that we can avoid button glitches causing issues.
+ ::std::chrono::nanoseconds freeze_delay = ::std::chrono::seconds(0);
// Freeze the target once the driver presses the button; if we haven't yet
// confirmed a target when the driver presses the button, we will not do
// anything and report not ready until we have a target.
- freeze_target_ = goal.controller_type == 3;
+ if (goal.controller_type == 3) {
+ last_enable_ = now;
+ // If we already acquired a target, we want to keep track if it.
+ if (have_target_) {
+ freeze_delay = ::std::chrono::milliseconds(500);
+ // If we've had the target acquired for a while, the driver is probably
+ // happy with the current target selection, so we really want to keep it.
+ if (now - start_of_target_acquire_ > ::std::chrono::milliseconds(1000)) {
+ freeze_delay = ::std::chrono::milliseconds(2000);
+ }
+ }
+ }
+ freeze_target_ = now <= freeze_delay + last_enable_;
+ // Set an adjustment that lets the driver tweak the offset for where we place
+ // the target left/right.
+ side_adjust_ = -goal.wheel * 0.1;
}
bool LineFollowDrivetrain::SetOutput(
@@ -177,7 +139,48 @@
return have_target_;
}
+double LineFollowDrivetrain::GoalTheta(
+ const ::Eigen::Matrix<double, 5, 1> &abs_state, double relative_y_offset,
+ double velocity_sign) {
+ // Calculates the goal angle for the drivetrain given our position.
+ // The calculated goal will be such that a point disc_rad to one side of the
+ // drivetrain (the side depends on where we approach from) will end up hitting
+ // the plane of the target exactly disc_rad from the center of the target.
+ // This allows us to better approach targets in the 2019 game from an
+ // angle--radii of zero imply driving straight in.
+ const double disc_rad = target_selector_->TargetRadius();
+ // Depending on whether we are to the right or left of the target, we work off
+ // of a different side of the robot.
+ const double edge_sign = relative_y_offset > 0 ? 1.0 : -1.0;
+ // Note side_adjust which is the input from the driver's wheel to allow
+ // shifting the goal target left/right.
+ const double edge_offset = edge_sign * disc_rad - side_adjust_;
+ // The point that we are trying to get the disc to hit.
+ const Pose corner = Pose(&target_pose_, {0.0, edge_offset, 0.0}, 0.0);
+ // A pose for the current robot position that is square to the target.
+ Pose square_robot =
+ Pose({abs_state.x(), abs_state.y(), 0.0}, 0.0).Rebase(&corner);
+ // To prevent numerical issues, we limit x so that when the localizer isn't
+ // working properly and ends up driving past the target, we still get sane
+ // results.
+ square_robot.mutable_pos()->x() =
+ ::std::min(::std::min(square_robot.mutable_pos()->x(), -disc_rad), -0.01);
+ // Distance from the edge of the disc on the robot to the velcro we want to
+ // hit on the target.
+ const double dist_to_corner = square_robot.xy_norm();
+ // The following actually handles calculating the heading we need the robot to
+ // take (relative to the plane of the target).
+ const double alpha = ::std::acos(disc_rad / dist_to_corner);
+ const double heading_to_robot = edge_sign * square_robot.heading();
+ double theta = -edge_sign * (M_PI - alpha - (heading_to_robot - M_PI_2));
+ if (velocity_sign < 0) {
+ theta = ::aos::math::NormalizeAngle(theta + M_PI);
+ }
+ return theta;
+}
+
void LineFollowDrivetrain::Update(
+ ::aos::monotonic_clock::time_point now,
const ::Eigen::Matrix<double, 5, 1> &abs_state) {
// Because we assume the target selector may have some internal state (e.g.,
// not confirming a target until some time as passed), we should call
@@ -189,6 +192,8 @@
// target before.
if (!have_target_ && new_target) {
have_target_ = true;
+ start_of_target_acquire_ = now;
+ velocity_sign_ = goal_velocity_ >= 0.0 ? 1.0 : -1.0;
target_pose_ = target_selector_->TargetPose();
}
} else {
@@ -197,12 +202,15 @@
have_target_ = new_target;
if (have_target_) {
target_pose_ = target_selector_->TargetPose();
+ velocity_sign_ = goal_velocity_ >= 0.0 ? 1.0 : -1.0;
}
}
-
// Get the robot pose in the target coordinate frame.
- const Pose relative_robot_pose = Pose({abs_state.x(), abs_state.y(), 0.0},
- abs_state(2, 0)).Rebase(&target_pose_);
+ relative_pose_ = Pose({abs_state.x(), abs_state.y(), 0.0}, abs_state(2, 0))
+ .Rebase(&target_pose_);
+ double goal_theta =
+ GoalTheta(abs_state, relative_pose_.rel_pos().y(), velocity_sign_);
+
// Always force a slight negative X, so that the driver can continue to drive
// past zero if they want.
// The "slight negative" needs to be large enough that we won't force
@@ -210,28 +218,38 @@
// line--because, in practice, the driver should never be trying to drive to a
// target where they are significantly off laterally at <0.1m from the target,
// this should not be a problem.
- const double relative_x = ::std::min(relative_robot_pose.rel_pos().x(), -0.1);
+ const double relative_x = ::std::min(relative_pose_.rel_pos().x(), -0.1);
const ::Eigen::Matrix<double, 5, 1> rel_state =
(::Eigen::Matrix<double, 5, 1>() << relative_x,
- relative_robot_pose.rel_pos().y(), relative_robot_pose.rel_theta(),
- abs_state(3, 0), abs_state(4, 0))
- .finished();
- ::Eigen::Matrix<double, 3, 1> controls_goal(
- GoalTheta(rel_state), goal_velocity_, GoalThetaDot(rel_state));
+ relative_pose_.rel_pos().y(), relative_pose_.rel_theta(),
+ abs_state(3, 0), abs_state(4, 0)).finished();
+ if (velocity_sign_ * goal_velocity_ < 0) {
+ goal_theta = rel_state(2, 0);
+ }
+ controls_goal_ << goal_theta, goal_velocity_, 0.0;
::Eigen::Matrix<double, 3, 1> controls_state;
controls_state(0, 0) = rel_state(2, 0);
controls_state.block<2, 1>(1, 0) =
dt_config_.Tlr_to_la() * rel_state.block<2, 1>(3, 0);
- ::Eigen::Matrix<double, 3, 1> controls_err = controls_goal - controls_state;
+ ::Eigen::Matrix<double, 3, 1> controls_err = controls_goal_ - controls_state;
// Because we are taking the difference of an angle, normaliez to [-pi, pi].
controls_err(0, 0) = ::aos::math::NormalizeAngle(controls_err(0, 0));
// TODO(james): Calculate the next goal so that we are properly doing
// feed-forwards.
::Eigen::Matrix<double, 2, 1> U_ff =
- Kff_ * (controls_goal - A_d_ * controls_goal);
+ Kff_ * (controls_goal_ - A_d_ * controls_goal_);
U_ = K_ * controls_err + U_ff;
- const double maxU = U_.lpNorm<::Eigen::Infinity>();
- U_ *= (maxU > kMaxVoltage) ? kMaxVoltage / maxU : 1.0;
+ const double maxU = ::std::max(U_(0, 0), U_(1, 0));
+ const double minU = ::std::min(U_(0, 0), U_(1, 0));
+ const double diffU = ::std::abs(U_(1, 0) - U_(0, 0));
+ const double maxAbsU = ::std::max(maxU, -minU);
+ if (diffU > 24.0) {
+ U_ *= (maxAbsU > kMaxVoltage) ? kMaxVoltage / maxAbsU : 1.0;
+ } else if (maxU > 12.0) {
+ U_ = U_ + U_.Ones() * (12.0 - maxU);
+ } else if (minU < -12.0) {
+ U_ = U_ - U_.Ones() * (minU + 12.0);
+ }
}
void LineFollowDrivetrain::PopulateStatus(
@@ -241,6 +259,11 @@
status->line_follow_logging.x = target_pose_.abs_pos().x();
status->line_follow_logging.y = target_pose_.abs_pos().y();
status->line_follow_logging.theta = target_pose_.abs_theta();
+ status->line_follow_logging.offset = relative_pose_.rel_pos().y();
+ status->line_follow_logging.distance_to_target =
+ -relative_pose_.rel_pos().x();
+ status->line_follow_logging.goal_theta = controls_goal_(0, 0);
+ status->line_follow_logging.rel_theta = relative_pose_.rel_theta();
}
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.h b/frc971/control_loops/drivetrain/line_follow_drivetrain.h
index de9ff5a..09b2080 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.h
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.h
@@ -17,25 +17,22 @@
// A drivetrain that permits a velocity input from the driver while controlling
// lateral motion.
-// TODO(james): Do we want to allow some driver input on lateral control? It may
-// just be too confusing to make work effectively, but it also wouldn't be too
-// hard to just allow the driver to specify an offset to the angle/angular
-// velocity.
class LineFollowDrivetrain {
public:
typedef TypedPose<double> Pose;
- LineFollowDrivetrain(
- const DrivetrainConfig<double> &dt_config,
- TargetSelectorInterface *target_selector);
+ LineFollowDrivetrain(const DrivetrainConfig<double> &dt_config,
+ TargetSelectorInterface *target_selector);
// Sets the current goal; the drivetrain queue contains throttle_velocity
// which is used to command overall robot velocity. The goal_pose is a Pose
- // representing where we are tring to go. This would typically be the Pose of
+ // representing where we are trying to go. This would typically be the Pose of
// a Target; the positive X-axis in the Pose's frame represents the direction
// we want to go (we approach the pose from the left-half plane).
- void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
+ void SetGoal(::aos::monotonic_clock::time_point now,
+ const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
// State: [x, y, theta, left_vel, right_vel]
- void Update(const ::Eigen::Matrix<double, 5, 1> &state);
+ void Update(::aos::monotonic_clock::time_point now,
+ const ::Eigen::Matrix<double, 5, 1> &state);
// Returns whether we set the output. If false, that implies that we do not
// yet have a target to track into and so some other drivetrain should take
// over.
@@ -48,24 +45,18 @@
// Nominal max voltage.
// TODO(james): Is there a config for this or anything?
static constexpr double kMaxVoltage = 12.0;
- // Degree of the polynomial to follow in. Should be strictly greater than 1.
- // A value of 1 would imply that we drive straight to the target (but not hit
- // it straight on, unless we happened to start right in front of it). A value
- // of zero would imply driving straight until we hit the plane of the target.
- // Values between 0 and 1 would imply hitting the target from the side.
- static constexpr double kPolyN = 3.0;
- static_assert(kPolyN > 1.0,
- "We want to hit targets straight on (see above comments).");
- double GoalTheta(const ::Eigen::Matrix<double, 5, 1> &state) const;
- double GoalThetaDot(const ::Eigen::Matrix<double, 5, 1> &state) const;
+ double GoalTheta(const ::Eigen::Matrix<double, 5, 1> &abs_state,
+ double relative_y_offset, double velocity_sign);
const DrivetrainConfig<double> dt_config_;
// TODO(james): These should probably be factored out somewhere.
+ // TODO(james): This controller is not actually asymptotically stable, due to
+ // the varying goal theta.
const ::Eigen::DiagonalMatrix<double, 3> Q_ =
(::Eigen::DiagonalMatrix<double, 3>().diagonal()
- << 1.0 / ::std::pow(0.01, 2),
- 1.0 / ::std::pow(0.1, 2), 1.0 / ::std::pow(10.0, 2))
+ << 1.0 / ::std::pow(0.1, 2),
+ 1.0 / ::std::pow(1.0, 2), 1.0 / ::std::pow(1.0, 2))
.finished()
.asDiagonal();
const ::Eigen::DiagonalMatrix<double, 2> R_ =
@@ -86,6 +77,20 @@
bool have_target_ = false;
Pose target_pose_;
double goal_velocity_ = 0.0;
+ // The amount to shift the center of the goal target side-to-side, to give the
+ // driver an input that lets them account for panels that are offset on the
+ // grabber.
+ double side_adjust_ = 0.0; // meters
+ double velocity_sign_ = 1.0;
+
+ // The last time at which we saw an enable button.
+ ::aos::monotonic_clock::time_point last_enable_;
+ // The time at which we first acquired the current target.
+ ::aos::monotonic_clock::time_point start_of_target_acquire_;
+ // Most recent relative pose to target, used for debugging.
+ Pose relative_pose_;
+ // Current goal state, used for debugging.
+ ::Eigen::Matrix<double, 3, 1> controls_goal_;
// Voltage output to apply
::Eigen::Matrix<double, 2, 1> U_;
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
index 65cf9bf..414ec81 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
@@ -53,8 +53,8 @@
::frc971::control_loops::DrivetrainQueue::Goal goal;
goal.throttle = driver_model_(state_);
goal.controller_type = freeze_target_ ? 3 : 0;
- drivetrain_.SetGoal(goal);
- drivetrain_.Update(state_);
+ drivetrain_.SetGoal(t_, goal);
+ drivetrain_.Update(t_, state_);
::frc971::control_loops::DrivetrainQueue::Output output;
EXPECT_EQ(expect_output_, drivetrain_.SetOutput(&output));
@@ -85,51 +85,26 @@
simulation_ur_.push_back(U(1, 0));
simulation_x_.push_back(state_.x());
simulation_y_.push_back(state_.y());
+ simulation_theta_.push_back(state_(3, 0));
}
// Check that left/right velocities are near zero and the absolute position is
// near that of the goal Pose.
void VerifyNearGoal() const {
- EXPECT_NEAR(goal_pose().abs_pos().x(), state_(0, 0), 1e-3);
- EXPECT_NEAR(goal_pose().abs_pos().y(), state_(1, 0), 1e-3);
- // state should be at 0 or PI (we should've come in striaght on or straight
- // backwards).
- const double angle_err =
- ::aos::math::DiffAngle(goal_pose().abs_theta(), state_(2, 0));
- const double angle_pi_err = ::aos::math::DiffAngle(angle_err, M_PI);
- EXPECT_LT(::std::min(::std::abs(angle_err), ::std::abs(angle_pi_err)),
- 1e-3);
+ EXPECT_NEAR(goal_pose().abs_pos().x(), state_(0, 0), 1e-2);
+ EXPECT_NEAR(goal_pose().abs_pos().y(), state_(1, 0), 1e-2);
// Left/right velocities are zero:
- EXPECT_NEAR(0.0, state_(3, 0), 1e-3);
- EXPECT_NEAR(0.0, state_(4, 0), 1e-3);
+ EXPECT_NEAR(0.0, state_(3, 0), 1e-2);
+ EXPECT_NEAR(0.0, state_(4, 0), 1e-2);
}
- void CheckGoalTheta(double x, double y, double v, double expected_theta,
- double expected_thetadot) {
+ double GoalTheta(double x, double y, double v, double throttle) {
+ ::frc971::control_loops::DrivetrainQueue::Goal goal;
+ goal.throttle = throttle;
+ drivetrain_.SetGoal(t_, goal);
::Eigen::Matrix<double, 5, 1> state;
state << x, y, 0.0, v, v;
- const double theta = drivetrain_.GoalTheta(state);
- const double thetadot = drivetrain_.GoalThetaDot(state);
- EXPECT_EQ(expected_theta, theta) << "x " << x << " y " << y << " v " << v;
- EXPECT_EQ(expected_thetadot, thetadot)
- << "x " << x << " y " << y << " v " << v;
- }
-
- void CheckGoalThetaDotAtState(double x, double y, double v) {
- ::Eigen::Matrix<double, 5, 1> state;
- state << x, y, 0.0, v, v;
- const double theta = drivetrain_.GoalTheta(state);
- const double thetadot = drivetrain_.GoalThetaDot(state);
- const double dx = v * ::std::cos(theta);
- const double dy = v * ::std::sin(theta);
- constexpr double kEps = 1e-5;
- state(0, 0) += dx * kEps;
- state(1, 0) += dy * kEps;
- const double next_theta = drivetrain_.GoalTheta(state);
- EXPECT_NEAR(thetadot, ::aos::math::DiffAngle(next_theta, theta) / kEps,
- 1e-4)
- << "theta: " << theta << " nexttheta: " << next_theta << " x " << x
- << " y " << y << " v " << v;
+ return drivetrain_.GoalTheta(state, y, throttle > 0.0 ? 1.0 : -1.0);
}
void TearDown() override {
@@ -137,6 +112,7 @@
matplotlibcpp::figure();
matplotlibcpp::plot(time_, simulation_ul_, {{"label", "ul"}});
matplotlibcpp::plot(time_, simulation_ur_, {{"label", "ur"}});
+ matplotlibcpp::plot(time_, simulation_theta_, {{"label", "theta"}});
matplotlibcpp::legend();
TypedPose<double> target = goal_pose();
@@ -194,18 +170,33 @@
::std::vector<double> simulation_ur_;
::std::vector<double> simulation_x_;
::std::vector<double> simulation_y_;
+ ::std::vector<double> simulation_theta_;
};
-TEST_F(LineFollowDrivetrainTest, ValidGoalThetaDot) {
+TEST_F(LineFollowDrivetrainTest, BasicGoalThetaCheck) {
for (double x : {1.0, 0.0, -1.0, -2.0, -3.0}) {
for (double y : {-3.0, -2.0, -1.0, 0.0, 1.0, 2.0, 3.0}) {
for (double v : {-3.0, -2.0, -1.0, 0.0, 1.0, 2.0, 3.0}) {
- if (x == 0.0 && y == 0.0) {
- // When x/y are zero, we can encounter singularities. The code should
- // just provide zeros in this case.
- CheckGoalTheta(x, y, v, 0.0, 0.0);
- } else {
- CheckGoalThetaDotAtState(x, y, v);
+ for (double throttle : {-1.0, 1.0}) {
+ target_selector_.set_target_radius(0.0);
+ const double zero_rad_theta = GoalTheta(x, y, v, throttle);
+ EXPECT_NEAR(
+ 0.0,
+ ::aos::math::DiffAngle((throttle > 0.0 ? M_PI : 0.0) +
+ ::std::atan2(y, ::std::min(-0.01, x)),
+ zero_rad_theta),
+ 1e-14);
+ target_selector_.set_target_radius(0.05);
+ const double small_rad_theta = GoalTheta(x, y, v, throttle);
+ if (y > 0) {
+ EXPECT_LT(small_rad_theta, zero_rad_theta);
+ } else if (y == 0) {
+ EXPECT_NEAR(0.0,
+ ::aos::math::DiffAngle(small_rad_theta, zero_rad_theta),
+ 1e-14);
+ } else {
+ EXPECT_GT(small_rad_theta, zero_rad_theta);
+ }
}
}
}
@@ -215,8 +206,12 @@
// If the driver commands the robot to keep going, we should just run straight
// off the end and keep going along the line:
TEST_F(LineFollowDrivetrainTest, RunOffEnd) {
- state_ << -1.0, 0.1, 0.0, 0.0, 0.0;
- driver_model_ = [](const ::Eigen::Matrix<double, 5, 1> &) { return 1.0; };
+ freeze_target_ = true;
+ state_ << -1.0, 0.0, 0.0, 0.0, 0.0;
+ // TODO(james): This test currently relies on the scalar on the throttle to
+ // velocity conversion being 4.0. This should probably be moved out into a
+ // config.
+ driver_model_ = [](const ::Eigen::Matrix<double, 5, 1> &) { return 0.25; };
RunForTime(chrono::seconds(10));
EXPECT_LT(6.0, state_.x());
EXPECT_NEAR(0.0, state_.y(), 1e-4);
@@ -250,7 +245,8 @@
set_goal_pose({{1.0, 1.0, 0.0}, M_PI_2});
RunForTime(::std::chrono::seconds(5));
- EXPECT_NEAR(0.0, state_.squaredNorm(), 1e-25);
+ EXPECT_NEAR(0.0, state_.squaredNorm(), 1e-25)
+ << "Expected state of zero, got: " << state_.transpose();
}
// Tests that when we freeze the controller without having acquired a target, we
@@ -268,9 +264,9 @@
// Now, provide a target:
target_selector_.set_has_target(true);
- set_goal_pose({{1.0, 1.0, 0.0}, M_PI_2});
+ set_goal_pose({{1.0, 2.0, 0.0}, M_PI_2});
driver_model_ = [this](const ::Eigen::Matrix<double, 5, 1> &state) {
- return (state.topRows<2>() - goal_pose().abs_pos().topRows<2>()).norm();
+ return -(state.y() - goal_pose().abs_pos().y());
};
expect_output_ = true;
@@ -289,10 +285,13 @@
: public LineFollowDrivetrainTest,
public ::testing::WithParamInterface<Pose> {};
TEST_P(LineFollowDrivetrainTargetParamTest, NonZeroTargetTest) {
+ freeze_target_ = true;
+ target_selector_.set_has_target(true);
// Start the state at zero and then put the target in a
state_.setZero();
driver_model_ = [this](const ::Eigen::Matrix<double, 5, 1> &state) {
- return (state.topRows<2>() - GetParam().abs_pos().topRows<2>()).norm();
+ return 0.2 *
+ (state.topRows<2>() - GetParam().abs_pos().topRows<2>()).norm();
};
set_goal_pose(GetParam());
RunForTime(chrono::seconds(10));
@@ -353,13 +352,13 @@
.finished()),
::testing::Values(
[](const ::Eigen::Matrix<double, 5, 1> &state) {
- return -5.0 * state.x();
+ return -1.0 * state.x();
},
[](const ::Eigen::Matrix<double, 5, 1> &state) {
- return 5.0 * state.x();
+ return 1.0 * state.x();
},
[](const ::Eigen::Matrix<double, 5, 1> &state) {
- return -1.0 * ::std::abs(state.x()) - 0.5 * state.x() * state.x();
+ return -0.25 * ::std::abs(state.x()) - 0.125 * state.x() * state.x();
})));
} // namespace testing