00001 00018 #include <cstdio> 00019 #include <iostream> 00020 #include <cstring> 00021 #include <fstream> 00022 #include <optimizer_gnuplot.h> 00023 00024 using namespace robotLibPbD; 00025 00026 00027 bool COptimizerGnuplot::generateGnuPlotCoords(double *st, std::string filenameScript, std::string filenameData) 00028 { 00029 setData(examples[0]); 00030 setValue(st); 00031 std::string text; 00032 00033 CMatrix pose; 00034 std::vector<CVec> points; 00035 const double len = 20.0; 00036 points.push_back(CVec(0.0, 0.0, 0.0)); 00037 points.push_back(CVec(len, 0.0, 0.0)); 00038 points.push_back(CVec(0.0, 0.0, 0.0)); 00039 points.push_back(CVec(0.0, len, 0.0)); 00040 points.push_back(CVec(0.0, 0.0, 0.0)); 00041 points.push_back(CVec(0.0, 0.0, len)); 00042 00043 std::vector<int> linestyle; 00044 linestyle.push_back(1); 00045 linestyle.push_back(1); 00046 linestyle.push_back(2); 00047 linestyle.push_back(2); 00048 linestyle.push_back(3); 00049 linestyle.push_back(3); 00050 00051 std::string plotdata; 00052 for (unsigned int i=0; i<frames.getFrames().size(); i++) 00053 { 00054 pose = frames.getFrame(i)->getRelativeToBase(); 00055 for (unsigned int j=0; j<points.size(); j++) 00056 plotdata += (pose * points[j]).toString() + "\n"; 00057 } 00058 00059 std::ofstream textstream2(filenameData.c_str()); 00060 if (textstream2.fail()) 00061 return false; 00062 00063 textstream2 << plotdata; 00064 textstream2.close(); 00065 00066 std::vector<double> mins(3, 1.0e30); 00067 std::vector<double> maxs(3, -1.0e30); 00068 for (unsigned int i=0; i<frames.getFrames().size(); i++) 00069 { 00070 pose = frames.getFrame(i)->getRelativeToBase(); 00071 for (unsigned int k=0; k<3; k++) 00072 if (pose.a[12 + k] < mins[k]) 00073 mins[k] = pose.a[12 + k]; 00074 else if (pose.a[12 + k] > maxs[k]) 00075 maxs[k] = pose.a[12 + k]; 00076 00077 for (unsigned int j=0; j<points.size(); j+=2) 00078 { 00079 CVec t1, t2; 00080 t1 = pose * points[j]; 00081 t2 = pose * points[j+1]; 00082 text += printToString("set arrow from %f,%f,%f to %f,%f,%f linestyle %d\n", 00083 t1.x, t1.y, t1.z, 00084 t2.x, t2.y, t2.z, 00085 linestyle[i]); 00086 00087 } 00088 } 00089 00090 text += "splot 'plot_data' with points ps 0\n"; 00091 text += "pause -1\n"; 00092 00093 00094 std::ofstream textstream(filenameScript.c_str()); 00095 if (textstream.fail()) 00096 return false; 00097 00098 textstream << text; 00099 textstream.close(); 00100 return true; 00101 } 00102 00103 bool COptimizerGnuplot::generateGnuPlot(double *st, std::string filenameScript, std::string filenameData) 00104 { 00105 std::string text; 00106 std::string plotdata; 00107 CVec t1, t2; 00108 std::vector<CVec> t1start(functions.size()); 00109 for (unsigned int i=0; i<examples.size(); i++) 00110 { 00111 setData(examples[i]); 00112 setValue(st); 00113 for (unsigned int j=0; j<functions.size(); j++) 00114 { 00115 functions[j]->getDistance(t1, t2); 00116 00117 if (i == 0) 00118 t1start[j] = t1; 00119 00120 text += printToString("set arrow from %f,%f,%f to %f,%f,%f linestyle %d\n", 00121 t1.x, t1.y, t1.z, 00122 t2.x, t2.y, t2.z, 00123 1); 00124 00125 plotdata += printToString("%f %f %f\n", 00126 t1.x, t1.y, t1.z); 00127 00128 functions[j]->getDistance(t2); 00129 text += printToString("set arrow from %f,%f,%f to %f,%f,%f linestyle %d\n", 00130 t1start[j].x, t1start[j].y, t1start[j].z, 00131 t1start[j].x + t2.x, t1start[j].y + t2.y, t1start[j].z + t2.z, 00132 3); 00133 } 00134 } 00135 00136 text += "splot 'plot_data' with points ps 0\n"; 00137 text += "pause -1\n"; 00138 00139 std::ofstream textstream(filenameScript.c_str()); 00140 if (textstream.fail()) 00141 return false; 00142 00143 textstream << text; 00144 textstream.close(); 00145 00146 std::ofstream textstream2(filenameData.c_str()); 00147 if (textstream2.fail()) 00148 return false; 00149 00150 textstream2 << plotdata; 00151 textstream2.close(); 00152 return true; 00153 } 00154