void test_stateToHT() { const int MAXNS = NUMSTATE(WmrModel::MAXNF); //make WmrModel object WmrModel mdl; Real state[MAXNS]; //rocky(mdl,state,0); zoe(mdl,state,0); //get from WmrModel int nf = mdl.get_nf(); int ns = NUMSTATE(nf); //convert state to homogeneous transforms HomogeneousTransform HT_parent[WmrModel::MAXNF]; HomogeneousTransform HT_world[WmrModel::MAXNF]; stateToHT(mdl, state, HT_parent, HT_world); //DEBUGGING, PRINT std::cout << "state=\n"; printMatReal(ns,1,state,-1,-1); std::cout << std::endl; for (int i=0; i<nf; i++) { std::cout << "HT_parent[" << i << "]=\n"; printHT(HT_parent[i],-1,-1); std::cout << std::endl; } std::cout << std::endl; for (int i=0; i<nf; i++) { std::cout << "HT_world[" << i << "]=\n"; printHT(HT_world[i],-1,-1); std::cout << std::endl; } if (1) { //time it timeval t0, t1; int n= (int) 1e6; gettimeofday(&t0, NULL); for (int i=0; i<n; i++) { stateToHT(mdl, state, HT_parent, HT_world); } gettimeofday(&t1, NULL); std::cout << "stateToHT()\n"; std::cout << "iterations: " << (Real) n << std::endl; std::cout << "clock (sec): " << tosec(t1)-tosec(t0) << std::endl << std::endl; } }
void test_updateWheelContactGeom() { const Real rad = .325; WheelContactGeom contact; //terrain SurfaceVector surfs; //flat(surfs); //ramp(surfs); grid(surfs, ResourceDir() + std::string("gridsurfdata.txt") ); //wheel pose HomogeneousTransform HT_wheel_to_world; VecOrient orient = {0,0,0}; //assume WMRSIM_USE_QUATERNION 0 Vec3 pos = {2.5, -1.0, rad}; //adjust height of wheel Real zsurf; surfacesHeight(surfs,pos,zsurf); pos[2] += zsurf; poseToHT(orient,pos,HT_wheel_to_world); //assumes WMRSIM_USE_QUATERNION 0 updateWheelContactGeom(surfs, HT_wheel_to_world, rad, contact ); //print std::cout << "wheel pos = \n"; printVec3(pos,-1,-1); std::cout << "dz = " << contact.dz << std::endl; std::cout << "contact angle (deg)= " << RADTODEG(contact.angle) << std::endl; std::cout << "\nHT_contact_to_wheel=\n"; printHT(contact.HT_wheel,-1,-1); std::cout << "\nHT_contact_to_world=\n"; printHT(contact.HT_world,-1,-1); if (1) { //time it int nw = 4; timeval t0, t1; int n= (int) 1e5; gettimeofday(&t0, NULL); for (int i=0; i<n*nw; i++) { updateWheelContactGeom(surfs, HT_wheel_to_world, rad, contact ); } gettimeofday(&t1, NULL); std::cout << "num wheels: " << nw << std::endl; std::cout << "iterations: " << (Real) n << std::endl; std::cout << "clock (sec): " << tosec(t1)-tosec(t0) << std::endl; } }
int main(int argc,char* argv[]) { Hash_item* hashtable=populateHashtable(argv[1]); FILE* fp=fopen(argv[2],"w"); printHT(*hashtable,T_SIZE,fp); //printf("%d\n",findEntry(*hashtable,T_SIZE,4654,"LQWWRLLH")); return 0; }
void test_transform() { VecEuler euler; Vec3 translation; HomogeneousTransform HT,HT2; setEuler(DEGTORAD(10),DEGTORAD(20),DEGTORAD(30),euler); setVec3(1,2,3,translation); poseToHT(euler,translation,HT); std::cout << "HT=\n"; printHT(HT,-1,-1); //test rotation matrix Mat3 Rx,Ry,Rz,Rot,Tmp; Rotx(euler[0],Rx); Roty(euler[1],Ry); Rotz(euler[2],Rz); multMatMat3(Ry,Rx,Tmp); multMatMat3(Rz,Tmp,Rot); std::cout << "Rotz(yaw)*Roty(pit)*Rotx(rol)=\n"; printMat3(Rot,-1,-1); //test invert transform invertHT(HT,HT2); std::cout << "inv(HT)=\n"; printHT(HT2,-1,-1); //test compose HT if (1) { //time it int n = (int) 1e8; timeval t0, t1; //compose Homogeneous Transforms gettimeofday(&t0, NULL); for (int i=0; i<n; i++) { composeHT(HT,HT,HT2); } gettimeofday(&t1, NULL); std::cout << "HT*HT=\n"; printHT(HT2,-1,-1); std::cout << "iterations: " << (Real) n << std::endl; std::cout << "clock (sec): " << tosec(t1)-tosec(t0) << std::endl; } else { composeHT(HT,HT,HT2); std::cout << "HT*HT=\n"; printHT(HT2,-1,-1); } composeInvHT(HT,HT,HT2); std::cout << "inv(HT)*HT=\n"; printHT(HT2,-1,-1); //test transform point Vec3 p,q; setVec3(2,3,4,p); std::cout << "p=\n"; printVec3(p,-1,-1); applyHT(HT,p,q); std::cout << "q=HT*p=\n"; printVec3(q,-1,-1); applyInvHT(HT,p,q); std::cout << "q=inv(HT)*p=\n"; printVec3(q,-1,-1); //test converting between angular velocity and Euler rates Vec3 vel={1,2,3}; VecEuler eulerrate; MatEuler T; std::cout << "angular velocity=\n"; printVec3(vel,-1,-1); velToEulerrate(euler,vel,eulerrate,T); std::cout << "eulerrate=\n"; printEuler(eulerrate,-1,-1); std::cout << "T_vel_to_eulerrate=\n"; printMatReal(3,3,T,-1,-1); std::cout << "convert back:\n"; eulerrateToVel(euler,eulerrate,vel,0); std::cout << "angular velocity=\n"; printVec3(vel,-1,-1); std::cout << std::endl; //test at singularity setEuler(0,M_PI/2,0,euler); setVec3(0,0,1,vel); velToEulerrate(euler,vel,eulerrate,T); std::cout << "test at singularity:\n"; std::cout << "euler=\n"; printEuler(euler,-1,-1); std::cout << "angular velocity=\n"; printVec3(vel,-1,-1); std::cout << "eulerrate=\n"; printEuler(eulerrate,-1,-1); }
void test_spatial() { //for timing int n; timeval t0, t1; //construct Homogeneous Transform VecEuler euler; Vec3 translation; HomogeneousTransform HT; setEuler(DEGTORAD(10),DEGTORAD(20),DEGTORAD(30),euler); setVec3(1,2,3,translation); poseToHT(euler,translation,HT); std::cout << "HT=\n"; printHT(HT,-1,-1); //convert to Plucker transform Mat6b P; HTToPlucker(HT,P); std::cout << "P=\n"; printMat6b(P,-1,-1); //inverse Plucker transform Mat6b P2; if (1) { //time it n= (int) 1e8; gettimeofday(&t0, NULL); for (int i=0; i<n; i++) { invHTToPlucker(HT,P2); } gettimeofday(&t1, NULL); std::cout << "inv(P)=\n"; printMat6b(P2,-1,-1); std::cout << "iterations: " << (Real) n << std::endl; std::cout << "clock (sec): " << tosec(t1)-tosec(t0) << std::endl; std::cout << std::endl; } else { invHTToPlucker(HT,P2); std::cout << "inv(P)=\n"; printMat6b(P2,-1,-1); } Vec6b m,f,v,v2; setVec6b(1,2,3,4,5,6,m); setVec6b(2,3,4,5,6,7,f); std::cout << "m=\n"; printVec6b(m,-1,-1); std::cout << "f=\n"; printVec6b(f,-1,-1); //cross products crossVec6bMotion(m,f,v); std::cout << "crossMotion(m)*f=\n"; printVec6b(v,-1,-1); crossVec6bForce(m,f,v); std::cout << "crossForce(m)*f=\n"; printVec6b(v,-1,-1); //multiply vector by Plucker transform std::cout << "using spatial.h\n"; if (1) { //time it n= (int) 1e8; gettimeofday(&t0, NULL); for (int i=0; i<n; i++) { multPluckerVec6b(P,m,v); multPluckerTVec6b(P,m,v2); } gettimeofday(&t1, NULL); std::cout << "P*m=\n"; printVec6b(v,-1,-1); std::cout << "P'*m=\n"; printVec6b(v2,-1,-1); std::cout << "iterations: " << (Real) n << std::endl; std::cout << "clock (sec): " << tosec(t1)-tosec(t0) << std::endl; std::cout << std::endl; } else { multPluckerVec6b(P,m,v); std::cout << "P*m=\n"; printVec6b(v,-1,-1); multPluckerTVec6b(P,m,v2); std::cout << "P'*m=\n"; printVec6b(v2,-1,-1); } if (1) { //using Eigen Eigen::Matrix<Real,6,6> P_; Eigen::Matrix<Real,6,1> m_, v_, v2_; //copy to Eigen matrices copyMat6bToArray(P,P_.data()); copyVec6bToArray(m,m_.data()); gettimeofday(&t0, NULL); for (int i=0; i<n; i++) { v_ = P_*m_; v2_ = P_.transpose()*m_; } gettimeofday(&t1, NULL); std::cout << "using Eigen\n"; std::cout << "P*m=\n" << v_ << std::endl; std::cout << "P'*m=\n" << v2_ << std::endl; std::cout << "iterations: " << (Real) n << std::endl; std::cout << "clock (sec): " << tosec(t1)-tosec(t0) << std::endl; } //compute P'*I*P //copy tricycle body frame Real mass = 10; Real L = 1; Real W = .8; Real H = .25; Vec3 cm; Mat3 I; setVec3(L/2,0,H/2,cm); inertiaBox(mass,L,W,H,I); //using spatial.h Mat6b Is; //spatial inertia toSpatialInertia(mass,cm,I,Is); std::cout << "Is=\n"; printMat6b(Is,-1,-1); Mat6b Is2; std::cout << "using spatial.h\n"; if (1) { //time it n = (int) 1e7; gettimeofday(&t0, NULL); for (int i=0; i<n; i++) { //multPluckerTMat6bPlucker(P,Is,Is2); //deprecated multPluckerTInertiaPlucker(P,Is,Is2); } gettimeofday(&t1, NULL); std::cout << "P'*Is*P=\n"; printMat6b(Is2,-1,-1); std::cout << "iterations: " << (Real) n << std::endl; std::cout << "clock (sec): " << tosec(t1)-tosec(t0) << std::endl; std::cout << std::endl; } else { //multPluckerTMat6bPlucker(P,Is,Is2); multPluckerTInertiaPlucker(P,Is,Is2); std::cout << "P'*Is*P=\n"; printMat6b(Is2,-1,-1); } if (1) { //using Eigen Eigen::Matrix<Real,6,6> P_, Is_, Is2_; //copy to Eigen matrices copyMat6bToArray(P,P_.data()); copyMat6bToArray(Is,Is_.data()); gettimeofday(&t0, NULL); for (int i=0; i<n; i++) { Is2_ = P_.transpose()*Is_*P_; } gettimeofday(&t1, NULL); std::cout << "using Eigen\n"; std::cout << "P'*Is*P=\n" << Is2_ << std::endl; std::cout << "iterations: " << (Real) n << std::endl; std::cout << "clock (sec): " << tosec(t1)-tosec(t0) << std::endl; } }
int main(int argc, char* argv[]) { hash_item* ht = populateHashtable(argv[1]); printHT(&ht, 10); return 0; }