Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix ompl tests builds #178

Merged
merged 4 commits into from
Apr 18, 2017
Merged

Fix ompl tests builds #178

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
c0fab6a
Remove if-statement that doesn't work
jslee02 Apr 17, 2017
5b6890f
Don't build ompl tests without aikido_planner_ompl
jslee02 Apr 17, 2017
372517b
Fix ompl tests
jslee02 Apr 18, 2017
6c4b5ec
Merge branch 'master' into test/fix_planner_ompl
mkoval Apr 18, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 1 addition & 5 deletions tests/planner/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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")

44 changes: 12 additions & 32 deletions tests/planner/ompl/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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")
40 changes: 14 additions & 26 deletions tests/planner/ompl/OMPLTestHelpers.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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);
Expand All @@ -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);
}

Expand All @@ -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
Expand All @@ -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);
Expand Down Expand Up @@ -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);
Expand All @@ -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;
}

Expand Down Expand Up @@ -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) {
Expand All @@ -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;
};


Expand All @@ -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;
}
Expand All @@ -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;
}
Expand Down
21 changes: 0 additions & 21 deletions tests/planner/ompl/test_CRRT.cpp
View file
Open in desktop

This file was deleted.

44 changes: 20 additions & 24 deletions tests/planner/ompl/test_OMPLPlanner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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
Expand All @@ -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));
}

Expand All @@ -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;
Expand All @@ -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
Expand All @@ -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;
Expand Down Expand Up @@ -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
Expand All @@ -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;
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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
Expand Down