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;
}
