commit c7a0a3da74a137c6c0a9757fec0fb2ca978a959c
author Austin Schuh <austin.linux@gmail.com> Sat Oct 15 16:22:47 2016 -0700
committer Austin Schuh <austin.linux@gmail.com> Sat Oct 29 13:49:17 2016 -0700
tree fd54badbdc6b6c98d963f54fd3ba7ac2c3061aae
parent e5f194c6c20dcb61aea387f401f16facb125edc9

Removed mallocs from polydrive code

Since most of the places where we create a polytope are just affine
transformations of another polytope, we can transform the original
vertices to avoid needing libcdd at runtime.  This is in preparation
for the battlebot.

Change-Id: Ic35af2d918095dc23b90a4ab248d0c142cf3588a

frc971/control_loops/drivetrain/ssdrivetrain.cc

diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 235738d..ff7ed6e 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -13,8 +13,6 @@
 namespace control_loops {
 namespace drivetrain {
 
-using ::frc971::control_loops::DoCoerceGoal;
-
 void DrivetrainMotorsSS::ScaleCapU(Eigen::Matrix<double, 2, 1> *U) {
   output_was_capped_ =
       ::std::abs((*U)(0, 0)) > 12.0 || ::std::abs((*U)(1, 0)) > 12.0;
@@ -53,9 +51,15 @@
   Eigen::Matrix<double, 2, 1> U_integral;
   U_integral << kf_->X_hat(4, 0), kf_->X_hat(5, 0);
 
-  const ::aos::controls::HPolytope<2> pos_poly(
-      U_poly_, position_K * T_,
-      -velocity_K * velocity_error + U_integral - kf_->ff_U());
+  const ::aos::controls::HVPolytope<2, 4, 4> pos_poly_hv(
+      U_poly_.static_H() * position_K * T_,
+      U_poly_.static_H() *
+              (-velocity_K * velocity_error + U_integral - kf_->ff_U()) +
+          U_poly_.static_k(),
+      (position_K * T_).inverse() *
+          ::aos::controls::ShiftPoints<2, 4>(
+              U_poly_.StaticVertices(),
+              -velocity_K * velocity_error + U_integral - kf_->ff_U()));
 
   Eigen::Matrix<double, 2, 1> adjusted_pos_error;
   {
@@ -81,10 +85,10 @@
 
     bool is_inside_h, is_inside_45;
     const auto adjusted_pos_error_h =
-        DoCoerceGoal(pos_poly, LH, wh, drive_error, &is_inside_h);
+        DoCoerceGoal(pos_poly_hv, LH, wh, drive_error, &is_inside_h);
     const auto adjusted_pos_error_45 =
-        DoCoerceGoal(pos_poly, L45, w45, intersection, &is_inside_45);
-    if (pos_poly.IsInside(intersection)) {
+        DoCoerceGoal(pos_poly_hv, L45, w45, intersection, &is_inside_45);
+    if (pos_poly_hv.IsInside(intersection)) {
       adjusted_pos_error = adjusted_pos_error_h;
     } else {
       if (is_inside_h) {
@@ -115,10 +119,16 @@
                                        double *integrated_kf_heading)
     : dt_config_(dt_config),
       kf_(kf),
-      U_poly_(
-          (Eigen::Matrix<double, 4, 2>() << 1, 0, -1, 0, 0, 1, 0, -1)
-              .finished(),
-          (Eigen::Matrix<double, 4, 1>() << 12.0, 12.0, 12.0, 12.0).finished()),
+      U_poly_((Eigen::Matrix<double, 4, 2>() << /*[[*/ 1, 0 /*]*/,
+               /*[*/ -1, 0 /*]*/,
+               /*[*/ 0, 1 /*]*/,
+               /*[*/ 0, -1 /*]]*/).finished(),
+              (Eigen::Matrix<double, 4, 1>() << /*[[*/ 12 /*]*/,
+               /*[*/ 12 /*]*/,
+               /*[*/ 12 /*]*/,
+               /*[*/ 12 /*]]*/).finished(),
+              (Eigen::Matrix<double, 2, 4>() << /*[[*/ 12, 12, -12, -12 /*]*/,
+               /*[*/ -12, 12, 12, -12 /*]*/).finished()),
       linear_profile_(::aos::controls::kLoopFrequency),
       angular_profile_(::aos::controls::kLoopFrequency),
       integrated_kf_heading_(integrated_kf_heading) {