Alignment stuff.

Fix more alignment issues.

drake/systems/controllers/InstantaneousQPController.cpp

@@ -281,13 +281,12 @@ VectorXd InstantaneousQPController::velocityReference(
   return qd_ref;
 }
 
-std::vector<SupportStateElement, Eigen::aligned_allocator<SupportStateElement>>
+drake::eigen_aligned_std_vector<SupportStateElement>
 InstantaneousQPController::loadAvailableSupports(
     const drake::lcmt_qp_controller_input& qp_input) {
   // Parse a qp_input LCM message to extract its available supports as a vector
   // of SupportStateElements
-  std::vector<SupportStateElement,
-              Eigen::aligned_allocator<SupportStateElement>> available_supports;
+  drake::eigen_aligned_std_vector<SupportStateElement> available_supports;
   available_supports.resize(qp_input.num_support_data);
   for (int i = 0; i < qp_input.num_support_data; i++) {
     available_supports[i].body_idx =
@@ -389,7 +388,7 @@ double averageContactPointHeight(
 
 Vector2d computeCoP(const RigidBodyTree& robot,
                     const KinematicsCache<double>& cache,
-                    const std::map<Side, ForceTorqueMeasurement>&
+                    const drake::eigen_aligned_std_map<Side, ForceTorqueMeasurement>&
                         foot_force_torque_measurements,
                     Vector3d point_on_contact_plane, Eigen::Vector3d normal) {
   std::vector<ForceTorqueMeasurement> force_torque_measurements;
@@ -406,10 +405,10 @@ Vector2d computeCoP(const RigidBodyTree& robot,
 
 void InstantaneousQPController::estimateCoMBasedOnMeasuredZMP(
     const QPControllerParams& params,
-    std::vector<SupportStateElement,
-                Eigen::aligned_allocator<SupportStateElement>>& active_supports,
-    int num_contact_points, const std::map<Side, ForceTorqueMeasurement>&
-                                foot_force_torque_measurements,
+    drake::eigen_aligned_std_vector <SupportStateElement>& active_supports,
+    int num_contact_points,
+    const drake::eigen_aligned_std_map<Side, ForceTorqueMeasurement>&
+        foot_force_torque_measurements,
     double dt, Vector3d& xcom, Vector3d& xcomdot) {
   /*
    * Derivation:
@@ -589,8 +588,7 @@ std::unordered_map<std::string, int> computeBodyOrFrameNameToIdMap(
 const QPControllerParams& InstantaneousQPController::FindParams(
     const std::string& param_set_name) {
   // look up the param set by name
-  std::map<std::string, QPControllerParams>::iterator it;
-  it = param_sets.find(param_set_name);
+  auto it = param_sets.find(param_set_name);
   if (it == param_sets.end()) {
     std::cout
         << "Got a param set I don't recognize! Using standing params instead";
@@ -611,7 +609,7 @@ int InstantaneousQPController::setupAndSolveQP(
     const drake::lcmt_qp_controller_input& qp_input,
     const DrakeRobotState& robot_state,
     const Ref<const Matrix<bool, Dynamic, 1>>& contact_detected,
-    const std::map<Side, ForceTorqueMeasurement>&
+    const drake::eigen_aligned_std_map<Side, ForceTorqueMeasurement>&
         foot_force_torque_measurements,
     QPControllerOutput& qp_output, QPControllerDebugData* debug) {
   // The primary solve loop for our controller. This constructs and solves a
@@ -651,11 +649,9 @@ int InstantaneousQPController::setupAndSolveQP(
   std::vector<SupportStateElement,
               Eigen::aligned_allocator<SupportStateElement>>
       available_supports = loadAvailableSupports(qp_input);
-  std::vector<SupportStateElement,
-              Eigen::aligned_allocator<SupportStateElement>> active_supports =
-      getActiveSupports(*robot, robot_state.q, robot_state.qd,
-                        available_supports, contact_detected,
-                        params.contact_threshold);
+  drake::eigen_aligned_std_vector<SupportStateElement> active_supports = getActiveSupports(*robot, robot_state.q, robot_state.qd,
+                                          available_supports, contact_detected,
+                                          params.contact_threshold);
 
   // // whole_body_data
   if (qp_input.whole_body_data.num_positions != nq)

drake/systems/controllers/InstantaneousQPController.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include "drake/common/eigen_stl_types.h"
 #include <memory>
 #include "QPCommon.h"
 #include "drake/solvers/gurobi_qp.h"
@@ -18,7 +19,7 @@ class DRAKEQPCOMMON_EXPORT InstantaneousQPController {
  public:
   InstantaneousQPController(
       std::unique_ptr<RigidBodyTree> robot_in,
-      const std::map<std::string, QPControllerParams>& param_sets_in,
+      const drake::eigen_aligned_std_map<std::string, QPControllerParams>& param_sets_in,
       const RobotPropertyCache& rpc_in)
       : robot(std::move(robot_in)),
         param_sets(param_sets_in),
@@ -51,18 +52,20 @@ class DRAKEQPCOMMON_EXPORT InstantaneousQPController {
       const DrakeRobotState& robot_state,
       const Eigen::Ref<const Eigen::Matrix<bool, Eigen::Dynamic, 1>>&
           contact_detected,
-      const std::map<Side, ForceTorqueMeasurement>&
+      const drake::eigen_aligned_std_map<Side, ForceTorqueMeasurement>&
           foot_force_torque_measurements,
       QPControllerOutput& qp_output, QPControllerDebugData* debug = NULL);
 
   const RigidBodyTree& getRobot() const { return *robot; }
 
   std::unordered_map<std::string, int> body_or_frame_name_to_id;
 
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
  private:
   GRBenv* env;
   std::unique_ptr<RigidBodyTree> robot;
-  std::map<std::string, QPControllerParams> param_sets;
+  drake::eigen_aligned_std_map<std::string, QPControllerParams> param_sets;
   RobotPropertyCache rpc;
   Eigen::VectorXd umin, umax;
   int use_fast_qp;
@@ -103,16 +106,13 @@ class DRAKEQPCOMMON_EXPORT InstantaneousQPController {
       const Eigen::Ref<const Eigen::VectorXd>& qdd, bool foot_contact[2],
       const VRefIntegratorParams& params);
 
-  std::vector<SupportStateElement,
-              Eigen::aligned_allocator<SupportStateElement>>
+  drake::eigen_aligned_std_vector<SupportStateElement>
   loadAvailableSupports(const drake::lcmt_qp_controller_input& qp_input);
 
   void estimateCoMBasedOnMeasuredZMP(
       const QPControllerParams& params,
-      std::vector<SupportStateElement,
-                  Eigen::aligned_allocator<SupportStateElement>>&
-          active_supports,
-      int num_contact_points, const std::map<Side, ForceTorqueMeasurement>&
+      drake::eigen_aligned_std_vector<SupportStateElement>& active_supports,
+      int num_contact_points, const drake::eigen_aligned_std_map<Side, ForceTorqueMeasurement>&
                                   foot_force_torque_measurements,
       double dt, Eigen::Vector3d& xcom, Eigen::Vector3d& xcomdot);
 

drake/systems/controllers/QPCommon.h

@@ -1,9 +1,9 @@
 #pragma once
 
-#include <vector>
 
 #include <Eigen/Core>
 
+#include "drake/common/eigen_stl_types.h"
 #include "drake/systems/controllers/controlUtil.h"
 #include "drake/systems/plants/ForceTorqueMeasurement.h"
 #include "drake/systems/plants/RigidBodyTree.h"
@@ -28,6 +28,8 @@ struct QPControllerState {
   int* cbasis;
   int vbasis_len;
   int cbasis_len;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 struct PositionIndices {
@@ -166,6 +168,8 @@ struct BodyMotionParams {
     return lhs.Kp.isApprox(rhs.Kp) && lhs.Kd.isApprox(rhs.Kd) &&
            lhs.accel_bounds == rhs.accel_bounds && lhs.weight == rhs.weight;
   }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 struct HardwareGains {
@@ -238,7 +242,7 @@ struct QPControllerParams {
         center_of_mass_observer_gain(Eigen::Matrix4d::Zero()) {}
 
   WholeBodyParams whole_body;
-  std::vector<BodyMotionParams> body_motion;
+  drake::eigen_aligned_std_vector<BodyMotionParams> body_motion;
   VRefIntegratorParams vref_integrator;
   JointSoftLimitParams joint_soft_limits;
   HardwareParams hardware;
@@ -271,6 +275,8 @@ struct QPControllerParams {
             rhs.center_of_mass_observer_gain);
     return is_equal;
   }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 std::unordered_map<std::string, int> computeBodyOrFrameNameToIdMap(
@@ -289,6 +295,8 @@ struct DesiredBodyAcceleration {
   KinematicPath body_path;
   Eigen::Isometry3d T_task_to_world;
   Vector6d weight_multiplier;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 struct QPControllerOutput {
@@ -299,9 +307,7 @@ struct QPControllerOutput {
 };
 
 struct QPControllerDebugData {
-  std::vector<SupportStateElement,
-              Eigen::aligned_allocator<SupportStateElement>>
-      active_supports;
+  drake::eigen_aligned_std_vector<SupportStateElement> active_supports;
   int nc;
   Eigen::MatrixXd normals;
   Eigen::MatrixXd B;
@@ -323,6 +329,8 @@ struct QPControllerDebugData {
   Eigen::MatrixXd Jcom;
   Eigen::VectorXd Jcomdotv;
   Eigen::VectorXd beta;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 struct PIDOutput {

drake/systems/controllers/controlUtil.cpp

@@ -53,8 +53,7 @@ void angleDiff(MatrixBase<DerivedPhi1> const& phi1,
 }
 
 bool inSupport(
-    const std::vector<SupportStateElement,
-                      Eigen::aligned_allocator<SupportStateElement>>& supports,
+    const drake::eigen_aligned_std_vector<SupportStateElement>& supports,
     int body_idx) {
   // HANDLE IF BODY_IDX IS A FRAME ID?
   for (size_t i = 0; i < supports.size(); i++) {
@@ -107,8 +106,7 @@ int contactPhi(const RigidBodyTree& r, const KinematicsCache<double>& cache,
 int contactConstraintsBV(
     const RigidBodyTree& r, const KinematicsCache<double>& cache, int nc,
     std::vector<double> support_mus,
-    std::vector<SupportStateElement,
-                Eigen::aligned_allocator<SupportStateElement>>& supp,
+    drake::eigen_aligned_std_vector<SupportStateElement>& supp,
     MatrixXd& B, MatrixXd& JB, MatrixXd& Jp, VectorXd& Jpdotv,
     MatrixXd& normals) {
   int j, k = 0, nq = r.number_of_positions();
@@ -123,10 +121,7 @@ int contactConstraintsBV(
   MatrixXd J(3, nq);
   Matrix<double, 3, m_surface_tangents> d;
 
-  for (std::vector<SupportStateElement,
-                   Eigen::aligned_allocator<SupportStateElement>>::iterator
-           iter = supp.begin();
-       iter != supp.end(); iter++) {
+  for (auto iter = supp.begin(); iter != supp.end(); iter++) {
     double mu = support_mus[iter - supp.begin()];
     double norm = sqrt(1 + mu * mu);  // because normals and ds are orthogonal,
                                       // the norm has a simple form
@@ -169,9 +164,7 @@ int contactConstraintsBV(
 
 MatrixXd individualSupportCOPs(
     const RigidBodyTree& r, const KinematicsCache<double>& cache,
-    const std::vector<SupportStateElement,
-                      Eigen::aligned_allocator<SupportStateElement>>&
-        active_supports,
+    const drake::eigen_aligned_std_vector<SupportStateElement>& active_supports,
     const MatrixXd& normals, const MatrixXd& B, const VectorXd& beta) {
   const int n_basis_vectors_per_contact =
       static_cast<int>(B.cols() / normals.cols());
@@ -253,8 +246,7 @@ bool isSupportElementActive(SupportStateElement* se,
 
 Matrix<bool, Dynamic, 1> getActiveSupportMask(
     const RigidBodyTree& r, VectorXd q, VectorXd qd,
-    std::vector<SupportStateElement,
-                Eigen::aligned_allocator<SupportStateElement>>&
+    drake::eigen_aligned_std_vector<SupportStateElement>&
         available_supports,
     const Ref<const Matrix<bool, Dynamic, 1>>& contact_force_detected,
     double contact_threshold) {
@@ -303,19 +295,16 @@ Matrix<bool, Dynamic, 1> getActiveSupportMask(
   return active_supp_mask;
 }
 
-std::vector<SupportStateElement, Eigen::aligned_allocator<SupportStateElement>>
-getActiveSupports(
+drake::eigen_aligned_std_vector<SupportStateElement> getActiveSupports(
     const RigidBodyTree& r, const VectorXd& q, const VectorXd& qd,
-    std::vector<SupportStateElement,
-                Eigen::aligned_allocator<SupportStateElement>>&
+    drake::eigen_aligned_std_vector<SupportStateElement>&
         available_supports,
     const Ref<const Matrix<bool, Dynamic, 1>>& contact_force_detected,
     double contact_threshold) {
   Matrix<bool, Dynamic, 1> active_supp_mask = getActiveSupportMask(
       r, q, qd, available_supports, contact_force_detected, contact_threshold);
 
-  std::vector<SupportStateElement,
-              Eigen::aligned_allocator<SupportStateElement>> active_supports;
+  drake::eigen_aligned_std_vector<SupportStateElement> active_supports;
 
   for (size_t i = 0; i < available_supports.size(); i++) {
     if (active_supp_mask(i)) {

drake/systems/controllers/controlUtil.h

@@ -24,7 +24,7 @@ typedef struct _support_state_element {
   bool support_logic_map[4];
   Eigen::Vector4d support_surface;  // 4-vector describing a support surface:
                                     // [v; b] such that v' * [x;y;z] + b == 0
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 } SupportStateElement;
 
 struct DrakeRobotState {
@@ -106,7 +106,7 @@ DRAKECONTROLUTIL_EXPORT void evaluateXYZExpmapCubicSpline(
 struct RobotJointIndexMap {
   Eigen::VectorXi drake_to_robot;
   Eigen::VectorXi robot_to_drake;
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 struct JointNames {

drake/systems/controllers/instantaneousQPControllermex.cpp

@@ -85,7 +85,8 @@ void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
     debug.reset(new QPControllerDebugData());
   }
 
-  std::map<Side, ForceTorqueMeasurement> foot_force_torque_measurements;
+  drake::eigen_aligned_std_map<Side, ForceTorqueMeasurement>
+      foot_force_torque_measurements;
   if (nrhs > 5) {
     const mxArray* mex_foot_force_torque_measurements = prhs[narg++];
     if (!mxIsEmpty(mex_foot_force_torque_measurements)) {

drake/systems/controllers/supportDetectmex.cpp

@@ -88,8 +88,7 @@ void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
 
   //---------------------------------------------------------------------
   // Compute active support from desired supports -----------------------
-  vector<SupportStateElement, Eigen::aligned_allocator<SupportStateElement>>
-      active_supports;
+  drake::eigen_aligned_std_vector<SupportStateElement> active_supports;
   set<int> contact_bodies;  // redundant, clean up later
   int num_active_contact_pts = 0;
   if (!mxIsEmpty(prhs[desired_support_argid])) {
@@ -159,9 +158,7 @@ void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
                                    mxREAL);
     pr = mxGetPr(plhs[0]);
     int i = 0;
-    for (vector<SupportStateElement,
-                Eigen::aligned_allocator<SupportStateElement>>::iterator iter =
-             active_supports.begin();
+    for (auto iter = active_supports.begin();
          iter != active_supports.end(); iter++) {
       pr[i++] = (double)(iter->body_idx + 1);
     }

drake/systems/plants/ForceTorqueMeasurement.h

@@ -6,4 +6,5 @@ struct ForceTorqueMeasurement {
   int frame_idx;
   Eigen::Matrix<double, 6, 1> wrench;  // [torque; force], expressed in frame
                                        // corresponding to frame_idx;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
 };

drake/util/yaml/yamlUtil.cpp

@@ -323,9 +323,9 @@ QPControllerParams loadSingleParamSet(const YAML::Node& config,
   return params;
 }
 
-std::map<std::string, QPControllerParams> loadAllParamSetsFromExpandedConfig(
+drake::eigen_aligned_std_map<std::string, QPControllerParams> loadAllParamSetsFromExpandedConfig(
     YAML::Node config, const RigidBodyTree& robot) {
-  auto param_sets = std::map<std::string, QPControllerParams>();
+  drake::eigen_aligned_std_map<std::string, QPControllerParams> param_sets;
   for (auto config_it = config.begin(); config_it != config.end();
        ++config_it) {
     std::cout << "loading param set: " << config_it->first << std::endl;
@@ -336,13 +336,13 @@ std::map<std::string, QPControllerParams> loadAllParamSetsFromExpandedConfig(
   return param_sets;
 }
 
-std::map<std::string, QPControllerParams> loadAllParamSets(
+drake::eigen_aligned_std_map<std::string, QPControllerParams> loadAllParamSets(
     YAML::Node config, const RigidBodyTree& robot) {
   config = expandDefaults(config);
   return loadAllParamSetsFromExpandedConfig(config, robot);
 }
 
-std::map<std::string, QPControllerParams> loadAllParamSets(
+drake::eigen_aligned_std_map<std::string, QPControllerParams> loadAllParamSets(
     YAML::Node config, const RigidBodyTree& robot,
     std::ofstream& debug_output_file) {
   config = expandDefaults(config);

drake/util/yaml/yamlUtil.h

@@ -2,6 +2,8 @@
 
 #include <map>
 #include <fstream>
+
+#include "drake/common/eigen_stl_types.h"
 #include "yaml-cpp/yaml.h"
 #include "drake/systems/controllers/QPCommon.h"
 #include "drake/systems/plants/RigidBodyTree.h"
@@ -19,9 +21,9 @@ DRAKEYAMLUTIL_EXPORT YAML::Node get(
 
 DRAKEYAMLUTIL_EXPORT QPControllerParams loadSingleParamSet(
   const YAML::Node& config, const RigidBodyTree& robot);
-DRAKEYAMLUTIL_EXPORT std::map<std::string, QPControllerParams> loadAllParamSets(
+DRAKEYAMLUTIL_EXPORT drake::eigen_aligned_std_map<std::string, QPControllerParams> loadAllParamSets(
     YAML::Node config, const RigidBodyTree& robot);
-DRAKEYAMLUTIL_EXPORT std::map<std::string, QPControllerParams> loadAllParamSets(
+DRAKEYAMLUTIL_EXPORT drake::eigen_aligned_std_map<std::string, QPControllerParams> loadAllParamSets(
     YAML::Node config, const RigidBodyTree& robot,
     std::ofstream& debug_output_file);
 DRAKEYAMLUTIL_EXPORT RobotPropertyCache parseKinematicTreeMetadata(