void collectLocusData(GList<GenomicSeqData>& ref_data) {
int locus_num=0;
for (int g=0;g<ref_data.Count();g++) {
GenomicSeqData* gdata=ref_data[g];
for (int l=0;l<gdata->loci.Count();l++) {
GffLocus& loc=*(gdata->loci[l]);
GHash<int> gnames(true); //gene names in this locus
GHash<int> geneids(true); //Entrez GeneID: numbers
for (int i=0;i<loc.rnas.Count();i++) {
GffObj& t=*(loc.rnas[i]);
GStr gname(t.getGeneName());
if (!gname.is_empty()) {
gname.upper();
int* prevg=gnames.Find(gname.chars());
if (prevg!=NULL) (*prevg)++;
else gnames.Add(gname, new int(1));
}
//parse GeneID xrefs, if any:
GStr xrefs(t.getAttr("xrefs"));
if (!xrefs.is_empty()) {
xrefs.startTokenize(",");
GStr token;
while (xrefs.nextToken(token)) {
token.upper();
if (token.startsWith("GENEID:")) {
token.cut(0,token.index(':')+1);
int* prevg=geneids.Find(token.chars());
if (prevg!=NULL) (*prevg)++;
else geneids.Add(token, new int(1));
}
} //for each xref
} //xrefs parsing
}//for each transcript
locus_num++;
loc.locus_num=locus_num;
if (gnames.Count()>0) { //collect all gene names associated to this locus
gnames.startIterate();
int* gfreq=NULL;
char* key=NULL;
while ((gfreq=gnames.NextData(key))!=NULL) {
loc.gene_names.AddIfNew(new CGeneSym(key,*gfreq));
}
} //added collected gene_names
if (loc.gene_ids.Count()>0) { //collect all GeneIDs names associated to this locus
geneids.startIterate();
int* gfreq=NULL;
char* key=NULL;
while ((gfreq=geneids.NextData(key))!=NULL) {
loc.gene_ids.AddIfNew(new CGeneSym(key,*gfreq));
}
}
} //for each locus
}//for each genomic sequence
}
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);
}
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");
}
