Merge pull request RobotLocomotion#1416 from RussTedrake/win32

fixes memory errors on win32
Nguyenski91 · Oct 13, 2015 · 2e5b3fb · 2e5b3fb
2 parents 3bdd980 + 6c29e95
commit 2e5b3fb
Show file tree

Hide file tree

Showing 18 changed files with 38 additions and 30 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -2,13 +2,15 @@ cmake_minimum_required(VERSION 2.8.0)
 project(drake-superbuild)
 
 # PODs out-of-source build logic
-if (CMAKE_INSTALL_PREFIX STREQUAL "/usr/local" OR CMAKE_INSTALL_PREFIX STREQUAL "C:/Program Files/${PROJECT_NAME}")
+if (CMAKE_INSTALL_PREFIX_INITIALIZED_TO_DEFAULT)
 	find_file(_build_dir build PATHS ${PROJECT_SOURCE_DIR} ${PROJECT_SOURCE_DIR}/.. ${PROJECT_SOURCE_DIR}/../.. ${PROJECT_SOURCE_DIR}/../../.. ${PROJECT_SOURCE_DIR}/../../../..)
 	if (_build_dir)
-		set(CMAKE_INSTALL_PREFIX "${_build_dir}")
+		set(CMAKE_INSTALL_PREFIX "${_build_dir}" CACHE STRING
+		"install prefix" FORCE)
 	else()
 		execute_process(COMMAND ${CMAKE_COMMAND} -E make_directory ${PROJECT_SOURCE_DIR}/build)
-		set(CMAKE_INSTALL_PREFIX ${PROJECT_SOURCE_DIR}/build)
+		set(CMAKE_INSTALL_PREFIX ${PROJECT_SOURCE_DIR}/build
+		CACHE STRING "install prefix" FORCE)
 	endif()
 endif()
 message(STATUS CMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX})
