SE2MultiRigidBodyPlanning.cpp
1 /*********************************************************************
2 * Rice University Software Distribution License
3 *
4 * Copyright (c) 2012, Rice University
5 * All Rights Reserved.
6 *
7 * For a full description see the file named LICENSE.
8 *
9 *********************************************************************/
10 
11 /* Author: Ryan Luna */
12 
13 #include <omplapp/apps/SE2MultiRigidBodyPlanning.h>
14 #include <omplapp/config.h>
15 #include <ompl/geometric/planners/rrt/RRTConnect.h>
16 
17 using namespace ompl;
18 
19 int main()
20 {
21  // plan for two bodies in SE2
23 
24  // load the robot and the environment
25  std::string robot_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/2D/car1_planar_robot.dae";
26  std::string env_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/2D/Maze_planar_env.dae";
27  setup.setRobotMesh(robot_fname.c_str()); // The first mesh should use setRobotMesh.
28  setup.addRobotMesh(robot_fname.c_str()); // Subsequent robot meshes MUST use addRobotMesh!
29  setup.setEnvironmentMesh(env_fname.c_str());
30 
31  // constructing start and goal states
32  base::ScopedState<base::CompoundStateSpace> start(setup.getSpaceInformation());
33  base::ScopedState<base::CompoundStateSpace> goal(setup.getSpaceInformation());
34 
35  // define starting state for robot 1
37  start1->setXY(0., 0.);
38  start1->setYaw(0.);
39  // define goal state for robot 1
41  goal1->setXY(26., 0.);
42  goal1->setYaw(0.);
43 
44  // define starting state for robot 2
46  start2->setXY(26., 0.);
47  start2->setYaw(0.);
48  // define goal state for robot 2
50  goal2->setXY(-30., 0.);
51  goal2->setYaw(0.);
52 
53  // set the start & goal states
54  setup.setStartAndGoalStates(start, goal);
55 
56  // use RRTConnect for planning
57  setup.setPlanner(std::make_shared<geometric::RRTConnect>(setup.getSpaceInformation()));
58 
59  setup.setup();
60  setup.print(std::cout);
61  // attempt to solve the problem, and print it to screen if a solution is found
62  if (setup.solve(60))
63  {
64  setup.simplifySolution();
65  setup.getSolutionPath().printAsMatrix(std::cout);
66  }
67 
68  return 0;
69 }
A state in SE(2): (x, y, yaw)
Definition: SE2StateSpace.h:53
Definition of a scoped state.
Definition: ScopedState.h:56
void setYaw(double yaw)
Set the yaw component of the state. This is the rotation in plane, with respect to the Z axis...
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
void setXY(double x, double y)
Set the X and Y components of the state.
Definition: SE2StateSpace.h:93
Wrapper for ompl::app::RigidBodyPlanning that plans for multiple rigid bodies in SE2.