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
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 }
virtual void printAsMatrix(std::ostream &out) const
Print the path as a real-valued matrix where the i-th row represents the i-th state along the path...
PathGeometric & getSolutionPath() const
Get the solution path. Throw an exception if no solution is available.
Definition of a scoped state.
Definition: ScopedState.h:56
void setStartAndGoalStates(const base::ScopedState<> &start, const base::ScopedState<> &goal, double threshold=std::numeric_limits< double >::epsilon())
Set the start and goal states to use.
Definition: SimpleSetup.cpp:90
Wrapper for ompl::app::RigidBodyPlanning that plans for rigid bodies in SE3.
void setup() override
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition: AppBase.h:98
const base::SpaceInformationPtr & getSpaceInformation() const
Get the current instance of the space information.
Definition: SimpleSetup.h:75
virtual bool setEnvironmentMesh(const std::string &env)
This function specifies the name of the CAD file representing the environment (env). Returns 1 on success, 0 on failure.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
virtual base::PlannerStatus solve(double time=1.0)
Run the planner for up to a specified amount of time (default is 1 second)
virtual void print(std::ostream &out=std::cout) const
Print information about the current setup.
void simplifySolution(double duration=0.0)
Attempt to simplify the current solution path. Spent at most duration seconds in the simplification p...
virtual bool setRobotMesh(const std::string &robot)
This function specifies the name of the CAD file representing the robot (robot). Returns 1 on success...