Improving line following
This makes it so that we try to drive the robot to have the edge of the
hatch panel hit the edge of the velcro on the target.
Also contains a few other tweaks that happened in the interim.
Change-Id: I2d2bffa2b88423b9ac94107daaa0602892ff4da2
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