iCub-main
test.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2007-2010 RobotCub Consortium, European Commission FP6 Project IST-004370
3  * author: Arjan Gijsberts
4  * email: arjan.gijsberts@iit.it
5  * website: www.robotcub.org
6  * Permission is granted to copy, distribute, and/or modify this program
7  * under the terms of the GNU General Public License, version 2 or any
8  * later version published by the Free Software Foundation.
9  *
10  * A copy of the license can be found at
11  * http://www.robotcub.org/icub/license/gpl.txt
12  *
13  * This program is distributed in the hope that it will be useful, but
14  * WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16  * Public License for more details
17  */
18 
19 #include <algorithm>
20 #include <string>
21 #include <vector>
22 #include <cmath>
23 #include <iostream>
24 #include <sstream>
25 #include <fstream>
26 #include <stdexcept>
27 #include <cassert>
28 
29 #include <yarp/os/Network.h>
30 #include <yarp/os/ResourceFinder.h>
31 #include <yarp/os/RFModule.h>
32 #include <yarp/os/PortablePair.h>
33 #include <yarp/sig/Vector.h>
34 #include <yarp/os/Port.h>
35 #include <yarp/os/BufferedPort.h>
36 #include <yarp/os/Time.h>
37 #include <yarp/os/Vocab.h>
38 
39 #define TWOPI 6.283185307179586
40 
41 using namespace yarp::os;
42 using namespace yarp::sig;
43 
44 namespace iCub {
45 namespace learningmachine {
46 namespace test {
47 
48 // the Prediction class is compatible with a PortablePair of Vector objects.
49 // in this class, we demonstrate that we in fact only need standard Yarp
50 // classes to make use of the learningMachine library: no learningMachine headers
51 // or library linking required!
52 typedef PortablePair<Vector,Vector> Prediction;
53 
54 // it's 2011 and I still have to implement a double to string conversion? come on!
55 std::string doubletostring(double d) {
56  std::ostringstream output;
57  output << d;
58  return output.str();
59 }
60 
61 std::string inttostring(int i) {
62  std::ostringstream output;
63  output << i;
64  return output.str();
65 }
66 
67 std::string printVector(const std::vector<int>& v) {
68  std::ostringstream output;
69  output << "[";
70  for(unsigned int i = 0; i < v.size(); i++) {
71  if(i > 0) output << ", ";
72  output << v[i];
73  }
74  output << "]";
75  return output.str();
76 }
77 
78 std::string printVector(const Vector& v) {
79  std::ostringstream output;
80  output << "[";
81  for(size_t i = 0; i < v.size(); i++) {
82  if(i > 0) output << ",";
83  output << v[i];
84  }
85  output << "]";
86  return output.str();
87 }
88 
89 std::string printPrediction(Prediction& p) {
90  std::ostringstream output;
91  Vector v = p.head;
92  output << "[";
93  for(size_t i = 0; i < v.size(); i++) {
94  if(i > 0) output << ",";
95  output << v[i];
96  }
97  v = p.body;
98  if(v.size() > 0) {
99  output << " +/- ";
100  for(size_t i = 0; i < v.size(); i++) {
101  if(i > 0) output << ",";
102  output << v[i];
103  }
104  }
105  output << "]";
106  return output.str();
107 }
108 
109 
110 class Dataset {
111 private:
112  int samplesRead;
113  std::ifstream file;
114  std::string filename;
115  std::vector<int> inputCols;
116  std::vector<int> outputCols;
117 
118 
119 public:
121  this->inputCols.resize(1);
122  this->outputCols.resize(1);
123 
124  this->inputCols.clear();
125  this->outputCols.clear();
126  }
127 
128  // manage input and output columns
129  void addInputColumn(int col) {
130  this->inputCols.push_back(col);
131  }
132 
133  void addOutputColumn(int col) {
134  this->outputCols.push_back(col);
135  }
136 
137  std::vector<int> getInputColumns() {
138  return this->inputCols;
139  }
140 
141  std::vector<int> getOutputColumns() {
142  return this->outputCols;
143  }
144 
145  // gets the filename of the open dataset
146  std::string getFilename() {
147  return this->filename;
148  }
149 
150  // sets the filename for the dataset
151  void setFilename(std::string filename) {
152  this->filename = filename;
153  }
154 
155  // manage file datastream
156  void open() {
157  this->open(this->filename);
158  }
159 
160  // open a datafile
161  void open(std::string filename) {
162  // close previous file
163  if(this->file.is_open()) {
164  this->file.close();
165  }
166 
167  this->file.open(filename.c_str());
168  if(!file.is_open() || file.fail()) {
169  std::string msg("could not open file '" + filename + "'");
170  this->setFilename("");
171  throw std::runtime_error(msg);
172  return; // will not be reached anyways...
173  }
174  this->setFilename(filename);
175  this->reset();
176  }
177 
178  bool hasNextSample() {
179  return !this->file.eof() && this->file.good();
180  }
181 
182  void reset() {
183  this->samplesRead = 0;
184  this->file.clear();
185  this->file.seekg(0, std::ios::beg);
186  }
187 
188  // retrieve new sample from datastream
189  std::pair<Vector,Vector> getNextSample() {
190  Vector input;
191  Vector output;
192 
193  int currentCol = 1;
194  double val = 0.0;
195 
196  std::string lineString;
197 
198  if(!this->hasNextSample()) {
199  throw std::runtime_error("at end of dataset");
200  }
201 
202  // find first valid string that does not start with #
203  do {
204  getline(file, lineString);
205  } while(lineString[0] == '#' && !file.eof());
206 
207  if(!this->hasNextSample()) {
208  throw std::runtime_error("at end of dataset");
209  }
210 
211  // read string as stream buffer
212  std::istringstream lineStream(lineString);
213  while(!lineStream.eof()) {
214  lineStream >> val;
215 
216  // add to input sample if appropriate
217  std::vector<int>::iterator inputFind = std::find(this->inputCols.begin(), this->inputCols.end(), currentCol);
218  if(inputFind != this->inputCols.end()) {
219  input.push_back(val);
220  }
221 
222  // add to output sample if appropriate
223  std::vector<int>::iterator outputFind = std::find(this->outputCols.begin(), this->outputCols.end(), currentCol);
224  if(outputFind != this->outputCols.end()) {
225  output.push_back(val);
226  }
227  currentCol++;
228  }
229 
230  std::pair<Vector,Vector> sample(input, output);
231  return sample;
232  }
233 };
234 
244 class MachineLearnerTestModule : public RFModule {
245 private:
246  BufferedPort<PortablePair<Vector,Vector> > train_out;
247  Port predict_inout;
248 
249  std::string portPrefix;
250 
251  Dataset dataset;
252 
253  int frequency; // in Hz
254 
255  void registerPort(Contactable& port, std::string name) {
256  if(port.open(name.c_str()) != true) {
257  std::string msg("could not register port ");
258  msg+=name;
259  throw std::runtime_error(msg);
260  }
261  }
262 
263  void registerAllPorts() {
264  this->registerPort(this->train_out, this->portPrefix + "/train:o");
265  this->train_out.setStrict();
266  this->registerPort(this->predict_inout, this->portPrefix + "/predict:io");
267  }
268 
269  void unregisterAllPorts() {
270  this->train_out.close();
271  this->predict_inout.close();
272  }
273 
274 public:
275  MachineLearnerTestModule(std::string pp = "/lm/test") : portPrefix(pp) {
276 
277  }
278 
279  void printOptions(std::string error = "") {
280  if(error != "") {
281  std::cout << "Error: " << error << std::endl;
282  }
283  std::cout << "Available options" << std::endl;
284  std::cout << "--help Display this help message" << std::endl;
285  std::cout << "--trainport port Data port for the training samples" << std::endl;
286  std::cout << "--predictport port Data port for the prediction samples" << std::endl;
287  std::cout << "--datafile file Filename containing the dataset" << std::endl;
288  std::cout << "--inputs (idx1, ..) List of indices to use as inputs" << std::endl;
289  std::cout << "--outputs (idx1, ..) List of indices to use as outputs" << std::endl;
290  std::cout << "--port pfx Prefix for registering the ports" << std::endl;
291  std::cout << "--frequency f Sampling frequency in Hz" << std::endl;
292  }
293 
294  void printConfig() {
295  std::cout << "* - Configuration -" << std::endl;
296  std::cout << "* Datafile: " << this->dataset.getFilename() << std::endl;
297  std::cout << "* Input columns: " << printVector(this->dataset.getInputColumns()) << std::endl;
298  std::cout << "* Output columns: " << printVector(this->dataset.getOutputColumns()) << std::endl;
299  }
300 
301 
302  virtual bool configure(ResourceFinder& opt) {
303  // read for the general specifiers:
304  Value* val;
305 
306  // check for help request
307  if(opt.check("help")) {
308  this->printOptions();
309  return false;
310  }
311 
312  // check for port specifier: portSuffix
313  if(opt.check("port", val)) {
314  this->portPrefix = val->asString().c_str();
315  }
316 
317  std::cout << "* Registering ports...";
318  this->registerAllPorts();
319  std::cout << "Done!" << std::endl;
320 
321  // check for train data port
322  if(opt.check("trainport", val)) {
323  Network::connect(this->train_out.where().getName().c_str(), val->asString().c_str());
324  } else {
325  // add message here if necessary
326  }
327 
328  // check for predict data port
329  if(opt.check("predictport", val)) {
330  Network::connect(this->predict_inout.where().getName().c_str(), val->asString().c_str());
331  } else {
332  // add message here if necessary
333  }
334 
335  // check for filename of the dataset
336  if(opt.check("datafile", val)) {
337  //this->dataset.setFilename(val->asString().c_str());
338  this->dataset.open(val->asString().c_str());
339  }
340 
341  // check for the columns of the dataset that should be used for inputs
342  if(opt.check("inputs", val)) {
343  // if it's a list,
344  if(val->isList()) {
345  Bottle* inputs = val->asList();
346  for(int i = 0; i < inputs->size(); i++) {
347  if(inputs->get(i).isInt32()) {
348  this->dataset.addInputColumn(inputs->get(i).asInt32());
349  }
350  }
351  } else if(val->isInt32()) {
352  this->dataset.addInputColumn(val->asInt32());
353  }
354  } else {
355  this->dataset.addInputColumn(1);
356  }
357 
358  // check for the columns of the dataset that should be used for outputs
359  if(opt.check("outputs", val)) {
360  // if it's a list,
361  if(val->isList()) {
362  Bottle* outputs = val->asList();
363  for(int i = 0; i < outputs->size(); i++) {
364  if(outputs->get(i).isInt32()) {
365  this->dataset.addOutputColumn(outputs->get(i).asInt32());
366  }
367  }
368  } else if(val->isInt32()) {
369  this->dataset.addOutputColumn(val->asInt32());
370  }
371  } else {
372  this->dataset.addOutputColumn(2);
373  }
374 
375  // check for port specifier: portSuffix
376  if(opt.check("frequency", val)) {
377  this->frequency = val->asInt32();
378  } else {
379  this->frequency = 0;
380  }
381 
382  this->printConfig();
383 
384  this->attachTerminal();
385 
386  return true;
387  }
388 
389  void sendTrainSample(Vector input, Vector output) {
390  PortablePair<Vector,Vector>& sample = this->train_out.prepare();
391  sample.head = input;
392  sample.body = output;
393  this->train_out.writeStrict();
394  }
395 
397  Prediction prediction;
398  this->predict_inout.write(input, prediction);
399  return prediction;
400  }
401 
402  bool updateModule() {
403  Time::delay(1.);
404  return true;
405  }
406 
407  bool respond(const Bottle& cmd, Bottle& reply) {
408  bool success = false;
409 
410  try {
411  switch(cmd.get(0).asVocab32()) {
412  case yarp::os::createVocab32('h','e','l','p'): // print help information
413  success = true;
414  reply.add(Value::makeVocab32("help"));
415 
416  reply.addString("Testing module configuration options");
417  reply.addString(" help Displays this message");
418  reply.addString(" conf Print configuration");
419  reply.addString(" train [n] Send training samples");
420  reply.addString(" predict [n] Send testing samples");
421  reply.addString(" skip [n] Skip samples");
422  reply.addString(" reset Reset dataset");
423  reply.addString(" shoot v1 ... vn Shoot a single prediction sample");
424  reply.addString(" open fname Opens a datafile");
425  reply.addString(" freq f Sampling frequency in Hertz (0 for disabled)");
426  break;
427  case yarp::os::createVocab32('c','o','n','f'): // print the configuration
428  {
429  success = true;
430  this->printConfig();
431  break;
432  }
433 
434  case yarp::os::createVocab32('s','k','i','p'): // skip some training sample(s)
435  {
436  success = true;
437  int noSamples = 1;
438  if(cmd.get(1).isInt32()) {
439  noSamples = cmd.get(1).asInt32();
440  }
441 
442  for(int i = 0; i < noSamples; i++) {
443  std::pair<Vector,Vector> sample = this->dataset.getNextSample();
444  // now do nothing with it... poor sample!
445  }
446  reply.addString("Done!");
447 
448  break;
449  }
450 
451  case yarp::os::createVocab32('t','r','a','i'): // send training sample(s)
452  {
453  reply.add(Value::makeVocab32("help"));
454  success = true;
455  int noSamples = 1;
456  if(cmd.get(1).isInt32()) {
457  noSamples = cmd.get(1).asInt32();
458  }
459 
460  //double start = yarp::os::Time::now();
461  for(int i = 0; i < noSamples; i++) {
462  std::pair<Vector,Vector> sample = this->dataset.getNextSample();
463  this->sendTrainSample(sample.first, sample.second);
464 
465  // sleep
466  if(this->frequency > 0)
467  yarp::os::Time::delay(1. / this->frequency);
468  }
469  // this timing only measures how fast the module can dump the samples on the network
470  //double end = yarp::os::Time::now();
471  //std::string reply_str = "Timing: " + doubletostring(noSamples / (end - start)) + " samples per second";
472  //reply.addString(reply_str.c_str());
473  reply.addString("Done!");
474 
475  break;
476  }
477 
478  case yarp::os::createVocab32('p','r','e','d'): // send prediction sample(s)
479  {
480  reply.add(Value::makeVocab32("help"));
481  success = true;
482  int noSamples = 1;
483  if(cmd.get(1).isInt32()) {
484  noSamples = cmd.get(1).asInt32();
485  }
486 
487  // initiate error vector (not completely failsafe!)
488  Vector error(this->dataset.getOutputColumns().size());
489  Vector nmlp(this->dataset.getOutputColumns().size());
490  error = 0.0;
491  nmlp = 0.0;
492 
493  // make predictions and keep track of errors (MSE)
494  double start = yarp::os::Time::now();
495  for(int i = 0; i < noSamples; i++) {
496  std::pair<Vector,Vector> sample = this->dataset.getNextSample();
497  Prediction prediction = this->sendPredictSample(sample.first);
498 
499  //std::cout << "head[" << prediction.head.size() << "]: " << prediction.head.toString() << std::endl;
500  //std::cout << "variance[" << prediction.body.size() << "]: " << prediction.body.toString() << std::endl;
501 
502  Vector expected = prediction.head;
503  if(expected.size() != sample.second.size()) {
504  std::string msg("incoming prediction has incorrect dimension");
505  throw std::runtime_error(msg);
506  }
507  for(size_t j = 0; j < error.size(); j++) {
508  double dist = sample.second[j] - expected[j];
509  error[j] += (dist * dist);
510  }
511 
512  Vector variance = prediction.body;
513  if(variance.size() > 0 && variance.size() != sample.second.size()) {
514  std::string msg("incoming predictive variance has invalid dimension");
515  throw std::runtime_error(msg);
516  }
517  for(size_t j = 0; j < nmlp.size(); j++) {
518  double normdist = (sample.second[j] - expected[j]) / variance[j];
519  nmlp[j] += 0.5 * ((normdist * normdist) + log(TWOPI) + 2 * log(variance[j]));
520  }
521 
522  // sleep
523  if(this->frequency > 0)
524  yarp::os::Time::delay(1. / this->frequency);
525  }
526  double end = yarp::os::Time::now();
527  std::string reply_str = "Timing: " + doubletostring(noSamples / (end - start)) + " samples per second";
528  reply.addString(reply_str.c_str());
529 
530  // take mean of cumulated errors
531  for(size_t i = 0; i < error.size(); i++) {
532  error[i] = error[i] / double(noSamples);
533  }
534  for(size_t i = 0; i < error.size(); i++) {
535  nmlp[i] = nmlp[i] / double(noSamples);
536  }
537 
538  reply_str = "MSE: " + printVector(error);
539  reply.addString(reply_str.c_str());
540 
541  reply_str = "NMLP: " + printVector(nmlp);
542  reply.addString(reply_str.c_str());
543 
544  break;
545  }
546 
547  case yarp::os::createVocab32('s','h','o','o'):
548  {
549  success = true;
550  Vector input;
551  for(int i = 1; i <= cmd.size(); i++) {
552  if(cmd.get(i).isFloat64() || cmd.get(i).isInt32()) {
553  input.push_back(cmd.get(i).asFloat64());
554  }
555  }
556  Prediction prediction = this->sendPredictSample(input);
557  std::string reply_str = printVector(input) + " -> " + printPrediction(prediction);
558  reply.addString(reply_str.c_str());
559 
560  break;
561  }
562 
563  case yarp::os::createVocab32('o','p','e','n'):
564  success = true;
565  if(cmd.get(1).isString()) {
566  std::cout << "Open..." << std::endl;
567  this->dataset.open(cmd.get(1).asString().c_str());
568  }
569  reply.addString((std::string("Opened dataset: ") + cmd.get(1).asString().c_str()).c_str());
570  break;
571 
572  case yarp::os::createVocab32('r','e','s','e'):
573  case yarp::os::createVocab32('r','s','t'):
574  success = true;
575  this->dataset.reset();
576  reply.addString("Dataset reset to beginning");
577  break;
578 
579  case yarp::os::createVocab32('f','r','e','q'): // set sampling frequency
580  {
581  if(cmd.size() > 1 && cmd.get(1).isInt32()) {
582  success = true;
583  this->frequency = cmd.get(1).asInt32();
584  reply.addString((std::string("Current frequency: ") + inttostring(this->frequency)).c_str());
585  }
586  break;
587  }
588 
589  case yarp::os::createVocab32('s','e','t'): // set some configuration options
590  // implement some options
591  break;
592 
593  default:
594  break;
595  }
596  } catch(const std::exception& e) {
597  success = true; // to make sure YARP prints the error message
598  std::string msg = std::string("Error: ") + e.what();
599  reply.addString(msg.c_str());
600  } catch(...) {
601  success = true; // to make sure YARP prints the error message
602  std::string msg = std::string("Error. (something bad happened, but I wouldn't know what!)");
603  reply.addString(msg.c_str());
604  }
605 
606  return success;
607  }
608 
609  bool close() {
610  this->unregisterAllPorts();
611  return true;
612  }
613 
614 };
615 
616 } // test
617 } // learningmachine
618 } // iCub
619 
620 using namespace iCub::learningmachine::test;
621 
622 int main(int argc, char *argv[]) {
623  Network yarp;
624  int ret;
625 
626  ResourceFinder rf;
627  rf.setDefaultContext("learningMachine");
628  rf.configure(argc, argv);
629 
631  try {
632  ret = module.runModule(rf);
633  //module.attachTerminal();
634  } catch(const std::exception& e) {
635  std::cerr << "Error: " << e.what() << std::endl;
636  module.close();
637  return 1;
638  } catch(char* msg) {
639  std::cerr << "Error: " << msg << std::endl;
640  module.close();
641  return 1;
642  }
643  return ret;
644 }
645 
void open(std::string filename)
Definition: test.cpp:161
std::vector< int > getInputColumns()
Definition: test.cpp:137
std::pair< Vector, Vector > getNextSample()
Definition: test.cpp:189
std::vector< int > getOutputColumns()
Definition: test.cpp:141
void setFilename(std::string filename)
Definition: test.cpp:151
The MachineLearnerTestModule is used to feed datasets loaded from a file to the learning machines and...
Definition: test.cpp:244
virtual bool configure(ResourceFinder &opt)
Definition: test.cpp:302
void sendTrainSample(Vector input, Vector output)
Definition: test.cpp:389
bool respond(const Bottle &cmd, Bottle &reply)
Definition: test.cpp:407
MachineLearnerTestModule(std::string pp="/lm/test")
Definition: test.cpp:275
cmd
Definition: dataTypes.h:30
static float test[3]
Definition: iCub_Sim.cpp:77
static int v
Definition: iCub_Sim.cpp:42
FILE * file
Definition: main.cpp:81
int main(int argc, char *argv[])
Definition: test.cpp:622
#define TWOPI
Definition: test.cpp:39
std::string printPrediction(Prediction &p)
Definition: test.cpp:89
PortablePair< Vector, Vector > Prediction
Definition: test.cpp:52
std::string doubletostring(double d)
Definition: test.cpp:55
std::string printVector(const Vector &v)
Definition: test.cpp:78
std::string inttostring(int i)
Definition: test.cpp:61
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.
Copyright (C) 2008 RobotCub Consortium.
ms frequency
Definition: sine.m:2