Separated CoerceGoal out from drivetrain.
diff --git a/frc971/control_loops/coerce_goal.cc b/frc971/control_loops/coerce_goal.cc
new file mode 100644
index 0000000..8ef26e7
--- /dev/null
+++ b/frc971/control_loops/coerce_goal.cc
@@ -0,0 +1,58 @@
+#include "frc971/control_loops/coerce_goal.h"
+
+#include "Eigen/Dense"
+
+#include "aos/controls/polytope.h"
+
+namespace frc971 {
+namespace control_loops {
+
+Eigen::Matrix<double, 2, 1> CoerceGoal(aos::controls::HPolytope<2> ®ion,
+ const Eigen::Matrix<double, 1, 2> &K,
+ double w,
+ const Eigen::Matrix<double, 2, 1> &R) {
+ if (region.IsInside(R)) {
+ return R;
+ }
+ Eigen::Matrix<double, 2, 1> parallel_vector;
+ Eigen::Matrix<double, 2, 1> perpendicular_vector;
+ perpendicular_vector = K.transpose().normalized();
+ parallel_vector << perpendicular_vector(1, 0), -perpendicular_vector(0, 0);
+
+ aos::controls::HPolytope<1> t_poly(
+ region.H() * parallel_vector,
+ region.k() - region.H() * perpendicular_vector * w);
+
+ Eigen::Matrix<double, 1, Eigen::Dynamic> vertices = t_poly.Vertices();
+ if (vertices.innerSize() > 0) {
+ double min_distance_sqr = 0;
+ Eigen::Matrix<double, 2, 1> closest_point;
+ for (int i = 0; i < vertices.innerSize(); i++) {
+ Eigen::Matrix<double, 2, 1> point;
+ point = parallel_vector * vertices(0, i) + perpendicular_vector * w;
+ const double length = (R - point).squaredNorm();
+ if (i == 0 || length < min_distance_sqr) {
+ closest_point = point;
+ min_distance_sqr = length;
+ }
+ }
+ return closest_point;
+ } else {
+ Eigen::Matrix<double, 2, Eigen::Dynamic> region_vertices =
+ region.Vertices();
+ double min_distance = INFINITY;
+ int closest_i = 0;
+ for (int i = 0; i < region_vertices.outerSize(); i++) {
+ const double length = ::std::abs(
+ (perpendicular_vector.transpose() * (region_vertices.col(i)))(0, 0));
+ if (i == 0 || length < min_distance) {
+ closest_i = i;
+ min_distance = length;
+ }
+ }
+ return region_vertices.col(closest_i);
+ }
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/coerce_goal.h b/frc971/control_loops/coerce_goal.h
new file mode 100644
index 0000000..43707b4
--- /dev/null
+++ b/frc971/control_loops/coerce_goal.h
@@ -0,0 +1,23 @@
+#ifndef FRC971_CONTROL_LOOPS_COERCE_GOAL_H_
+#define FRC971_CONTROL_LOOPS_COERCE_GOAL_H_
+
+#include "Eigen/Dense"
+
+#include "aos/controls/polytope.h"
+
+namespace frc971 {
+namespace control_loops {
+
+// Intersects a line with a region, and finds the closest point to R.
+// Finds a point that is closest to R inside the region, and on the line
+// defined by K X = w. If it is not possible to find a point on the line,
+// finds a point that is inside the region and closest to the line.
+Eigen::Matrix<double, 2, 1> CoerceGoal(aos::controls::HPolytope<2> ®ion,
+ const Eigen::Matrix<double, 1, 2> &K,
+ double w,
+ const Eigen::Matrix<double, 2, 1> &R);
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_COERCE_GOAL_H_
diff --git a/frc971/control_loops/control_loops.gyp b/frc971/control_loops/control_loops.gyp
index 273063c..29cd29e 100644
--- a/frc971/control_loops/control_loops.gyp
+++ b/frc971/control_loops/control_loops.gyp
@@ -25,6 +25,21 @@
'includes': ['../../aos/build/queues.gypi'],
},
{
+ 'target_name': 'coerce_goal',
+ 'type': 'static_library',
+ 'sources': [
+ 'coerce_goal.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):eigen',
+ '<(DEPTH)/aos/build/externals.gyp:libcdd',
+ ],
+ 'export_dependent_settings': [
+ '<(EXTERNALS):eigen',
+ '<(DEPTH)/aos/build/externals.gyp:libcdd',
+ ],
+ },
+ {
'target_name': 'state_feedback_loop',
'type': 'static_library',
'sources': [
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index e7aae01..b54949d 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -13,6 +13,7 @@
#include "aos/common/logging/queue_logging.h"
#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/drivetrain/polydrivetrain_cim_plant.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/queues/gyro_angle.q.h"
@@ -26,53 +27,6 @@
// Width of the robot.
const double width = 22.0 / 100.0 * 2.54;
-Eigen::Matrix<double, 2, 1> CoerceGoal(aos::controls::HPolytope<2> ®ion,
- const Eigen::Matrix<double, 1, 2> &K,
- double w,
- const Eigen::Matrix<double, 2, 1> &R) {
- if (region.IsInside(R)) {
- return R;
- }
- Eigen::Matrix<double, 2, 1> parallel_vector;
- Eigen::Matrix<double, 2, 1> perpendicular_vector;
- perpendicular_vector = K.transpose().normalized();
- parallel_vector << perpendicular_vector(1, 0), -perpendicular_vector(0, 0);
-
- aos::controls::HPolytope<1> t_poly(
- region.H() * parallel_vector,
- region.k() - region.H() * perpendicular_vector * w);
-
- Eigen::Matrix<double, 1, Eigen::Dynamic> vertices = t_poly.Vertices();
- if (vertices.innerSize() > 0) {
- double min_distance_sqr = 0;
- Eigen::Matrix<double, 2, 1> closest_point;
- for (int i = 0; i < vertices.innerSize(); i++) {
- Eigen::Matrix<double, 2, 1> point;
- point = parallel_vector * vertices(0, i) + perpendicular_vector * w;
- const double length = (R - point).squaredNorm();
- if (i == 0 || length < min_distance_sqr) {
- closest_point = point;
- min_distance_sqr = length;
- }
- }
- return closest_point;
- } else {
- Eigen::Matrix<double, 2, Eigen::Dynamic> region_vertices =
- region.Vertices();
- double min_distance = INFINITY;
- int closest_i = 0;
- for (int i = 0; i < region_vertices.outerSize(); i++) {
- const double length = ::std::abs(
- (perpendicular_vector.transpose() * (region_vertices.col(i)))(0, 0));
- if (i == 0 || length < min_distance) {
- closest_i = i;
- min_distance = length;
- }
- }
- return region_vertices.col(closest_i);
- }
-}
-
class DrivetrainMotorsSS {
public:
DrivetrainMotorsSS()
diff --git a/frc971/control_loops/drivetrain/drivetrain.gyp b/frc971/control_loops/drivetrain/drivetrain.gyp
index adb9f94..5941f63 100644
--- a/frc971/control_loops/drivetrain/drivetrain.gyp
+++ b/frc971/control_loops/drivetrain/drivetrain.gyp
@@ -46,6 +46,7 @@
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/aos/build/externals.gyp:libcdd',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:coerce_goal',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
'<(AOS)/common/util/util.gyp:log_interval',
'<(AOS)/common/logging/logging.gyp:queue_logging',
@@ -53,6 +54,7 @@
'export_dependent_settings': [
'<(DEPTH)/aos/build/externals.gyp:libcdd',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:coerce_goal',
'<(AOS)/common/common.gyp:controls',
'drivetrain_loop',
],
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 9ee7a27..2d5e9c9 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -12,11 +12,6 @@
namespace frc971 {
namespace control_loops {
-Eigen::Matrix<double, 2, 1> CoerceGoal(aos::controls::HPolytope<2> ®ion,
- const Eigen::Matrix<double, 1, 2> &K,
- double w,
- const Eigen::Matrix<double, 2, 1> &R);
-
class DrivetrainLoop
: public aos::control_loops::ControlLoop<control_loops::Drivetrain, true, false> {
public:
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index eb1ede2..73f8525 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -11,6 +11,7 @@
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "frc971/queues/gyro_angle.q.h"