urop/pendulum.h at master · hanssusilo/urop · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
#ifndef PENDULUM_H
#define PENDULUM_H

#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <ctime>
#include <fstream>
#include <iostream>
#include <GL/glut.h>
#include <GL/freeglut.h>

#include "node.h"
#include "bounds.h"
#include "obstacles.h"
#include "dynamics.cpp"
#include "kdtree.h"
#include "point.h"

using namespace std;

#ifndef st_type
#define st_type
typedef vector<double> state_type;
#endif

class Pendulum {
  Point b; // goal
  Dynamics d;
  Node* root; // start

  double max_u;
  double x_epsilon;
  double v_epsilon;
  double theta_epsilon;
  double w_epsilon;

  Bounds* bounds;
  vector<Obstacles*> obstacles;
  kdtree* nodes_tree;
  vector<Node*> nodes;


  void LoadBounds();
  void LoadObstacles();

  bool CheckBounds(Node* n);
  bool CheckCollisions(Node* n);
  bool Feasible(Node* n);
  bool CheckGoal(Node* n);


//  vector<Node*> bestPath;

//  vector<Node*> nodesToDelete;

//  Node* GetNodeToExpand();
  Node* GetNodeToExpandUniform();
  Node* GetNodeToExpandRRT();

//  Node* GetNewNode(Node* parent, double u, double omega, double dist);
//  void AddNode(Node* n);
//  void DeleteNode(Node* n);

 public:
  Pendulum(Node* start, Point goal);
  ~Pendulum();
//  vector<Node*> FindPathSolutions(int iterations);
  vector<Node*> FindPathIterations(int iterations);
//  vector<Node*> GetNodes();

    vector<Node*> TraverseNodes(Node* n);
    vector<Node*> FindOptimalPathIterations(int iterations);
    void PhaseDiagram(int* argc, char** argv);
};
#endif