SE3RigidBodyPlanning.cpp
1 /*********************************************************************
2 * Rice University Software Distribution License
3 *
4 * Copyright (c) 2010, Rice University
5 * All Rights Reserved.
6 *
7 * For a full description see the file named LICENSE.
8 *
9 *********************************************************************/
10 
11 /* Author: Ioan Sucan */
12 
13 #include <omplapp/apps/SE3RigidBodyPlanning.h>
14 #include <omplapp/config.h>
15 
16 using namespace ompl;
17 
18 int main()
19 {
20  // plan in SE3
22 
23  // load the robot and the environment
24  std::string robot_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/3D/cubicles_robot.dae";
25  std::string env_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/3D/cubicles_env.dae";
26  setup.setRobotMesh(robot_fname);
27  setup.setEnvironmentMesh(env_fname);
28 
29  // define start state
30  base::ScopedState<base::SE3StateSpace> start(setup.getSpaceInformation());
31  start->setX(-4.96);
32  start->setY(-40.62);
33  start->setZ(70.57);
34  start->rotation().setIdentity();
35 
36  // define goal state
38  goal->setX(200.49);
39  goal->setY(-40.62);
40  goal->setZ(70.57);
41  goal->rotation().setIdentity();
42 
43  // set the start & goal states
44  setup.setStartAndGoalStates(start, goal);
45 
46  // setting collision checking resolution to 1% of the space extent
47  setup.getSpaceInformation()->setStateValidityCheckingResolution(0.01);
48 
49  // we call setup just so print() can show more information
50  setup.setup();
51  setup.print();
52 
53  // try to solve the problem
54  if (setup.solve(10))
55  {
56  // simplify & print the solution
57  setup.simplifySolution();
58  setup.getSolutionPath().printAsMatrix(std::cout);
59  }
60 
61  return 0;
62 }
Definition of a scoped state.
Definition: ScopedState.h:56
Wrapper for ompl::app::RigidBodyPlanning that plans for rigid bodies in SE3.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21