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/polydrivetrain.cc

diff --git a/frc971/control_loops/drivetrain/polydrivetrain.cc b/frc971/control_loops/drivetrain/polydrivetrain.cc
index 121a231..6d3de59 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain.cc
@@ -16,10 +16,6 @@
 namespace control_loops {
 namespace drivetrain {
 
-using ::frc971::control_loops::GearLogging;
-using ::frc971::control_loops::CIMLogging;
-using ::frc971::control_loops::CoerceGoal;
-
 PolyDrivetrain::PolyDrivetrain(const DrivetrainConfig &dt_config,
                                StateFeedbackLoop<7, 2, 3> *kf)
     : kf_(kf),
@@ -30,7 +26,9 @@
               (Eigen::Matrix<double, 4, 1>() << /*[[*/ 12 /*]*/,
                /*[*/ 12 /*]*/,
                /*[*/ 12 /*]*/,
-               /*[*/ 12 /*]]*/).finished()),
+               /*[*/ 12 /*]]*/).finished(),
+              (Eigen::Matrix<double, 2, 4>() << /*[[*/ 12, 12, -12, -12 /*]*/,
+               /*[*/ -12, 12, 12, -12 /*]*/).finished()),
       loop_(new StateFeedbackLoop<2, 2, 2>(dt_config.make_v_drivetrain_loop())),
       ttrust_(1.1),
       wheel_(0.0),
@@ -239,13 +237,16 @@
       const double equality_w = 0.0;
 
       // Construct a constraint on R by manipulating the constraint on U
-      ::aos::controls::HPolytope<2> R_poly = ::aos::controls::HPolytope<2>(
-          U_Poly_.H() * (loop_->K() + FF),
-          U_Poly_.k() + U_Poly_.H() * loop_->K() * loop_->X_hat());
+      ::aos::controls::HVPolytope<2, 4, 4> R_poly_hv(
+          U_Poly_.static_H() * (loop_->K() + FF),
+          U_Poly_.static_k() + U_Poly_.static_H() * loop_->K() * loop_->X_hat(),
+          (loop_->K() + FF).inverse() *
+              ::aos::controls::ShiftPoints<2, 4>(U_Poly_.StaticVertices(),
+                                                 loop_->K() * loop_->X_hat()));
 
       // Limit R back inside the box.
       loop_->mutable_R() =
-          CoerceGoal(R_poly, equality_k, equality_w, loop_->R());
+          CoerceGoal(R_poly_hv, equality_k, equality_w, loop_->R());
     }
 
     const Eigen::Matrix<double, 2, 1> FF_volts = FF * loop_->R();