int main() { // load trajectory char file[]="/media/fantaosha/Documents/JHU/Summer 2015/boat/boat_states.mat"; mxArray *mxState; mxArray *mxVb; mxArray *mxU; std::srand((unsigned int) time(0)); mxState=matGetVariable(matOpen(file,"r"), "state"); mxVb=matGetVariable(matOpen(file,"r"), "vb"); mxU=matGetVariable(matOpen(file,"r"), "u"); size_t N=mxGetN(mxU); void *pState, *pVb, *pU; std::list<boat::State> list_pars1; std::list<boat::State> list_pars2; std::list<Vec2> list_U; pState=mxGetPr(mxState); pVb=mxGetPr(mxVb); for(int i=0;i<N;i++) { Vec3 thxy; Vec3 v; Mat3 g; memcpy(thxy.data(), pState, sizeof(double)*3); memcpy(v.data(), pVb, sizeof(double)*3); g=SE2::exp(thxy); g(0,2)=thxy(1); g(1,2)=thxy(2); list_pars1.emplace_back(g,v); pState+=sizeof(double)*3; pVb+=sizeof(double)*3; } pState=mxGetPr(mxState); pVb=mxGetPr(mxVb); pU=mxGetPr(mxU); for(int i=0;i<N;i+=2) { Vec3 thxy; Vec3 v; Mat3 g; Vec2 u; memcpy(thxy.data(), pState, sizeof(double)*3); memcpy(v.data(), pVb, sizeof(double)*3); g=SE2::exp(thxy); g(0,2)=thxy(1); g(1,2)=thxy(2); list_pars2.emplace_back(g,v); memcpy(u.data(), pU, sizeof(double)*2); list_U.push_back(u); pState+=sizeof(double)*6; pVb+=sizeof(double)*6; pU+=sizeof(double)*4; } list_U.pop_back(); // system initialization double I=1; double m=1; boat::System sys(I,m); // cost function initialization Mat6 NM1=0.3*Mat6::Identity(); NM1(2,2)*=10; NM1.block(3,3,3,3)=0.05*Mat3::Identity(); Mat6 PM1=300*Mat6::Identity(); PM1(0,0)*=5; PM1(1,1)*=5; PM1(2,2)*=10; PM1.block(3,3,3,3)=600*Mat3::Identity(); // PM1(3,3)=100; // PM1(5,5)=PM1(2,2)/20; boat::Cost cost1=std::bind(boat_cost::cost_exp, std::placeholders::_1, std::placeholders::_2, std::ref(NM1)); boat::DCost dcost1=std::bind(boat_cost::dcost_exp, std::placeholders::_1, std::placeholders::_2, std::ref(NM1)); boat::Cost costN1=std::bind(boat_cost::cost_exp, std::placeholders::_1, std::placeholders::_2, std::ref(PM1)); boat::DCost dcostN1=std::bind(boat_cost::dcost_exp, std::placeholders::_1, std::placeholders::_2, std::ref(PM1)); SAC<boat>::Functions func1(cost1,dcost1, costN1, dcostN1); Mat6 NM2=0.3*Mat6::Identity(); NM2(2,2)*=10; NM2.block(3,3,3,3)=0.05*Mat3::Identity(); Mat6 PM2=300*Mat6::Identity(); PM2(2,2)*=40; PM2.block(3,3,3,3)=0*Mat3::Identity(); PM2(4,4)=PM2(2,2)/200; PM2(5,5)=PM2(2,2)*8; boat::Cost cost2=std::bind(boat_cost::cost, std::placeholders::_1, std::placeholders::_2, std::ref(NM2)); boat::DCost dcost2=std::bind(boat_cost::dcost, std::placeholders::_1, std::placeholders::_2, std::ref(NM2)); boat::Cost costN2=std::bind(boat_cost::cost, std::placeholders::_1, std::placeholders::_2, std::ref(PM2)); boat::DCost dcostN2=std::bind(boat_cost::dcost, std::placeholders::_1, std::placeholders::_2, std::ref(PM2)); SAC<boat>::Functions func2(cost2, dcost2, costN2, dcostN2); // set up SAC control parameters double Ts=0.02; SAC<boat>::Params params1; params1.alpha=-25; params1.beta=0.6; params1.Tc=1.28; params1.Tp=1.5; params1.Ts=Ts; params1.Tcal=0.01; params1.kmax=8; params1.R= 1e-8*Mat2::Identity(); SAC<boat>::Params params2; params2.alpha=-2; params2.beta=0.6; params2.Tc=0.64; params2.Tp=1; params2.Ts=Ts; params2.Tcal=0.01; params2.kmax=6; params2.R= Mat2::Identity(); // set up initial and final states boat::State state0(list_pars1.front()); // state0.g.block(0,0,2,2)<<0, -1, 1, 0; // state0.g(0,2)-=10; // state0.g(1,2)+=10; // state0.v*=0; unsigned seed = std::chrono::system_clock::now().time_since_epoch().count(); std::default_random_engine generator(seed); std::normal_distribution<double> distribution1(0,40); double P=distribution1(generator); state0.g.block(0,0,2,2)*=SO2::exp(P); state0.g.block(0,2,2,1)-=(Vec2::Random()).normalized()*15; state0.v-=(Vec3::Random()).normalized()*10; // initialize simulator double dt=0.01; Sim<boat>::State_Sim sim(sys,dt); sim.init(state0); std::cout<<sim.get_state().g<<std::endl; // initialize LQR Mat6 Q=Mat6::Identity(); Q.block(0,0,3,3)<<20, 0, 0, 0, 40, 0, 0, 0, 40; Q.block(3,3,3,3)*=1; std::cout<<Q<<std::endl; Mat2 R=2*Mat2::Identity(); Mat6 Pf=Q*5; std::cout<<list_pars2.size()<<std::endl; std::list<Eigen::Matrix<double,2,6> > list_K=boat::lqr(sys, dt, list_pars2, list_U, Q, R, Pf); // Another LQR controller // LQR<boat> lqr(sys); // lqr.build(list_pars2, list_U, Q, R, Pf, dt); // std::list<Eigen::Matrix<double,2,6> > list_K=lqr.get_K(); // initialize SAC SAC<boat> sac(sys,dt); Vec2 umax, umin; umax<<6,6; umin<<-6,-6; // simulation with SAC std::list<boat::U> list_u=list_U; int sN=int(Ts/dt+0.5); std::list<boat::State> list_pars; double J; clock_t start,end; bool lqr_in=0; for(double t=0; t<538-1e-5; t+=Ts) { boat::State state=sim.get_state(); boat::State state_ref=list_pars2.front(); Vec6 err=(Vec6()<<SO2::log(state_ref.g.block(0,0,2,2).transpose()*state.g.block(0,0,2,2)), state.g.block(0,2,2,1)-state_ref.g.block(0,2,2,1), state.v(0)-state_ref.v(0), state.g.block(0,0,2,2)*state.v.tail(2)-state_ref.g.block(0,0,2,2)*state_ref.v.tail(2)).finished(); // std::cout<<err.transpose()<<std::endl<<std::endl; if(err.norm()<3) { if(!lqr_in) { lqr_in=1; std::cout<<t<<std::endl; } for(int n=0;n<sN;n++) { Vec2 ud=list_u.front(); Eigen::Matrix<double,2,6> K=list_K.front(); Vec2 u=ud-K*err; sim.update(u); list_pars.push_back(list_pars2.front()); list_u.pop_front(); list_U.pop_front(); list_K.pop_front(); list_pars1.pop_front(); list_pars1.pop_front(); list_pars2.pop_front(); } } else { lqr_in=0; start=clock(); sac.init(sim.get_state(), params1, func1, list_pars1.begin(),list_pars1.end(), &list_U); J=sac.sac_ctrl(list_U,umin,umax); end=clock(); cout<<"time consumed is "<<static_cast<double>(end-start)/CLOCKS_PER_SEC*1000<<" ms"<<endl; for(int n=0; n<sN; n++) { sim.update(list_U.front()); list_pars.push_back(list_pars2.front()); list_u.pop_front(); list_U.pop_front(); list_K.pop_front(); list_pars1.pop_front(); list_pars1.pop_front(); list_pars2.pop_front(); } } // double error=err.norm(); // if(error>2) // sac.init(sim.get_state(), params1, func1, list_pars1.begin(),list_pars1.end(), &list_u); // else // sac.init(sim.get_state(), params2, func2, list_pars.begin(),list_pars.end(), &list_u); // // J=sac.sac_ctrl(list_u,umin,umax); // // std::cout<<J<<std::endl; // // for(int i=0;i<sN; i++) // { // sim.update(list_u.front()); // // list_Pars.push_back(list_pars1.front()); // list_pars1.pop_front(); // list_pars1.pop_front(); // list_inputs.push_back(list_u.front()); // list_u.pop_front(); // std::cout<<t<<" "<<list_u.size()<<std::endl; // } } boat::State::save(list_pars,"/media/fantaosha/Documents/JHU/Summer 2015/results/boat_Pars.mat"); sim.save("/media/fantaosha/Documents/JHU/Summer 2015/results/boat.mat"); // Eigen::tomat<2,1>(list_inputs,"/media/fantaosha/Documents/JHU/Summer 2015/results/boat_u.mat", "u"); return 0; }