Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 #include "inverse_dynamics/utils.h"
00040
00041 namespace utils {
00042
00043 struct timeval starttic;
00044
00045 void tic() {
00046 gettimeofday(&starttic, NULL);
00047 }
00048
00049 void toc() {
00050 struct timeval end;
00051 long mtime, seconds, useconds;
00052 gettimeofday(&end, NULL);
00053 seconds = end.tv_sec - starttic.tv_sec;
00054 useconds = end.tv_usec - starttic.tv_usec;
00055 mtime = ((seconds) * 1000 + useconds/1000.0) + 0.5;
00056 printf("Elapsed time: %ld milliseconds\n", mtime);
00057 }
00058
00059 bool writeMatrix(std::string fname, Eigen::MatrixXd * matrix) {
00060 std::ofstream file;
00061 std::ostream * out;
00062 file.open(fname.c_str());
00063 if (!file) {
00064 fprintf(stderr, "Error: can't open file %s.\n", fname.c_str());
00065 return false;
00066 }
00067 out = &file;
00068 *out << *matrix << std::endl;
00069 file.close();
00070 return true;
00071 }
00072
00073 bool readMatrix(std::string fname, Eigen::MatrixXd * matrix) {
00074
00075 std::ifstream file;
00076 std::istream * in;
00077 file.open(fname.c_str());
00078 if (!file) {
00079 fprintf(stderr, "Error: can't open file %s.\n", fname.c_str());
00080 return false;
00081 }
00082 in = &file;
00083
00084 std::vector<double> data;
00085 std::vector<double>::iterator it;
00086 size_t cols(0);
00087 size_t rows(0);
00088 for (; *in; rows++) {
00089 std::vector<double> row;
00090
00091 std::string line;
00092 getline(*in, line);
00093 std::istringstream ls(line);
00094
00095 double value;
00096 while (ls >> value) {
00097 row.push_back(value);
00098 }
00099
00100 if (0 == row.size())
00101 break;
00102
00103
00104 if (!cols) {
00105 cols = row.size();
00106 } else if (cols != row.size()) {
00107 fprintf(stderr, "Error reading matrix from \"%s\", line: %zu. Expected %zu columns but got %zu.\n", fname.c_str(), rows+1, cols, row.size());
00108 return false;
00109 }
00110
00111
00112 for (it = row.begin(); it < row.end(); it++)
00113 data.push_back(*it);
00114 }
00115
00116 if (0 == rows)
00117 return false;
00118
00119
00120 matrix->resize(rows, cols);
00121 int i = 0;
00122 for (it = data.begin(); it < data.end(); it++) {
00123 (*matrix)(i / cols, i % cols) = *it;
00124 i++;
00125 }
00126 file.close();
00127 return true;
00128 }
00129
00130 }