PaGMO  1.1.5
spheres.cpp
1 /*****************************************************************************
2  * Copyright (C) 2004-2015 The PaGMO development team, *
3  * Advanced Concepts Team (ACT), European Space Agency (ESA) *
4  * *
5  * https://github.com/esa/pagmo *
6  * *
7  * act@esa.int *
8  * *
9  * This program is free software; you can redistribute it and/or modify *
10  * it under the terms of the GNU General Public License as published by *
11  * the Free Software Foundation; either version 2 of the License, or *
12  * (at your option) any later version. *
13  * *
14  * This program is distributed in the hope that it will be useful, *
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
17  * GNU General Public License for more details. *
18  * *
19  * You should have received a copy of the GNU General Public License *
20  * along with this program; if not, write to the *
21  * Free Software Foundation, Inc., *
22  * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
23  *****************************************************************************/
24 
25 #include<gsl/gsl_odeiv2.h>
26 #include<gsl/gsl_errno.h>
27 #include<cmath>
28 #include<algorithm>
29 
30 #include "../exceptions.h"
31 #include "../types.h"
32 #include "../population.h"
33 #include "base_stochastic.h"
34 #include "spheres.h"
35 
36 static const int nr_input = 8;
37 static const int nr_output = 3;
38 static const int nr_spheres = 3;
39 static const int nr_eq = 9;
40 
41 static double norm2(double v[3]) {
42  return(v[0]*v[0] + v[1]*v[1] +v[2]*v[2]);
43 }
44 
45 namespace pagmo { namespace problem {
46 
47 spheres::spheres(int n_evaluations, int n_hidden_neurons,
48  double numerical_precision, unsigned int seed, bool symmetric, double sim_time, const std::vector<double>& sides) :
49  base_stochastic((nr_input/(int(symmetric)+1) + 1) * n_hidden_neurons + (n_hidden_neurons + 1) * nr_output, seed),
50  m_ffnn(nr_input,n_hidden_neurons,nr_output), m_n_evaluations(n_evaluations),
51  m_n_hidden_neurons(n_hidden_neurons), m_numerical_precision(numerical_precision),
52  m_ic(nr_eq), m_symm(symmetric), m_sim_time(sim_time), m_sides(sides) {
53  // Here we set the bounds for the problem decision vector, i.e. the nn weights
54  set_lb(-1);
55  set_ub(1);
56  // We then instantiate the ode integrator system using gsl
57  gsl_odeiv2_system sys = {ode_func,NULL,nr_eq,&m_ffnn};
58  m_sys = sys;
59  m_gsl_drv_pntr = gsl_odeiv2_driver_alloc_y_new(&m_sys, gsl_odeiv2_step_rk8pd, 1e-6,m_numerical_precision,0.0);
60  // And make sure the three sides are ordered and squared here
61  std::sort(m_sides.begin(),m_sides.end());
62  m_sides[0]*=m_sides[0]; m_sides[1]*=m_sides[1]; m_sides[2]*=m_sides[2];
63 }
64 
65 spheres::spheres(const spheres &other):
66  base_stochastic(other),
67  m_ffnn(other.m_ffnn),
68  m_n_evaluations(other.m_n_evaluations),m_n_hidden_neurons(other.m_n_hidden_neurons),
69  m_numerical_precision(other.m_numerical_precision),m_ic(other.m_ic), m_symm(other.m_symm), m_sim_time(other.m_sim_time),m_sides(other.m_sides)
70 {
71  // Here we set the bounds for the problem decision vector, i.e. the nn weights
72  gsl_odeiv2_system sys = {ode_func,NULL,nr_eq,&m_ffnn};
73  m_sys = sys;
74  m_gsl_drv_pntr = gsl_odeiv2_driver_alloc_y_new(&m_sys, gsl_odeiv2_step_rk8pd, 1e-6,m_numerical_precision,0.0);
75 }
76 
78  gsl_odeiv2_driver_free(m_gsl_drv_pntr);
79 }
80 
83 {
84  return base_ptr(new spheres(*this));
85 }
86 
87 //This function evaluates the fitness of a given spheres configuration ....
88 double spheres::single_fitness( const std::vector<double> &y, const ffnn& neural_net) const {
89 
90 
91  double fit = 0.0;
92  double context[8], vel_f[3];
93  int k;
94 
95  // for each sphere
96  for( int i = 0; i < nr_spheres; i++ ){ // i - is the sphere counter 0 .. 1 .. 2 ..
97  k = 0;
98 
99  // we now load in context the perceived data (as decoded from the world state y)
100  for( int n = 1; n <= nr_spheres - 1; n++ ){ // consider the vector from each other sphere
101  for( int j = 0; j < 3; j++ ){ // consider each component from the vectors
102  context[k++] = y[i*3 + j] - y[ (i*3 + j + n*3) % 9 ];
103  }
104  }
105 
106  // context now contains the relative position vectors (6 components) in the absolute frame
107  // we write, on the last two components of context, the norms2 of these relative positions
108  context[6] = context[0]*context[0] + context[1]*context[1] + context[2]*context[2];
109  context[7] = context[3]*context[3] + context[4]*context[4] + context[5]*context[5];
110 
111  //We evaluate the output from the neural net
112  neural_net.eval(vel_f, context);
113  vel_f[0] = vel_f[0] * 2 * 0.3 - 0.3;
114  vel_f[1] = vel_f[1] * 2 * 0.3 - 0.3;
115  vel_f[2] = vel_f[2] * 2 * 0.3 - 0.3;
116 
117  //and we add the final velocity violation (two times since we will divide by two later)
118  fit += 2 *(norm2(vel_f));
119 
120  //and we keep them within the box [-1 1]
121  //if (std::abs(y[i*3]) > 1) fit += std::abs(y[i*3]);
122  //if (std::abs(y[i*3+1]) > 1) fit += std::abs(y[i*3+1]);
123  //if (std::abs(y[i*3+2]) > 1) fit += std::abs(y[i*3+2]);
124  }
125  fit = fit/2;
126 
127  // Here we compute the violation from the desired velocity
128 #define SRT_PAIR(a,b) if ((b)<(a)) {(tmp=a);(a=b);(b=tmp);}
129  double tmp = 0;
130  double a2 = (y[0]-y[3])*(y[0]-y[3]) + (y[1]-y[4])*(y[1]-y[4]) + (y[2]-y[5])*(y[2]-y[5]);
131  double b2 = (y[0]-y[6])*(y[0]-y[6]) + (y[1]-y[7])*(y[1]-y[7]) + (y[2]-y[8])*(y[2]-y[8]);
132  double c2 = (y[6]-y[3])*(y[6]-y[3]) + (y[7]-y[4])*(y[7]-y[4]) + (y[8]-y[5])*(y[8]-y[5]);
133 
134  SRT_PAIR(a2,b2)
135  SRT_PAIR(b2,c2)
136  SRT_PAIR(a2,b2)
137 #undef SRT_PAIR
138 
139  fit += (a2-m_sides[0])*(a2-m_sides[0])+(b2-m_sides[1])*(b2-m_sides[1])+(c2-m_sides[2])*(c2-m_sides[2]);
140  //fit += std::abs(a2-m_sides[0])+std::abs(b2-m_sides[1])+std::abs(c2-m_sides[2]);
141  return fit;
142 }
143 
144 int spheres::ode_func( double t, const double y[], double f[], void *params ) {
145  (void)t;
146  // Here we recover the neural network
147  ffnn *ptr_ffnn = (ffnn*)params;
148 
149  // The fixed-size vector context represent the sensory data perceived from each sphere. These are
150  // the body axis components of the relative positions of the other spheres, and their modules
151  double context[nr_input];
152  double out[nr_output];
153 
154  // Here are some counters
155  int k;
156 
157 
158  for( int i = 0; i < nr_spheres; i++ ){ // i - is the sphere counter 0 .. 1 .. 2 ..
159  k = 0;
160 
161  // we now load in context the perceived data (as decoded from the world state y)
162  for( int n = 1; n <= nr_spheres - 1; n++ ){ // consider the vector from each other sphere
163  for( int j = 0; j < 3; j++ ){ // consider each component from the vectors
164  context[k++] = y[i*3 + j] - y[ (i*3 + j + n*3) % 9 ];
165  }
166  }
167 
168  // context now contains the relative position vectors (6 components) in the absolute frame
169  // we write, on the last two components of context, the norms of these relative positions
170  context[6] = context[0]*context[0] + context[1]*context[1] + context[2]*context[2];
171  context[7] = context[3]*context[3] + context[4]*context[4] + context[5]*context[5];
172 
173  //We evaluate the output from the neural net
174  ptr_ffnn->eval(out, context);
175 
176  //Here we set the dynamics transforming the nn output [0,1] in desired velocities [-0/3,0.3]
177  f[i*3] = out[0] * 0.3 * 2 - 0.3;
178  f[i*3+1] = out[1] * 0.3 * 2 - 0.3;
179  f[i*3+2] = out[2] * 0.3 * 2 - 0.3;
180 
181  }
182  return GSL_SUCCESS;
183 }
184 
185 spheres::ffnn::ffnn(const unsigned int n_inputs, const unsigned int n_hidden,const unsigned int n_outputs) :
186  m_n_inputs(n_inputs), m_n_hidden(n_hidden), m_n_outputs(n_outputs),
187  m_weights((n_inputs + 1) * n_hidden + (n_hidden + 1) * n_outputs), m_hidden(n_hidden)
188 {}
189 
190 void spheres::ffnn::set_weights(const std::vector<double> &weights) {
191  m_weights = weights;
192 }
193 
194 void spheres::ffnn::eval(double out[], const double in[]) const {
195  // Offset for the weights to the output nodes
196  unsigned int offset = m_n_hidden * (m_n_inputs + 1);
197 
198  // -- PROCESS CONTEXT USING THE NEURAL NETWORK --
199  for( unsigned int i = 0; i < m_n_hidden; i++ ){
200  // Set the bias (the first weight to the i'th hidden node)
201  m_hidden[i] = m_weights[i * (m_n_inputs + 1)];
202 
203  for( unsigned int j = 0; j < m_n_inputs; j++ ){
204  // Compute the weight number
205  int ji = i * (m_n_inputs + 1) + (j + 1);
206  // Add the weighted input
207  m_hidden[i] += m_weights[ji] * in[j];
208  }
209 
210  // Apply the transfer function (a sigmoid with output in [0,1])
211  m_hidden[i] = 1.0 / ( 1 + std::exp( -m_hidden[i] ));
212  }
213 
214  // generate values for the output nodes
215  for( unsigned int i = 0; i < m_n_outputs; i++ ){
216  // add the bias (weighted by the first weight to the i^th output node
217  out[i] = m_weights[offset + i * (m_n_hidden + 1)];
218 
219  for( unsigned int j = 0; j < m_n_hidden; j++ ){
220  // compute the weight number
221  int ji = offset + i * (m_n_hidden + 1) + (j + 1);
222  // add the weighted input
223  out[i] += m_weights[ji] * m_hidden[j];
224  }
225 
226  out[i] = 1.0 / ( 1 + std::exp( -out[i] ));
227  }
228 }
229 
231  f[0]=0;
232  // Make sure the pseudorandom sequence will always be the same
233  m_drng.seed(m_seed);
234  // Set the ffnn weights from x, by accounting for symmetries in neurons weights
235  set_nn_weights(x);
236  // Loop over the number of repetitions
237  for (int count=0;count<m_n_evaluations;++count) {
238  // Creates the initial conditions at random
239  // Positions starts in a [-1,1] box
240  for (int i=0; i<6; ++i) {
241  m_ic[i] = (m_drng()*2 - 1);
242  }
243 
244  // Centered around the origin
245  m_ic[6] = - (m_ic[0] + m_ic[3]);
246  m_ic[7] = - (m_ic[1] + m_ic[4]);
247  m_ic[8] = - (m_ic[2] + m_ic[5]);
248 
249  // Integrate the system
250  double t0 = 0.0;
251  double tf = m_sim_time;
252  //gsl_odeiv2_driver_set_hmin (m_gsl_drv_pntr, 1e-6);
253  int status = gsl_odeiv2_driver_apply( m_gsl_drv_pntr, &t0, tf, &m_ic[0] );
254  // Not sure if this help or what it does ....
255  gsl_odeiv2_driver_reset (m_gsl_drv_pntr);
256  if( status != GSL_SUCCESS ){
257  printf ("ERROR: gsl_odeiv2_driver_apply returned value = %d\n", status);
258  break;
259  }
260  f[0] += single_fitness(m_ic,m_ffnn);
261 
262  }
263  f[0] /= m_n_evaluations;
264 }
265 
266 static bool my_sort_function (std::vector<double> i,std::vector<double> j) { return (i[9] < j[9]); }
267 
268 std::vector<std::vector<double> > spheres::post_evaluate(const decision_vector & x, int N, unsigned int seed) const {
269  std::vector<double> one_row(10,0.0);
270  std::vector<std::vector<double> > ret(N,one_row);
271  // Make sure the pseudorandom sequence will always be the same
272  m_drng.seed(seed);
273  // Set the ffnn weights
274  set_nn_weights(x);
275  // Loop over the number of repetitions
276  for (int count=0;count<N;++count) {
277  // Creates the initial conditions at random
278 
279  // Position starts in a [-1,1] box (evolution is in [-2,2])
280  for (int i=0; i<6; ++i) {
281  m_ic[i] = (m_drng()*2 - 1);
282  }
283  // Centered around the origin
284  m_ic[6] = - (m_ic[0] + m_ic[3]);
285  m_ic[7] = - (m_ic[1] + m_ic[4]);
286  m_ic[8] = - (m_ic[2] + m_ic[5]);
287 
288  for (int i=0; i<9; ++i) {
289  one_row[i] = m_ic[i];
290  }
291 
292  // Integrate the system
293  double t0 = 0.0;
294  double tf = m_sim_time;
295  //gsl_odeiv2_driver_set_hmin (m_gsl_drv_pntr, 1e-6);
296  int status = gsl_odeiv2_driver_apply( m_gsl_drv_pntr, &t0, tf, &m_ic[0] );
297  // Not sure if this help or what it does ....
298  //gsl_odeiv2_driver_reset (m_gsl_drv_pntr);
299  if( status != GSL_SUCCESS ){
300  printf ("ERROR: gsl_odeiv2_driver_apply returned value = %d\n", status);
301  break;
302  }
303  one_row[9] = single_fitness(m_ic,m_ffnn);
304  ret[count] = one_row;
305  }
306  // sorting by fitness
307  std::sort (ret.begin(), ret.end(), my_sort_function);
308  return ( ret );
309 }
310 
311 void spheres::set_nn_weights(const decision_vector &x) const {
312  if (m_symm) { //symmetric weigths activated
313  int w = 0;
314  for(unsigned int h = 0; h < m_ffnn.m_n_hidden; h++)
315  {
316  int start_index = h * 5; // (nr_input/2+1)
317  // bias, dx1, dy1, dz1
318  for(int j = 0; j < 4; j++)
319  {
320  m_ffnn.m_weights[w] = x[start_index+j]; w++;
321  }
322  // dx2, dy2, dz2
323  for(int j = 1; j <= 3; j++)
324  {
325  m_ffnn.m_weights[w] = x[start_index+j]; w++;
326  }
327  // distance 1
328  m_ffnn.m_weights[w] = x[start_index+4]; w++;
329  // distance 2
330  m_ffnn.m_weights[w] = x[start_index+4]; w++;
331  }
332  int ind = 0;
333  for(unsigned int ww = w; ww < m_ffnn.m_weights.size(); ww++)
334  {
335  m_ffnn.m_weights[ww] = x[(nr_input/2+1)*m_ffnn.m_n_hidden+ind];
336  ind++;
337  }
338  } else {//no symmetric weights
339  m_ffnn.m_weights = x;
340  }
341 }
342 
343 std::vector<std::vector<double> > spheres::simulate(const decision_vector &x, const std::vector<double> &ic, int N) const {
344  std::vector<double> y0(ic);
345  std::vector<double> one_row(10,0.0);
346  std::vector<std::vector<double> > ret;
347  // Set the ffnn weights
348  set_nn_weights(x);
349  // Integrate the system
350  double ti, t0=0;
351  double tf = m_sim_time;
352 
353  // pushing back the initial conditions
354  one_row[0] = 0.0;
355  std::copy(y0.begin(),y0.end(),one_row.begin()+1);
356  ret.push_back(one_row);
357 
358  for( int i = 1; i <= N; i++ ){
359  ti = i * tf / N;
360  int status = gsl_odeiv2_driver_apply( m_gsl_drv_pntr, &t0, ti, &y0[0] );
361  //pushing_back the result
362  one_row[0] = ti;
363  std::copy(y0.begin(),y0.end(),one_row.begin()+1);
364  ret.push_back(one_row);
365  if( status != GSL_SUCCESS ){
366  printf ("ERROR: gsl_odeiv2_driver_apply returned value = %d\n", status);
367  break;
368  }
369  }
370  //Not sure if this help or what it does ....
371  //gsl_odeiv2_driver_reset (m_gsl_drv_pntr);
372  return ( ret );
373 }
374 
375 std::vector<double> spheres::get_nn_weights(decision_vector x) const {
376  set_nn_weights(x);
377  return m_ffnn.m_weights;
378 }
379 
380 std::string spheres::get_name() const
381 {
382  return "MIT SPHERES - Neurocontroller Evolution";
383 }
384 
386 
389 std::string spheres::human_readable_extra() const
390 {
391  std::ostringstream oss;
392  oss << "\n\tSample Size: " << m_n_evaluations << '\n';
393  oss << "\tHidden Neurons: " << m_n_hidden_neurons << '\n';
394  oss << "\tODE precision: " << m_numerical_precision << '\n';
395  oss << "\tSeed: " << m_seed << '\n';
396  oss << "\tSymmetric Weights: " << m_symm << '\n';
397  oss << "\tSimulation time: " << m_sim_time << '\n';
398  oss << "\tTriangle sides (squared): " << m_sides << '\n';
399  return oss.str();
400 }
401 
402 } //namespace problem
403 } //namespace pagmo
404 
405 BOOST_CLASS_EXPORT_IMPLEMENT(pagmo::problem::spheres)
Root PaGMO namespace.
boost::shared_ptr< base > base_ptr
Alias for shared pointer to base problem.
Definition: problem/base.h:62
std::vector< double > decision_vector
Decision vector type.
Definition: types.h:40
~spheres()
Destructor.
Definition: spheres.cpp:77
base_ptr clone() const
Clone method.
Definition: spheres.cpp:82
rng_double m_drng
Random number generator for double-precision floating point values.
std::string human_readable_extra() const
Extra human readable info for the problem.
Definition: spheres.cpp:389
void set_lb(const decision_vector &)
Set lower bounds from pagmo::decision_vector.
unsigned int m_seed
Seed of the random number generator.
void set_ub(const decision_vector &)
Set upper bounds from pagmo::decision_vector.
std::vector< std::vector< double > > post_evaluate(const decision_vector &x, int N=25000, unsigned int seed=0) const
Post evaluation of the neural controller.
Definition: spheres.cpp:268
std::vector< double > fitness_vector
Fitness vector type.
Definition: types.h:42
std::string get_name() const
Get problem's name.
Definition: spheres.cpp:380
Base Stochastic Optimization Problem.
void objfun_impl(fitness_vector &, const decision_vector &) const
Objective function implementation.
Definition: spheres.cpp:230
Evolutionary Neuro-Controller for the MIT Spheres (perception-action defined in the absolute frame) ...
Definition: spheres.h:70
std::vector< std::vector< double > > simulate(const decision_vector &x, const std::vector< double > &ic, int N) const
Performs one "detailed" simulation.
Definition: spheres.cpp:343
std::vector< double > get_nn_weights(decision_vector x) const
Gets the weights of the neural network.
Definition: spheres.cpp:375
spheres(int n_evaluations=10, int n_hidden=10, double ode_prec=1E-6, unsigned int seed=0, bool symmetric=false, double sim_time=50.0, const std::vector< double > &sides=std::vector< double >(3, 0.5))
Constructor.
Definition: spheres.cpp:47