Esempio n. 1
0
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;
}