00001 #include "WallTestBehavior.h"
00002 #include "Shared/newmat/newmat.h"
00003 #include "Shared/newmat/newmatap.h"
00004 #include "Events/EventRouter.h"
00005 #include "Motion/MotionManager.h"
00006 #include "Motion/MotionSequenceMC.h"
00007 #include "Shared/Config.h"
00008 #include "Shared/WorldState.h"
00009 #include "Shared/newmat/newmatio.h"
00010 #include <fstream>
00011
00012 void
00013 WallTestBehavior::DoStart() {
00014 BehaviorBase::DoStart();
00015
00016
00017
00018 int startrec,stoprec;
00019
00020
00021 SharedObject<SmallMotionSequenceMC> pan;
00022
00023 pan->setTime(startrec=reposTime);
00024 pan->setPose(PostureEngine(config->motion.makePath("stand.pos").c_str()));
00025 pan->setOutputCmd(HeadOffset+TiltOffset,0);
00026 pan->setOutputCmd(HeadOffset+PanOffset,outputRanges[HeadOffset+PanOffset][MaxRange]);
00027 pan->setOutputCmd(HeadOffset+RollOffset,0);
00028
00029 stoprec=pan->advanceTime(panTime);
00030 pan->setOutputCmd(HeadOffset+TiltOffset,0);
00031 pan->setOutputCmd(HeadOffset+PanOffset,outputRanges[HeadOffset+PanOffset][MinRange]);
00032 pan->setOutputCmd(HeadOffset+RollOffset,0);
00033
00034 pan->advanceTime(reposTime);
00035 pan->setOutputCmd(HeadOffset+TiltOffset,0);
00036 pan->setOutputCmd(HeadOffset+PanOffset,0);
00037 pan->setOutputCmd(HeadOffset+RollOffset,0);
00038
00039 pan->advanceTime(reposTime);
00040 pan->setOutputCmd(HeadOffset+TiltOffset,0);
00041 pan->setOutputCmd(HeadOffset+PanOffset,0);
00042 pan->setOutputCmd(HeadOffset+RollOffset,0);
00043
00044
00045 motman->addPrunableMotion(pan);
00046
00047
00048
00049 erouter->addTimer(this,0,startrec+lagTime,false);
00050 erouter->addTimer(this,1,stoprec+lagTime,false);
00051 }
00052
00053 void
00054 WallTestBehavior::DoStop() {
00055 erouter->removeListener(this);
00056 BehaviorBase::DoStop();
00057 }
00058
00059 void
00060 WallTestBehavior::processEvent(const EventBase& e) {
00061 if(e.getGeneratorID()==EventBase::sensorEGID) {
00062
00063
00064 #ifdef TGT_ERS7
00065 float nd = state->sensors[NearIRDistOffset];
00066
00067
00068
00069
00070
00071
00072
00073 if(false && nd<350)
00074 usedNear.push_back(true);
00075 else {
00076 nd=state->sensors[FarIRDistOffset];
00077 usedNear.push_back(false);
00078 }
00079 #else //not TGT_ERS7
00080 float nd = state->sensors[IRDistOffset];
00081 #endif //not TGT_ERS7
00082 float na = state->outputs[HeadOffset+PanOffset];
00083
00084
00085 d.push_back(nd);
00086 a.push_back(na);
00087
00088 } else if(e.getSourceID()==0) {
00089
00090
00091 erouter->addListener(this,EventBase::sensorEGID,SensorSourceID::UpdatedSID);
00092
00093 } else if(e.getSourceID()==1) {
00094
00095
00096 erouter->removeListener(this,EventBase::sensorEGID);
00097
00098 PostureEngine pose;
00099 pose.clear();
00100
00101
00102
00103
00104 cout << "Logging Non-Kinematic calculations to /data/raw_xy.txt" << endl;
00105 {
00106 ofstream rawxy(config->portPath("data/raw_xy.txt").c_str());
00107 if(!rawxy) {
00108 cout << "Could not open file" << endl;
00109 } else {
00110 cout << "Columns are:\tx\ty" << endl;
00111 for(unsigned int i=0; i<d.size(); i++)
00112 rawxy << d[i]*cos(a[i]) << '\t' << d[i]*sin(a[i]) << '\n';
00113 }
00114 }
00115
00116
00117
00118
00119
00120 cout << "Logging uncalibrated kinematic calculations to /data/k_xyz.txt" << endl;
00121 {
00122 ofstream kxys(config->portPath("data/k_xyz.txt").c_str());
00123 if(!kxys) {
00124 cout << "Could not open file" << endl;
00125 } else {
00126
00127 #ifdef TGT_ERS7
00128 cout << "Columns are:\tx\ty\tz\tis_using_near" << endl;
00129 float off=NEWMAT::ColumnVector(pose.getJointInterestPoint(CameraFrameOffset,"NearIR"))(3);
00130 #else //not TGT_ERS7
00131 cout << "Columns are:\tx\ty\tz" << endl;
00132 float off=NEWMAT::ColumnVector(pose.getJointInterestPoint(CameraFrameOffset,"IR"))(3);
00133 #endif //TGT_ERS7 check
00134 for(unsigned int i=0; i<d.size(); i++) {
00135 pose(HeadOffset+PanOffset)=a[i];
00136 NEWMAT::ColumnVector hit=pose.jointToBase(CameraFrameOffset)*Kinematics::pack(0,0,d[i]+off);
00137 #ifdef TGT_ERS7
00138 kxys << hit(1) << '\t' << hit(2) << '\t' << hit(3) << '\t' << usedNear[i] << '\n';
00139 #else //not TGT_ERS7
00140 kxys << hit(1) << '\t' << hit(2) << '\t' << hit(3) << '\n';
00141 #endif //TGT_ERS7 check
00142 }
00143 }
00144 }
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155 cout << "Logging calibrated kinematic calculations to /data/ck_xyz.txt" << endl;
00156 {
00157 ofstream ckxys(config->portPath("data/ck_xyz.txt").c_str());
00158 if(!ckxys) {
00159 cout << "Could not open file" << endl;
00160 } else {
00161 #ifdef TGT_ERS7
00162 cout << "Columns are:\tx\ty\tz\tis_using_near" << endl;
00163 #else //not TGT_ERS7
00164 cout << "Columns are:\tx\ty\tz" << endl;
00165 #endif //not TGT_ERS7
00166 for(unsigned int i=0; i<d.size(); i++) {
00167 pose(HeadOffset+PanOffset)=a[i];
00168 #ifdef TGT_ERS7
00169 unsigned int frame=usedNear[i]?NearIRFrameOffset:FarIRFrameOffset;
00170 NEWMAT::ColumnVector hit=pose.jointToBase(frame)*Kinematics::pack(0,0,d[i]);
00171 ckxys << hit(1) << '\t' << hit(2) << '\t' << hit(3) << '\t' << usedNear[i] << '\n';
00172 #else //not TGT_ERS7
00173 NEWMAT::ColumnVector hit=pose.jointToBase(IRFrameOffset)*Kinematics::pack(0,0,d[i]);
00174 ckxys << hit(1) << '\t' << hit(2) << '\t' << hit(3) << '\n';
00175 #endif //not TGT_ERS7
00176 }
00177 }
00178 }
00179
00180
00181
00182
00183
00184 unsigned int start[3];
00185 unsigned int stop[3];
00186 start[0]=stop[0]=0;
00187 while(a[stop[0]++]>60*M_PI/180 && stop[0]<a.size()) ;
00188 start[1]=stop[0];
00189 while(a[start[1]++]>30*M_PI/180 && start[1]<a.size()) ;
00190 stop[1]=start[1];
00191 while(a[stop[1]++]>-30*M_PI/180 && stop[1]<a.size()) ;
00192 start[2]=stop[1];
00193 while(a[start[2]++]>-60*M_PI/180 && start[2]<a.size()) ;
00194 stop[2]=a.size();
00195
00196 cout << "Regions are: ";
00197 for(int i=0; i<3; i++)
00198 cout << start[i] << "-" << stop[i] << (i==2?"":", ");
00199 cout << endl;
00200
00201
00202 for(int w=0; w<3; w++) {
00203 cout << "Wall "<<w<<": ";
00204
00205 int n=stop[w]-start[w];
00206 if(n<3) {
00207 cout << "not enough data - did you forget to turn off estop?" << endl;
00208 continue;
00209 }
00210 vector<float> x,y;
00211 for(unsigned int i=start[w]; i<stop[w]; i++) {
00212 #ifdef TGT_ERS7
00213 if(d[i]==200 || d[i]>1500)
00214 continue;
00215 #else //not ers-7
00216 if(d[i]==100 || d[i]>700)
00217 continue;
00218 #endif //not ers-7
00219 x.push_back(d[i]*cos(a[i]));
00220 y.push_back(d[i]*sin(a[i]));
00221 }
00222 cout << "(" << x.size() << " valid samples)" << endl;
00223 if(x.size()<3) {
00224 cout << "No wall" << endl;
00225 continue;
00226 }
00227
00228
00229
00230
00231
00232
00233 float x0=0,x1=0;
00234 TimeET t;
00235
00236 t.Set();
00237 solve1(x,y,x0,x1);
00238 cout << " QR: 'y = "<<x0<<"x + "<<x1<<"'\tTime:"<<t.Age()<<endl;
00239
00240 t.Set();
00241 solve2(x,y,x0,x1);
00242 cout << " SVD: '"<<x0<<"x + "<<x1<<"y = 1'\tTime:"<<t.Age()<<endl;
00243 }
00244 }
00245 }
00246
00247 void
00248 WallTestBehavior::solve1(const std::vector<float>& x, const std::vector<float>& y, float& x0, float& x1) {
00249 NEWMAT::Matrix A(x.size(),2);
00250 NEWMAT::ColumnVector b(x.size());
00251 for(unsigned int i=0; i<x.size(); i++) {
00252 A(i+1,1)=x[i];
00253 A(i+1,2)=1;
00254 b(i+1)=y[i];
00255 }
00256
00257 NEWMAT::UpperTriangularMatrix U;
00258 NEWMAT::Matrix M;
00259
00260 NEWMAT::QRZ(A,U);
00261 NEWMAT::QRZ(A,b,M);
00262 NEWMAT::ColumnVector dq = U.i()*M;
00263
00264 x0=dq(1);
00265 x1=dq(2);
00266 }
00267
00268 void
00269 WallTestBehavior::solve2(const std::vector<float>& x, const std::vector<float>& y, float& x0, float& x1) {
00270 NEWMAT::Matrix A(x.size(),3);
00271 for(unsigned int i=0; i<x.size(); i++) {
00272 A(i+1,1)=x[i];
00273 A(i+1,2)=y[i];
00274 A(i+1,3)=-1;
00275 }
00276
00277 NEWMAT::DiagonalMatrix Q;
00278 NEWMAT::Matrix U,V;
00279
00280 NEWMAT::SVD(A,Q,U,V,false,true);
00281
00282 x0=V(1,V.ncols())/V(3,V.ncols());
00283 x1=V(2,V.ncols())/V(3,V.ncols());
00284 }