planOpenDE.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 "OpenDEWorld.inc"
14 #include "OMPLEnvironment.inc"
15 #include "OMPLSetup.inc"
16 
17 #include "displayOpenDE.h"
18 #include "omplapp/config.h"
19 
20 #include <drawstuff/drawstuff.h>
21 
22 #include <thread>
23 
24 #ifdef dDOUBLE
25 #define dsDrawBox dsDrawBoxD
26 #define dsDrawSphere dsDrawSphereD
27 #define dsDrawCylinder dsDrawCylinderD
28 #define dsDrawCapsule dsDrawCapsuleD
29 #endif
30 
31 static DisplayOpenDESpaces DISP;
32 static std::vector<std::pair<double, double>> POINTS;
33 static bool drawTree = true;
34 
35 static void start()
36 {
37  dAllocateODEDataForThread(dAllocateMaskAll);
38  static float xyz[3] = {3.8548f, 9.0843f, 7.5900f};
39  static float hpr[3] = {-145.5f, -22.5f, 0.25f};
40 
41  dsSetViewpoint(xyz, hpr);
42 }
43 
44 static void command(int cmd)
45 {
46  if ((char)cmd == 't')
47  drawTree = !drawTree;
48 }
49 
50 static void simLoop(int /*pause*/)
51 {
52  DISP.displaySpaces();
53 
54  if (drawTree)
55  {
56  glPointSize(2.0);
57  glColor3f(1.0f, 0.0f, 0.0f);
58  glBegin(GL_POINTS);
59  for (auto &i : POINTS)
60  glVertex3d(i.first, i.second, 0.05);
61  glEnd();
62  }
63 
64  static ompl::time::duration d = ompl::time::seconds(0.001);
65  std::this_thread::sleep_for(d);
66 }
67 
68 static void playPath(oc::OpenDESimpleSetup *ss)
69 {
70  while (true)
71  {
72  ss->playSolutionPath(0.005);
74  std::this_thread::sleep_for(d);
75  }
76 }
77 
78 int main(int argc, char **argv)
79 {
80  dsFunctions fn;
81  fn.version = DS_VERSION;
82  fn.start = &start;
83  fn.step = &simLoop;
84  fn.command = &command;
85  fn.stop = nullptr;
86 
87  auto *textures = (char *)alloca(strlen(OMPLAPP_RESOURCE_DIR) + 10);
88  strcpy(textures, OMPLAPP_RESOURCE_DIR);
89  strcat(textures, "/textures");
90  fn.path_to_textures = textures;
91 
92  dInitODE2(0);
93 
94  bodies = 0;
95  joints = 0;
96  boxes = 0;
97  spheres = 0;
98  resetSimulation();
99  DISP.addSpace(space, 0.9, 0.9, 0.5);
100  DISP.setGeomColor(avoid_box_geom, 0.9, 0.0, 0.0);
101  DISP.setGeomColor(movable_box_geom[0], 0.1, 0.8, 0.8);
102  DISP.setGeomColor(movable_box_geom[1], 0.1, 0.8, 0.8);
103  DISP.setGeomColor(movable_box_geom[2], 0.1, 0.8, 0.8);
104  DISP.setGeomColor(movable_box_geom[3], 0.1, 0.8, 0.8);
105  DISP.setGeomColor(goal_geom, 0.0, 0.9, 0.1);
106 
107  oc::OpenDEEnvironmentPtr ce = std::make_shared<CarEnvironment>();
108  ob::StateSpacePtr sm = std::make_shared<CarStateSpace>(ce);
109 
110  oc::ControlSpacePtr cm = std::make_shared<CarControlSpace>(sm);
111 
112  oc::OpenDESimpleSetup ss(cm);
113  ss.setGoal(std::make_shared<CarGoal>(ss.getSpaceInformation(), GOAL_X, GOAL_Y));
114  ob::RealVectorBounds vb(3);
115  vb.low[0] = -50;
116  vb.low[1] = -50;
117  vb.low[2] = -5;
118  vb.high[0] = 50;
119  vb.high[1] = 50;
120  vb.high[2] = 10;
121  ss.setVolumeBounds(vb);
122 
123  ss.setup();
124  ss.print();
125  std::shared_ptr<std::thread> th;
126 
127  std::cout << "Planning for at most 60 seconds ..." << std::endl;
128 
129  if (ss.solve(60))
130  {
131  std::cout << "Solved!" << std::endl;
133  last = ss.getSolutionPath().getStates().back();
134  std::cout << "Reached: " << last->getBodyPosition(0)[0] << " " << last->getBodyPosition(0)[1] << std::endl;
135 
136  POINTS.clear();
138  ss.getPlannerData(pd);
139  for (unsigned int i = 0; i < pd.numVertices(); ++i)
140  {
141  const double *pos = pd.getVertex(i).getState()->as<oc::OpenDEStateSpace::StateType>()->getBodyPosition(0);
142  POINTS.push_back(std::make_pair(pos[0], pos[1]));
143  }
144 
145  th = std::make_shared<std::thread>([&ss] { return playPath(&ss); });
146  }
147 
148  // run simulation
149  dsSimulationLoop(argc, argv, 640, 480, &fn);
150  if (th)
151  {
152  // th->interrupt();
153  th->join();
154  }
155 
156  dSpaceDestroy(space);
157  dWorldDestroy(world);
158  dCloseODE();
159 
160  return 0;
161 }
void setup() override
This method will create the necessary classes for planning. The solve() method will call this functio...
void setVolumeBounds(const base::RealVectorBounds &bounds)
Set the bounds for the planning volume.
const SpaceInformationPtr & getSpaceInformation() const
Get the current instance of the space information.
Definition: SimpleSetup.h:138
Create the set of classes typically needed to solve a control problem when forward propagation is com...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
void getPlannerData(base::PlannerData &pd) const
Get information about the exploration data structure the motion planner used.
void playSolutionPath(double timeFactor=1.0) const
Call playPath() on the solution path, if one is available.
std::vector< base::State * > & getStates()
Get the states that make up the path (as a reference, so it can be modified, hence the function is no...
Definition: PathControl.h:168
virtual base::PlannerStatus solve(double time=1.0)
Run the planner for a specified amount of time (default is 1 second)
Definition: SimpleSetup.cpp:88
std::chrono::system_clock::duration duration
Representation of a time duration.
Definition: Time.h:119
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:641
PathControl & getSolutionPath() const
Get the solution path. Throw an exception if no solution is available.
void setGoal(const base::GoalPtr &goal)
Set the goal for planning. This call is not needed if setStartAndGoalStates() has been called.
Definition: SimpleSetup.h:280
Definition of a scoped state.
Definition: ScopedState.h:120
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:128
virtual void print(std::ostream &out=std::cout) const
Print information about the current setup.
The lower and upper bounds for an Rn space.