Merge changes Ifbe2348f,Ie5e236b3,I79499c1d,I82072de1
* changes:
Recalibrated back camera after tightening the bolts
Retuned setpoints for the elevator at Utah
Tune rejection score to work on the side of the cargo ship
Raise exposure slightly at Utah so we can see further
diff --git a/aos/vision/math/BUILD b/aos/vision/math/BUILD
index 3233bc9..0fbdf7a 100644
--- a/aos/vision/math/BUILD
+++ b/aos/vision/math/BUILD
@@ -13,6 +13,7 @@
],
deps = [
"//third_party/eigen",
+ "@com_google_ceres_solver//:ceres",
],
)
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index d4c765f..05889e4 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -106,6 +106,13 @@
drivetrain_motor_plant_.GetRightPosition(), 1e-3);
}
+ void VerifyNearPosition(double x, double y) {
+ my_drivetrain_queue_.status.FetchLatest();
+ auto actual = drivetrain_motor_plant_.GetPosition();
+ EXPECT_NEAR(actual(0), x, 1e-2);
+ EXPECT_NEAR(actual(1), y, 1e-2);
+ }
+
void VerifyNearSplineGoal() {
my_drivetrain_queue_.status.FetchLatest();
double expected_x = my_drivetrain_queue_.status->trajectory_logging.x;
@@ -720,6 +727,29 @@
VerifyNearSplineGoal();
}
+//Tests that a trajectory can be executed after it is planned.
+TEST_F(DrivetrainTest, SplineExecuteAfterPlan) {
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ goal->controller_type = 2;
+ goal->spline.spline_idx = 1;
+ goal->spline.spline_count = 1;
+ goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ goal->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+ goal.Send();
+ WaitForTrajectoryPlan();
+ RunForTime(chrono::milliseconds(2000));
+
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+ start_goal = my_drivetrain_queue_.goal.MakeMessage();
+ start_goal->controller_type = 2;
+ start_goal->spline_handle = 1;
+ start_goal.Send();
+ RunForTime(chrono::milliseconds(2000));
+
+ VerifyNearPosition(1.0, 1.0);
+}
+
// The LineFollowDrivetrain logic is tested in line_follow_drivetrain_test. This
// tests that the integration with drivetrain_lib worked properly.
TEST_F(DrivetrainTest, BasicLineFollow) {
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 0b6026d..a37ca0e 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -93,7 +93,7 @@
const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
current_spline_handle_ = goal.spline_handle;
// If told to stop, set the executing spline to an invalid index.
- if (current_spline_handle_ == 0) {
+ if (current_spline_handle_ == 0 && has_started_execution_) {
current_spline_idx_ = -1;
}
@@ -119,6 +119,7 @@
// Reset internal state to a trajectory start position.
current_xva_ = current_trajectory_->FFAcceleration(0);
current_xva_(1) = 0.0;
+ has_started_execution_ = false;
}
mutex_.Unlock();
}
@@ -133,6 +134,7 @@
::Eigen::Matrix<double, 2, 1> U_ff = ::Eigen::Matrix<double, 2, 1>::Zero();
if (!IsAtEnd() &&
current_spline_handle_ == current_spline_idx_) {
+ has_started_execution_ = true;
// TODO(alex): It takes about a cycle for the outputs to propagate to the
// motors. Consider delaying the output by a cycle.
U_ff = current_trajectory_->FFVoltage(current_xva_(0));
@@ -187,7 +189,7 @@
status->trajectory_logging.right_velocity = goal_state(4);
}
status->trajectory_logging.planning_state = static_cast<int8_t>(plan_state_.load());
- status->trajectory_logging.is_executing = !IsAtEnd();
+ status->trajectory_logging.is_executing = !IsAtEnd() && has_started_execution_;
status->trajectory_logging.goal_spline_handle = current_spline_handle_;
status->trajectory_logging.current_spline_idx = current_spline_idx_;
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index 3e47f5a..d70cb6d 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -70,6 +70,7 @@
int32_t current_spline_handle_ = 0; // Current spline told to excecute.
int32_t current_spline_idx_ = 0; // Current executing spline.
+ bool has_started_execution_ = false;
::std::unique_ptr<DistanceSpline> current_distance_spline_;
::std::unique_ptr<Trajectory> current_trajectory_;
diff --git a/y2019/constants.cc b/y2019/constants.cc
index 89fc009..ae6cb96 100644
--- a/y2019/constants.cc
+++ b/y2019/constants.cc
@@ -194,8 +194,8 @@
break;
case kPracticeTeamNumber:
- elevator_params->zeroing_constants.measured_absolute_position = 0.132216;
- elevator->potentiometer_offset = -0.022320 + 0.020567 - 0.022355 - 0.006497 + 0.019690 + 0.009151;
+ elevator_params->zeroing_constants.measured_absolute_position = 0.141526;
+ elevator->potentiometer_offset = -0.022320 + 0.020567 - 0.022355 - 0.006497 + 0.019690 + 0.009151 - 0.007513;
intake->zeroing_constants.measured_absolute_position = 1.756847;
diff --git a/y2019/vision/target_geometry.cc b/y2019/vision/target_geometry.cc
index 00375c1..efa2def 100644
--- a/y2019/vision/target_geometry.cc
+++ b/y2019/vision/target_geometry.cc
@@ -6,7 +6,6 @@
#include "aos/util/math.h"
-using ceres::NumericDiffCostFunction;
using ceres::CENTRAL;
using ceres::CostFunction;
using ceres::Problem;
@@ -55,77 +54,6 @@
left.inside, left.bottom}};
}
-Vector<2> Project(Vector<2> pt, const IntrinsicParams &intrinsics,
- const ExtrinsicParams &extrinsics) {
- const double y = extrinsics.y; // height
- const double z = extrinsics.z; // distance
- const double r1 = extrinsics.r1; // skew
- const double r2 = extrinsics.r2; // heading
- const double rup = intrinsics.mount_angle;
- const double rbarrel = intrinsics.barrel_mount;
- const double fl = intrinsics.focal_length;
-
- // Start by translating point in target-space to be at correct height.
- ::Eigen::Matrix<double, 1, 3> pts{pt.x(), pt.y() + y, 0.0};
-
- {
- // Rotate to compensate for skew angle, to get into a frame still at the
- // same (x, y) position as the target but rotated to be facing straight
- // towards the camera.
- const double theta = r1;
- const double s = sin(theta);
- const double c = cos(theta);
- pts = (::Eigen::Matrix<double, 3, 3>() << c, 0, -s, 0, 1, 0, s, 0,
- c).finished() *
- pts.transpose();
- }
-
- // Translate the coordinate frame to have (x, y) centered at the camera, but
- // still oriented to be facing along the line from the camera to the target.
- pts(2) += z;
-
- {
- // Rotate out the heading so that the frame is oriented to line up with the
- // camera's viewpoint in the yaw-axis.
- const double theta = r2;
- const double s = sin(theta);
- const double c = cos(theta);
- pts = (::Eigen::Matrix<double, 3, 3>() << c, 0, -s, 0, 1, 0, s, 0,
- c).finished() *
- pts.transpose();
- }
-
- // TODO: Apply 15 degree downward rotation.
- {
- // Compensate for rotation in the pitch of the camera up/down to get into
- // the coordinate frame lined up with the plane of the camera sensor.
- const double theta = rup;
- const double s = sin(theta);
- const double c = cos(theta);
-
- pts = (::Eigen::Matrix<double, 3, 3>() << 1, 0, 0, 0, c, -s, 0, s,
- c).finished() *
- pts.transpose();
- }
-
- // Compensate for rotation of the barrel of the camera, i.e. about the axis
- // that points straight out from the camera lense, using an AngleAxis instead
- // of manually constructing the rotation matrices because once you get into
- // this frame you no longer need to be masochistic.
- // TODO: Maybe barrel should be extrinsics to allow rocking?
- // Also, in this case, barrel should go above the rotation above?
- pts = ::Eigen::AngleAxis<double>(rbarrel, ::Eigen::Vector3d(0.0, 0.0, 1.0)) *
- pts.transpose();
-
- // TODO: Final image projection.
- const ::Eigen::Matrix<double, 1, 3> res = pts;
-
- // Finally, scale to account for focal length and translate to get into
- // pixel-space.
- const float scale = fl / res.z();
- return Vector<2>(res.x() * scale + 320.0, 240.0 - res.y() * scale);
-}
-
Target Project(const Target &target, const IntrinsicParams &intrinsics,
const ExtrinsicParams &extrinsics) {
Target new_targ;
@@ -150,11 +78,13 @@
IntrinsicParams intrinsics)
: result(result), template_pt(template_pt), intrinsics(intrinsics) {}
- bool operator()(const double *const x, double *residual) const {
- auto extrinsics = ExtrinsicParams::get(x);
- auto pt = result - Project(template_pt, intrinsics, extrinsics);
- residual[0] = pt.x();
- residual[1] = pt.y();
+ template <typename T>
+ bool operator()(const T *const x, T *residual) const {
+ const auto extrinsics = TemplatedExtrinsicParams<T>::get(x);
+ ::Eigen::Matrix<T, 2, 1> pt =
+ Project<T>(ToEigenMatrix<T>(template_pt), intrinsics, extrinsics);
+ residual[0] = result.x() - pt(0, 0);
+ residual[1] = result.y() - pt(0, 1);
return true;
}
@@ -170,15 +100,18 @@
IntrinsicParams intrinsics)
: result(result), template_seg(template_seg), intrinsics(intrinsics) {}
- bool operator()(const double *const x, double *residual) const {
- const auto extrinsics = ExtrinsicParams::get(x);
- const Vector<2> p1 = Project(template_seg.A(), intrinsics, extrinsics);
- const Vector<2> p2 = Project(template_seg.B(), intrinsics, extrinsics);
- // distance from line (P1, P2) to point result
- double dx = p2.x() - p1.x();
- double dy = p2.y() - p1.y();
- double denom = p2.DistanceTo(p1);
- residual[0] = ::std::abs(dy * result.x() - dx * result.y() +
+ template <typename T>
+ bool operator()(const T *const x, T *residual) const {
+ const auto extrinsics = TemplatedExtrinsicParams<T>::get(x);
+ const ::Eigen::Matrix<T, 2, 1> p1 =
+ Project<T>(ToEigenMatrix<T>(template_seg.A()), intrinsics, extrinsics);
+ const ::Eigen::Matrix<T, 2, 1> p2 =
+ Project<T>(ToEigenMatrix<T>(template_seg.B()), intrinsics, extrinsics);
+ // Distance from line(P1, P2) to point result
+ T dx = p2.x() - p1.x();
+ T dy = p2.y() - p1.y();
+ T denom = (p2-p1).norm();
+ residual[0] = ceres::abs(dy * result.x() - dx * result.y() +
p2.x() * p1.y() - p2.y() * p1.x()) /
denom;
return true;
@@ -199,24 +132,28 @@
template_seg_(template_seg),
intrinsics_(intrinsics) {}
- bool operator()(const double *const x, double *residual) const {
- const ExtrinsicParams extrinsics = ExtrinsicParams::get(x);
- const Vector<2> p1 = Project(template_seg_.A(), intrinsics_, extrinsics);
- const Vector<2> p2 = Project(template_seg_.B(), intrinsics_, extrinsics);
+ template <typename T>
+ bool operator()(const T *const x, T *residual) const {
+ const auto extrinsics = TemplatedExtrinsicParams<T>::get(x);
+ const ::Eigen::Matrix<T, 2, 1> p1 = Project<T>(
+ ToEigenMatrix<T>(template_seg_.A()), intrinsics_, extrinsics);
+ const ::Eigen::Matrix<T, 2, 1> p2 = Project<T>(
+ ToEigenMatrix<T>(template_seg_.B()), intrinsics_, extrinsics);
// Construct a vector pointed perpendicular to the line. This vector is
// pointed down out the bottom of the target.
- ::Eigen::Vector2d down_axis(-(p1.y() - p2.y()), p1.x() - p2.x());
+ ::Eigen::Matrix<T, 2, 1> down_axis;
+ down_axis << -(p1.y() - p2.y()), p1.x() - p2.x();
down_axis.normalize();
// Positive means out.
- const double component =
- down_axis.transpose() * (bottom_point_ - p1.GetData().transpose());
+ const T component =
+ down_axis.transpose() * (bottom_point_ - p1);
- if (component > 0) {
+ if (component > T(0)) {
residual[0] = component * 1.0;
} else {
- residual[0] = 0.0;
+ residual[0] = T(0);
}
return true;
}
@@ -254,18 +191,18 @@
aos::vision::Segment<2> line = Segment<2>(a, a2);
problem_4point.AddResidualBlock(
- new NumericDiffCostFunction<LineCostFunctor, CENTRAL, 1, 4>(
+ new ceres::AutoDiffCostFunction<LineCostFunctor, 1, 4>(
new LineCostFunctor(b, line, intrinsics_)),
NULL, ¶ms_4point[0]);
} else {
problem_4point.AddResidualBlock(
- new NumericDiffCostFunction<PointCostFunctor, CENTRAL, 2, 4>(
+ new ceres::AutoDiffCostFunction<PointCostFunctor, 2, 4>(
new PointCostFunctor(b, a, intrinsics_)),
NULL, ¶ms_4point[0]);
}
problem_8point.AddResidualBlock(
- new NumericDiffCostFunction<PointCostFunctor, CENTRAL, 2, 4>(
+ new ceres::AutoDiffCostFunction<PointCostFunctor, 2, 4>(
new PointCostFunctor(b, a, intrinsics_)),
NULL, ¶ms_8point[0]);
}
@@ -284,7 +221,7 @@
// Now, add a large cost for the bottom point being below the bottom line.
problem_4point.AddResidualBlock(
- new NumericDiffCostFunction<BottomPointCostFunctor, CENTRAL, 1, 4>(
+ new ceres::AutoDiffCostFunction<BottomPointCostFunctor, 1, 4>(
new BottomPointCostFunctor(target.left.bottom_point,
Segment<2>(target_template_.left.outside,
target_template_.left.bottom),
@@ -293,7 +230,7 @@
// Make sure to point the segment the other way so when we do a -pi/2 rotation
// on the line, it points down in target space.
problem_4point.AddResidualBlock(
- new NumericDiffCostFunction<BottomPointCostFunctor, CENTRAL, 1, 4>(
+ new ceres::AutoDiffCostFunction<BottomPointCostFunctor, 1, 4>(
new BottomPointCostFunctor(target.right.bottom_point,
Segment<2>(target_template_.right.bottom,
target_template_.right.outside),
diff --git a/y2019/vision/target_types.h b/y2019/vision/target_types.h
index e97c051..aba9300 100644
--- a/y2019/vision/target_types.h
+++ b/y2019/vision/target_types.h
@@ -1,6 +1,8 @@
#ifndef _Y2019_VISION_TARGET_TYPES_H_
#define _Y2019_VISION_TARGET_TYPES_H_
+#include "Eigen/Dense"
+
#include "aos/vision/math/segment.h"
#include "aos/vision/math/vector.h"
#include "y2019/vision/constants.h"
@@ -54,28 +56,29 @@
std::array<aos::vision::Vector<2>, 8> ToPointList() const;
};
-struct ExtrinsicParams {
+template <typename Scalar>
+struct TemplatedExtrinsicParams {
static constexpr size_t kNumParams = 4;
// Height of the target
- double y = 18.0 * 0.0254;
+ Scalar y = Scalar(18.0 * 0.0254);
// Distance to the target
- double z = 23.0 * 0.0254;
+ Scalar z = Scalar(23.0 * 0.0254);
// Skew of the target relative to the line-of-sight from the camera to the
// target.
- double r1 = 1.0 / 180 * M_PI;
+ Scalar r1 = Scalar(1.0 / 180 * M_PI);
// Heading from the camera to the target, relative to the center of the view
// from the camera.
- double r2 = -1.0 / 180 * M_PI;
+ Scalar r2 = Scalar(-1.0 / 180 * M_PI);
- void set(double *data) {
+ void set(Scalar *data) {
data[0] = y;
data[1] = z;
data[2] = r1;
data[3] = r2;
}
- static ExtrinsicParams get(const double *data) {
- ExtrinsicParams out;
+ static TemplatedExtrinsicParams get(const Scalar *data) {
+ TemplatedExtrinsicParams out;
out.y = data[0];
out.z = data[1];
out.r1 = data[2];
@@ -84,10 +87,18 @@
}
};
+using ExtrinsicParams = TemplatedExtrinsicParams<double>;
+
// Projects a point from idealized template space to camera space.
+template <typename Extrinsics>
aos::vision::Vector<2> Project(aos::vision::Vector<2> pt,
- const IntrinsicParams &intrinsics,
- const ExtrinsicParams &extrinsics);
+ const IntrinsicParams &intrinsics,
+ const Extrinsics &extrinsics);
+
+template <typename T, typename Extrinsics>
+::Eigen::Matrix<T, 2, 1> Project(::Eigen::Matrix<T, 2, 1> pt,
+ const IntrinsicParams &intrinsics,
+ const Extrinsics &extrinsics);
Target Project(const Target &target, const IntrinsicParams &intrinsics,
const ExtrinsicParams &extrinsics);
@@ -130,6 +141,96 @@
float skew;
};
+template<typename T>
+::Eigen::Matrix<T, 2, 1> ToEigenMatrix(aos::vision::Vector<2> pt) {
+ return (::Eigen::Matrix<T, 2, 1>() << T(pt.x()), T(pt.y())).finished();
+}
+
+template <typename Extrinsics>
+aos::vision::Vector<2> Project(aos::vision::Vector<2> pt,
+ const IntrinsicParams &intrinsics,
+ const Extrinsics &extrinsics) {
+ const ::Eigen::Matrix<double, 2, 1> eigen_pt = ToEigenMatrix<double>(pt);
+ const ::Eigen::Matrix<double, 2, 1> res =
+ Project(eigen_pt, intrinsics, extrinsics);
+ return aos::vision::Vector<2>(res(0, 0), res(1, 0));
+}
+
+template <typename T, typename Extrinsics>
+::Eigen::Matrix<T, 2, 1> Project(::Eigen::Matrix<T, 2, 1> pt,
+ const IntrinsicParams &intrinsics,
+ const Extrinsics &extrinsics) {
+ const T y = extrinsics.y; // height
+ const T z = extrinsics.z; // distance
+ const T r1 = extrinsics.r1; // skew
+ const T r2 = extrinsics.r2; // heading
+ const double rup = intrinsics.mount_angle;
+ const double rbarrel = intrinsics.barrel_mount;
+ const double fl = intrinsics.focal_length;
+
+ // Start by translating point in target-space to be at correct height.
+ ::Eigen::Matrix<T, 3, 1> pts{pt(0, 0), pt(1, 0) + y, T(0.0)};
+
+ {
+ // Rotate to compensate for skew angle, to get into a frame still at the
+ // same (x, y) position as the target but rotated to be facing straight
+ // towards the camera.
+ const T theta = r1;
+ const T s = sin(theta);
+ const T c = cos(theta);
+ pts = (::Eigen::Matrix<T, 3, 3>() << c, T(0), -s, T(0), T(1), T(0), s, T(0),
+ c).finished() *
+ pts;
+ }
+
+ // Translate the coordinate frame to have (x, y) centered at the camera, but
+ // still oriented to be facing along the line from the camera to the target.
+ pts(2) += z;
+
+ {
+ // Rotate out the heading so that the frame is oriented to line up with the
+ // camera's viewpoint in the yaw-axis.
+ const T theta = r2;
+ const T s = sin(theta);
+ const T c = cos(theta);
+ pts = (::Eigen::Matrix<T, 3, 3>() << c, T(0), -s, T(0), T(1), T(0), s, T(0),
+ c).finished() *
+ pts;
+ }
+
+ // TODO: Apply 15 degree downward rotation.
+ {
+ // Compensate for rotation in the pitch of the camera up/down to get into
+ // the coordinate frame lined up with the plane of the camera sensor.
+ const double theta = rup;
+ const T s = T(sin(theta));
+ const T c = T(cos(theta));
+
+ pts = (::Eigen::Matrix<T, 3, 3>() << T(1), T(0), T(0), T(0), c, -s, T(0), s,
+ c).finished() *
+ pts;
+ }
+
+ // Compensate for rotation of the barrel of the camera, i.e. about the axis
+ // that points straight out from the camera lense, using an AngleAxis instead
+ // of manually constructing the rotation matrices because once you get into
+ // this frame you no longer need to be masochistic.
+ // TODO: Maybe barrel should be extrinsics to allow rocking?
+ // Also, in this case, barrel should go above the rotation above?
+ pts = ::Eigen::AngleAxis<T>(T(rbarrel),
+ ::Eigen::Matrix<T, 3, 1>(T(0), T(0), T(1))) *
+ pts;
+
+ // TODO: Final image projection.
+ const ::Eigen::Matrix<T, 3, 1> res = pts;
+
+ // Finally, scale to account for focal length and translate to get into
+ // pixel-space.
+ const T scale = fl / res.z();
+ return ::Eigen::Matrix<T, 2, 1>(res.x() * scale + 320.0,
+ 240.0 - res.y() * scale);
+}
+
} // namespace vision
} // namespace y2019