Fix failed unit tests on 32-bit machine

include/aikido/constraint/uniform/RnBoxConstraint.hpp

@@ -85,6 +85,9 @@ class RBoxConstraint : public constraint::Differentiable,
   std::unique_ptr<common::RNG> mRng;
   VectorNd mLowerLimits;
   VectorNd mUpperLimits;
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(VectorNd::NeedsToAlign)
 };
 
 using R0BoxConstraint = RBoxConstraint<0>;

include/aikido/constraint/uniform/RnConstantSampler.hpp

@@ -35,6 +35,9 @@ class RConstantSampler : public constraint::Sampleable
 private:
   std::shared_ptr<statespace::R<N>> mSpace;
   VectorNd mValue;
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(VectorNd::NeedsToAlign)
 };
 
 using R0ConstantSampler = RConstantSampler<0>;

include/aikido/constraint/uniform/SE2BoxConstraint.hpp

@@ -79,6 +79,9 @@ class SE2BoxConstraint : public constraint::Projectable,
 
   // DOF of the joint
   std::size_t mDimension;
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 } // namespace uniform

include/aikido/constraint/uniform/detail/RnConstantSampler-impl.hpp

@@ -45,6 +45,9 @@ class RnConstantSamplerSampleGenerator : public constraint::SampleGenerator
 private:
   std::shared_ptr<statespace::R<N>> mSpace;
   VectorNd mValue;
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(VectorNd::NeedsToAlign)
 };
 
 //==============================================================================

include/aikido/distance/SE2Weighted.hpp

@@ -41,6 +41,9 @@ class SE2Weighted : public DistanceMetric
   std::shared_ptr<statespace::SE2> mStateSpace;
 
   Eigen::Vector2d mWeights;
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 } // namespace distance

include/aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp

