Fix ompl tests builds (#178)

personalrobotics · Apr 18, 2017 · 2af5c55 · 2af5c55
1 parent 38ce8e6
commit 2af5c55
Show file tree

Hide file tree

Showing 5 changed files with 47 additions and 108 deletions.
diff --git a/tests/planner/CMakeLists.txt b/tests/planner/CMakeLists.txt
@@ -1,13 +1,9 @@
 add_subdirectory("parabolic")
-
-if(${OMPL_FOUND})
-  add_subdirectory("ompl")
-endif()
+add_subdirectory("ompl")
 
 aikido_add_test(test_SnapPlanner test_SnapPlanner.cpp)
 target_link_libraries(test_SnapPlanner
   "${PROJECT_NAME}_constraint"
   "${PROJECT_NAME}_distance"
   "${PROJECT_NAME}_trajectory"
   "${PROJECT_NAME}_planner")
-
diff --git a/tests/planner/ompl/CMakeLists.txt b/tests/planner/ompl/CMakeLists.txt
@@ -1,41 +1,21 @@
+if(NOT TARGET "${PROJECT_NAME}_planner_ompl")
+  return()
+endif()
+
 aikido_add_test(test_GeometricStateSpace test_GeometricStateSpace.cpp)
-target_link_libraries(test_GeometricStateSpace
-  "${PROJECT_NAME}_distance"
-  "${PROJECT_NAME}_planner_ompl"
-  "${PROJECT_NAME}_trajectory"
-  "${PROJECT_NAME}_statespace")
+target_link_libraries(test_GeometricStateSpace "${PROJECT_NAME}_planner_ompl")
 
 aikido_add_test(test_GoalRegion test_GoalRegion.cpp)
-target_link_libraries(test_GoalRegion
-  "${PROJECT_NAME}_distance"
-  "${PROJECT_NAME}_planner_ompl"
-  "${PROJECT_NAME}_trajectory"
-  "${PROJECT_NAME}_statespace")
+target_link_libraries(test_GoalRegion "${PROJECT_NAME}_planner_ompl")
 
 aikido_add_test(test_MotionValidator test_MotionValidator.cpp)
-target_link_libraries(test_MotionValidator
-  "${PROJECT_NAME}_distance"
-  "${PROJECT_NAME}_planner_ompl"
-  "${PROJECT_NAME}_trajectory"
-  "${PROJECT_NAME}_statespace")
+target_link_libraries(test_MotionValidator "${PROJECT_NAME}_planner_ompl")
+
+aikido_add_test(test_OMPLPlanner test_OMPLPlanner.cpp)
+target_link_libraries(test_OMPLPlanner "${PROJECT_NAME}_planner_ompl")
 
 aikido_add_test(test_StateSampler test_StateSampler.cpp)
-target_link_libraries(test_StateSampler
-  "${PROJECT_NAME}_distance"
-  "${PROJECT_NAME}_planner_ompl"
-  "${PROJECT_NAME}_trajectory"
-  "${PROJECT_NAME}_statespace")
+target_link_libraries(test_StateSampler "${PROJECT_NAME}_planner_ompl")
 
 aikido_add_test(test_ValidityChecker test_ValidityChecker.cpp)
-target_link_libraries(test_ValidityChecker
-  "${PROJECT_NAME}_distance"
-  "${PROJECT_NAME}_planner_ompl"
-  "${PROJECT_NAME}_trajectory"
-  "${PROJECT_NAME}_statespace")
-
-aikido_add_test(test_OMPLPlanner test_OMPLPlanner.cpp)
-target_link_libraries(test_OMPLPlanner
-  "${PROJECT_NAME}_distance"
-  "${PROJECT_NAME}_planner_ompl"
-  "${PROJECT_NAME}_trajectory"
-  "${PROJECT_NAME}_statespace")
+target_link_libraries(test_ValidityChecker "${PROJECT_NAME}_planner_ompl")
diff --git a/tests/planner/ompl/OMPLTestHelpers.hpp b/tests/planner/ompl/OMPLTestHelpers.hpp
@@ -16,7 +16,7 @@
 
 using dart::common::make_unique;
 using aikido::statespace::CartesianProduct;
-using aikido::statespace::Rn;
+using aikido::statespace::R3;
 using aikido::planner::ompl::GeometricStateSpace;
 using aikido::util::RNGWrapper;
 using aikido::util::RNG;
@@ -31,8 +31,7 @@ dart::dynamics::SkeletonPtr createTranslationalRobot()
 {
   // Create robot
   auto robot = dart::dynamics::Skeleton::create("robot");
-  auto jn_bn =
-      robot->createJointAndBodyNodePair<dart::dynamics::TranslationalJoint>();
+  robot->createJointAndBodyNodePair<dart::dynamics::TranslationalJoint>();
 
   // Set bounds on the skeleton
   robot->setPositionLowerLimit(0, -5);
@@ -52,9 +51,7 @@ void setTranslationalState(
 {
   auto st = _state->as<aikido::planner::ompl::GeometricStateSpace::StateType>();
   auto cst = static_cast<CartesianProduct::State*>(st->mState);
-  auto subState =
-      _stateSpace->getSubStateHandle<aikido::statespace::Rn>(
-          cst, 0);
+  auto subState = _stateSpace->getSubStateHandle<R3>(cst, 0);
   subState.setValue(_value);
 }
 
@@ -64,11 +61,9 @@ Eigen::Vector3d getTranslationalState(
 {
   auto st = _state->as<GeometricStateSpace::StateType>();
   auto cst = static_cast<CartesianProduct::State*>(st->mState);
-  auto subState =
-      _stateSpace->getSubStateHandle<Rn>(
-          cst, 0);
-  return subState.getValue();
+  auto subState = _stateSpace->getSubStateHandle<R3>(cst, 0);
 
+  return subState.getValue();
 }
 
 class MockConstrainedSampleGenerator
@@ -91,7 +86,7 @@ class MockConstrainedSampleGenerator
   bool sample(aikido::statespace::StateSpace::State *_state) override {
     mSampleGenerator->sample(_state);
     auto outstate = static_cast<CartesianProduct::State *>(_state);
-    auto outSubState = mStateSpace->getSubStateHandle<Rn>(outstate, 0);
+    auto outSubState = mStateSpace->getSubStateHandle<R3>(outstate, 0);
     auto val = outSubState.getValue();
     Eigen::Vector3d newval(mValue, val(1), val(2));
     outSubState.setValue(newval);
@@ -140,12 +135,8 @@ class MockProjectionConstraint : public aikido::constraint::Projectable,
         static_cast<const CartesianProduct::State *>(_s);
     auto outstate =
         static_cast<CartesianProduct::State *>(_out);
-    auto inSubState =
-        mStateSpace
-            ->getSubStateHandle<Rn>(instate, 0);
-    auto outSubState =
-        mStateSpace
-            ->getSubStateHandle<Rn>(outstate, 0);
+    auto inSubState = mStateSpace->getSubStateHandle<R3>(instate, 0);
+    auto outSubState = mStateSpace->getSubStateHandle<R3>(outstate, 0);
     auto val = inSubState.getValue();
     Eigen::Vector3d newval(mValue, val(1), val(2));
     outSubState.setValue(newval);
@@ -157,7 +148,7 @@ class MockProjectionConstraint : public aikido::constraint::Projectable,
   {
     auto state =
         static_cast<const CartesianProduct::State *>(_s);
-    auto val = mStateSpace->getSubStateHandle<Rn>(state, 0).getValue();
+    auto val = mStateSpace->getSubStateHandle<R3>(state, 0).getValue();
     return std::fabs(val[0] - mValue) < 1e-6;
   }
 
@@ -191,11 +182,8 @@ class MockTranslationalRobotConstraint : public aikido::constraint::Testable
   bool isSatisfied(
       const aikido::statespace::StateSpace::State *_state) const override
   {
-    auto cst =
-        static_cast<const CartesianProduct::State *>(_state);
-    auto subState =
-        mStateSpace
-            ->getSubStateHandle<Rn>(cst, 0);
+    auto cst = static_cast<const CartesianProduct::State *>(_state);
+    auto subState = mStateSpace->getSubStateHandle<R3>(cst, 0);
     auto val = subState.getValue();
 
     for (size_t i = 0; i < 3; ++i) {
@@ -215,8 +203,8 @@ class MockTranslationalRobotConstraint : public aikido::constraint::Testable
 
 private:
   aikido::statespace::dart::MetaSkeletonStateSpacePtr mStateSpace;
-  Eigen::Vector3d mUpperBounds;
   Eigen::Vector3d mLowerBounds;
+  Eigen::Vector3d mUpperBounds;
 };
 
 
@@ -233,7 +221,7 @@ class EmptySampleGenerator : public aikido::constraint::SampleGenerator
     return mStateSpace;
   }
 
-  bool sample(aikido::statespace::StateSpace::State* _state) override
+  bool sample(aikido::statespace::StateSpace::State* /*_state*/) override
   {
     return true;
   }
@@ -256,7 +244,7 @@ class FailedSampleGenerator : public aikido::constraint::SampleGenerator {
     return mStateSpace;
   }
 
-  bool sample(aikido::statespace::StateSpace::State* _state) override
+  bool sample(aikido::statespace::StateSpace::State* /*_state*/) override
   {
     return false;
   }

diff --git a/tests/planner/ompl/test_CRRT.cpp b/tests/planner/ompl/test_CRRT.cpp
diff --git a/tests/planner/ompl/test_OMPLPlanner.cpp b/tests/planner/ompl/test_OMPLPlanner.cpp
@@ -12,7 +12,6 @@
 #include <ompl/geometric/planners/rrt/RRTConnect.h>
 
 using StateSpace = aikido::statespace::dart::MetaSkeletonStateSpace;
-using Rn = aikido::statespace::Rn;
 using aikido::planner::ompl::getSpaceInformation;
 using aikido::planner::ompl::CRRT;
 using aikido::planner::ompl::CRRTConnect;
@@ -23,13 +22,11 @@ TEST_F(PlannerTest, PlanToConfiguration)
   Eigen::Vector3d goalPose(5, 5, 0);
 
   auto startState = stateSpace->createState();
-  auto subState1 =
-      stateSpace->getSubStateHandle<Rn>(startState, 0);
+  auto subState1 = stateSpace->getSubStateHandle<R3>(startState, 0);
   subState1.setValue(startPose);
 
   auto goalState = stateSpace->createState();
-  auto subState2 =
-      stateSpace->getSubStateHandle<Rn>(goalState, 0);
+  auto subState2 = stateSpace->getSubStateHandle<R3>(goalState, 0);
   subState2.setValue(goalPose);
 
   // Plan
@@ -41,12 +38,12 @@ TEST_F(PlannerTest, PlanToConfiguration)
   // Check the first waypoint
   auto s0 = stateSpace->createState();
   traj->evaluate(0, s0);
-  auto r0 = s0.getSubStateHandle<Rn>(0);
+  auto r0 = s0.getSubStateHandle<R3>(0);
   EXPECT_TRUE(r0.getValue().isApprox(startPose));
 
   // Check the last waypoint
   traj->evaluate(traj->getDuration(), s0);
-  r0 = s0.getSubStateHandle<Rn>(0);
+  r0 = s0.getSubStateHandle<R3>(0);
   EXPECT_TRUE(r0.getValue().isApprox(goalPose));
 }
 
@@ -55,13 +52,12 @@ TEST_F(PlannerTest, PlanToGoalRegion)
   auto startState = stateSpace->createState();
   Eigen::Vector3d startPose(-5, -5, 0);
 
-  auto subState1 =
-      stateSpace->getSubStateHandle<Rn>(startState, 0);
+  auto subState1 = stateSpace->getSubStateHandle<R3>(startState, 0);
   subState1.setValue(startPose);
 
   auto boxConstraint =
-      std::make_shared<aikido::constraint::RnBoxConstraint>(
-          stateSpace->getSubspace<Rn>(0), make_rng(),
+      std::make_shared<aikido::constraint::R3BoxConstraint>(
+          stateSpace->getSubspace<R3>(0), make_rng(),
           Eigen::Vector3d(4, 4, 0), Eigen::Vector3d(5, 5, 0));
   std::vector<std::shared_ptr<aikido::constraint::Sampleable>>
       sConstraints;
@@ -85,7 +81,7 @@ TEST_F(PlannerTest, PlanToGoalRegion)
   // Check the first waypoint
   auto s0 = stateSpace->createState();
   traj->evaluate(0, s0);
-  auto r0 = s0.getSubStateHandle<Rn>(0);
+  auto r0 = s0.getSubStateHandle<R3>(0);
   EXPECT_TRUE(r0.getValue().isApprox(startPose));
 
   // Check the last waypoint
@@ -100,12 +96,12 @@ TEST_F(PlannerTest, PlanConstrainedCRRTConnect)
 
   auto startState = stateSpace->createState();
   auto subState1 =
-      stateSpace->getSubStateHandle<Rn>(startState, 0);
+      stateSpace->getSubStateHandle<R3>(startState, 0);
   subState1.setValue(startPose);
 
   auto boxConstraint =
-      std::make_shared<aikido::constraint::RnBoxConstraint>(
-          stateSpace->getSubspace<Rn>(0), make_rng(),
+      std::make_shared<aikido::constraint::R3BoxConstraint>(
+          stateSpace->getSubspace<R3>(0), make_rng(),
           Eigen::Vector3d(constraintVal-1, 4, 0), Eigen::Vector3d(constraintVal+1, 5, 0));
   std::vector<std::shared_ptr<aikido::constraint::Sampleable>>
       sConstraints;
@@ -134,7 +130,7 @@ TEST_F(PlannerTest, PlanConstrainedCRRTConnect)
   // Check the first waypoint
   auto s0 = stateSpace->createState();
   traj->evaluate(0, s0);
-  auto r0 = s0.getSubStateHandle<Rn>(0);
+  auto r0 = s0.getSubStateHandle<R3>(0);
   EXPECT_TRUE(r0.getValue().isApprox(startPose));
 
   // Check the last waypoint
@@ -157,12 +153,12 @@ TEST_F(PlannerTest, PlanConstrainedCRRT)
 
   auto startState = stateSpace->createState();
   auto subState1 =
-      stateSpace->getSubStateHandle<Rn>(startState, 0);
+      stateSpace->getSubStateHandle<R3>(startState, 0);
   subState1.setValue(startPose);
 
   auto boxConstraint =
-      std::make_shared<aikido::constraint::RnBoxConstraint>(
-          stateSpace->getSubspace<Rn>(0), make_rng(),
+      std::make_shared<aikido::constraint::R3BoxConstraint>(
+          stateSpace->getSubspace<R3>(0), make_rng(),
           Eigen::Vector3d(constraintVal-1, 4, 0), Eigen::Vector3d(constraintVal+1, 5, 0));
   std::vector<std::shared_ptr<aikido::constraint::Sampleable>>
       sConstraints;
@@ -192,7 +188,7 @@ TEST_F(PlannerTest, PlanConstrainedCRRT)
   // Check the first waypoint
   auto s0 = stateSpace->createState();
   traj->evaluate(0, s0);
-  auto r0 = s0.getSubStateHandle<Rn>(0);
+  auto r0 = s0.getSubStateHandle<R3>(0);
   EXPECT_TRUE(r0.getValue().isApprox(startPose));
 
   // Check the last waypoint
@@ -214,7 +210,7 @@ TEST_F(PlannerTest, PlanThrowsOnNullGoalTestable)
   Eigen::Vector3d startPose(-5, -5, 0);
 
   auto subState1 =
-      stateSpace->getSubStateHandle<Rn>(startState, 0);
+      stateSpace->getSubStateHandle<R3>(startState, 0);
   subState1.setValue(startPose);
 
   // Easiest sampleable to get
@@ -236,7 +232,7 @@ TEST_F(PlannerTest, PlanThrowsOnGoalTestableMismatch)
   Eigen::Vector3d startPose(-5, -5, 0);
 
   auto subState1 =
-      stateSpace->getSubStateHandle<Rn>(startState, 0);
+      stateSpace->getSubStateHandle<R3>(startState, 0);
   subState1.setValue(startPose);
 
   // Easiest sampleable to get
@@ -263,7 +259,7 @@ TEST_F(PlannerTest, PlanThrowsOnNullGoalSampler)
   Eigen::Vector3d startPose(-5, -5, 0);
 
   auto subState1 =
-      stateSpace->getSubStateHandle<Rn>(startState, 0);
+      stateSpace->getSubStateHandle<R3>(startState, 0);
   subState1.setValue(startPose);
 
   // Dummy testable
@@ -284,7 +280,7 @@ TEST_F(PlannerTest, PlanThrowsOnGoalSamplerMismatch)
   Eigen::Vector3d startPose(-5, -5, 0);
 
   auto subState1 =
-      stateSpace->getSubStateHandle<Rn>(startState, 0);
+      stateSpace->getSubStateHandle<R3>(startState, 0);
   subState1.setValue(startPose);
 
   // Easiest sampleable to get