From dc813c86460edaec027e899a537715500ab53d4d Mon Sep 17 00:00:00 2001 From: daqing Date: Fri, 2 Mar 2018 16:43:24 -0800 Subject: [PATCH 01/14] add threshold to handle numeric precision --- src/common/StepSequence.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/common/StepSequence.cpp b/src/common/StepSequence.cpp index 717345d528..c8938bd838 100644 --- a/src/common/StepSequence.cpp +++ b/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; From dd6b018bd1b8a3d2d428606b49449b1fdcdfdb72 Mon Sep 17 00:00:00 2001 From: Daqing Yi Date: Fri, 2 Mar 2018 17:20:24 -0800 Subject: [PATCH 02/14] fix code style --- src/common/StepSequence.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/common/StepSequence.cpp b/src/common/StepSequence.cpp index c8938bd838..c01b322f92 100644 --- a/src/common/StepSequence.cpp +++ b/src/common/StepSequence.cpp @@ -115,7 +115,7 @@ void StepSequence::updateLength() // 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 (std::abs(mEndPoint - endPivot) > COMPARE_PRECISION) { if (mIncludeEndPoint) ++mNumSteps; From eaca03b279c9cafccab3bac1ada4f77230be545d Mon Sep 17 00:00:00 2001 From: daqing Date: Tue, 6 Mar 2018 16:53:11 -0800 Subject: [PATCH 03/14] fixed size eigen matrix aligment issue --- include/aikido/constraint/uniform/RnBoxConstraint.hpp | 2 ++ .../aikido/constraint/uniform/detail/RnConstantSampler-impl.hpp | 1 + include/aikido/planner/parabolic/ParabolicTimer.hpp | 2 ++ src/planner/vectorfield/VectorFieldUtil.cpp | 2 ++ tests/planner/parabolic/test_ParabolicTimer.cpp | 2 ++ 5 files changed, 9 insertions(+) diff --git a/include/aikido/constraint/uniform/RnBoxConstraint.hpp b/include/aikido/constraint/uniform/RnBoxConstraint.hpp index 101630c2f0..6744408f3b 100644 --- a/include/aikido/constraint/uniform/RnBoxConstraint.hpp +++ b/include/aikido/constraint/uniform/RnBoxConstraint.hpp @@ -20,6 +20,8 @@ class RBoxConstraint : public constraint::Differentiable, public constraint::Testable { public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + using constraint::Projectable::project; using constraint::Differentiable::getValueAndJacobian; diff --git a/include/aikido/constraint/uniform/detail/RnConstantSampler-impl.hpp b/include/aikido/constraint/uniform/detail/RnConstantSampler-impl.hpp index 08a569ecc2..0daef9cb05 100644 --- a/include/aikido/constraint/uniform/detail/RnConstantSampler-impl.hpp +++ b/include/aikido/constraint/uniform/detail/RnConstantSampler-impl.hpp @@ -29,6 +29,7 @@ template class RnConstantSamplerSampleGenerator : public constraint::SampleGenerator { public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW using VectorNd = Eigen::Matrix; RnConstantSamplerSampleGenerator( diff --git a/include/aikido/planner/parabolic/ParabolicTimer.hpp b/include/aikido/planner/parabolic/ParabolicTimer.hpp index 0fe0a54cf3..0b32a2a3b9 100644 --- a/include/aikido/planner/parabolic/ParabolicTimer.hpp +++ b/include/aikido/planner/parabolic/ParabolicTimer.hpp @@ -73,6 +73,8 @@ std::unique_ptr convertToSpline( class ParabolicTimer : public aikido::planner::TrajectoryPostProcessor { public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + /// \param _velocityLimits Maximum velocity for each dimension. /// \param _accelerationLimits Maximum acceleration for each dimension. ParabolicTimer( diff --git a/src/planner/vectorfield/VectorFieldUtil.cpp b/src/planner/vectorfield/VectorFieldUtil.cpp index 95f53e52a6..ad6ca70d9a 100644 --- a/src/planner/vectorfield/VectorFieldUtil.cpp +++ b/src/planner/vectorfield/VectorFieldUtil.cpp @@ -17,6 +17,8 @@ namespace { class DesiredTwistFunction : public dart::optimizer::Function { public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + using Twist = Eigen::Vector6d; using Jacobian = dart::math::Jacobian; diff --git a/tests/planner/parabolic/test_ParabolicTimer.cpp b/tests/planner/parabolic/test_ParabolicTimer.cpp index 9eeb88f0da..7b013229bf 100644 --- a/tests/planner/parabolic/test_ParabolicTimer.cpp +++ b/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 { From 00a3f57a4537d6932ac224d5f54424bc32517c86 Mon Sep 17 00:00:00 2001 From: daqing Date: Tue, 6 Mar 2018 16:56:30 -0800 Subject: [PATCH 04/14] fixed size eigen matrix + STL --- include/aikido/constraint/uniform/RnConstantSampler.hpp | 1 + tests/constraint/test_CyclicSampleable.cpp | 2 ++ tests/constraint/test_FiniteSampleable.cpp | 4 +++- ..._BarrettHandKinematicSimulationPositionCommandExecutor.cpp | 2 ++ 4 files changed, 8 insertions(+), 1 deletion(-) diff --git a/include/aikido/constraint/uniform/RnConstantSampler.hpp b/include/aikido/constraint/uniform/RnConstantSampler.hpp index 372159bff9..f5ee6cd2ea 100644 --- a/include/aikido/constraint/uniform/RnConstantSampler.hpp +++ b/include/aikido/constraint/uniform/RnConstantSampler.hpp @@ -14,6 +14,7 @@ template class RConstantSampler : public constraint::Sampleable { public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW using VectorNd = Eigen::Matrix; /// Constructor. diff --git a/tests/constraint/test_CyclicSampleable.cpp b/tests/constraint/test_CyclicSampleable.cpp index 95d6465a98..ec05ff6c41 100644 --- a/tests/constraint/test_CyclicSampleable.cpp +++ b/tests/constraint/test_CyclicSampleable.cpp @@ -20,6 +20,8 @@ using aikido::common::RNGWrapper; using aikido::common::RNG; using DefaultRNG = RNGWrapper; +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2d) + static std::unique_ptr make_rng() { return make_unique>(0); diff --git a/tests/constraint/test_FiniteSampleable.cpp b/tests/constraint/test_FiniteSampleable.cpp index 074e12ea64..faafef3d18 100644 --- a/tests/constraint/test_FiniteSampleable.cpp +++ b/tests/constraint/test_FiniteSampleable.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include "../eigen_tests.hpp" #include @@ -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); diff --git a/tests/control/test_BarrettHandKinematicSimulationPositionCommandExecutor.cpp b/tests/control/test_BarrettHandKinematicSimulationPositionCommandExecutor.cpp index 9edeb6cef3..ef6816c217 100644 --- a/tests/control/test_BarrettHandKinematicSimulationPositionCommandExecutor.cpp +++ b/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 From 1af13719dd32094a4817819aa0f24a6e1cd695a7 Mon Sep 17 00:00:00 2001 From: daqing Date: Tue, 6 Mar 2018 17:36:21 -0800 Subject: [PATCH 05/14] fix code style --- tests/constraint/test_FiniteSampleable.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/constraint/test_FiniteSampleable.cpp b/tests/constraint/test_FiniteSampleable.cpp index faafef3d18..be7d62e3a4 100644 --- a/tests/constraint/test_FiniteSampleable.cpp +++ b/tests/constraint/test_FiniteSampleable.cpp @@ -1,7 +1,7 @@ +#include #include #include #include -#include #include "../eigen_tests.hpp" #include From c7b9a9daf8885ff9f024208e2407d467d4bb5d37 Mon Sep 17 00:00:00 2001 From: daqing Date: Mon, 12 Mar 2018 12:47:32 -0700 Subject: [PATCH 06/14] empty commit for sync --- include/aikido/constraint/uniform/RnConstantSampler.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/include/aikido/constraint/uniform/RnConstantSampler.hpp b/include/aikido/constraint/uniform/RnConstantSampler.hpp index f5ee6cd2ea..37e0022be4 100644 --- a/include/aikido/constraint/uniform/RnConstantSampler.hpp +++ b/include/aikido/constraint/uniform/RnConstantSampler.hpp @@ -15,6 +15,7 @@ class RConstantSampler : public constraint::Sampleable { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW + using VectorNd = Eigen::Matrix; /// Constructor. From 49b29f35c98edc7c79bfbab7bc3b9c5be43395bf Mon Sep 17 00:00:00 2001 From: daqing Date: Wed, 21 Mar 2018 15:28:04 -0700 Subject: [PATCH 07/14] fix eigen alignment --- .../constraint/uniform/RnBoxConstraint.hpp | 4 +- .../constraint/uniform/RnConstantSampler.hpp | 4 +- .../uniform/detail/RnConstantSampler-impl.hpp | 3 +- .../planner/parabolic/ParabolicTimer.hpp | 1 - .../MoveEndEffectorOffsetVectorField.hpp | 3 +- .../MoveEndEffectorPoseVectorField.hpp | 3 +- .../vectorfield/VectorFieldPlanner.cpp | 6 +- src/planner/vectorfield/VectorFieldUtil.cpp | 111 +++++++++--------- .../test_CartesianProductProjectable.cpp | 5 +- tests/constraint/test_FrameDifferentiable.cpp | 2 +- tests/constraint/test_RnBoxConstraint.cpp | 8 +- .../parabolic/test_TimePostProcessor.cpp | 2 - .../vectorfield/test_VectorFieldPlanner.cpp | 1 + 13 files changed, 78 insertions(+), 75 deletions(-) diff --git a/include/aikido/constraint/uniform/RnBoxConstraint.hpp b/include/aikido/constraint/uniform/RnBoxConstraint.hpp index 6744408f3b..38c72b5c87 100644 --- a/include/aikido/constraint/uniform/RnBoxConstraint.hpp +++ b/include/aikido/constraint/uniform/RnBoxConstraint.hpp @@ -20,8 +20,6 @@ class RBoxConstraint : public constraint::Differentiable, public constraint::Testable { public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - using constraint::Projectable::project; using constraint::Differentiable::getValueAndJacobian; @@ -87,6 +85,8 @@ class RBoxConstraint : public constraint::Differentiable, std::unique_ptr mRng; VectorNd mLowerLimits; VectorNd mUpperLimits; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(VectorNd::NeedsToAlign) }; using R0BoxConstraint = RBoxConstraint<0>; diff --git a/include/aikido/constraint/uniform/RnConstantSampler.hpp b/include/aikido/constraint/uniform/RnConstantSampler.hpp index 37e0022be4..8387af949b 100644 --- a/include/aikido/constraint/uniform/RnConstantSampler.hpp +++ b/include/aikido/constraint/uniform/RnConstantSampler.hpp @@ -14,8 +14,6 @@ template class RConstantSampler : public constraint::Sampleable { public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - using VectorNd = Eigen::Matrix; /// Constructor. @@ -37,6 +35,8 @@ class RConstantSampler : public constraint::Sampleable private: std::shared_ptr> mSpace; VectorNd mValue; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(VectorNd::NeedsToAlign) }; using R0ConstantSampler = RConstantSampler<0>; diff --git a/include/aikido/constraint/uniform/detail/RnConstantSampler-impl.hpp b/include/aikido/constraint/uniform/detail/RnConstantSampler-impl.hpp index 0daef9cb05..ae43a62f41 100644 --- a/include/aikido/constraint/uniform/detail/RnConstantSampler-impl.hpp +++ b/include/aikido/constraint/uniform/detail/RnConstantSampler-impl.hpp @@ -29,7 +29,6 @@ template class RnConstantSamplerSampleGenerator : public constraint::SampleGenerator { public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW using VectorNd = Eigen::Matrix; RnConstantSamplerSampleGenerator( @@ -46,6 +45,8 @@ class RnConstantSamplerSampleGenerator : public constraint::SampleGenerator private: std::shared_ptr> mSpace; VectorNd mValue; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(VectorNd::NeedsToAlign) }; //============================================================================== diff --git a/include/aikido/planner/parabolic/ParabolicTimer.hpp b/include/aikido/planner/parabolic/ParabolicTimer.hpp index 0b32a2a3b9..2eef93537f 100644 --- a/include/aikido/planner/parabolic/ParabolicTimer.hpp +++ b/include/aikido/planner/parabolic/ParabolicTimer.hpp @@ -73,7 +73,6 @@ std::unique_ptr convertToSpline( class ParabolicTimer : public aikido::planner::TrajectoryPostProcessor { public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// \param _velocityLimits Maximum velocity for each dimension. /// \param _accelerationLimits Maximum acceleration for each dimension. diff --git a/include/aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp b/include/aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp index f5253e3f45..77627df397 100644 --- a/include/aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp +++ b/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,8 @@ class MoveEndEffectorOffsetVectorField : public BodyNodePoseVectorField /// Start pose of the end-effector. Eigen::Isometry3d mStartPose; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace vectorfield diff --git a/include/aikido/planner/vectorfield/MoveEndEffectorPoseVectorField.hpp b/include/aikido/planner/vectorfield/MoveEndEffectorPoseVectorField.hpp index 22cb89fb33..c9c00c220e 100644 --- a/include/aikido/planner/vectorfield/MoveEndEffectorPoseVectorField.hpp +++ b/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,8 @@ class MoveEndEffectorPoseVectorField : public BodyNodePoseVectorField /// Conversion ratio from radius to meter. double mConversionRatioFromRadiusToMeter; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace vectorfield diff --git a/src/planner/vectorfield/VectorFieldPlanner.cpp b/src/planner/vectorfield/VectorFieldPlanner.cpp index 1105af5730..65cac3458f 100644 --- a/src/planner/vectorfield/VectorFieldPlanner.cpp +++ b/src/planner/vectorfield/VectorFieldPlanner.cpp @@ -40,7 +40,7 @@ std::unique_ptr followVectorField( boost::numeric::odeint::vector_space_algebra>; std::size_t dimension = vectorField.getStateSpace()->getDimension(); - auto integrator = std::make_shared( + auto integrator = dart::common::make_aligned_shared( &vectorField, &constraint, timelimit.count(), checkConstraintResolution); integrator->start(); @@ -165,7 +165,7 @@ std::unique_ptr planToEndEffectorOffset( metaskeleton, MetaSkeletonStateSaver::Options::POSITIONS); DART_UNUSED(saver); - auto vectorfield = std::make_shared( + auto vectorfield = dart::common::make_aligned_shared( stateSpace, metaskeleton, bn, @@ -217,7 +217,7 @@ std::unique_ptr planToEndEffectorPose( metaskeleton, MetaSkeletonStateSaver::Options::POSITIONS); DART_UNUSED(saver); - auto vectorfield = std::make_shared( + auto vectorfield = dart::common::make_aligned_shared( stateSpace, metaskeleton, bn, diff --git a/src/planner/vectorfield/VectorFieldUtil.cpp b/src/planner/vectorfield/VectorFieldUtil.cpp index ad6ca70d9a..03f7d70f50 100644 --- a/src/planner/vectorfield/VectorFieldUtil.cpp +++ b/src/planner/vectorfield/VectorFieldUtil.cpp @@ -17,8 +17,6 @@ namespace { class DesiredTwistFunction : public dart::optimizer::Function { public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - using Twist = Eigen::Vector6d; using Jacobian = dart::math::Jacobian; @@ -54,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 }; } @@ -81,58 +82,58 @@ bool computeJointVelocityFromTwist( using Eigen::VectorXd; // Use LBFGS to find joint angles that won't violate the joint limits. - const Jacobian jacobian = metaSkeleton->getWorldJacobian(bodyNode); - - const std::size_t numDofs = metaSkeleton->getNumDofs(); - - jointVelocity = Eigen::VectorXd::Zero(numDofs); - VectorXd positions = metaSkeleton->getPositions(); - VectorXd initialGuess = metaSkeleton->getVelocities(); - VectorXd positionLowerLimits = metaSkeleton->getPositionLowerLimits(); - VectorXd positionUpperLimits = metaSkeleton->getPositionUpperLimits(); - VectorXd velocityLowerLimits = jointVelocityLowerLimits; - VectorXd velocityUpperLimits = jointVelocityUpperLimits; - - const auto problem = std::make_shared(numDofs); - if (enforceJointVelocityLimits) - { - for (std::size_t i = 0; i < numDofs; ++i) - { - const double position = positions[i]; - const double positionLowerLimit = positionLowerLimits[i]; - const double positionUpperLimit = positionUpperLimits[i]; - const double velocityLowerLimit = velocityLowerLimits[i]; - const double velocityUpperLimit = velocityUpperLimits[i]; - - if (position + stepSize * velocityLowerLimit - <= positionLowerLimit + jointLimitPadding) - { - velocityLowerLimits[i] = 0.0; - } - - if (position + stepSize * velocityUpperLimit - >= positionUpperLimit - jointLimitPadding) - { - velocityUpperLimits[i] = 0.0; - } - - initialGuess[i] = common::clamp( - initialGuess[i], velocityLowerLimits[i], velocityUpperLimits[i]); - } - problem->setLowerBounds(velocityLowerLimits); - problem->setUpperBounds(velocityUpperLimits); - } - - problem->setInitialGuess(initialGuess); - problem->setObjective( - std::make_shared(desiredTwist, jacobian)); - - dart::optimizer::NloptSolver solver(problem, nlopt::LD_LBFGS); - if (!solver.solve()) - { - return false; - } - jointVelocity = problem->getOptimalSolution(); +// const Jacobian jacobian = metaSkeleton->getWorldJacobian(bodyNode); + +// const std::size_t numDofs = metaSkeleton->getNumDofs(); + +// jointVelocity = Eigen::VectorXd::Zero(numDofs); +// VectorXd positions = metaSkeleton->getPositions(); +// VectorXd initialGuess = metaSkeleton->getVelocities(); +// VectorXd positionLowerLimits = metaSkeleton->getPositionLowerLimits(); +// VectorXd positionUpperLimits = metaSkeleton->getPositionUpperLimits(); +// VectorXd velocityLowerLimits = jointVelocityLowerLimits; +// VectorXd velocityUpperLimits = jointVelocityUpperLimits; + +// const auto problem = std::make_shared(numDofs); +// if (enforceJointVelocityLimits) +// { +// for (std::size_t i = 0; i < numDofs; ++i) +// { +// const double position = positions[i]; +// const double positionLowerLimit = positionLowerLimits[i]; +// const double positionUpperLimit = positionUpperLimits[i]; +// const double velocityLowerLimit = velocityLowerLimits[i]; +// const double velocityUpperLimit = velocityUpperLimits[i]; + +// if (position + stepSize * velocityLowerLimit +// <= positionLowerLimit + jointLimitPadding) +// { +// velocityLowerLimits[i] = 0.0; +// } + +// if (position + stepSize * velocityUpperLimit +// >= positionUpperLimit - jointLimitPadding) +// { +// velocityUpperLimits[i] = 0.0; +// } + +// initialGuess[i] = common::clamp( +// initialGuess[i], velocityLowerLimits[i], velocityUpperLimits[i]); +// } +// problem->setLowerBounds(velocityLowerLimits); +// problem->setUpperBounds(velocityUpperLimits); +// } + +// problem->setInitialGuess(initialGuess); +// problem->setObjective( +// dart::common::make_aligned_shared(desiredTwist, jacobian)); + +// dart::optimizer::NloptSolver solver(problem, nlopt::LD_LBFGS); +// if (!solver.solve()) +// { +// return false; +// } +// jointVelocity = problem->getOptimalSolution(); return true; } diff --git a/tests/constraint/test_CartesianProductProjectable.cpp b/tests/constraint/test_CartesianProductProjectable.cpp index f0d9ed4d1d..65df33a5f7 100644 --- a/tests/constraint/test_CartesianProductProjectable.cpp +++ b/tests/constraint/test_CartesianProductProjectable.cpp @@ -1,4 +1,5 @@ #include +#include #include #include #include @@ -23,9 +24,9 @@ class CartesianProductProjectableTest : public testing::Test rvss2 = std::make_shared(); // Constraints - rvBox1 = std::make_shared( + rvBox1 = dart::common::make_aligned_shared( rvss1, nullptr, Eigen::Vector3d(1, 1, 1), Eigen::Vector3d(2, 1, 1)); - rvBox2 = std::make_shared( + rvBox2 = dart::common::make_aligned_shared( rvss2, nullptr, Eigen::Vector2d(1, 1), Eigen::Vector2d(2, 2)); projectables.push_back(rvBox1); diff --git a/tests/constraint/test_FrameDifferentiable.cpp b/tests/constraint/test_FrameDifferentiable.cpp index 5223bb91f5..982a5408df 100644 --- a/tests/constraint/test_FrameDifferentiable.cpp +++ b/tests/constraint/test_FrameDifferentiable.cpp @@ -31,7 +31,7 @@ class FrameDifferentiableTest : public ::testing::Test virtual void SetUp() { - tsr = std::make_shared( + tsr = dart::common::make_aligned_shared( std::unique_ptr(new RNGWrapper(0))); Eigen::MatrixXd Bw = Eigen::Matrix::Zero(); diff --git a/tests/constraint/test_RnBoxConstraint.cpp b/tests/constraint/test_RnBoxConstraint.cpp index 9029ff1b36..6340d3f3e2 100644 --- a/tests/constraint/test_RnBoxConstraint.cpp +++ b/tests/constraint/test_RnBoxConstraint.cpp @@ -592,7 +592,7 @@ TEST_F( //============================================================================== TEST_F(RnBoxConstraintTests, R2_createSampleGenerator) { - auto constraint = std::make_shared( + auto constraint = dart::common::make_aligned_shared( 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( + auto constraint = dart::common::make_aligned_shared( 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( + auto unbounded1 = dart::common::make_aligned_shared( mR2StateSpace, mRng->clone(), noLowerBound, mUpperLimits); EXPECT_THROW({ unbounded1->createSampleGenerator(); }, std::runtime_error); - auto unbounded2 = std::make_shared( + auto unbounded2 = dart::common::make_aligned_shared( mR2StateSpace, mRng->clone(), mLowerLimits, noUpperBound); EXPECT_THROW({ unbounded2->createSampleGenerator(); }, std::runtime_error); } diff --git a/tests/planner/parabolic/test_TimePostProcessor.cpp b/tests/planner/parabolic/test_TimePostProcessor.cpp index 13cf5e8f94..715fb0cec9 100644 --- a/tests/planner/parabolic/test_TimePostProcessor.cpp +++ b/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 { diff --git a/tests/planner/vectorfield/test_VectorFieldPlanner.cpp b/tests/planner/vectorfield/test_VectorFieldPlanner.cpp index 3612f9a014..5d5d99e526 100644 --- a/tests/planner/vectorfield/test_VectorFieldPlanner.cpp +++ b/tests/planner/vectorfield/test_VectorFieldPlanner.cpp @@ -17,6 +17,7 @@ using std::make_shared; class VectorFieldPlannerTest : public ::testing::Test { public: + using FCLCollisionDetector = dart::collision::FCLCollisionDetector; using DistanceMetric = aikido::distance::DistanceMetric; using MetaSkeletonStateSpacePtr From cec37c37e47f8db34f561c8f9e40a789e7e8461b Mon Sep 17 00:00:00 2001 From: daqing Date: Wed, 21 Mar 2018 15:35:13 -0700 Subject: [PATCH 08/14] fix eigen alignment --- .../planner/parabolic/ParabolicTimer.hpp | 1 - .../vectorfield/VectorFieldPlanner.cpp | 2 +- src/planner/vectorfield/VectorFieldUtil.cpp | 104 +++++++++--------- .../vectorfield/test_VectorFieldPlanner.cpp | 1 - 4 files changed, 53 insertions(+), 55 deletions(-) diff --git a/include/aikido/planner/parabolic/ParabolicTimer.hpp b/include/aikido/planner/parabolic/ParabolicTimer.hpp index 2eef93537f..0fe0a54cf3 100644 --- a/include/aikido/planner/parabolic/ParabolicTimer.hpp +++ b/include/aikido/planner/parabolic/ParabolicTimer.hpp @@ -73,7 +73,6 @@ std::unique_ptr convertToSpline( class ParabolicTimer : public aikido::planner::TrajectoryPostProcessor { public: - /// \param _velocityLimits Maximum velocity for each dimension. /// \param _accelerationLimits Maximum acceleration for each dimension. ParabolicTimer( diff --git a/src/planner/vectorfield/VectorFieldPlanner.cpp b/src/planner/vectorfield/VectorFieldPlanner.cpp index 65cac3458f..0e396126b2 100644 --- a/src/planner/vectorfield/VectorFieldPlanner.cpp +++ b/src/planner/vectorfield/VectorFieldPlanner.cpp @@ -40,7 +40,7 @@ std::unique_ptr followVectorField( boost::numeric::odeint::vector_space_algebra>; std::size_t dimension = vectorField.getStateSpace()->getDimension(); - auto integrator = dart::common::make_aligned_shared( + auto integrator = std::make_shared( &vectorField, &constraint, timelimit.count(), checkConstraintResolution); integrator->start(); diff --git a/src/planner/vectorfield/VectorFieldUtil.cpp b/src/planner/vectorfield/VectorFieldUtil.cpp index 03f7d70f50..b52583ced2 100644 --- a/src/planner/vectorfield/VectorFieldUtil.cpp +++ b/src/planner/vectorfield/VectorFieldUtil.cpp @@ -82,58 +82,58 @@ bool computeJointVelocityFromTwist( using Eigen::VectorXd; // Use LBFGS to find joint angles that won't violate the joint limits. -// const Jacobian jacobian = metaSkeleton->getWorldJacobian(bodyNode); - -// const std::size_t numDofs = metaSkeleton->getNumDofs(); - -// jointVelocity = Eigen::VectorXd::Zero(numDofs); -// VectorXd positions = metaSkeleton->getPositions(); -// VectorXd initialGuess = metaSkeleton->getVelocities(); -// VectorXd positionLowerLimits = metaSkeleton->getPositionLowerLimits(); -// VectorXd positionUpperLimits = metaSkeleton->getPositionUpperLimits(); -// VectorXd velocityLowerLimits = jointVelocityLowerLimits; -// VectorXd velocityUpperLimits = jointVelocityUpperLimits; - -// const auto problem = std::make_shared(numDofs); -// if (enforceJointVelocityLimits) -// { -// for (std::size_t i = 0; i < numDofs; ++i) -// { -// const double position = positions[i]; -// const double positionLowerLimit = positionLowerLimits[i]; -// const double positionUpperLimit = positionUpperLimits[i]; -// const double velocityLowerLimit = velocityLowerLimits[i]; -// const double velocityUpperLimit = velocityUpperLimits[i]; - -// if (position + stepSize * velocityLowerLimit -// <= positionLowerLimit + jointLimitPadding) -// { -// velocityLowerLimits[i] = 0.0; -// } - -// if (position + stepSize * velocityUpperLimit -// >= positionUpperLimit - jointLimitPadding) -// { -// velocityUpperLimits[i] = 0.0; -// } - -// initialGuess[i] = common::clamp( -// initialGuess[i], velocityLowerLimits[i], velocityUpperLimits[i]); -// } -// problem->setLowerBounds(velocityLowerLimits); -// problem->setUpperBounds(velocityUpperLimits); -// } - -// problem->setInitialGuess(initialGuess); -// problem->setObjective( -// dart::common::make_aligned_shared(desiredTwist, jacobian)); - -// dart::optimizer::NloptSolver solver(problem, nlopt::LD_LBFGS); -// if (!solver.solve()) -// { -// return false; -// } -// jointVelocity = problem->getOptimalSolution(); + const Jacobian jacobian = metaSkeleton->getWorldJacobian(bodyNode); + + const std::size_t numDofs = metaSkeleton->getNumDofs(); + + jointVelocity = Eigen::VectorXd::Zero(numDofs); + VectorXd positions = metaSkeleton->getPositions(); + VectorXd initialGuess = metaSkeleton->getVelocities(); + VectorXd positionLowerLimits = metaSkeleton->getPositionLowerLimits(); + VectorXd positionUpperLimits = metaSkeleton->getPositionUpperLimits(); + VectorXd velocityLowerLimits = jointVelocityLowerLimits; + VectorXd velocityUpperLimits = jointVelocityUpperLimits; + + const auto problem = std::make_shared(numDofs); + if (enforceJointVelocityLimits) + { + for (std::size_t i = 0; i < numDofs; ++i) + { + const double position = positions[i]; + const double positionLowerLimit = positionLowerLimits[i]; + const double positionUpperLimit = positionUpperLimits[i]; + const double velocityLowerLimit = velocityLowerLimits[i]; + const double velocityUpperLimit = velocityUpperLimits[i]; + + if (position + stepSize * velocityLowerLimit + <= positionLowerLimit + jointLimitPadding) + { + velocityLowerLimits[i] = 0.0; + } + + if (position + stepSize * velocityUpperLimit + >= positionUpperLimit - jointLimitPadding) + { + velocityUpperLimits[i] = 0.0; + } + + initialGuess[i] = common::clamp( + initialGuess[i], velocityLowerLimits[i], velocityUpperLimits[i]); + } + problem->setLowerBounds(velocityLowerLimits); + problem->setUpperBounds(velocityUpperLimits); + } + + problem->setInitialGuess(initialGuess); + problem->setObjective( + dart::common::make_aligned_shared(desiredTwist, jacobian)); + + dart::optimizer::NloptSolver solver(problem, nlopt::LD_LBFGS); + if (!solver.solve()) + { + return false; + } + jointVelocity = problem->getOptimalSolution(); return true; } diff --git a/tests/planner/vectorfield/test_VectorFieldPlanner.cpp b/tests/planner/vectorfield/test_VectorFieldPlanner.cpp index 0e8de986e3..1e3a300b4a 100644 --- a/tests/planner/vectorfield/test_VectorFieldPlanner.cpp +++ b/tests/planner/vectorfield/test_VectorFieldPlanner.cpp @@ -17,7 +17,6 @@ using std::make_shared; class VectorFieldPlannerTest : public ::testing::Test { public: - using FCLCollisionDetector = dart::collision::FCLCollisionDetector; using DistanceMetric = aikido::distance::DistanceMetric; using MetaSkeletonStateSpacePtr From 0c7ad90eed98e78c033d500f3a6b9220456de815 Mon Sep 17 00:00:00 2001 From: daqing Date: Wed, 21 Mar 2018 17:03:19 -0700 Subject: [PATCH 09/14] fix eigen alignment --- .../aikido/constraint/uniform/SE2BoxConstraint.hpp | 2 ++ include/aikido/distance/SE2Weighted.hpp | 2 ++ tests/constraint/test_RnConstantSampler.cpp | 2 +- tests/constraint/test_SE2BoxConstraint.cpp | 12 ++++++------ 4 files changed, 11 insertions(+), 7 deletions(-) diff --git a/include/aikido/constraint/uniform/SE2BoxConstraint.hpp b/include/aikido/constraint/uniform/SE2BoxConstraint.hpp index 93ff48e3a4..7168ef6ee8 100644 --- a/include/aikido/constraint/uniform/SE2BoxConstraint.hpp +++ b/include/aikido/constraint/uniform/SE2BoxConstraint.hpp @@ -79,6 +79,8 @@ class SE2BoxConstraint : public constraint::Projectable, // DOF of the joint std::size_t mDimension; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace uniform diff --git a/include/aikido/distance/SE2Weighted.hpp b/include/aikido/distance/SE2Weighted.hpp index 9e7e9eee3c..b77f32917d 100644 --- a/include/aikido/distance/SE2Weighted.hpp +++ b/include/aikido/distance/SE2Weighted.hpp @@ -41,6 +41,8 @@ class SE2Weighted : public DistanceMetric std::shared_ptr mStateSpace; Eigen::Vector2d mWeights; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace distance diff --git a/tests/constraint/test_RnConstantSampler.cpp b/tests/constraint/test_RnConstantSampler.cpp index 90d4f65b3e..68dcfbeae7 100644 --- a/tests/constraint/test_RnConstantSampler.cpp +++ b/tests/constraint/test_RnConstantSampler.cpp @@ -78,7 +78,7 @@ void testSampleGenerator(int dimension = N) auto stateSpace = std::make_shared>(dimension); EXPECT_EQ(stateSpace->getDimension(), dimension); - auto sampler = std::make_shared>( + auto sampler = dart::common::make_aligned_shared>( stateSpace, value); EXPECT_EQ(sampler->getStateSpace(), stateSpace); EXPECT_TRUE( diff --git a/tests/constraint/test_SE2BoxConstraint.cpp b/tests/constraint/test_SE2BoxConstraint.cpp index fda86712b1..d681d51186 100644 --- a/tests/constraint/test_SE2BoxConstraint.cpp +++ b/tests/constraint/test_SE2BoxConstraint.cpp @@ -28,8 +28,8 @@ class SE2BoxConstraintTests : public ::testing::Test void SetUp() override { - mSE2StateSpace = std::make_shared(); - mSE2Distance = std::make_shared(mSE2StateSpace); + mSE2StateSpace = dart::common::make_aligned_shared(); + mSE2Distance = dart::common::make_aligned_shared(mSE2StateSpace); mRng = make_unique>(0); mLowerLimits = Vector2d(-1., 1.); @@ -211,7 +211,7 @@ TEST_F(SE2BoxConstraintTests, projectUnSatisfiedConstraintProjects) //============================================================================== TEST_F(SE2BoxConstraintTests, createSampleGenerator) { - auto constraint = std::make_shared( + auto constraint = dart::common::make_aligned_shared( mSE2StateSpace, mRng->clone(), mLowerLimits, mUpperLimits); auto generator = constraint->createSampleGenerator(); @@ -232,7 +232,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( + auto constraint = dart::common::make_aligned_shared( mSE2StateSpace, nullptr, mLowerLimits, mUpperLimits); EXPECT_THROW({ constraint->createSampleGenerator(); }, std::invalid_argument); @@ -249,11 +249,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( + auto unbounded1 = dart::common::make_aligned_shared( mSE2StateSpace, mRng->clone(), noLowerBound, mUpperLimits); EXPECT_THROW({ unbounded1->createSampleGenerator(); }, std::runtime_error); - auto unbounded2 = std::make_shared( + auto unbounded2 = dart::common::make_aligned_shared( mSE2StateSpace, mRng->clone(), mLowerLimits, noUpperBound); EXPECT_THROW({ unbounded2->createSampleGenerator(); }, std::runtime_error); } From 1f740f9fc2681433999be8302545e54889eb5bc8 Mon Sep 17 00:00:00 2001 From: daqing Date: Wed, 21 Mar 2018 17:11:17 -0700 Subject: [PATCH 10/14] fix code format --- .../constraint/uniform/RnBoxConstraint.hpp | 1 + .../constraint/uniform/RnConstantSampler.hpp | 1 + .../uniform/detail/RnConstantSampler-impl.hpp | 1 + .../MoveEndEffectorOffsetVectorField.hpp | 1 + .../MoveEndEffectorPoseVectorField.hpp | 1 + .../vectorfield/VectorFieldPlanner.cpp | 42 ++++++++++--------- src/planner/vectorfield/VectorFieldUtil.cpp | 3 +- .../test_CartesianProductProjectable.cpp | 2 +- 8 files changed, 30 insertions(+), 22 deletions(-) diff --git a/include/aikido/constraint/uniform/RnBoxConstraint.hpp b/include/aikido/constraint/uniform/RnBoxConstraint.hpp index 38c72b5c87..ef3f3e0c9c 100644 --- a/include/aikido/constraint/uniform/RnBoxConstraint.hpp +++ b/include/aikido/constraint/uniform/RnBoxConstraint.hpp @@ -85,6 +85,7 @@ class RBoxConstraint : public constraint::Differentiable, std::unique_ptr mRng; VectorNd mLowerLimits; VectorNd mUpperLimits; + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(VectorNd::NeedsToAlign) }; diff --git a/include/aikido/constraint/uniform/RnConstantSampler.hpp b/include/aikido/constraint/uniform/RnConstantSampler.hpp index 8387af949b..76517fbc5c 100644 --- a/include/aikido/constraint/uniform/RnConstantSampler.hpp +++ b/include/aikido/constraint/uniform/RnConstantSampler.hpp @@ -35,6 +35,7 @@ class RConstantSampler : public constraint::Sampleable private: std::shared_ptr> mSpace; VectorNd mValue; + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(VectorNd::NeedsToAlign) }; diff --git a/include/aikido/constraint/uniform/detail/RnConstantSampler-impl.hpp b/include/aikido/constraint/uniform/detail/RnConstantSampler-impl.hpp index ae43a62f41..f5146da6c8 100644 --- a/include/aikido/constraint/uniform/detail/RnConstantSampler-impl.hpp +++ b/include/aikido/constraint/uniform/detail/RnConstantSampler-impl.hpp @@ -45,6 +45,7 @@ class RnConstantSamplerSampleGenerator : public constraint::SampleGenerator private: std::shared_ptr> mSpace; VectorNd mValue; + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(VectorNd::NeedsToAlign) }; diff --git a/include/aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp b/include/aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp index 77627df397..7862631037 100644 --- a/include/aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp +++ b/include/aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp @@ -76,6 +76,7 @@ class MoveEndEffectorOffsetVectorField : public BodyNodePoseVectorField /// Start pose of the end-effector. Eigen::Isometry3d mStartPose; + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/include/aikido/planner/vectorfield/MoveEndEffectorPoseVectorField.hpp b/include/aikido/planner/vectorfield/MoveEndEffectorPoseVectorField.hpp index c9c00c220e..be63793503 100644 --- a/include/aikido/planner/vectorfield/MoveEndEffectorPoseVectorField.hpp +++ b/include/aikido/planner/vectorfield/MoveEndEffectorPoseVectorField.hpp @@ -61,6 +61,7 @@ class MoveEndEffectorPoseVectorField : public BodyNodePoseVectorField /// Conversion ratio from radius to meter. double mConversionRatioFromRadiusToMeter; + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/src/planner/vectorfield/VectorFieldPlanner.cpp b/src/planner/vectorfield/VectorFieldPlanner.cpp index 0e396126b2..5fbf26af75 100644 --- a/src/planner/vectorfield/VectorFieldPlanner.cpp +++ b/src/planner/vectorfield/VectorFieldPlanner.cpp @@ -165,17 +165,18 @@ std::unique_ptr planToEndEffectorOffset( metaskeleton, MetaSkeletonStateSaver::Options::POSITIONS); DART_UNUSED(saver); - auto vectorfield = dart::common::make_aligned_shared( - stateSpace, - metaskeleton, - bn, - direction, - minDistance, - maxDistance, - positionTolerance, - angularTolerance, - initialStepSize, - jointLimitTolerance); + auto vectorfield + = dart::common::make_aligned_shared( + stateSpace, + metaskeleton, + bn, + direction, + minDistance, + maxDistance, + positionTolerance, + angularTolerance, + initialStepSize, + jointLimitTolerance); auto compoundConstraint = std::make_shared(stateSpace); @@ -217,15 +218,16 @@ std::unique_ptr planToEndEffectorPose( metaskeleton, MetaSkeletonStateSaver::Options::POSITIONS); DART_UNUSED(saver); - auto vectorfield = dart::common::make_aligned_shared( - stateSpace, - metaskeleton, - bn, - goalPose, - poseErrorTolerance, - conversionRatioInGeodesicDistance, - initialStepSize, - jointLimitTolerance); + auto vectorfield + = dart::common::make_aligned_shared( + stateSpace, + metaskeleton, + bn, + goalPose, + poseErrorTolerance, + conversionRatioInGeodesicDistance, + initialStepSize, + jointLimitTolerance); auto compoundConstraint = std::make_shared(stateSpace); diff --git a/src/planner/vectorfield/VectorFieldUtil.cpp b/src/planner/vectorfield/VectorFieldUtil.cpp index b52583ced2..88ae78c8b1 100644 --- a/src/planner/vectorfield/VectorFieldUtil.cpp +++ b/src/planner/vectorfield/VectorFieldUtil.cpp @@ -126,7 +126,8 @@ bool computeJointVelocityFromTwist( problem->setInitialGuess(initialGuess); problem->setObjective( - dart::common::make_aligned_shared(desiredTwist, jacobian)); + dart::common::make_aligned_shared( + desiredTwist, jacobian)); dart::optimizer::NloptSolver solver(problem, nlopt::LD_LBFGS); if (!solver.solve()) diff --git a/tests/constraint/test_CartesianProductProjectable.cpp b/tests/constraint/test_CartesianProductProjectable.cpp index 65df33a5f7..dfd49c0fa7 100644 --- a/tests/constraint/test_CartesianProductProjectable.cpp +++ b/tests/constraint/test_CartesianProductProjectable.cpp @@ -1,5 +1,5 @@ -#include #include +#include #include #include #include From b366ddd5fc2b6da0b33c6f90bfb77d70a81fb6eb Mon Sep 17 00:00:00 2001 From: daqing Date: Wed, 21 Mar 2018 17:42:20 -0700 Subject: [PATCH 11/14] fix code format --- include/aikido/constraint/uniform/SE2BoxConstraint.hpp | 1 + include/aikido/distance/SE2Weighted.hpp | 1 + tests/constraint/test_RnConstantSampler.cpp | 5 +++-- tests/constraint/test_SE2BoxConstraint.cpp | 3 ++- 4 files changed, 7 insertions(+), 3 deletions(-) diff --git a/include/aikido/constraint/uniform/SE2BoxConstraint.hpp b/include/aikido/constraint/uniform/SE2BoxConstraint.hpp index 7168ef6ee8..44e9b41780 100644 --- a/include/aikido/constraint/uniform/SE2BoxConstraint.hpp +++ b/include/aikido/constraint/uniform/SE2BoxConstraint.hpp @@ -79,6 +79,7 @@ class SE2BoxConstraint : public constraint::Projectable, // DOF of the joint std::size_t mDimension; + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/include/aikido/distance/SE2Weighted.hpp b/include/aikido/distance/SE2Weighted.hpp index b77f32917d..0990b23ee7 100644 --- a/include/aikido/distance/SE2Weighted.hpp +++ b/include/aikido/distance/SE2Weighted.hpp @@ -41,6 +41,7 @@ class SE2Weighted : public DistanceMetric std::shared_ptr mStateSpace; Eigen::Vector2d mWeights; + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/tests/constraint/test_RnConstantSampler.cpp b/tests/constraint/test_RnConstantSampler.cpp index 68dcfbeae7..886039ee02 100644 --- a/tests/constraint/test_RnConstantSampler.cpp +++ b/tests/constraint/test_RnConstantSampler.cpp @@ -78,8 +78,9 @@ void testSampleGenerator(int dimension = N) auto stateSpace = std::make_shared>(dimension); EXPECT_EQ(stateSpace->getDimension(), dimension); - auto sampler = dart::common::make_aligned_shared>( - stateSpace, value); + auto sampler = dart::common:: + make_aligned_shared>( + stateSpace, value); EXPECT_EQ(sampler->getStateSpace(), stateSpace); EXPECT_TRUE( tests::CompareEigenMatrices(sampler->getConstantValue(), value, 1e-6)); diff --git a/tests/constraint/test_SE2BoxConstraint.cpp b/tests/constraint/test_SE2BoxConstraint.cpp index d681d51186..e340d6f53a 100644 --- a/tests/constraint/test_SE2BoxConstraint.cpp +++ b/tests/constraint/test_SE2BoxConstraint.cpp @@ -29,7 +29,8 @@ class SE2BoxConstraintTests : public ::testing::Test void SetUp() override { mSE2StateSpace = dart::common::make_aligned_shared(); - mSE2Distance = dart::common::make_aligned_shared(mSE2StateSpace); + mSE2Distance + = dart::common::make_aligned_shared(mSE2StateSpace); mRng = make_unique>(0); mLowerLimits = Vector2d(-1., 1.); From 5c4bc3b42b5b9fc8cda513f50d1ddb2e39e9d9ba Mon Sep 17 00:00:00 2001 From: daqing Date: Mon, 9 Apr 2018 13:11:33 -0700 Subject: [PATCH 12/14] remove unnecessary MACRO declaration --- include/aikido/constraint/uniform/SE2BoxConstraint.hpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/include/aikido/constraint/uniform/SE2BoxConstraint.hpp b/include/aikido/constraint/uniform/SE2BoxConstraint.hpp index 44e9b41780..93ff48e3a4 100644 --- a/include/aikido/constraint/uniform/SE2BoxConstraint.hpp +++ b/include/aikido/constraint/uniform/SE2BoxConstraint.hpp @@ -79,9 +79,6 @@ class SE2BoxConstraint : public constraint::Projectable, // DOF of the joint std::size_t mDimension; - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace uniform From 6fc3a0a702fa775dc702987c5d2ad545fb651b75 Mon Sep 17 00:00:00 2001 From: Daqing Yi Date: Tue, 10 Apr 2018 21:33:57 -0700 Subject: [PATCH 13/14] update CHANGELOG.md --- CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 8897b4419d..f405ab8314 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -31,6 +31,10 @@ * Added Robot, Manipulator, Hand interfaces, and ConcreteRobot, ConcreteManipulator classes. [#325] https://github.com/personalrobotics/aikido/pull/325) +* Build & Testing & ETC + + * Fixed Eigen memory alignment issues on 32bit Ubuntu. [#368](https://github.com/personalrobotics/aikido/pull/368) + ### 0.2.0 (2018-01-09) * State Space From b7dc4d9af249974a656393d2487ef848c4a97366 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 10 Apr 2018 21:47:38 -0700 Subject: [PATCH 14/14] Update CHANGELOG.md --- CHANGELOG.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index f405ab8314..35c56c2b7b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -29,11 +29,11 @@ * Robot - * Added Robot, Manipulator, Hand interfaces, and ConcreteRobot, ConcreteManipulator classes. [#325] https://github.com/personalrobotics/aikido/pull/325) + * Added Robot, Manipulator, Hand interfaces, and ConcreteRobot, ConcreteManipulator classes: [#325] https://github.com/personalrobotics/aikido/pull/325) * Build & Testing & ETC - * Fixed Eigen memory alignment issues on 32bit Ubuntu. [#368](https://github.com/personalrobotics/aikido/pull/368) + * Fixed Eigen memory alignment issues on 32bit Ubuntu: [#368](https://github.com/personalrobotics/aikido/pull/368) ### 0.2.0 (2018-01-09)