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