@@ -21,7 +21,6 @@ namespace vectorfield {
 class MoveEndEffectorOffsetVectorField : public BodyNodePoseVectorField
 {
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   /// Constructor
   ///
   /// \param[in] stateSpace MetaSkeleton state space.
@@ -77,6 +76,9 @@ class MoveEndEffectorOffsetVectorField : public BodyNodePoseVectorField
 
   /// Start pose of the end-effector.
   Eigen::Isometry3d mStartPose;
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 } // namespace vectorfield

include/aikido/planner/vectorfield/MoveEndEffectorPoseVectorField.hpp

@@ -20,7 +20,6 @@ namespace vectorfield {
 class MoveEndEffectorPoseVectorField : public BodyNodePoseVectorField
 {
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   /// Constructor.
   ///
   /// \param[in] stateSpace MetaSkeleton state space.
@@ -62,6 +61,9 @@ class MoveEndEffectorPoseVectorField : public BodyNodePoseVectorField
 
   /// Conversion ratio from radius to meter.
   double mConversionRatioFromRadiusToMeter;
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 } // namespace vectorfield

src/common/StepSequence.cpp

@@ -6,6 +6,8 @@
 namespace aikido {
 namespace common {
 
+constexpr double COMPARE_PRECISION = 1e-7;
+
 //==============================================================================
 StepSequence::StepSequence(
     double stepSize,
@@ -111,7 +113,9 @@ void StepSequence::updateLength()
 
   double endPivot = mStartPoint + floorStepRatio * mStepSize;
 
-  if (mEndPoint > endPivot)
+  // When endPivot is close to mEndPoint (below COMPARE_PRECISION threshold), we
+  // will think endPivot equals to mEndPoint. This is because of numeric issue.
+  if (std::abs(mEndPoint - endPivot) > COMPARE_PRECISION)
   {
     if (mIncludeEndPoint)
       ++mNumSteps;

src/planner/vectorfield/VectorFieldPlanner.cpp

@@ -165,17 +165,18 @@ std::unique_ptr<aikido::trajectory::Spline> planToEndEffectorOffset(
       metaskeleton, MetaSkeletonStateSaver::Options::POSITIONS);
   DART_UNUSED(saver);
 
-  auto vectorfield = std::make_shared<MoveEndEffectorOffsetVectorField>(
-      stateSpace,
-      metaskeleton,
-      bn,
-      direction,
-      minDistance,
-      maxDistance,
-      positionTolerance,
-      angularTolerance,
-      initialStepSize,
-      jointLimitTolerance);
+  auto vectorfield
+      = dart::common::make_aligned_shared<MoveEndEffectorOffsetVectorField>(
+          stateSpace,
+          metaskeleton,
+          bn,
+          direction,
+          minDistance,
+          maxDistance,
+          positionTolerance,
+          angularTolerance,
+          initialStepSize,
+          jointLimitTolerance);
 
   auto compoundConstraint
       = std::make_shared<constraint::TestableIntersection>(stateSpace);
@@ -217,15 +218,16 @@ std::unique_ptr<aikido::trajectory::Spline> planToEndEffectorPose(
       metaskeleton, MetaSkeletonStateSaver::Options::POSITIONS);
   DART_UNUSED(saver);
 
-  auto vectorfield = std::make_shared<MoveEndEffectorPoseVectorField>(
-      stateSpace,
-      metaskeleton,
-      bn,
-      goalPose,
-      poseErrorTolerance,
-      conversionRatioInGeodesicDistance,
-      initialStepSize,
-      jointLimitTolerance);
+  auto vectorfield
+      = dart::common::make_aligned_shared<MoveEndEffectorPoseVectorField>(
+          stateSpace,
+          metaskeleton,
+          bn,
+          goalPose,
+          poseErrorTolerance,
+          conversionRatioInGeodesicDistance,
+          initialStepSize,
+          jointLimitTolerance);
 
   auto compoundConstraint
       = std::make_shared<aikido::constraint::TestableIntersection>(stateSpace);

src/planner/vectorfield/VectorFieldUtil.cpp

@@ -52,12 +52,15 @@ class DesiredTwistFunction : public dart::optimizer::Function
     grad = mJacobian.transpose() * (mJacobian * qd - mTwist);
   }
 
-private:
+protected:
   /// Twist.
   Twist mTwist;
 
   /// Jacobian of Meta Skeleton.
   Jacobian mJacobian;
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 }
 
@@ -123,7 +126,8 @@ bool computeJointVelocityFromTwist(
 
   problem->setInitialGuess(initialGuess);
   problem->setObjective(
-      std::make_shared<DesiredTwistFunction>(desiredTwist, jacobian));
+      dart::common::make_aligned_shared<DesiredTwistFunction>(
+          desiredTwist, jacobian));
 
   dart::optimizer::NloptSolver solver(problem, nlopt::LD_LBFGS);
   if (!solver.solve())

tests/constraint/test_CartesianProductProjectable.cpp

@@ -1,3 +1,4 @@
+#include <dart/common/Memory.hpp>
 #include <gtest/gtest.h>
 #include <aikido/constraint/CartesianProductProjectable.hpp>
 #include <aikido/constraint/Projectable.hpp>
@@ -23,9 +24,9 @@ class CartesianProductProjectableTest : public testing::Test
     rvss2 = std::make_shared<R2>();
 
     // Constraints
-    rvBox1 = std::make_shared<R3BoxConstraint>(
+    rvBox1 = dart::common::make_aligned_shared<R3BoxConstraint>(
         rvss1, nullptr, Eigen::Vector3d(1, 1, 1), Eigen::Vector3d(2, 1, 1));
-    rvBox2 = std::make_shared<R2BoxConstraint>(
+    rvBox2 = dart::common::make_aligned_shared<R2BoxConstraint>(
         rvss2, nullptr, Eigen::Vector2d(1, 1), Eigen::Vector2d(2, 2));
 
     projectables.push_back(rvBox1);

tests/constraint/test_CyclicSampleable.cpp

@@ -20,6 +20,8 @@ using aikido::common::RNGWrapper;
 using aikido::common::RNG;
 using DefaultRNG = RNGWrapper<std::default_random_engine>;
 
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2d)
+
 static std::unique_ptr<DefaultRNG> make_rng()
 {
   return make_unique<RNGWrapper<std::default_random_engine>>(0);

tests/constraint/test_FiniteSampleable.cpp

@@ -1,3 +1,4 @@
+#include <Eigen/StdVector>
 #include <aikido/constraint/FiniteSampleable.hpp>
 #include <aikido/statespace/Rn.hpp>
 #include <aikido/statespace/StateSpace.hpp>
@@ -11,6 +12,8 @@ using aikido::constraint::FiniteSampleable;
 using aikido::constraint::SampleGenerator;
 using State = aikido::statespace::StateSpace::State;
 
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2d)
+
 TEST(FiniteSampleableTest, ConstructorThrowsOnNullStateSpace)
 {
   R1 rvss;
@@ -80,7 +83,6 @@ TEST(FiniteSampleableTest, SingleSampleGenerator)
 
 TEST(FiniteSampleableTest, FiniteSampleGenerator)
 {
-
   // Finite-samples
   Eigen::Vector2d v1(0, 1);
   Eigen::Vector2d v2(2, 3);

tests/constraint/test_FrameDifferentiable.cpp

@@ -31,7 +31,7 @@ class FrameDifferentiableTest : public ::testing::Test
   virtual void SetUp()
   {
 
-    tsr = std::make_shared<TSR>(
+    tsr = dart::common::make_aligned_shared<TSR>(
         std::unique_ptr<RNG>(new RNGWrapper<std::default_random_engine>(0)));
 
     Eigen::MatrixXd Bw = Eigen::Matrix<double, 6, 2>::Zero();

tests/constraint/test_RnBoxConstraint.cpp

@@ -592,7 +592,7 @@ TEST_F(
 //==============================================================================
 TEST_F(RnBoxConstraintTests, R2_createSampleGenerator)
 {
-  auto constraint = std::make_shared<R2BoxConstraint>(
+  auto constraint = dart::common::make_aligned_shared<R2BoxConstraint>(
       mR2StateSpace, mRng->clone(), mLowerLimits, mUpperLimits);
 
   auto generator = constraint->createSampleGenerator();
@@ -632,7 +632,7 @@ TEST_F(RnBoxConstraintTests, R2_createSampleGenerator_RNGIsNull_Throws)
 {
   // We need to use make_shared here because createSampleGenerator calls
   // shared_from_this, provided by enable_shared_from_this.
-  auto constraint = std::make_shared<R2BoxConstraint>(
+  auto constraint = dart::common::make_aligned_shared<R2BoxConstraint>(
       mR2StateSpace, nullptr, mLowerLimits, mUpperLimits);
 
   EXPECT_THROW({ constraint->createSampleGenerator(); }, std::invalid_argument);
@@ -660,11 +660,11 @@ TEST_F(RnBoxConstraintTests, R2_createSampleGenerator_Unbounded_Throws)
 
   // We need to use make_shared here because createSampleGenerator calls
   // shared_from_this, provided by enable_shared_from_this.
-  auto unbounded1 = std::make_shared<R2BoxConstraint>(
+  auto unbounded1 = dart::common::make_aligned_shared<R2BoxConstraint>(
       mR2StateSpace, mRng->clone(), noLowerBound, mUpperLimits);
   EXPECT_THROW({ unbounded1->createSampleGenerator(); }, std::runtime_error);
 
-  auto unbounded2 = std::make_shared<R2BoxConstraint>(
+  auto unbounded2 = dart::common::make_aligned_shared<R2BoxConstraint>(
       mR2StateSpace, mRng->clone(), mLowerLimits, noUpperBound);
   EXPECT_THROW({ unbounded2->createSampleGenerator(); }, std::runtime_error);
 }

tests/constraint/test_RnConstantSampler.cpp

@@ -78,8 +78,9 @@ void testSampleGenerator(int dimension = N)
   auto stateSpace = std::make_shared<statespace::R<N>>(dimension);
   EXPECT_EQ(stateSpace->getDimension(), dimension);
 
-  auto sampler = std::make_shared<constraint::uniform::RConstantSampler<N>>(
-      stateSpace, value);
+  auto sampler = dart::common::
+      make_aligned_shared<constraint::uniform::RConstantSampler<N>>(
+          stateSpace, value);
   EXPECT_EQ(sampler->getStateSpace(), stateSpace);
   EXPECT_TRUE(
       tests::CompareEigenMatrices(sampler->getConstantValue(), value, 1e-6));

tests/constraint/test_SE2BoxConstraint.cpp

@@ -28,8 +28,9 @@ class SE2BoxConstraintTests : public ::testing::Test
 
   void SetUp() override
   {
-    mSE2StateSpace = std::make_shared<SE2>();
-    mSE2Distance = std::make_shared<SE2Weighted>(mSE2StateSpace);
+    mSE2StateSpace = dart::common::make_aligned_shared<SE2>();
+    mSE2Distance
+        = dart::common::make_aligned_shared<SE2Weighted>(mSE2StateSpace);
     mRng = make_unique<RNGWrapper<std::default_random_engine>>(0);
 
     mLowerLimits = Vector2d(-1., 1.);
@@ -211,7 +212,7 @@ TEST_F(SE2BoxConstraintTests, projectUnSatisfiedConstraintProjects)
 //==============================================================================
 TEST_F(SE2BoxConstraintTests, createSampleGenerator)
 {
-  auto constraint = std::make_shared<SE2BoxConstraint>(
+  auto constraint = dart::common::make_aligned_shared<SE2BoxConstraint>(
       mSE2StateSpace, mRng->clone(), mLowerLimits, mUpperLimits);
 
   auto generator = constraint->createSampleGenerator();
@@ -232,7 +233,7 @@ TEST_F(SE2BoxConstraintTests, createSampleGeneratorThrowOnNUllRNG)
 {
   // We need to use make_shared here because createSampleGenerator calls
   // shared_from_this, provided by enable_shared_from_this.
-  auto constraint = std::make_shared<SE2BoxConstraint>(
+  auto constraint = dart::common::make_aligned_shared<SE2BoxConstraint>(
       mSE2StateSpace, nullptr, mLowerLimits, mUpperLimits);
 
   EXPECT_THROW({ constraint->createSampleGenerator(); }, std::invalid_argument);
@@ -249,11 +250,11 @@ TEST_F(SE2BoxConstraintTests, createSampleGeneratorThrowsIfUnbounded)
 
   // We need to use make_shared here because createSampleGenerator calls
   // shared_from_this, provided by enable_shared_from_this.
-  auto unbounded1 = std::make_shared<SE2BoxConstraint>(
+  auto unbounded1 = dart::common::make_aligned_shared<SE2BoxConstraint>(
       mSE2StateSpace, mRng->clone(), noLowerBound, mUpperLimits);
   EXPECT_THROW({ unbounded1->createSampleGenerator(); }, std::runtime_error);
 
-  auto unbounded2 = std::make_shared<SE2BoxConstraint>(
+  auto unbounded2 = dart::common::make_aligned_shared<SE2BoxConstraint>(
       mSE2StateSpace, mRng->clone(), mLowerLimits, noUpperBound);
   EXPECT_THROW({ unbounded2->createSampleGenerator(); }, std::runtime_error);
 }

tests/control/test_BarrettHandKinematicSimulationPositionCommandExecutor.cpp

@@ -35,6 +35,8 @@ class BarrettHandKinematicSimulationPositionCommandExecutorTest
     : public testing::Test
 {
 public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
   void setGeometry(const BodyNodePtr& bn)
   {
     // Create a BoxShape to be used for both visualization and collision

tests/planner/parabolic/test_ParabolicTimer.cpp

@@ -20,6 +20,8 @@ using aikido::planner::parabolic::convertToSpline;
 
 class ParabolicTimerTests : public ::testing::Test
 {
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 protected:
   void SetUp() override
   {

tests/planner/parabolic/test_TimePostProcessor.cpp

@@ -14,8 +14,6 @@ using aikido::planner::parabolic::ParabolicTimer;
 
 class TimePostProcessorTests : public ::testing::Test
 {
-public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 protected:
   void SetUp() override
   {