urop/dynamics.cpp 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
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
#ifndef DYNAMICS_CPP
#define DYNAMICS_CPP
#include <iostream>
#include <boost/array.hpp>
#include <boost/numeric/odeint.hpp>
#include "node.h"

#define DT 0.1
#define dt 0.01

#ifndef PI
#define PI 3.14159265358979
#endif


using namespace std;
using namespace boost::numeric::odeint;

/*
States
0: x
1: x_dot
2: theta
3: theta_dot
*/


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

struct record
{
    vector<state_type>& m_states;
    vector< double >& m_times;

    record(vector<state_type> &states, vector< double > &times)
    : m_states(states), m_times( times ) { }

    void operator()(const state_type &y, double t )
    {
        m_states.push_back(y);
        m_times.push_back(t);
    }
};


class Dynamics {

  public:
  //PARAMETERS
    double G,M,m,L,I,f;
    Dynamics(){
        G = 9.8;//Gravitational Acceleration (m/s^2)
        M = 10.0;//Mass of object (kg)
        m = 5.0;//Pendulum mass (kg)
        L = 2.5;//Length of pendulum to COM of rod(m)
        I = 2.0;//Moment of Inertia of pendulum (kg*m^2)
	}

    Node* update(Node* n, double input){
        f = input;
	    state_type x(4);
        vector<state_type> states;
        vector<double> times;

        double t1 = n->t;
        double t2 = t1+DT;
        double cost = n->cost + (t2-t1)*(t2-t1) + f*f;
        x[0] = n->x;
        x[1] = n->v;
        x[2] = n->theta;
        x[3] = n->w;
	    integrate(*this, x, t1, t2, dt, record(states, times));
        states.pop_back();
        times.pop_back();

        /*
        #ifndef WRITE_OUTPUT_FUN
        cout << t2 << '\t' << '\t' << "x:" <<  x[0] << '\t' << '\t' << "x_d:" << x[1] << '\t' << '\t' << "th:" << x[2] << '\t' << '\t' << "th_d:" << x[3] << endl;
        for (int i =0; i < states.size(); i++){
            cout << "Time: " << times[i] << " " << states[i][2] << endl;
        }
        #endif
        */

        //Normalizing the theta value
        x[2] = fmod(x[2], 2.0*PI);
        if (x[2] < 0){
            x[2] = x[2]+2.0*PI;
        }
        for (int i = 0; i < states.size(); i++){
            states[i][2] = fmod(states[i][2], 2.0*PI);
            if (x[2] < 0){
                states[i][2] = states[i][2]+2.0*PI;
            }
        }

        return new Node(t2, x[0], x[1], x[2], x[3], input, cost, n, times, states);
    }


    void operator() ( const state_type &x, state_type &dxdt, const double /*t*/)
    {
	    dxdt[0] = x[1];
	    dxdt[1] = (((I+m*L*L)*(f+m*L*x[3]*x[3]*sin(x[2]))+(m*m*L*L)*cos(x[2])*sin(x[2])*G)/((M+m)*(I+m*L*L)-(m*m*L*L)*cos(x[2])*cos(x[2])));
	    dxdt[2] = x[3];
	    dxdt[3] = (((-m*L*cos(x[2]))*(f+m*L*x[3]*x[3]*sin(x[2]))+(M+m)*(-m*G*L*sin(x[2])))/((M+m)*(I+m*L*L)-m*m*L*L*cos(x[2])*cos(x[2])));
    }
};


/*

 * state update_state(state s, double f, double dt){
	state s_out;
	double x = s.x;
	double v = s.v;
	double theta = s.theta;
	double omega = s.omega;

	double sinth = sin(theta);
	double costh = cos(theta);

	s_out.x = v*dt;
	s_out.v = (((I+m*L*L)*(f+m*L*omega*omega*sinth)+(m*m*L*L)*costh*sinth*G)/((M+m)*(I+m*L*L)-(m*m*L*L)*costh*costh))*dt;
	s_out.theta = omega*dt;
	s_out.omega = (((-m*L*costh)*(f+m*L*theta*theta*sinth)+(M+m)*(-m*G*L*sinth))/((M+m)*(I+m*L*L)-m*m*L*L*costh*costh))*dt;

	return s_out;
}*/

#endif