37 #include <ompl/base/spaces/SE3StateSpace.h>
38 #include <ompl/geometric/SimpleSetup.h>
39 #include <ompl/base/goals/GoalLazySamples.h>
40 #include <ompl/geometric/ik/GAIK.h>
42 #include <ompl/config.h>
45 namespace ob = ompl::base;
46 namespace og = ompl::geometric;
85 for (
int i = 0 ; i < 100 ; ++i)
86 if (g.solve(0.05, *region, result))
94 std::cout <<
"Found goal state: " << std::endl;
95 si->printState(result);
102 void planWithIK(
void)
119 start->setXYZ(0, 0, 0);
120 start->rotation().setIdentity();
121 ss.addStartState(start);
124 MyGoalRegion region(ss.getSpaceInformation());
128 ob::GoalSamplingFn samplingFunction = boost::bind(®ionSamplingWithGAIK, ss.getSpaceInformation(), ss.getProblemDefinition(), ®ion, _1, _2);
141 std::cout <<
"Found solution:" << std::endl;
143 ss.simplifySolution();
144 ss.getSolutionPath().print(std::cout);
147 std::cout <<
"No solution found" << std::endl;
155 int main(
int,
char **)
157 std::cout <<
"OMPL version: " << OMPL_VERSION << std::endl;