bool available_poly(void)
{ size_t size = 10;
size_t repeat = 1;
CppAD::vector<double> a(size), z(1), ddp(1);
return link_poly(size, repeat, a, z, ddp);
}
bool correct_poly(bool is_package_double)
{ size_t size = 10;
size_t repeat = 1;
CppAD::vector<double> a(size), z(1), ddp(1);
link_poly(size, repeat, a, z, ddp);
size_t k;
double check;
if( is_package_double )
k = 0;
else k = 2;
check = CppAD::Poly(k, a, z[0]);
bool ok = CppAD::NearEqual(check, ddp[0], 1e-10, 1e-10);
return ok;
}
DTC::LineupList* Channel::GetDDLineupList( const QString &sSource,
const QString &sUserId,
const QString &sPassword )
{
int source = 0;
DTC::LineupList *pLineups = new DTC::LineupList();
if (sSource.toLower() == "schedulesdirect1" ||
sSource.toLower() == "schedulesdirect" ||
sSource.isEmpty())
{
source = 1;
DataDirectProcessor ddp(source, sUserId, sPassword);
if (!ddp.GrabLineupsOnly())
{
throw( QString("Unable to grab lineups. Check info."));
}
const DDLineupList lineups = ddp.GetLineups();
DDLineupList::const_iterator it;
for (it = lineups.begin(); it != lineups.end(); ++it)
{
DTC::Lineup *pLineup = pLineups->AddNewLineup();
pLineup->setLineupId((*it).lineupid);
pLineup->setName((*it).name);
pLineup->setDisplayName((*it).displayname);
pLineup->setType((*it).type);
pLineup->setPostal((*it).postal);
pLineup->setDevice((*it).device);
}
}
return pLineups;
}
int main(int argc, char** argv)
{
quadrotor::State xref;
xref.R=SO3::exp((Vec3()<<0,0,0).finished());
xref.xq=Vec3::Zero();
xref.omega=Vec3::Zero();
xref.vq=Vec3::Zero();
std::vector<quadrotor::State> xrefs(4000,xref);
std::vector<quadrotor::U> us(4000, 1*(Vec4()<<1,0,1,0).finished());
std::srand((unsigned int) time(0));
// Set up quadrotor parameters
double m=0.6; // mass of the quadrotor
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;
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)*=2;
Mf.block(3,3,3,3)*=6;
Mf.block(6,6,6,6)=Mat6::Identity()*2;
Mat12 M=Mf/2;
// M.block(6,6,6,6)*=4;
Mat4 R=Mat4::Identity()*2;
DDP<quadrotor>::Params params(M,R,Mf);
// Set up initial state
quadrotor::State x0=xrefs[0];
x0.xq-=(Vec3::Random()).normalized()*10;
x0.R*=SO3::exp(Vec3::Random().normalized()*3);
x0.omega=Vec3::Random().normalized()*1;
x0.vq-=Vec3::Random().normalized()*1;
// Set up simulator
double dt=0.01;
size_t num=200;
Sim<quadrotor> sim(sys,dt);
sim.init(x0,num);
Vec4 umin=-Vec4::Ones()*3;
Vec4 umax=Vec4::Ones()*3;
DDP<quadrotor> ddp(sys,dt);
int sn=1;
int itr_max=200;
for(int i=0;i<1000;i+=sn)
{
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.block(3,0,3,1).norm()<0.05 && error.head(3).norm()<0.4)
break;
double err=error.head(6).norm();
if(err<2.5)
itr_max=2000;
else
itr_max=1000;
if(i==0)
itr_max=10000;
if(err<2)
{
M=2.5*Mat12::Identity()*10;
M.block(0,0,3,3)*=2;
M.block(3,3,3,3)*=6;
M.block(6,6,6,6)=Mat6::Identity()*std::max(0.01, 1.25*err*err);
Mf=5*Mat12::Identity()*100;
Mf.block(0,0,3,3)*=2;
Mf.block(3,3,3,3)*=6;
Mf.block(6,6,6,6)=Mat6::Identity()*0.1;
R=Mat4::Identity()*std::max(0.001, 0.5*err*err);
}
else
{
Mf=5*Mat12::Identity()*10;
Mf.block(0,0,3,3)*=4;
Mf.block(3,3,3,3)*=6;
Mf.block(6,6,6,6)=Mat6::Identity()*5;
M=Mf;
R=Mat4::Identity()*1;
}
if(i%150==0 || (i%50==0 && us[0].norm()+us[10].norm()<0.5))
{
Vec4 u0=(Vec4()<<0.62,0.52,0.42,0.32).finished();
for(int i=0;i<20;i++)
us[i]=u0;
ddp.init(sim.get_state(), us, xrefs, params, umin, umax, 150);
ddp.iterate(40000,us);
}
else
{
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());
}
}
std::string path="/media/fantaosha/Documents/Northwestern/2016/Spring/Machine-Learning/Project/Data/quadrotor_ddp_1";
if (argc>=2)
{
path.append("_");
path.append(argv[1]);
}
path.append(".mat");
sim.save(path);
}
void geometry::print_ascii(ostream& ost)
{
INT v, b, i, j, a, l;
v = X().s_m();
b = X().s_n();
if (v > 0 && b > 0) {
ost << "GEOMETRY " << number() << " " << label().s() << endl;
ost << "v=" << v << " b=" << b << endl;
if (f_incidence_matrix()) {
ost << "INCIDENCE_MATRIX" << endl;
for (i = 0; i < v; i++) {
for (j = 0; j < b; j++) {
a = X().s_iji(i, j);
if (a)
ost << "X";
else
ost << ".";
}
ost << endl;
}
}
else {
ost << "INTEGER_MATRIX" << endl;
ost << X();
}
ost << "LABELLING_OF_POINTS" << endl;
point_labels().print_unformatted(ost);
ost << endl;
ost << "LABELLING_OF_BLOCKS" << endl;
block_labels().print_unformatted(ost);
ost << endl;
if (f_row_decomp()) {
ost << "DECOMPOSITION_OF_POINTS" << endl;
ost << row_decomp().s_l() << " ";
row_decomp().print_unformatted(ost);
ost << endl;
}
if (f_col_decomp()) {
ost << "DECOMPOSITION_OF_BLOCKS" << endl;
ost << col_decomp().s_l() << " ";
col_decomp().print_unformatted(ost);
ost << endl;
}
if (f_ddp()) {
ost << "DDP" << endl;
ddp().print_unformatted(ost);
ost << endl;
}
if (f_ddb()) {
ost << "DDB" << endl;
ddb().print_unformatted(ost);
ost << endl;
}
if (f_canonical_labelling_points()) {
ost << "CANONICAL_LABELLING_OF_POINTS" << endl;
canonical_labelling_points().print_unformatted(ost);
ost << endl;
}
if (f_canonical_labelling_blocks()) {
ost << "CANONICAL_LABELLING_OF_BLOCKS" << endl;
canonical_labelling_blocks().print_unformatted(ost);
ost << endl;
}
if (f_aut_gens()) {
ost << "AUT_GENS (group order " << ago() << ")" << endl;
l = aut_gens().s_l();
ost << l << endl;
for (i = 0; i < l; i++) {
INT ll = aut_gens().s_i(i).as_permutation().s_l();
for (j = 0; j < ll; j++) {
ost << aut_gens().s_i(i).as_permutation()[j] << " ";
}
// aut_gens().s_i(i).as_permutation().print_unformatted(ost);
ost << endl;
}
}
ost << "END" << endl << endl;
}
else {
ost << "#GEOMETRY (not allocated)" << endl;
}
}
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");
}
void speed_poly(size_t size, size_t repeat)
{ CppAD::vector<double> a(size), z(1), ddp(1);
link_poly(size, repeat, a, z, ddp);
return;
}