@@ -97,7 +99,7 @@ set(director_GIT_TAG 9e2cfbc3812cb5c12915981fe63fed76e711e1b7)
 set(director_SOURCE_DIR ${PROJECT_SOURCE_DIR}/externals/director/distro/pods/drake-distro)
 set(director_IS_PUBLIC TRUE)
 set(eigen_GIT_REPOSITORY https://github.com/RobotLocomotion/eigen-pod.git)
-set(eigen_GIT_TAG 3ec6bbfcb41a9ea74921720d62f86863fdf96217)
+set(eigen_GIT_TAG 9061e9d5481fe0fdf55d5de322dfb1ed9d380a1c)
 set(eigen_IS_CMAKE_POD TRUE)
 set(eigen_IS_PUBLIC TRUE)
 set(gtk_GIT_REPOSITORY https://github.com/RobotLocomotion/gtk-pod.git)

diff --git a/Makefile b/Makefile
@@ -26,9 +26,9 @@ configure: pod-build
 .PHONY: options
 options: configure
 ifeq ($(OS),Windows_NT)
-	cmake-gui pod-build
+	cmake-gui $(CMAKE_FLAGS) pod-build
 else
-	ccmake pod-build
+	ccmake $(CMAKE_FLAGS) pod-build
 endif
 
 .PHONY: clean

diff --git a/drake/examples/Pendulum/runDynamics.cpp b/drake/examples/Pendulum/runDynamics.cpp
@@ -9,7 +9,7 @@ int main(int argc, char* argv[]) {
   if(!lcm->good())
    return 1;
 
-  DrakeSystemPtr p = make_shared<PendulumWithBotVis>(lcm);
+  DrakeSystemPtr p(new PendulumWithBotVis(lcm));
   runLCM(p,*lcm,0,50,p->getRandomState());
 
   cout << "output frame: " << p->getOutputFrame() << endl;

diff --git a/drake/examples/Pendulum/runLQR.cpp b/drake/examples/Pendulum/runLQR.cpp
@@ -9,7 +9,7 @@ int main(int argc, char* argv[]) {
   if(!lcm->good())
     return 1;
 
-  std::shared_ptr<PendulumWithBotVis> pendulum = make_shared<PendulumWithBotVis>(lcm);
+  std::shared_ptr<PendulumWithBotVis> pendulum(new PendulumWithBotVis(lcm));
   DrakeSystemPtr controller = pendulum->balanceLQR();
 
   DrakeSystemPtr sys = feedback(pendulum,controller);

diff --git a/drake/systems/controllers/QPCommon.cpp b/drake/systems/controllers/QPCommon.cpp
@@ -480,7 +480,7 @@ int setupAndSolveQP(
 
   // handle external wrenches to compensate for
   typedef GradientVar<double, TWIST_SIZE, 1> WrenchGradientVarType;
-  std::unordered_map<RigidBody const *, GradientVar<double, TWIST_SIZE, 1> > f_ext;
+  eigen_aligned_unordered_map<RigidBody const *, GradientVar<double, TWIST_SIZE, 1> > f_ext;
   for (auto it = qp_input->body_wrench_data.begin(); it != qp_input->body_wrench_data.end(); ++it) {
     const drake::lcmt_body_wrench_data& body_wrench_data = *it;
     int body_id = body_wrench_data.body_id - 1;

diff --git a/drake/systems/controllers/QPControllermex.cpp b/drake/systems/controllers/QPControllermex.cpp
@@ -275,7 +275,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     }
   }
 
-  std::unordered_map<RigidBody const *, GradientVar<double, TWIST_SIZE, 1> > f_ext;
+  eigen_aligned_unordered_map<RigidBody const *, GradientVar<double, TWIST_SIZE, 1> > f_ext;
   pdata->H = pdata->r->massMatrix(cache).value();
   pdata->C = pdata->r->dynamicsBiasTerm(cache, f_ext).value();
 

diff --git a/drake/systems/plants/KinematicsCache.h b/drake/systems/plants/KinematicsCache.h
@@ -62,7 +62,7 @@ template <typename Scalar>
 class KinematicsCache
 {
 private:
-  std::unordered_map<RigidBody const *, KinematicsCacheElement<Scalar>> elements;
+  std::unordered_map<RigidBody const *, KinematicsCacheElement<Scalar>, std::hash<RigidBody const *>, std::equal_to<RigidBody const *>, Eigen::aligned_allocator<std::pair<RigidBody const* const, KinematicsCacheElement<Scalar> > > > elements;
   Eigen::Matrix<Scalar, Eigen::Dynamic, 1> q;
   Eigen::Matrix<Scalar, Eigen::Dynamic, 1> v;
   bool velocity_vector_valid;

diff --git a/drake/systems/plants/RigidBodyManipulator.cpp b/drake/systems/plants/RigidBodyManipulator.cpp
@@ -42,7 +42,8 @@ RigidBodyManipulator::RigidBodyManipulator(const std::string &urdf_filename, con
 {
   a_grav << 0, 0, 0, 0, 0, -9.81;
 
-  shared_ptr<RigidBody> b = make_shared<RigidBody>();
+  shared_ptr<RigidBody> b(new RigidBody());
+  make_shared<RigidBody>();
   b->linkname = "world";
   b->robotnum = 0;
   b->body_index = 0;
@@ -58,7 +59,7 @@ RigidBodyManipulator::RigidBodyManipulator(void) :
 {
   a_grav << 0, 0, 0, 0, 0, -9.81;
 
-  shared_ptr<RigidBody> b = make_shared<RigidBody>();
+  shared_ptr<RigidBody> b(new RigidBody());
   b->linkname = "world";
   b->robotnum = 0;
   b->body_index = 0;
@@ -1241,7 +1242,7 @@ GradientVar<Scalar, Eigen::Dynamic, Eigen::Dynamic> RigidBodyManipulator::massMa
  * Note that the wrenches in f_ext are expressed in body frame.
  */
 template <typename Scalar>
-GradientVar<Scalar, Eigen::Dynamic, 1> RigidBodyManipulator::dynamicsBiasTerm(KinematicsCache<Scalar>& cache, const std::unordered_map<RigidBody const *, GradientVar<Scalar, TWIST_SIZE, 1> >& f_ext, int gradient_order) const
+GradientVar<Scalar, Eigen::Dynamic, 1> RigidBodyManipulator::dynamicsBiasTerm(KinematicsCache<Scalar>& cache, const eigen_aligned_unordered_map<RigidBody const *, GradientVar<Scalar, TWIST_SIZE, 1> >& f_ext, int gradient_order) const
 {
   GradientVar<Scalar, Eigen::Dynamic, 1> vd(num_velocities, 1, num_positions + num_velocities, gradient_order);
   vd.value().setZero();
@@ -1256,7 +1257,7 @@ GradientVar<Scalar, Eigen::Dynamic, 1> RigidBodyManipulator::dynamicsBiasTerm(Ki
  */
 template <typename Scalar>
 GradientVar<Scalar, Eigen::Dynamic, 1> RigidBodyManipulator::inverseDynamics(KinematicsCache<Scalar>& cache,
-    const std::unordered_map<RigidBody const *, GradientVar<Scalar, TWIST_SIZE, 1> >& f_ext,
+    const eigen_aligned_unordered_map<RigidBody const *, GradientVar<Scalar, TWIST_SIZE, 1> >& f_ext,
     const GradientVar<Scalar, Eigen::Dynamic, 1>& vd, int gradient_order) const
 {
   if (gradient_order > 1)
@@ -1888,8 +1889,8 @@ template DLLEXPORT_RBM GradientVar<double, TWIST_SIZE, 1> RigidBodyManipulator::
 template DLLEXPORT_RBM GradientVar<double, 6, Dynamic> RigidBodyManipulator::worldMomentumMatrix<double>(KinematicsCache<double>&, int, const std::set<int>&, bool) const;
 template DLLEXPORT_RBM GradientVar<double, 6, 1> RigidBodyManipulator::transformSpatialAcceleration<double>(const KinematicsCache<double>&, GradientVar<double, 6, 1> const&, int, int, int, int) const;
 template DLLEXPORT_RBM GradientVar<double, 6, 1> RigidBodyManipulator::worldMomentumMatrixDotTimesV<double>(KinematicsCache<double>&, int, const std::set<int>&) const;
-template DLLEXPORT_RBM GradientVar<double, -1, 1> RigidBodyManipulator::inverseDynamics<double>(KinematicsCache<double>&, unordered_map<RigidBody const*, GradientVar<double, 6, 1> > const&, GradientVar<double, -1, 1, -1, 1> const&, int) const;
-template DLLEXPORT_RBM GradientVar<double, -1, 1> RigidBodyManipulator::dynamicsBiasTerm<double>(KinematicsCache<double>&, unordered_map<RigidBody const*, GradientVar<double, 6, 1> > const&, int) const;
+template DLLEXPORT_RBM GradientVar<double, -1, 1> RigidBodyManipulator::inverseDynamics<double>(KinematicsCache<double>&, eigen_aligned_unordered_map<RigidBody const*, GradientVar<double, 6, 1> > const&, GradientVar<double, -1, 1, -1, 1> const&, int) const;
+template DLLEXPORT_RBM GradientVar<double, -1, 1> RigidBodyManipulator::dynamicsBiasTerm<double>(KinematicsCache<double>&, eigen_aligned_unordered_map<RigidBody const*, GradientVar<double, 6, 1> > const&, int) const;
 
 template DLLEXPORT_RBM GradientVar<double, Eigen::Dynamic, 1> RigidBodyManipulator::positionConstraints(const KinematicsCache<double>&, int) const;
 

diff --git a/drake/systems/plants/RigidBodyManipulator.h b/drake/systems/plants/RigidBodyManipulator.h
@@ -12,6 +12,7 @@
 #include "KinematicPath.h"
 #include "ForceTorqueMeasurement.h"
 #include "GradientVar.h"
+#include "drakeUtil.h"
 #include <stdexcept>
 
 
@@ -358,10 +359,10 @@ class DLLEXPORT_RBM RigidBodyManipulator
   GradientVar<Scalar, Eigen::Dynamic, Eigen::Dynamic> massMatrix(KinematicsCache<Scalar>& cache, int gradient_order = 0) const;
 
   template <typename Scalar>
-  GradientVar<Scalar, Eigen::Dynamic, 1> dynamicsBiasTerm(KinematicsCache<Scalar>& cache, const std::unordered_map<RigidBody const *, GradientVar<Scalar, TWIST_SIZE, 1> >& f_ext, int gradient_order = 0) const;
+  GradientVar<Scalar, Eigen::Dynamic, 1> dynamicsBiasTerm(KinematicsCache<Scalar>& cache, const eigen_aligned_unordered_map<RigidBody const *, GradientVar<Scalar, TWIST_SIZE, 1> >& f_ext, int gradient_order = 0) const;
 
   template <typename Scalar>
-  GradientVar<Scalar, Eigen::Dynamic, 1> inverseDynamics(KinematicsCache<Scalar>& cache, const std::unordered_map<RigidBody const *, GradientVar<Scalar, TWIST_SIZE, 1> >& f_ext, const GradientVar<Scalar, Eigen::Dynamic, 1>& vd, int gradient_order = 0) const;
+  GradientVar<Scalar, Eigen::Dynamic, 1> inverseDynamics(KinematicsCache<Scalar>& cache, const eigen_aligned_unordered_map<RigidBody const *, GradientVar<Scalar, TWIST_SIZE, 1> >& f_ext, const GradientVar<Scalar, Eigen::Dynamic, 1>& vd, int gradient_order = 0) const;
 
   template <typename DerivedV>
   GradientVar<typename DerivedV::Scalar, Eigen::Dynamic, 1> frictionTorques(Eigen::MatrixBase<DerivedV> const & v, int gradient_order = 0) const;

diff --git a/drake/systems/plants/RigidBodyManipulatorURDF.cpp b/drake/systems/plants/RigidBodyManipulatorURDF.cpp
@@ -713,7 +713,7 @@ bool parseLoop(RigidBodyManipulator* model, TiXmlElement* node)
     cerr << "ERROR parsing loop joint rpy" << endl;
     return false;
   }
-  std::shared_ptr<RigidBodyFrame> frameA = make_shared<RigidBodyFrame>(name+"FrameA",body,xyz,rpy);
+  std::shared_ptr<RigidBodyFrame> frameA = allocate_shared<RigidBodyFrame>(Eigen::aligned_allocator<RigidBodyFrame>(), name+"FrameA",body,xyz,rpy);
 
   link_node = node->FirstChildElement("link2");
   linkname = link_node->Attribute("link");
@@ -732,7 +732,7 @@ bool parseLoop(RigidBodyManipulator* model, TiXmlElement* node)
     cerr << "ERROR parsing loop joint rpy" << endl;
     return false;
   }
-  std::shared_ptr<RigidBodyFrame> frameB = make_shared<RigidBodyFrame>(name+"FrameB",body,xyz,rpy);
+  std::shared_ptr<RigidBodyFrame> frameB = allocate_shared<RigidBodyFrame>(Eigen::aligned_allocator<RigidBodyFrame>(), name+"FrameB",body,xyz,rpy);
 
   TiXmlElement* axis_node = node->FirstChildElement("axis");
   if (axis_node && !parseVectorAttribute(axis_node, "xyz", axis)) {
@@ -778,7 +778,7 @@ bool parseFrame(RigidBodyManipulator* model, TiXmlElement* node)
   Matrix4d T;
   T << rpy2rotmat(rpy), xyz, 0,0,0,1;
 
-  std::shared_ptr<RigidBodyFrame> frame = make_shared<RigidBodyFrame>(frame_name,body,T);
+  std::shared_ptr<RigidBodyFrame> frame = allocate_shared<RigidBodyFrame>(Eigen::aligned_allocator<RigidBodyFrame>(), frame_name,body,T);
   model->addFrame(frame);
 
 

diff --git a/drake/systems/plants/constraint/RigidBodyConstraint.cpp b/drake/systems/plants/constraint/RigidBodyConstraint.cpp
@@ -2657,7 +2657,7 @@ void GravityCompensationTorqueConstraint::eval(const double* t, KinematicsCache<
 {
   VectorXd qd = VectorXd::Zero(robot->num_velocities);
   KinematicsCache<double> cache_zero_velocity = robot->doKinematics(cache.getQ(), qd, 1);
-  std::unordered_map<RigidBody const *, GradientVar<double, TWIST_SIZE, 1> > f_ext;
+  eigen_aligned_unordered_map<RigidBody const *, GradientVar<double, TWIST_SIZE, 1> > f_ext;
   auto G_gradientvar = robot->dynamicsBiasTerm(cache_zero_velocity, f_ext, 1);
 
   c.resize(num_constraint);

diff --git a/drake/systems/plants/constructModelmex.cpp b/drake/systems/plants/constructModelmex.cpp
@@ -70,7 +70,7 @@ void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
     //DEBUG
     //mexPrintf("constructModelmex: body %d\n",i);
     //END_DEBUG
-    shared_ptr<RigidBody> b = make_shared<RigidBody>();
+    shared_ptr<RigidBody> b(new RigidBody());
     b->body_index = i;
 
     b->linkname = mxGetStdString(mxGetPropertySafe(pBodies, i, "linkname"));
@@ -352,7 +352,7 @@ void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
   //mexPrintf("constructModelmex: Parsing frames\n");
   //END_DEBUG
   for (int i = 0; i < num_frames; i++) {
-    shared_ptr<RigidBodyFrame> fr = make_shared<RigidBodyFrame>();
+    shared_ptr<RigidBodyFrame> fr(new RigidBodyFrame());
 
 	fr->name = mxGetStdString(mxGetPropertySafe(pFrames, i, "name"));
 

diff --git a/drake/systems/plants/rigidBodyManipulatorMexFunctions.cpp b/drake/systems/plants/rigidBodyManipulatorMexFunctions.cpp
@@ -205,7 +205,7 @@ void dynamicsBiasTermmex(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prh
 
   auto lambda = [](const RigidBodyManipulator &model, KinematicsCache<Scalar> &cache, const MatrixBase<DerivedF> &f_ext_value, const MatrixBase<DerivedDF> &f_ext_gradient, int gradient_order) {
 
-    unordered_map<const RigidBody *, GradientVar<Scalar, 6, 1> > f_ext;
+    eigen_aligned_unordered_map<const RigidBody *, GradientVar<Scalar, 6, 1> > f_ext;
 
     if (f_ext_value.size() > 0) {
       assert(f_ext_value.cols() == model.bodies.size());

diff --git a/drake/systems/plants/test/compareParsersmex.cpp b/drake/systems/plants/test/compareParsersmex.cpp
@@ -75,7 +75,7 @@ void mexFunction( int nlhs, mxArray *plhs[],int nrhs, const mxArray *prhs[] ) {
 		KinematicsCache<double> cpp_cache = cpp_model->doKinematics(cpp_q, cpp_v, 1);
 
 		{ // compare H, C, and B
-		  unordered_map<RigidBody const *, GradientVar<double, TWIST_SIZE, 1> > f_ext;
+			eigen_aligned_unordered_map<RigidBody const *, GradientVar<double, TWIST_SIZE, 1> > f_ext;
 
 		  auto matlab_H = matlab_model->massMatrix(matlab_cache);
 		  auto cpp_H = cpp_model->massMatrix(cpp_cache);

diff --git a/drake/systems/plants/test/debugManipulatorDynamics.cpp b/drake/systems/plants/test/debugManipulatorDynamics.cpp
@@ -26,7 +26,7 @@ int main()
   auto M = model.massMatrix<double>(cache, gradient_order);
   cout << M.value() << endl << endl;
 
-  unordered_map<RigidBody const *, GradientVar<double, TWIST_SIZE, 1> > f_ext;
+  eigen_aligned_unordered_map<RigidBody const *, GradientVar<double, TWIST_SIZE, 1> > f_ext;
   GradientVar<double, TWIST_SIZE, 1> f_ext_r_foot(TWIST_SIZE, 1, nq + nv, gradient_order);
   f_ext_r_foot.value().setRandom();
   f_ext_r_foot.gradient().value().setRandom();

diff --git a/drake/systems/plants/test/testKinematicsCacheChecks.cpp b/drake/systems/plants/test/testKinematicsCacheChecks.cpp
@@ -52,7 +52,7 @@ void performChecks(RigidBodyManipulator& model, KinematicsCache<double>& cache,
   spatial_acceleration.value().setRandom();
   if (gradient_order > 0)
     spatial_acceleration.gradient().value().setRandom();
-  std::unordered_map<RigidBody const *, GradientVar<double, TWIST_SIZE, 1> > f_ext;
+  eigen_aligned_unordered_map<RigidBody const *, GradientVar<double, TWIST_SIZE, 1> > f_ext;
 
   if (gradient_order == 0) {
     checkForErrors(settings.expect_error_on_configuration_methods, model, &RigidBodyManipulator::centerOfMass<double>, cache, RigidBodyManipulator::default_robot_num_set);

diff --git a/drake/systems/plants/test/urdfManipulatorDynamicsTest.cpp b/drake/systems/plants/test/urdfManipulatorDynamicsTest.cpp
@@ -43,7 +43,7 @@ int main(int argc, char* argv[])
   auto H = model->massMatrix(cache);
   cout << H.value() << endl;
 
-  unordered_map<RigidBody const *, GradientVar<double, TWIST_SIZE, 1> > f_ext;
+  eigen_aligned_unordered_map<RigidBody const *, GradientVar<double, TWIST_SIZE, 1> > f_ext;
   auto C = model->dynamicsBiasTerm(cache, f_ext);
   cout << C.value() << endl;
 

diff --git a/drake/util/drakeUtil.h b/drake/util/drakeUtil.h
@@ -9,6 +9,7 @@
 #include <vector>
 #include <utility>
 #include <Eigen/Core>
+#include <unordered_map>
 
 #ifndef DRAKE_UTIL_H_
 #define DRAKE_UTIL_H_
@@ -24,6 +25,9 @@
     #define DLLEXPORT
 #endif
 
+template <typename Key, typename T>
+using eigen_aligned_unordered_map = std::unordered_map<Key, T, std::hash<Key>, std::equal_to<Key>, Eigen::aligned_allocator<std::pair<Key const, T > > >;
+
 template <typename Derived>
 inline void sizecheck(const Eigen::MatrixBase<Derived>& mat, size_t rows, size_t cols){
   if ((mat.rows() != rows) || (mat.cols() != cols))