PointCoordinateMatrices ConverterPlaneFromTo3d::projectTo3d(Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> image) { PointCoordinateMatrices reshapedCoordinates; //std::cout << "image: " << std::endl << image << std::endl << std::endl; //As defined for the example data sets to get the distance in meters Z = depth / 5000 TODO change for kinect data (maybe is the same...) Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> z = image.array() / 1;// 5000; Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> x = z.cwiseProduct(xAdjustment_); Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> y = z.cwiseProduct(yAdjustment_); //std::cout << "x: " << std::endl << x << std::endl << std::endl; //std::cout << "y: " << std::endl << y << std::endl << std::endl; //std::cout << "z: " << std::endl << z << std::endl << std::endl; reshapedCoordinates.x = Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(x.data(), 1, image.size()); reshapedCoordinates.y = Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(y.data(), 1, image.size()); reshapedCoordinates.z = Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(z.data(), 1, image.size()); return reshapedCoordinates; }
template<int N> void calculateGaussPdf(Eigen::Matrix<double,Eigen::Dynamic,N> X, Eigen::Matrix<double,1,N> mu, Eigen::Matrix<double,N,N> C, double* result){ Eigen::Matrix<double,N,N> L = C.llt().matrixL().transpose(); // cholesky decomposition Eigen::Matrix<double,N,N> Linv = L.inverse(); double det = L.diagonal().prod(); //determinant of L is equal to square rooot of determinant of C double lognormconst = -log(2 * M_PI)*X.cols()/2 - log(fabs(det)); Eigen::Matrix<double,1,N> x = mu; Eigen::Matrix<double,1,N> tmp = x; for (int i=0; i<X.rows(); i++){ x.noalias() = X.row(i) - mu; tmp.noalias() = x*Linv; double exponent = -0.5 * (tmp.cwiseProduct(tmp)).sum(); result[i] = lognormconst+exponent; } /* Eigen::Matrix<double,Eigen::Dynamic,N> X0 = (X.rowwise() - mu)*Linv; Eigen::Map<Eigen::Matrix<double,Eigen::Dynamic,1> > resultMap(result, X.rows()); resultMap = (X0.rowwise().squaredNorm()).array() * (-0.5) + lognormconst; */ fmath::expd_v(result, X.rows()); }
int main() { //******************************************************** // Initialization //******************************************************** double M=0.6; double Ix=8*1e-3; // moment of inertia in X- Y- and Z-axis double Iy=7.5*1e-3; double Iz=13.5*1e-3; double d=0.2; // displacement of rotors double kt=0.6; double km=0.15; char file[]="/media/fantaosha/Documents/JHU/Summer 2015/quad_rotor_traj/traj_full_12s_1_small.mat"; mxArray *mxR; mxArray *mxw; mxArray *mxxq; mxArray *mxvq; mxArray *mxU; mxR=matGetVariable(matOpen(file,"r"),"state_R"); mxw=matGetVariable(matOpen(file,"r"),"state_w"); mxxq=matGetVariable(matOpen(file,"r"),"state_xq"); mxvq=matGetVariable(matOpen(file,"r"),"state_vq"); #ifdef ROT_INPUT mxU=matGetVariable(matOpen(file,"r"),"U"); #else mxU=matGetVariable(matOpen(file,"r"),"F"); #endif size_t N=mxGetN(mxU); size_t sN=5; void *pR=mxGetPr(mxR); void *pw=mxGetPr(mxw); void *pxq=mxGetPr(mxxq); void *pvq=mxGetPr(mxvq); void *pU=mxGetPr(mxU); std::list<typename quadrotor::State> list_par1; std::list<typename quadrotor::State> list_par2; for(int i=0;i<N;i+=sN) { Eigen::Matrix<double,3,3> R; Eigen::Matrix<double,3,1> w,xq,vq; memcpy(R.data(),pR,sizeof(double)*R.rows()*R.cols()); memcpy(w.data(),pw,sizeof(double)*w.rows()*w.cols()); memcpy(xq.data(),pxq,sizeof(double)*xq.rows()*xq.cols()); memcpy(vq.data(),pvq,sizeof(double)*vq.rows()*vq.cols()); list_par1.emplace_back(R,xq,w,R.transpose()*vq); if((i+1)%2) list_par2.emplace_back(R,xq,w,R.transpose()*vq); pR+=sizeof(double)*R.rows()*R.cols()*sN; pw+=sizeof(double)*w.rows()*w.cols()*sN; pxq+=sizeof(double)*xq.rows()*xq.cols()*sN; pvq+=sizeof(double)*vq.rows()*vq.cols()*sN; } sN=10; std::list<typename quadrotor::U> list_u1; std::list<typename quadrotor::U> list_u2; #ifdef ROT_INPUT for(int i=0;i<N;i+=sN) { Eigen::Matrix<double,4,1> u; memcpy(u.data(),pU,sizeof(double)*u.rows()*u.cols()); list_u1.push_back(u.cwiseProduct(u)); list_u2.push_back(u); pU+=sizeof(double)*u.rows()*u.cols()*sN; } #else for(int i=0;i<N;i+=sN) { Eigen::Matrix<double,4,1> u; memcpy(u.data(),pU,sizeof(double)*u.rows()*u.cols()); list_u.push_back(u); pU+=sizeof(double)*u.rows()*u.cols()*sN; } #endif mxDestroyArray(mxR); mxDestroyArray(mxw); mxDestroyArray(mxxq); mxDestroyArray(mxvq); mxDestroyArray(mxU); std::srand((unsigned int) time(0)); // initialize SAC Mat12 PM=2500*Mat12::Identity(); PM.block(0,0,3,3)*=10; PM.block(3,3,3,3)*=40; PM.block(6,6,6,6)=PM.block(0,0,6,6)/8; Mat12 NM=PM/20; quadrotor::Cost cost=std::bind(quad_cost::cost_exp, std::placeholders::_1, std::placeholders::_2, NM); quadrotor::DCost dcost=std::bind(quad_cost::dcost_exp, std::placeholders::_1, std::placeholders::_2, NM); quadrotor::Cost costN=std::bind(quad_cost::cost_exp, std::placeholders::_1, std::placeholders::_2, PM); quadrotor::DCost dcostN=std::bind(quad_cost::dcost_exp, std::placeholders::_1, std::placeholders::_2, PM); SAC<quadrotor>::Functions func(cost, dcost, costN, dcostN); SAC<quadrotor>::Params params; //-------------------------------------------------------------------- // Parameter one //-------------------------------------------------------------------- params.alpha=-10; params.beta=0.6; params.Tc=1.28; params.Tp=1.5; params.Ts=0.02; params.Tcal=0.01; params.kmax=8; params.R=1*Eigen::Matrix<double,4,4>::Identity(); //-------------------------------------------------------------------- // Parameter two //-------------------------------------------------------------------- double dt=0.01; quadrotor::System sys(Ix,Iy,Iz,M,d,km,kt); SAC<quadrotor> sac(sys,dt); double t=0; double ts=params.Ts; double T=108+params.Tp; quadrotor::U umax,umin; umax<<6,6,6,6; umin<<0.0,0.0,0.0,0.0; // initialize LQR Mat12 Q=50*Mat12::Identity(); Q.block(0,0,3,3)*=10; Q.block(3,3,3,3)*=5; Q.block(6,6,6,6)=50*Mat6::Identity(); Mat12 Pf=5*Q; Mat4 R=2000*Mat4::Identity(); std::list<Eigen::Matrix<double,4,12> > list_K=quadrotor::lqr(sys, dt, list_par2, list_u2, Q, R, Pf); // set up initial configuration quadrotor::State state0; #ifdef STATE0 char file0[]="/media/fantaosha/Documents/JHU/Summer 2015/results/plot_quadrotor/quadrotor_track_12s_important.mat"; mxArray *mxR0; mxArray *mxw0; mxArray *mxxq0; mxArray *mxvq0; mxR0=matGetVariable(matOpen(file0,"r"),"R"); mxw0=matGetVariable(matOpen(file0,"r"),"omega"); mxxq0=matGetVariable(matOpen(file0,"r"),"xq"); mxvq0=matGetVariable(matOpen(file0,"r"),"vq"); Mat3 R0; Vec3 xq0,vq0,w0; memcpy(R0.data(),mxGetPr(mxR0),sizeof(double)*9); memcpy(w0.data(),mxGetPr(mxw0),sizeof(double)*3); memcpy(xq0.data(),mxGetPr(mxxq0),sizeof(double)*3); memcpy(vq0.data(),mxGetPr(mxvq0),sizeof(double)*3); state0.g.block(0,0,3,3)=R0; state0.g.block(0,3,3,1)=xq0; state0.v.head(3)=w0; state0.v.tail(3)=vq0; mxDestroyArray(mxR0); mxDestroyArray(mxw0); mxDestroyArray(mxxq0); mxDestroyArray(mxvq0); #else state0=list_par1.front(); state0.g.block(0,3,3,1)-=(Vec3::Random()).normalized()*30; state0.g.block(0,0,3,3)*=SO3::exp(Vec3::Random().normalized()*PI); state0.v.head(3)-=Vec3::Random().normalized()*0; state0.v.tail(3)-=Vec3::Random().normalized()*0; #endif // set up simulator Sim<quadrotor>::State_Sim sim(sys,dt); sim.init(state0); Vec4 u2=Vec4::Zero(); int SN=int(params.Ts/dt+0.5); std::list<Eigen::Matrix<double,4,12> >::const_iterator itr_K=list_K.cbegin(); std::list<typename quadrotor::State>::const_iterator itr_par2=list_par2.begin(); std::list<typename quadrotor::U>::const_iterator itr_u2=list_u2.begin(); bool lqr=0; clock_t start,end; Vec4 u_curr=list_u1.front(); double K=10; double Aht=exp(-K*dt); start=clock(); for(;T-t>params.Tp;t+=ts) { // start=clock(); quadrotor::State state=sim.get_state(); quadrotor::State state_ref=list_par1.front(); Vec12 error=(Vec12()<<SE3::log(SE3::inverse(state_ref.g)*state.g), state.v-state_ref.v).finished(); double err_lqr=6; if(lqr) err_lqr=8.5; if(error.norm()>err_lqr) { timespec T_start, T_end; clock_gettime(CLOCK_MONOTONIC,&T_start); sac.init(state, params, func, list_par1.begin(), list_par1.end(), &list_u1); sac.sac_ctrl(list_u1,umin,umax,K,u_curr); clock_gettime(CLOCK_MONOTONIC,&T_end); cout<<"time consumed is "<<(T_end.tv_sec-T_start.tv_sec)+(T_end.tv_nsec-T_start.tv_nsec)/1000000000.0<<"s"<<endl; if(lqr) { lqr=0; std::cout<<"SAC: "<<t<<std::endl; } for(int i=0;i<SN;i++) { u_curr=list_u1.front(); sim.update(u_curr); list_u1.pop_front(); list_par1.pop_front(); list_par1.pop_front(); itr_par2++; itr_u2++; itr_K++; } } else { quadrotor::State state_ref=*itr_par2; Mat3 R_ref=state_ref.g.block(0,0,3,3); Mat3 R=state.g.block(0,0,3,3); Vec3 xq_ref=state_ref.g.block(0,3,3,1); Vec3 xq=state.g.block(0,3,3,1); Vec3 omega_ref=state_ref.v.head(3); Vec3 omega=state.v.head(3); Vec3 vq_ref=R_ref*state_ref.v.tail(3); Vec3 vq=R*state.v.tail(3); Vec12 err=(Vec12()<<SO3::log(R_ref.transpose()*R), xq-xq_ref, omega-omega_ref, vq-vq_ref).finished(); u2=*itr_u2-*itr_K*err; u2=u2.cwiseProduct(u2); Vec4 ut=sim.get_inputs().back(); u2=u2+Aht*(ut-u2); if(u2(0)>umax(0) || u2(1)>umax(1) || u2(2)>umax(2) || u2(3)>umax(3)) { std::cout<<t<<"s "<<": LQR switches to SAC"<<std::endl; sac.init(state, params, func, list_par1.begin(), list_par1.end(), &list_u1); sac.sac_ctrl(list_u1,umin,umax,K,u_curr); if(lqr) { lqr=0; std::cout<<"SAC: "<<t<<"s"<<std::endl; } for(int i=0;i<SN;i++) { u_curr=list_u1.front(); sim.update(u_curr); list_u1.pop_front(); list_par1.pop_front(); list_par1.pop_front(); itr_par2++; itr_u2++; itr_K++; } } else { u_curr=u2; for(int i=0;i<SN;i++) { sim.update(u_curr); list_u1.pop_front(); list_par1.pop_front(); list_par1.pop_front(); itr_par2++; itr_u2++; itr_K++; } if(!lqr) { lqr=1; std::cout<<"LQR: "<<t<<"s: "<<(u2).transpose()<<std::endl; end=clock(); cout<<"time consumed is "<<static_cast<double>(end-start)/CLOCKS_PER_SEC*1000<<" ms"<<endl; } } } } sim.save("/media/fantaosha/Documents/JHU/Summer 2015/results/quadrotor_track_12s.mat"); return 0; }
int main() { double M=0.6; double d=0.2; // displacement of rotors double kt=0.6; double km=0.15; double K=5; double Ix1=8*5e-3; // moment of inertia in X- Y- and Z-axis double Iy1=7.5*5e-3; double Iz1=13.5*5e-3; char file1[]="/media/fantaosha/Documents/JHU/Summer 2015/quad_rotor_traj/traj_full_12s_ud_2.mat"; double Ix2=8*1e-3; // moment of inertia in X- Y- and Z-axis double Iy2=7.5*1e-3; double Iz2=13.5*1e-3; char file2[]="/media/fantaosha/Documents/JHU/Summer 2015/quad_rotor_traj/traj_full_12s_ud_small_5.mat"; Eigen::Matrix<double,4,4> B=(Eigen::Matrix<double,4,4>() << 0, kt*d, 0, -kt*d, -kt*d, 0, kt*d, 0, km, -km, km, -km, kt, kt, kt, kt).finished(); Eigen::Matrix<double,4,4> Binv=(Eigen::Matrix<double,4,4>() << 0, -1/(2*d*kt), 1/(4*km), 1/(4*kt), 1/(2*d*kt), 0, -1/(4*km), 1/(4*kt), 0, 1/(2*d*kt), 1/(4*km), 1/(4*kt), -1/(2*d*kt), 0, -1/(4*km), 1/(4*kt)).finished(); // load control inputs for SAC mxArray *mxR1; mxArray *mxw1; mxArray *mxxq1; mxArray *mxvq1; mxArray *mxU1; mxArray *mxUd1; mxR1=matGetVariable(matOpen(file1,"r"),"state_R"); mxw1=matGetVariable(matOpen(file1,"r"),"state_w"); mxxq1=matGetVariable(matOpen(file1,"r"),"state_xq"); mxvq1=matGetVariable(matOpen(file1,"r"),"state_vq"); mxU1=matGetVariable(matOpen(file1,"r"),"U"); mxUd1=matGetVariable(matOpen(file1,"r"),"Ud"); size_t N=mxGetN(mxU1); size_t sN=5; void *pR1=mxGetPr(mxR1); void *pw1=mxGetPr(mxw1); void *pxq1=mxGetPr(mxxq1); void *pvq1=mxGetPr(mxvq1); void *pU1=mxGetPr(mxU1); std::list<typename quadrotor_ext::State> list_ref1; for(int i=0;i<N;i+=sN) { Eigen::Matrix<double,3,3> R; Eigen::Matrix<double,3,1> w,xq,vq; Eigen::Matrix<double,4,1> u; memcpy(R.data(),pR1,sizeof(double)*R.rows()*R.cols()); memcpy(w.data(),pw1,sizeof(double)*w.rows()*w.cols()); memcpy(xq.data(),pxq1,sizeof(double)*xq.rows()*xq.cols()); memcpy(vq.data(),pvq1,sizeof(double)*vq.rows()*vq.cols()); memcpy(u.data(),pU1,sizeof(double)*u.rows()*u.cols()); list_ref1.emplace_back(R,xq,w,R.transpose()*vq,u.cwiseProduct(u)); pR1+=sizeof(double)*R.rows()*R.cols()*sN; pw1+=sizeof(double)*w.rows()*w.cols()*sN; pxq1+=sizeof(double)*xq.rows()*xq.cols()*sN; pvq1+=sizeof(double)*vq.rows()*vq.cols()*sN; pU1+=sizeof(double)*u.rows()*u.cols()*sN; } sN=10; std::list<typename quadrotor_ext::U> list_ud1; void *pUd1=mxGetPr(mxUd1)+sizeof(double)*5*4; for(int i=sN/2;i<N;i+=sN) { Eigen::Matrix<double,4,1> ud; memcpy(ud.data(),pUd1,sizeof(double)*ud.rows()*ud.cols()); list_ud1.push_back(ud.cwiseProduct(ud)); pUd1+=sizeof(double)*ud.rows()*ud.cols()*sN; } mxDestroyArray(mxR1); mxDestroyArray(mxw1); mxDestroyArray(mxxq1); mxDestroyArray(mxvq1); mxDestroyArray(mxU1); mxDestroyArray(mxUd1); // load control inputs for LQR mxArray *mxR2; mxArray *mxw2; mxArray *mxxq2; mxArray *mxvq2; mxArray *mxU2; mxArray *mxUd2; mxR2=matGetVariable(matOpen(file2,"r"),"state_R"); mxw2=matGetVariable(matOpen(file2,"r"),"state_w"); mxxq2=matGetVariable(matOpen(file2,"r"),"state_xq"); mxvq2=matGetVariable(matOpen(file2,"r"),"state_vq"); mxU2=matGetVariable(matOpen(file2,"r"),"U"); mxUd2=matGetVariable(matOpen(file2,"r"),"Ud"); N=mxGetN(mxU2); sN=10; void *pR2=mxGetPr(mxR2); void *pw2=mxGetPr(mxw2); void *pxq2=mxGetPr(mxxq2); void *pvq2=mxGetPr(mxvq2); void *pU2=mxGetPr(mxU2); std::list<typename quadrotor_ext::State> list_ref2; for(int i=0;i<N;i+=sN) { Eigen::Matrix<double,3,3> R; Eigen::Matrix<double,3,1> w,xq,vq; Eigen::Matrix<double,4,1> u; memcpy(R.data(),pR2,sizeof(double)*R.rows()*R.cols()); memcpy(w.data(),pw2,sizeof(double)*w.rows()*w.cols()); memcpy(xq.data(),pxq2,sizeof(double)*xq.rows()*xq.cols()); memcpy(vq.data(),pvq2,sizeof(double)*vq.rows()*vq.cols()); memcpy(u.data(),pU2,sizeof(double)*u.rows()*u.cols()); list_ref2.emplace_back(R,xq,w,R.transpose()*vq,u.cwiseProduct(u)); pR2+=sizeof(double)*R.rows()*R.cols()*sN; pw2+=sizeof(double)*w.rows()*w.cols()*sN; pxq2+=sizeof(double)*xq.rows()*xq.cols()*sN; pvq2+=sizeof(double)*vq.rows()*vq.cols()*sN; pU2+=sizeof(double)*u.rows()*u.cols()*sN; } std::list<typename quadrotor_ext::U> list_ud2; void *pUd2=mxGetPr(mxUd2)+sizeof(double)*4*5; for(int i=sN/2;i<N;i+=sN) { Eigen::Matrix<double,4,1> ud; memcpy(ud.data(),pUd2,sizeof(double)*ud.rows()*ud.cols()); list_ud2.push_back(ud.cwiseProduct(ud)); pUd2+=sizeof(double)*ud.rows()*ud.cols()*sN; } mxDestroyArray(mxR2); mxDestroyArray(mxw2); mxDestroyArray(mxxq2); mxDestroyArray(mxvq2); mxDestroyArray(mxU2); mxDestroyArray(mxUd2); std::srand((unsigned int) time(0)); Mat16 PM=250*Mat16::Identity(); PM.block(0,0,3,3)*=8; PM.block(3,3,3,3)*=40; PM.block(6,6,6,6)=PM.block(0,0,6,6)/8; PM.block(12,12,4,4)*=1e-2; Mat16 NM=PM/20; quadrotor_ext::Cost cost=std::bind(quad_ext_cost::cost_exp, std::placeholders::_1, std::placeholders::_2, NM); quadrotor_ext::DCost dcost=std::bind(quad_ext_cost::dcost_exp, std::placeholders::_1, std::placeholders::_2, NM); quadrotor_ext::Cost costN=std::bind(quad_ext_cost::cost_exp, std::placeholders::_1, std::placeholders::_2, PM); quadrotor_ext::DCost dcostN=std::bind(quad_ext_cost::dcost_exp, std::placeholders::_1, std::placeholders::_2, PM); SAC<quadrotor_ext>::Functions func(cost, dcost, costN, dcostN); SAC<quadrotor_ext>::Params params; params.alpha=-10; params.beta=0.6; params.Tc=1.28; params.Tp=1.5; params.Ts=0.02; params.Tcal=0.01; params.kmax=8; params.R=0.02*Eigen::Matrix<double,4,4>::Identity(); quadrotor_ext::System sys1(Ix1,Iy1,Iz1,M,d,km,kt,K); double dt=0.01; SAC<quadrotor_ext> sac(sys1,dt); double t=0; double ts=params.Ts; double T=108+params.Tp; quadrotor_ext::U udmax,udmin; udmax<<6,6,6,6; udmin<<0.0,0.0,0.0,0.0; quadrotor_ext::System sys2(Ix2,Iy2,Iz2,M,d,km,kt,K); Mat16 Q=50*Mat16::Identity(); Q.block(0,0,3,3)*=20; Q.block(3,3,3,3)*=5; Q.block(6,6,6,6)=50*Mat6::Identity(); Q.block(12,12,4,4)=10*Mat4::Identity(); Mat16 Pf=5*Q; Mat4 R=4000*Mat4::Identity(); std::list<Eigen::Matrix<double,4,16> > list_K=quadrotor_ext::lqr(sys2, dt, list_ref2, list_ud2, Q, R, Pf); quadrotor_ext::State state0; state0=list_ref2.front(); state0.g.block(0,3,3,1)-=(Vec3::Random()).normalized()*0; state0.g.block(0,0,3,3)*=SO3::exp(Vec3::Random().normalized()*PI*0); state0.v.head(3)=Vec3::Random().normalized()*0; state0.v.tail(3)=Vec3::Random().normalized()*0; // state0.u*=0; Sim<quadrotor_ext> sim(sys2,dt); sim.init(state0); Vec4 Du((Vec4()<<6,6,6,6).finished()); Vec4 ud_curr=list_ud1.front(); std::list<Eigen::Matrix<double,4,16> >::const_iterator itr_K=list_K.cbegin(); std::list<typename quadrotor_ext::U>::const_iterator itr_ud2=list_ud2.begin(); size_t SN=size_t(params.Ts/sim.dt+0.5); double err_lqr=6.5; double err_sac=8.5; bool is_sac=true; for(double t=sim.dt*1e-3;t<=96;t+=params.Ts) { quadrotor_ext::State state=sim.get_state(); quadrotor_ext::State state_ref=list_ref2.front(); Vec12 error=(Vec12()<<SO3::log(SO3::inverse(state_ref.g.block(0,0,3,3))*state.g.block(0,0,3,3)), state.g.block(0,3,3,1)-state_ref.g.block(0,3,3,1), state.v-state_ref.v).finished(); double err=error.norm(); if(is_sac) { if(err<=err_lqr) { std::cout<<t<<std::endl; is_sac=false; } } else if(err>err_sac) is_sac=true; // is_sac=false; if(is_sac) { SAC: Vec4 F=B*state.u; F.head(3)*=5; state.u=Binv*F; sac.init(state, params, func, list_ref1.begin(), list_ref1.end(), &list_ud1); sac.sac_ctrl(list_ud1,udmin,udmax,Du,ud_curr); for(size_t n=0;n<SN;n++) { Vec4 ud=list_ud1.front(); Vec4 Fd=B*ud; Fd.head(3)/=5; ud_curr=Binv*Fd; sim.update(ud_curr); list_ud1.pop_front(); list_ref1.pop_front(); list_ref1.pop_front(); list_ref2.pop_front(); itr_K++; itr_ud2++; } } else { Mat3 R_ref=state_ref.g.block(0,0,3,3); Mat3 R=state.g.block(0,0,3,3); Vec3 xq_ref=state_ref.g.block(0,3,3,1); Vec3 xq=state.g.block(0,3,3,1); Vec3 omega_ref=state_ref.v.head(3); Vec3 omega=state.v.head(3); Vec3 vq_ref=R_ref*state_ref.v.tail(3); Vec3 vq=R*state.v.tail(3); Vec16 error((Vec16()<<SO3::log(R_ref.transpose()*R), xq-xq_ref, omega-omega_ref, vq-vq_ref, state.u-state_ref.u).finished()); // std::cout<<error.transpose()<<std::endl; Vec4 ud2=*itr_ud2-*itr_K*error; if(ud2(0)>udmax(0) || ud2(1)> udmax(1) || ud2(2)>udmax(2) || ud2(3)>udmax(3) || ud2(0)<udmin(0) || ud2(1)< udmin(1) || ud2(2)<udmin(2) || ud2(3)<udmin(3)) { is_sac=true; goto SAC; } else { ud_curr=ud2; for(size_t n=0;n<SN;n++) { sim.update(ud_curr); list_ud1.pop_front(); list_ref1.pop_front(); list_ref1.pop_front(); list_ref2.pop_front(); itr_K++; itr_ud2++; } } } } sim.save("/media/fantaosha/Documents/JHU/Summer 2015/results/quadrotor_track_12s_ud.mat"); return 0; }
int main() { #ifdef TRACK char file[]="/media/fantaosha/Documents/JHU/Summer 2015/quad_rotor_traj/traj_full_12s_1_small.mat"; mxArray *mxR; mxArray *mxw; mxArray *mxxq; mxArray *mxvq; mxArray *mxU; mxR=matGetVariable(matOpen(file,"r"),"state_R"); mxw=matGetVariable(matOpen(file,"r"),"state_w"); mxxq=matGetVariable(matOpen(file,"r"),"state_xq"); mxvq=matGetVariable(matOpen(file,"r"),"state_vq"); mxU=matGetVariable(matOpen(file,"r"),"U"); size_t N=mxGetN(mxU); size_t sN=10; void *pR=mxGetPr(mxR); void *pw=mxGetPr(mxw); void *pxq=mxGetPr(mxxq); void *pvq=mxGetPr(mxvq); void *pU=mxGetPr(mxU); std::vector<typename quadrotor::State> xrefs; xrefs.reserve(N/sN+1); for(int i=0;i<N;i+=sN) { Eigen::Matrix<double,3,3> R; Eigen::Matrix<double,3,1> w,xq,vq; memcpy(R.data(),pR,sizeof(double)*R.rows()*R.cols()); // R=(Vec3()<<1,-1,-1).finished().asDiagonal(); // w<<0,0,0; memcpy(w.data(),pw,sizeof(double)*w.rows()*w.cols()); memcpy(xq.data(),pxq,sizeof(double)*xq.rows()*xq.cols()); memcpy(vq.data(),pvq,sizeof(double)*vq.rows()*vq.cols()); xrefs.emplace_back(R,xq,w,R.transpose()*vq); pR+=sizeof(double)*R.rows()*R.cols()*sN; pw+=sizeof(double)*w.rows()*w.cols()*sN; pxq+=sizeof(double)*xq.rows()*xq.cols()*sN; pvq+=sizeof(double)*vq.rows()*vq.cols()*sN; } sN=10; std::vector<typename quadrotor::U> us; us.reserve(N/sN+1); for(int i=0;i<N;i+=sN) { Eigen::Matrix<double,4,1> u; memcpy(u.data(),pU,sizeof(double)*u.rows()*u.cols()); us.push_back(u.cwiseProduct(u)); pU+=sizeof(double)*u.rows()*u.cols()*sN; } #else quadrotor::State xref; xref.g.block(0,0,3,3)=SO3::exp((Vec3()<<0,0,0).finished()); xref.g.block(0,3,3,1)=(Vec3()<<0,0,0).finished(); std::vector<quadrotor::State> xrefs(4000,xref); std::vector<quadrotor::U> us(4000, Vec4::Zero()); #endif std::srand((unsigned int) time(0)); // Set up quadrotor parameters double m=0.6; double Ix=8*1e-3; // moment of inertia in X- Y- and Z-axis double Iy=7.5*1e-3; double Iz=13.5*1e-3; double d=0.2; // displacement of rotors double kt=0.6; double km=0.15; quadrotor::System sys(Ix,Iy,Iz,m,d,km,kt); // Set up cost function Mat12 Mf=5*Mat12::Identity()/10; Mf.block(0,0,3,3)*=1; Mf.block(3,3,3,3)*=6; // Mf.block(6,6,6,6)=Mf.block(0,0,6,6); Mat12 M=Mf/2; Mat4 R=Mat4::Identity()/50; DDP<quadrotor>::Params params(M,R,Mf); // Set up initial state quadrotor::State x0=xrefs[0]; x0.g.block(0,3,3,1)-=(Vec3::Random()).normalized()*3; x0.g.block(0,0,3,3)*=SO3::exp(Vec3::Random().normalized()*1); // x0.g.block(0,0,3,3)*=SO3::exp((Vec3()<<3.14,0,0).finished()); x0.v.head(3)-=Vec3::Random().normalized()*0; x0.v.tail(3)-=Vec3::Random().normalized()*0; // Set up simulator double dt=0.01; size_t num=200; Sim<quadrotor> sim(sys,dt); sim.init(x0,num); Vec4 umin=-Vec4::Ones()*0; Vec4 umax=Vec4::Ones()*6; DDP<quadrotor> ddp(sys,dt); double ts=0.02; double T=36; double Tp=1.5; size_t SN=size_t(ts/dt+0.5); size_t ND=size_t(Tp/dt+0.5)+1; int sn=2; int itr_max=20; for(int i=0;i<2000;i+=sn) { timespec T_start, T_end; clock_gettime(CLOCK_MONOTONIC,&T_start); ddp.init(sim.get_state(), us, xrefs, params, umin, umax, 150); ddp.iterate(itr_max,us); clock_gettime(CLOCK_MONOTONIC,&T_end); std::cout<<"time consumed is "<<(T_end.tv_sec-T_start.tv_sec)+(T_end.tv_nsec-T_start.tv_nsec)/1000000000.0<<"s"<<std::endl; for(int j=0;j<sn;j++) { sim.update(us.front()); xrefs.erase(xrefs.begin()); us.erase(us.begin()); } Vec12 error=quadrotor::State::diff(sim.get_state(),xrefs[0]); std::cout<<dt*i<<": "<<error.head(3).norm()<<" "<<error.head(6).tail(3).norm()<<std::endl; if(error.norm()<0.1) break; } sim.save("/media/fantaosha/Documents/JHU/Summer 2015/results/quadrotor_ddp.mat"); }
int main() { //******************************************************** // Initialization //******************************************************** double M=0.6; double Ix=8*5e-3; // moment of inertia in X- Y- and Z-axis double Iy=7.5*5e-3; double Iz=13.5*5e-3; double d=0.2; // displacement of rotors double kt=0.6; double km=0.15; char file[]="/media/fantaosha/Documents/JHU/Summer 2015/quad_rotor_traj/traj_full_12s_7.mat"; // double M=2; // mass of quadrotor // double Ix=1.2; // moment of inertia in X- Y- and Z-axis // double Iy=1.2; // double Iz=2.3; // double d=0.25; // displacement of rotors // double kt=1; // double km=0.20; // // char file[]="/media/fantaosha/Documents/JHU/Summer 2015/quad_rotor_traj/traj_full_12s_2.mat"; mxArray *mxR; mxArray *mxw; mxArray *mxxq; mxArray *mxvq; mxArray *mxU; mxR=matGetVariable(matOpen(file,"r"),"state_R"); mxw=matGetVariable(matOpen(file,"r"),"state_w"); mxxq=matGetVariable(matOpen(file,"r"),"state_xq"); mxvq=matGetVariable(matOpen(file,"r"),"state_vq"); #ifdef ROT_INPUT mxU=matGetVariable(matOpen(file,"r"),"U"); #else mxU=matGetVariable(matOpen(file,"r"),"F"); #endif size_t N=mxGetN(mxU); size_t sN=5; void *pR=mxGetPr(mxR); void *pw=mxGetPr(mxw); void *pxq=mxGetPr(mxxq); void *pvq=mxGetPr(mxvq); void *pU=mxGetPr(mxU); std::list<typename quadrotor::State> list_PAR1; std::list<typename quadrotor::State> list_PAR2; for(int i=0;i<N;i+=sN) { Eigen::Matrix<double,3,3> R; Eigen::Matrix<double,3,1> w,xq,vq; memcpy(R.data(),pR,sizeof(double)*R.rows()*R.cols()); memcpy(w.data(),pw,sizeof(double)*w.rows()*w.cols()); memcpy(xq.data(),pxq,sizeof(double)*xq.rows()*xq.cols()); memcpy(vq.data(),pvq,sizeof(double)*vq.rows()*vq.cols()); list_PAR1.emplace_back(R,xq,w,R.transpose()*vq); if((i+1)%2) list_PAR2.emplace_back(R,xq,w,R.transpose()*vq); pR+=sizeof(double)*R.rows()*R.cols()*sN; pw+=sizeof(double)*w.rows()*w.cols()*sN; pxq+=sizeof(double)*xq.rows()*xq.cols()*sN; pvq+=sizeof(double)*vq.rows()*vq.cols()*sN; } sN=10; std::list<typename quadrotor::U> list_U1; std::list<typename quadrotor::U> list_U2; #ifdef ROT_INPUT for(int i=0;i<N;i+=sN) { Eigen::Matrix<double,4,1> u; memcpy(u.data(),pU,sizeof(double)*u.rows()*u.cols()); list_U1.push_back(u.cwiseProduct(u)); list_U2.push_back(u); pU+=sizeof(double)*u.rows()*u.cols()*sN; } #else for(int i=0;i<N;i+=sN) { Eigen::Matrix<double,4,1> u; memcpy(u.data(),pU,sizeof(double)*u.rows()*u.cols()); list_u.push_back(u); pU+=sizeof(double)*u.rows()*u.cols()*sN; } #endif mxDestroyArray(mxR); mxDestroyArray(mxw); mxDestroyArray(mxxq); mxDestroyArray(mxvq); mxDestroyArray(mxU); std::srand((unsigned int) time(0)); // initialize SAC Mat12 PM=2500*Mat12::Identity(); PM.block(0,0,3,3)*=8; PM.block(3,3,3,3)*=4; PM.block(6,6,3,3)*=1.2; PM.block(9,9,3,3)*=2; Mat12 NM=PM/20; quadrotor::Cost cost=std::bind(quad_cost::cost_exp, std::placeholders::_1, std::placeholders::_2, NM); quadrotor::DCost dcost=std::bind(quad_cost::dcost_exp, std::placeholders::_1, std::placeholders::_2, NM); quadrotor::Cost costN=std::bind(quad_cost::cost_exp, std::placeholders::_1, std::placeholders::_2, PM); quadrotor::DCost dcostN=std::bind(quad_cost::dcost_exp, std::placeholders::_1, std::placeholders::_2, PM); SAC<quadrotor>::Functions func(cost, dcost, costN, dcostN); SAC<quadrotor>::Params params; params.alpha=-10; params.beta=0.6; params.Tc=1.28; params.Tp=1.5; params.Ts=0.02; params.Tcal=0.01; params.kmax=8; // mat4 k=(mat4()<<0, kt*d,0,-kt*d, // -kt*d, 0, kt*d, 0, // km,-km, km, -km, // kt, kt, kt, kt).finished(); // // vec4 vv=(vec4()<<5,5,10,0.2).finished(); // params.R=2*K.transpose()*vv.asDiagonal()*K; params.R=1*Eigen::Matrix<double,4,4>::Identity(); double dt=0.01; quadrotor::System sys(Ix,Iy,Iz,M,d,km,kt); double ts=params.Ts; double T=72+params.Tp; quadrotor::U umax,umin; umax<<6,6,6,6; umin<<0,0,0,0; // initialize LQR Mat12 Q=50*Mat12::Identity(); Q.block(0,0,3,3)*=10; Q.block(3,3,3,3)*=5; Q.block(6,6,6,6)=50*Mat6::Identity(); Mat12 Pf=5*Q; Mat4 R=2000*Mat4::Identity(); std::list<Eigen::Matrix<double,4,12> > list_K=quadrotor::lqr(sys, dt, list_PAR2, list_U2, Q, R, Pf); // set up initial configuration int testN=200; int xqN,RN; double err_dxq=3,err_dR=PI/10; std::ofstream results1,results2; results1.open("results1.txt"); results2.open("results2.txt"); omp_set_num_threads(6); timespec T_start,T_end; clock_gettime(CLOCK_MONOTONIC,&T_start); std::list<quadrotor::State> list_fail; for(xqN=10;xqN<=10;xqN++) for(RN=10;RN<=10;RN++) { double const err_xq=err_dxq*xqN; double const err_R=err_dR*RN; int succeed=0; #ifdef PARALLEL #pragma omp parallel for reduction(+:succeed) schedule(dynamic) #endif for(int test=1;test<=testN;test++) { SAC<quadrotor> sac(sys,dt); quadrotor::State state0=list_PAR1.front(); state0.g.block(0,3,3,1)-=(Vec3::Random()).normalized()*err_xq; state0.g.block(0,0,3,3)*=SO3::exp(Vec3::Random().normalized()*err_R); // state0.v.head(3)-=Vec3::Random().normalized()*0; // state0.v.tail(3)-=Vec3::Random().normalized()*0; state0.v.head(3)*=0; state0.v.tail(3)*=0; std::list<typename quadrotor::State> list_par1=list_PAR1; std::list<typename quadrotor::State> list_par2=list_PAR2; std::list<typename quadrotor::U> list_u1=list_U1; // set up simulator Sim<quadrotor>::State_Sim sim(sys,dt); sim.init(state0); Vec4 u2=Vec4::Zero(); int SN=int(params.Ts/dt+0.5); std::list<Eigen::Matrix<double,4,12> >::const_iterator itr_K=list_K.cbegin(); std::list<typename quadrotor::State>::const_iterator itr_par2=list_par2.begin(); std::list<typename quadrotor::U>::const_iterator itr_u2=list_U2.begin(); bool lqr=0; clock_t start,end; #ifdef LOW_PASS std::list<quadrotor::U> list_U; size_t UN=5; double weights[]={6,5,4,3,2,1}; double sum_weights[]={6,11,15,18,20,21}; #endif Vec4 u_curr=list_u1.front(); #ifdef SMOOTH_INPUTS Vec4 Du=1*(Vec4()<<0.5,0.5,0.5,0.5).finished(); #endif start=clock(); #ifdef PARALLEL #pragma omp critical { std::cout<<"================================================================="<<std::endl; std::cout<<omp_get_thread_num()<<" "<<omp_get_num_threads()<<std::endl; std::cout<<state0.g<<std::endl; std::cout<<"================================================================="<<std::endl; } #else std::cout<<"==============================================================================="<<std::endl; std::cout<<state0.g<<std::endl; std::cout<<"================================================================="<<std::endl; #endif double TTS=T; // Time to drive the system to the basin of attraction for LQR Vec12 error=Vec12::Zero(); for(double t=0;T-t>params.Tp;t+=ts) { quadrotor::State state=sim.get_state(); quadrotor::State state_ref=list_par1.front(); // Vec12 error=(Vec12()<<SE3::log(SE3::inverse(state_ref.g)*state.g), state.v-state_ref.v).finished(); error=(Vec12()<<SE3::log(SE3::inverse(state_ref.g)*state.g), state.v-state_ref.v).finished(); double err_lqr=6; if(lqr) err_lqr=8.5; if(error.norm()>err_lqr) { sac.init(state, params, func, list_par1.begin(), list_par1.end(), &list_u1); #ifdef SMOOTH_INPUTS sac.sac_ctrl(list_u1,umin,umax,Du,u_curr); #else sac.sac_ctrl(list_u1,umin,umax); #endif if(lqr) { TTS=T; lqr=0; } for(int i=0;i<SN;i++) { #ifdef LOW_PASS list_U.push_back(list_u1.front()); Vec4 u1=Vec4::Zero(); std::list<quadrotor::U>::const_reverse_iterator rit_u=list_U.rbegin(); size_t index=0; while(rit_u!=list_U.rend()) { u1+=*rit_u*weights[index]; rit_u++; index++; } index--; u1/=sum_weights[index]; if(index>=UN) list_U.pop_front(); u_curr=u1; #else u_curr=list_u1.front(); #endif // std::cout<<t<<": "<<u_curr.transpose()<<std::endl; sim.update(u_curr); list_u1.pop_front(); list_par1.pop_front(); list_par1.pop_front(); itr_par2++; itr_u2++; itr_K++; } } else { quadrotor::State state_ref=*itr_par2; Mat3 R_ref=state_ref.g.block(0,0,3,3); Mat3 R=state.g.block(0,0,3,3); Vec3 xq_ref=state_ref.g.block(0,3,3,1); Vec3 xq=state.g.block(0,3,3,1); Vec3 omega_ref=state_ref.v.head(3); Vec3 omega=state.v.head(3); Vec3 vq_ref=R_ref*state_ref.v.tail(3); Vec3 vq=R*state.v.tail(3); Vec12 err=(Vec12()<<SO3::log(R_ref.transpose()*R), xq-xq_ref, omega-omega_ref, vq-vq_ref).finished(); u2=*itr_u2-*itr_K*err; u2=u2.cwiseProduct(u2); if(u2(0)>umax(0) || u2(1)>umax(1) || u2(2)>umax(2) || u2(3)>umax(3)) { sac.init(state, params, func, list_par1.begin(), list_par1.end(), &list_u1); #ifdef SMOOTH_INPUTS sac.sac_ctrl(list_u1,umin,umax,Du,u_curr); #else sac.sac_ctrl(list_u1,umin,umax); #endif if(lqr) { lqr=0; TTS=T; #ifndef PARALLEL std::cout<<"SAC: "<<t<<"s"<<std::endl; #endif } for(int i=0;i<SN;i++) { #ifdef LOW_PASS list_U.push_back(list_u1.front()); Vec4 u1=Vec4::Zero(); std::list<quadrotor::U>::const_reverse_iterator rit_u=list_U.rbegin(); size_t index=0; while(rit_u!=list_U.rend()) { u1+=*rit_u*weights[index]; rit_u++; index++; } index--; u1/=sum_weights[index]; if(index>=UN) list_U.pop_front(); u_curr=u1; #else u_curr=list_u1.front(); #endif sim.update(u_curr); list_u1.pop_front(); list_par1.pop_front(); list_par1.pop_front(); itr_par2++; itr_u2++; itr_K++; } } else { u_curr=u2; for(int i=0;i<SN;i++) { sim.update(u_curr); list_u1.pop_front(); list_par1.pop_front(); list_par1.pop_front(); #ifdef LOW_PASS if(!list_U.empty()) list_U.pop_front(); #endif itr_par2++; itr_u2++; itr_K++; } if(!lqr) { lqr=1; #ifndef PARALLEL std::cout<<"LQR: "<<t<<"s: "<<(u2).transpose()<<std::endl; #endif TTS=t; end=clock(); #ifndef PARALLEL std::cout<<"-----------------------------------------------------------------"<<std::endl; cout<<"time consumed is "<<static_cast<double>(end-start)/CLOCKS_PER_SEC*1000<<" ms"<<endl; std::cout<<"================================================================="<<std::endl; #endif } } } } #ifndef PARALLEL std::cout<<"==============================================================================="<<std::endl<<std::endl; #endif if(lqr) { #ifdef PARALLEL succeed+=1; #else succeed++; #endif } else { #ifdef PARALLEL #pragma omp critical { list_fail.push_back(state0); } #else list_fail.push_back(state0); #endif } #ifdef PARALLEL #pragma omp critical { // std::cout<<omp_get_thread_num()<<std::endl; std::cout<<"=================================="<<std::endl; std::cout<<(error.head(3)).norm()<<std::endl; std::cout<<"=================================="<<std::endl; results2<<TTS<<" "; } #else results2<<TTS<<" "; #endif } results2<<std::endl; std::cout<<"err_xq="<<err_dxq*xqN<<", err_R="<<0.1*RN<<"Pi"<<" : "<<testN<<" "<<succeed<<std::endl; results1<<"err_xq="<<err_dxq*xqN<<", err_R="<<0.1*RN<<"Pi"<<" : "<<testN<<" "<<succeed<<std::endl; } std::string path_fail="./failture.mat"; if(!list_fail.empty()) { std::cout<<list_fail.size()<<std::endl; quadrotor::State::save(list_fail,path_fail); } clock_gettime(CLOCK_MONOTONIC,&T_end); cout<<"time consumed is "<<(T_end.tv_sec-T_start.tv_sec)+(T_end.tv_nsec-T_start.tv_nsec)/1000000000.0<<"s"<<endl; // sim.save("/media/fantaosha/Documents/JHU/Summer 2015/results/quadrotor_track_12s.mat"); return 0; }