00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <ompl/base/SpaceInformation.h>
00038 #include <ompl/base/spaces/SE3StateSpace.h>
00039 #include <ompl/geometric/planners/rrt/RRTConnect.h>
00040 #include <ompl/geometric/SimpleSetup.h>
00041
00042 #include <ompl/config.h>
00043 #include <iostream>
00044
00045 namespace ob = ompl::base;
00046 namespace og = ompl::geometric;
00047
00048 bool isStateValid(const ob::State *state)
00049 {
00050
00051 const ob::SE3StateSpace::StateType *se3state = state->as<ob::SE3StateSpace::StateType>();
00052
00053
00054 const ob::RealVectorStateSpace::StateType *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0);
00055
00056
00057 const ob::SO3StateSpace::StateType *rot = se3state->as<ob::SO3StateSpace::StateType>(1);
00058
00059
00060
00061
00062
00063 return (void*)rot != (void*)pos;
00064 }
00065
00066 void plan(void)
00067 {
00068
00069 ob::StateSpacePtr space(new ob::SE3StateSpace());
00070
00071
00072 ob::RealVectorBounds bounds(3);
00073 bounds.setLow(-1);
00074 bounds.setHigh(1);
00075
00076 space->as<ob::SE3StateSpace>()->setBounds(bounds);
00077
00078
00079 ob::SpaceInformationPtr si(new ob::SpaceInformation(space));
00080
00081
00082 si->setStateValidityChecker(boost::bind(&isStateValid, _1));
00083
00084
00085 ob::ScopedState<> start(space);
00086 start.random();
00087
00088
00089 ob::ScopedState<> goal(space);
00090 goal.random();
00091
00092
00093 ob::ProblemDefinitionPtr pdef(new ob::ProblemDefinition(si));
00094
00095
00096 pdef->setStartAndGoalStates(start, goal);
00097
00098
00099 ob::PlannerPtr planner(new og::RRTConnect(si));
00100
00101
00102 planner->setProblemDefinition(pdef);
00103
00104
00105 planner->setup();
00106
00107
00108
00109 si->printSettings(std::cout);
00110
00111
00112 pdef->print(std::cout);
00113
00114
00115 bool solved = planner->solve(1.0);
00116
00117 if (solved)
00118 {
00119
00120
00121 ob::PathPtr path = pdef->getGoal()->getSolutionPath();
00122 std::cout << "Found solution:" << std::endl;
00123
00124
00125 path->print(std::cout);
00126 }
00127 else
00128 std::cout << "No solution found" << std::endl;
00129 }
00130
00131 void planWithSimpleSetup(void)
00132 {
00133
00134 ob::StateSpacePtr space(new ob::SE3StateSpace());
00135
00136
00137 ob::RealVectorBounds bounds(3);
00138 bounds.setLow(-1);
00139 bounds.setHigh(1);
00140
00141 space->as<ob::SE3StateSpace>()->setBounds(bounds);
00142
00143
00144 og::SimpleSetup ss(space);
00145
00146
00147 ss.setStateValidityChecker(boost::bind(&isStateValid, _1));
00148
00149
00150 ob::ScopedState<> start(space);
00151 start.random();
00152
00153
00154 ob::ScopedState<> goal(space);
00155 goal.random();
00156
00157
00158 ss.setStartAndGoalStates(start, goal);
00159
00160
00161 ss.setup();
00162 ss.print();
00163
00164
00165 bool solved = ss.solve(1.0);
00166
00167 if (solved)
00168 {
00169 std::cout << "Found solution:" << std::endl;
00170
00171 ss.simplifySolution();
00172 ss.getSolutionPath().print(std::cout);
00173 }
00174 else
00175 std::cout << "No solution found" << std::endl;
00176 }
00177
00178 int main(int, char **)
00179 {
00180 std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
00181
00182 plan();
00183
00184 std::cout << std::endl << std::endl;
00185
00186 planWithSimpleSetup();
00187
00188 return 0;
00189 }