//----------------------------------------------------------------------
void write_file(){
ofstream ofs("grid");
for(int i=GRID_SIZE_X-1;i>-1;i--){
for(int j=GRID_SIZE_Y-1;j>-1;j--){
ofs << grid[i][j] << " ";
}
ofs << endl;
}
ofstream ofs2("particle_number");
for(int i=GRID_SIZE_X-1;i>-1;i--){
for(int j=GRID_SIZE_Y-1;j>-1;j--){
ofs2 << particle_number[i][j] << " ";
}
ofs2 << endl;
}
ofstream ofs3("smoothing_particle");
for(int i=0;i<PARTICLES;i++){
ofs3 << x_smoothing[i] << " " << y_smoothing[i] << endl;
}
}
/**
* Description not yet available.
* \param
*/
void laplace_approximation_calculator::
do_newton_raphson_banded(function_minimizer * pfmin,double f_from_1,
int& no_converge_flag)
{
//quadratic_prior * tmpptr=quadratic_prior::ptr[0];
//cout << tmpptr << endl;
laplace_approximation_calculator::where_are_we_flag=2;
double maxg=fabs(evaluate_function(uhat,pfmin));
laplace_approximation_calculator::where_are_we_flag=0;
dvector uhat_old(1,usize);
for(int ii=1;ii<=num_nr_iters;ii++)
{
// test newton raphson
switch(hesstype)
{
case 3:
bHess->initialize();
break;
case 4:
Hess.initialize();
break;
default:
cerr << "Illegal value for hesstype here" << endl;
ad_exit(1);
}
grad.initialize();
//int check=initial_params::stddev_scale(scale,uhat);
//check=initial_params::stddev_curvscale(curv,uhat);
//max_separable_g=0.0;
sparse_count = 0;
step=get_newton_raphson_info_banded(pfmin);
//if (bHess)
// cout << "norm(*bHess) = " << norm(*bHess) << endl;
//cout << "norm(Hess) = " << norm(Hess) << endl;
//cout << grad << endl;
//check_pool_depths();
if (!initial_params::mc_phase)
cout << "Newton raphson " << ii << " ";
if (quadratic_prior::get_num_quadratic_prior()>0)
{
quadratic_prior::get_cHessian_contribution(Hess,xsize);
quadratic_prior::get_cgradient_contribution(grad,xsize);
}
int ierr=0;
if (hesstype==3)
{
if (use_dd_nr==0)
{
banded_lower_triangular_dmatrix bltd=choleski_decomp(*bHess,ierr);
if (ierr && no_converge_flag ==0)
{
no_converge_flag=1;
//break;
}
if (ierr)
{
double oldval;
evaluate_function(oldval,uhat,pfmin);
uhat=banded_calculations_trust_region_approach(uhat,pfmin);
}
else
{
if (dd_nr_flag==0)
{
dvector v=solve(bltd,grad);
step=-solve_trans(bltd,v);
//uhat_old=uhat;
uhat+=step;
}
else
{
#if defined(USE_DD_STUFF)
int n=grad.indexmax();
maxg=fabs(evaluate_function(uhat,pfmin));
uhat=dd_newton_raphson2(grad,*bHess,uhat);
#else
cerr << "high precision Newton Raphson not implemented" << endl;
ad_exit(1);
#endif
}
maxg=fabs(evaluate_function(uhat,pfmin));
if (f_from_1< pfmin->lapprox->fmc1.fbest)
{
uhat=banded_calculations_trust_region_approach(uhat,pfmin);
maxg=fabs(evaluate_function(uhat,pfmin));
}
}
}
else
{
cout << "error not used" << endl;
ad_exit(1);
/*
banded_symmetric_ddmatrix bHessdd=banded_symmetric_ddmatrix(*bHess);
ddvector gradd=ddvector(grad);
//banded_lower_triangular_ddmatrix bltdd=choleski_decomp(bHessdd,ierr);
if (ierr && no_converge_flag ==0)
{
no_converge_flag=1;
break;
}
if (ierr)
{
double oldval;
evaluate_function(oldval,uhat,pfmin);
uhat=banded_calculations_trust_region_approach(uhat,pfmin);
maxg=fabs(evaluate_function(uhat,pfmin));
}
else
{
ddvector v=solve(bHessdd,gradd);
step=-make_dvector(v);
//uhat_old=uhat;
uhat=make_dvector(ddvector(uhat)+step);
maxg=fabs(evaluate_function(uhat,pfmin));
if (f_from_1< pfmin->lapprox->fmc1.fbest)
{
uhat=banded_calculations_trust_region_approach(uhat,pfmin);
maxg=fabs(evaluate_function(uhat,pfmin));
}
}
*/
}
if (maxg < 1.e-13)
{
break;
}
}
else if (hesstype==4)
{
dvector step;
# if defined(USE_ATLAS)
if (!ad_comm::no_atlas_flag)
{
step=-atlas_solve_spd(Hess,grad,ierr);
}
else
{
dmatrix A=choleski_decomp_positive(Hess,ierr);
if (!ierr)
{
step=-solve(Hess,grad);
//step=-solve(A*trans(A),grad);
}
}
if (!ierr) break;
# else
if (sparse_hessian_flag)
{
//step=-solve(*sparse_triplet,Hess,grad,*sparse_symbolic);
dvector temp=solve(*sparse_triplet2,grad,*sparse_symbolic2,ierr);
if (ierr)
{
step=-temp;
}
else
{
cerr << "matrix not pos definite in sparse choleski" << endl;
pfmin->bad_step_flag=1;
int on;
int nopt;
if ((on=option_match(ad_comm::argc,ad_comm::argv,"-ieigvec",nopt))
>-1)
{
dmatrix M=make_dmatrix(*sparse_triplet2);
ofstream ofs3("inner-eigvectors");
ofs3 << "eigenvalues and eigenvectors " << endl;
dvector v=eigenvalues(M);
dmatrix ev=trans(eigenvectors(M));
ofs3 << "eigenvectors" << endl;
int i;
for (i=1;i<=ev.indexmax();i++)
{
ofs3 << setw(4) << i << " "
<< setshowpoint() << setw(14) << setprecision(10) << v(i)
<< " "
<< setshowpoint() << setw(14) << setprecision(10)
<< ev(i) << endl;
}
}
}
//cout << norm2(step-tmpstep) << endl;
//dvector step1=-solve(Hess,grad);
//cout << norm2(step-step1) << endl;
}
else
{
step=-solve(Hess,grad);
}
# endif
if (pmin->bad_step_flag)
break;
uhat_old=uhat;
uhat+=step;
double maxg_old=maxg;
maxg=fabs(evaluate_function(uhat,pfmin));
if (maxg>maxg_old)
{
uhat=uhat_old;
evaluate_function(uhat,pfmin);
break;
}
if (maxg < 1.e-13)
{
break;
}
}
if (sparse_hessian_flag==0)
{
for (int i=1;i<=usize;i++)
{
y(i+xsize)=uhat(i);
}
}
else
{
for (int i=1;i<=usize;i++)
{
value(y(i+xsize))=uhat(i);
}
}
}
}
/**
Symmetrize and invert the hessian
*/
void function_minimizer::hess_inv(void)
{
initial_params::set_inactive_only_random_effects();
int nvar=initial_params::nvarcalc(); // get the number of active parameters
independent_variables x(1,nvar);
initial_params::xinit(x); // get the initial values into the x vector
//double f;
dmatrix hess(1,nvar,1,nvar);
uistream ifs("admodel.hes");
int file_nvar = 0;
ifs >> file_nvar;
if (nvar != file_nvar)
{
cerr << "Number of active variables in file mod_hess.rpt is wrong"
<< endl;
}
for (int i = 1;i <= nvar; i++)
{
ifs >> hess(i);
if (!ifs)
{
cerr << "Error reading line " << i << " of the hessian"
<< " in routine hess_inv()" << endl;
exit(1);
}
}
int hybflag = 0;
ifs >> hybflag;
dvector sscale(1,nvar);
ifs >> sscale;
if (!ifs)
{
cerr << "Error reading sscale"
<< " in routine hess_inv()" << endl;
}
double maxerr=0.0;
for (int i = 1;i <= nvar; i++)
{
for (int j=1;j<i;j++)
{
double tmp=(hess(i,j)+hess(j,i))/2.;
double tmp1=fabs(hess(i,j)-hess(j,i));
tmp1/=(1.e-4+fabs(hess(i,j))+fabs(hess(j,i)));
if (tmp1>maxerr) maxerr=tmp1;
hess(i,j)=tmp;
hess(j,i)=tmp;
}
}
/*
if (maxerr>1.e-2)
{
cerr << "warning -- hessian aprroximation is poor" << endl;
}
*/
for (int i = 1;i <= nvar; i++)
{
int zero_switch=0;
for (int j=1;j<=nvar;j++)
{
if (hess(i,j)!=0.0)
{
zero_switch=1;
}
}
if (!zero_switch)
{
cerr << " Hessian is 0 in row " << i << endl;
cerr << " This means that the derivative if probably identically 0 "
" for this parameter" << endl;
}
}
int ssggnn;
ln_det(hess,ssggnn);
int on1=0;
{
ofstream ofs3((char*)(ad_comm::adprogram_name + adstring(".eva")));
{
dvector se=eigenvalues(hess);
ofs3 << setshowpoint() << setw(14) << setprecision(10)
<< "unsorted:\t" << se << endl;
se=sort(se);
ofs3 << setshowpoint() << setw(14) << setprecision(10)
<< "sorted:\t" << se << endl;
if (se(se.indexmin())<=0.0)
{
negative_eigenvalue_flag=1;
cout << "Warning -- Hessian does not appear to be"
" positive definite" << endl;
}
}
ivector negflags(0,hess.indexmax());
int num_negflags=0;
{
int on = option_match(ad_comm::argc,ad_comm::argv,"-eigvec");
on1=option_match(ad_comm::argc,ad_comm::argv,"-spmin");
if (on > -1 || on1 >-1 )
{
ofs3 << setshowpoint() << setw(14) << setprecision(10)
<< eigenvalues(hess) << endl;
dmatrix ev=trans(eigenvectors(hess));
ofs3 << setshowpoint() << setw(14) << setprecision(10)
<< ev << endl;
for (int i=1;i<=ev.indexmax();i++)
{
double lam=ev(i)*hess*ev(i);
ofs3 << setshowpoint() << setw(14) << setprecision(10)
<< lam << " " << ev(i)*ev(i) << endl;
if (lam<0.0)
{
num_negflags++;
negflags(num_negflags)=i;
}
}
if ( (on1>-1) && (num_negflags>0)) // we will try to get away from
{ // saddle point
negative_eigenvalue_flag=0;
spminflag=1;
if(negdirections)
{
delete negdirections;
}
negdirections = new dmatrix(1,num_negflags);
for (int i=1;i<=num_negflags;i++)
{
(*negdirections)(i)=ev(negflags(i));
}
}
int on2 = option_match(ad_comm::argc,ad_comm::argv,"-cross");
if (on2>-1)
{ // saddle point
dmatrix cross(1,ev.indexmax(),1,ev.indexmax());
for (int i = 1;i <= ev.indexmax(); i++)
{
for (int j=1;j<=ev.indexmax();j++)
{
cross(i,j)=ev(i)*ev(j);
}
}
ofs3 << endl << " e(i)*e(j) ";
ofs3 << endl << cross << endl;
}
}
}
if (spminflag==0)
{
if (num_negflags==0)
{
hess=inv(hess);
int on=0;
if ( (on=option_match(ad_comm::argc,ad_comm::argv,"-eigvec"))>-1)
{
int i;
ofs3 << "choleski decomp of correlation" << endl;
dmatrix ch=choleski_decomp(hess);
for (i=1;i<=ch.indexmax();i++)
ofs3 << ch(i)/norm(ch(i)) << endl;
ofs3 << "parameterization of choleski decomnp of correlation" << endl;
for (i=1;i<=ch.indexmax();i++)
{
dvector tmp=ch(i)/norm(ch(i));
ofs3 << tmp(1,i)/tmp(i) << endl;
}
}
}
}
}
if (spminflag==0)
{
if (on1<0)
{
for (int i = 1;i <= nvar; i++)
{
if (hess(i,i) <= 0.0)
{
hess_errorreport();
ad_exit(1);
}
}
}
{
adstring tmpstring="admodel.cov";
if (ad_comm::wd_flag)
tmpstring = ad_comm::adprogram_name + ".cov";
uostream ofs((char*)tmpstring);
ofs << nvar << hess;
ofs << gradient_structure::Hybrid_bounded_flag;
ofs << sscale;
}
}
}
void NEATRunner::runLoop()
{
//setting up signal handlers
(void)signal(SIGINT,signalhandler);
(void)signal(SIGTERM,signalhandler);
(void)signal(SIGKILL,signalhandler);
//
time_t startt,tmpt;long totaltime=0;
int countruns=0;
vector<double> beststate;
generations++;
Evaluator * bak = NULL;
if(nodes==0&&localFE==false){
localFE=true;
}else if(!localFE)
cerr << "running cluster code.." << endl;
writeRunfile(false,basefile,infoline,pid);
pop->fe = icb->fe;
stringstream sgfc; sgfc << sgf.str() << "-" << countruns << ".xml";
if(speciationGraph)
sg = new SpecGraph((int)pop->getMembers()->size(),generations,sgfc.str());
stringstream sCurrentGenomeFilesuffixless; sCurrentGenomeFilesuffixless << sCurrentGenomeFile.str();
stringstream sCurrentGenomeFilec; sCurrentGenomeFilec << sCurrentGenomeFile.str() << "-" << countruns;
stringstream sCurrentGenGenomeFilec; sCurrentGenGenomeFilec << sCurrentGenomeFilec.str();
stringstream sCurrentXMLGenomeFilec; sCurrentXMLGenomeFilec << sCurrentXMLGenomeFile.str() << "-" << countruns << ".xml";
stringstream sCurrentGraphFilec; sCurrentGraphFilec << sCurrentGraphFile.str() << "-" << countruns;
if(currentgraphf==NULL){
currentgraphf = new ofstream(sCurrentGraphFile.str().c_str());
}
ofstream ofs(sCurrentGenomeFilec.str().c_str());
ofstream ofs2(sCurrentGenomeFilesuffixless.str().c_str());
ofstream ofscurg(newestgenome.c_str());
bool slaveStop = false;
int * bestspecid = new int;
bool teststop = false;
int gc=0; double ftest = 0;
FitnessEvaluator * fe = ev->getFitnessEvaluator();
while(!stop){
if(coevo != NULL && pop->getGeneration() == (coevo->getStartGeneration()+1)){
cout << "doing coevo!?" << endl;
bak = ev;
ev = coevo;
}
teststop = false;
startt = time(0);
if(localFE){
ev->evaluate(pop->getMembers(),pop->getMembers()->size());
}else{
if(generations>0&&(pop->getGeneration()+2)==generations&&runs==(countruns+1))
slaveStop = true;
comm->outputPopulation(pop,nodes,coevo,mc,slaveStop); //stream the population out to nodes for evaluation
ev->evaluate(pop->getMembers(),mc);//sweet..
comm->readFitness(pop,mc); //read the corresponding returned fitness values
}
//checking and updating for the overall best phenotype.
//do population/species sorting and stat updating
pop->updateSpeciesStats();
pop->sortmembers();
pop->sortspecies();
avgf = pop->calcAvgFitness();
//keeping a copy of generation champ:
gbest = pop->getCopyOfCurrentBest();
setChamp(best,gbest,bestspecid);
if(ftest < pop->getMembers()->at(0)->getFitness()){
ftest = pop->getMembers()->at(0)->getFitness();
}
else if(ftest != 0 && ftest > pop->getMembers()->at(0)->getFitness()){
cout << "old genome:\n " << best->getGenome();
cout << "new genome:\n " << pop->getMembers()->at(0)->getGenome();
cout << "fitness went DOWN ftest: " << ftest << " pop->getMembers()->at(0)->getFitness(): " << pop->getMembers()->at(0)->getFitness() << endl;
Phenotype * p = pop->getMembers()->at(0);
vector<double> input = p->getNet()->getInput();
Phenotypes * testp = new Phenotypes();
testp->push_back(p);
testp->push_back(gbest);
for(unsigned int i=0;i<10;i++){
ev->evaluate(testp
, 1);
}
}
best->getGenome()->setSeed(rands);
sCurrentGenGenomeFilec.str("");
sCurrentGenGenomeFilec << sCurrentGenomeFilec.str();
sCurrentGenGenomeFilec << "-" << pop->getGeneration();
ofs.open(sCurrentGenomeFilec.str().c_str());
ofs << best->getGenome();
ofs.close();
ofs2.open(sCurrentGenomeFilesuffixless.str().c_str());
ofs2 << best->getGenome();
ofs2.close();
ofscurg.open(newestgenome.c_str());
ofscurg << best->getGenome();
ofscurg.close();
ofstream ofs3(sCurrentGenGenomeFilec.str().c_str());
ofs3 << best->getGenome();
ofs3.close();
// if(pop->getGeneration()%2==0){
// cerr << icb->fe->show(best);
// }
icb->best = best;
writenetwork(best,sCurrentXMLGenomeFilec.str());
if(speciationGraph)
sg->update(pop);
//writing stats to file
*currentgraphf << getStatString(pop,avgf);
currentgraphf->flush();
//updating smoothed graph data..
updateSmoothData(smoothdata,pop,avgf,countruns+1);
if(coevo!=NULL)
coevo->update(pop); // update the coevolution data..
tmpt = (time(0)-startt);
totaltime += tmpt;
cerr << (pop->getGeneration()+1) << ":"
<< " curmaxid: "<<pop->getMembers()->at(0)->getID()
<< "(" << pop->getMembers()->at(0)->getSpecies()->getID() << ")"
<< " curmax: " << pop->getMembers()->at(0)->getFitness()
<< " bestid: "<< best->getID()
<< "(" << *bestspecid << ")"
<< " bestfitness: "<< best->getFitness()
<< " maxfitness: " << pop->getHighestFitness()
<< " avgfitness: " << pop->calcAvgFitness()
<< " curmin: " << pop->getMembers()->at(pop->getMembers()->size()-1)->getFitness()
<< " species: " << pop->getSpecies()->size()
<< " (" << pop->spectarget << ") "
<< " size: " << pop->getMembers()->size()
<< " time: " << tmpt
<< " time/size: " << (double)tmpt/(double)pop->getMembers()->size() << endl;
//run the code that adjusts fitness according to species age and size..
//select the lucky ones for reprocicration..
sel->select(pop,sel->getySpeciesForElitism());
//do the mating
teststop = fe->stop(best);
rep->reproduce(pop);
if(((generations>0&&(pop->getGeneration()+1) ==generations) || teststop)
&& runs==(countruns+1)){ // stopconditions
if(teststop && comm!=NULL)
comm->outputPopulation(pop,nodes,coevo,mc,true);
gc += pop->getGeneration();
setChamp(sbest,best,bestspecid);
stop = true;
if(speciationGraph){
sg->writetofile();
delete sg;
}
}else{
if(generations>0&& ((pop->getGeneration()+1)==generations || teststop )){
gc += pop->getGeneration();
//generation run is over lets go on to the next run..
countruns++;
//keep superchamp across runs..
Phenotype * tmp = best;
best = NULL;
setChamp(sbest,tmp,bestspecid);
//reset population...
if(pop->spawn)
pop->resetSpawn();
else
pop->resetGenesis();
if(bak!=NULL){
ev = bak;
bak = NULL;
}
if(speciationGraph){
sg->writetofile();
delete sg;
sgfc.str(""); sgfc << sgf.str() << "-" << countruns << ".xml";
sg = new SpecGraph((int)pop->getMembers()->size(),generations,sgfc.str());
}
sCurrentGenomeFilec.str(""); sCurrentGenomeFilec << sCurrentGenomeFile.str() << "-" << countruns;
sCurrentXMLGenomeFilec.str(""); sCurrentXMLGenomeFilec << sCurrentXMLGenomeFile.str() << "-" << countruns << ".xml";
sCurrentGraphFilec.str(""); sCurrentGraphFilec << sCurrentGraphFile.str() << "-" << countruns;
ftest = 0;
delete currentgraphf; currentgraphf = new ofstream(sCurrentGraphFilec.str().c_str());
}
}
}
ofstream ofsuper(sFinalGenomeFile.c_str());
ofsuper << sbest->getGenome();
ofsuper.close();
cout << "final best fitness" << sbest->getFitness() << endl;
if(countruns>1){
cout << "avg gc: " << (double)gc/(double)(countruns+1) << endl;
}
delete sbest;
ofstream finalgraphf(finalgraphfile.c_str());
for(int i=0;i<generations-1;i++)
finalgraphf << smoothdata[i][0]/(double)runs << " "
<< smoothdata[i][1]/(double)runs << " "
<< smoothdata[i][2]/(double)runs << endl;
for(int i=0;i<generations-1;i++)
delete[] smoothdata[i];
delete[] smoothdata;
//close graph files..
finalgraphf.close();
currentgraphf->close();
delete currentgraphf;
delete bestspecid;
writeRunfile(true,basefile,infoline,pid);
}
int main()
{
//txtPostureData読み込み
std::ifstream ifs("GetPositionALL.txt");
std::string str;
int CountData = 0;
//postureDataを入れるvector確保
//std::vector<std::vector<double>>txtPosture(3, std::vector<double>(25));
std::vector<std::vector<std::vector<double>>> Posture(3, std::vector<std::vector<double>>(25, std::vector<double>(3000, 0)));//1595
std::vector< std::vector<double> > position( 25, std::vector<double>( 3 ) );
std::vector< std::vector<double> > PositionBase( 25, std::vector<double>( 3 ) );
std::vector< std::vector<double> > RotatePosition( 25, std::vector<double>( 3 ) );
std::vector< std::vector<double> > XRotatePosition( 25, std::vector<double>( 3 ) );
std::vector< std::vector<double> > YRotatePosition( 25, std::vector<double>( 3 ) );
std::vector< std::vector<double> > ZRotatePosition( 25, std::vector<double>( 3 ) );
std::ofstream ofs( "GetPositionALL.txt", std::ios::out | std::ios::app );
std::ofstream ofs2( "GetPositionArm.txt", std::ios::out | std::ios::app );
while (getline(ifs, str))
{
std::string tmp;
std::istringstream stream(str);
int CountXYZ = 0, CountPosture = 0;
while (getline(stream, tmp, '\t'))
{
//文字列から数字(double)に変換
std::istringstream is;
is.str(tmp);
double x;
is >> x;
//postureにtxtPostureDataを代入
Posture[CountXYZ][CountPosture][CountData] = x;
//std::cout <<"("<<CountXYZ<<":"<<CountPosture<<":"<<CountData<<")->"<< x << std::endl;
//countを取ってpostureに代入
CountXYZ++;
if (CountXYZ == 3)
{
CountPosture++;
CountXYZ = 0;
}
if (CountPosture == 25)
{
CountData++;
CountPosture = 0;
}
}
}
for ( int i = 0; i <= CountData; i++ )
{
//テキストベースでpositionを保存
for ( int count = 0; count <= 24; count++ )
{
for ( int PosCount = 0; PosCount <= 2; PosCount++ )
{
Posture[PosCount][count][i] = position[count][PosCount];
}
}
for ( int count = 0; count <= 24; count++ )
{
for ( int PosCount = 0; PosCount <= 2; PosCount++ )
{
PositionBase[count][PosCount] = position[count][PosCount] - position[0][PosCount];
}
}
//座標変換ユーザの角度
double crossX = ( PositionBase[8][1] * PositionBase[4][2] ) - ( PositionBase[8][2] * PositionBase[4][1] );
double crossY = ( PositionBase[8][2] * PositionBase[4][0] ) - ( PositionBase[8][0] * PositionBase[4][2] );
double crossZ = ( PositionBase[8][0] * PositionBase[4][1] ) - ( PositionBase[8][1] * PositionBase[4][0] );//外積
double nLength = sqrtf( ( crossX*crossX ) + ( crossY*crossY ) + ( crossZ*crossZ ) );//normalize
double nx = crossX / nLength;//1より大きかったらだめifブンツクレ
double ny = crossY / nLength;
double nz = crossZ / nLength;//面法線ベクトル
double nxx = crossX / sqrtf( ( crossX*crossX ) + ( crossY*crossY ) );
double nzz = crossZ / sqrtf( ( crossX*crossX ) + ( crossZ*crossZ ) );
/*
if ( nx >= 1 )nx = 1;
if ( ny >= 1 )ny = 1;
if ( nz >= 1 )nz = 1;
*/
double cosX = nx;//nx / nLength;
double cosY = ny;//ny / nLength;
double cosZ = nz;//nz / nLength;//面の傾き(cos)？
double cosXX = nxx;//x / √x^2+y^2
double cosZZ = nzz;
double cosZZZ = ( PositionBase[4][1] ) / sqrtf( ( PositionBase[4][1] * PositionBase[4][1] ) + ( PositionBase[4][2] * PositionBase[4][2] ) );
double sinX = crossZ / nLength;//sqrtf( 1 - ( cosX*cosX ) );//マイナスになってる?正じゃないといけない
double sinY = sqrtf( ( crossX*crossX ) + ( crossY*crossY ) ) / nLength;//sqrtf( 1 - ( cosY*cosY ) );
double sinZ = crossY / nLength;//sqrtf( 1 - ( cosZ*cosZ ) );//面の傾き(sin)?
double sinXX = sqrtf( 1 - ( cosXX*cosXX ) );
double sinZZ = crossX / sqrtf( ( crossX*crossX ) + ( crossZ*crossZ ) );
double sinZZZ = ( PositionBase[4][2] ) / sqrtf( ( PositionBase[4][1] * PositionBase[4][1] ) + ( PositionBase[4][2] * PositionBase[4][2] ) );
//各軸回転行列
std::vector< std::vector<double> > vectorRx( 3, std::vector<double>( 3 ) );
std::vector< std::vector<double> > vectorRy( 3, std::vector<double>( 3 ) );
std::vector< std::vector<double> > vectorRz( 3, std::vector<double>( 3 ) );
std::vector< std::vector<double> > vectorR( 3, std::vector<double>( 3 ) );
vectorRx[0][0] = 1;
vectorRx[0][1] = 0;
vectorRx[0][2] = 0;
vectorRx[1][0] = 0;
vectorRx[1][1] = sinY;//cosX(90"-")
vectorRx[1][2] = -cosY;//-sinX
vectorRx[2][0] = 0;
vectorRx[2][1] = cosY;//sinX
vectorRx[2][2] = sinY;//cosX//X軸周り回転行列
vectorRy[0][0] = cosZZ;//cosZ;//cosY
vectorRy[0][1] = 0;
vectorRy[0][2] = -sinZZ;//sinZ;//sinY
vectorRy[1][0] = 0;
vectorRy[1][1] = 1;
vectorRy[1][2] = 0;
vectorRy[2][0] = sinZZ;//-sinZ;//sinY
vectorRy[2][1] = 0;
vectorRy[2][2] = cosZZ;//cosY//Y軸周り回転行列
vectorRz[0][0] = cosX;//cosZ
vectorRz[0][1] = -sinX;//-sinZ
vectorRz[0][2] = 0;
vectorRz[1][0] = sinX;//sinZ
vectorRz[1][1] = cosX;//cosZ
vectorRz[1][2] = 0;
vectorRz[2][0] = 0;
vectorRz[2][1] = 0;
vectorRz[2][2] = 1;//Z軸周り回転行列
//行列計算->Y軸周りの計算
for ( int count = 0; count <= 24; count++ )
{
for ( int PosCount = 0; PosCount <= 2; PosCount++ )
{
//cout << position[count][PosCount] << endl;
//ofs << position[count][PosCount] << "\t";
if ( PosCount == 0 )
{
for ( int i = 0; i <= 2; i++ )
{
YRotatePosition[count][PosCount] += PositionBase[count][i] * vectorRy[0][i];
}
}
else if ( PosCount == 1 )
{
for ( int i = 0; i <= 2; i++ )
{
YRotatePosition[count][PosCount] += PositionBase[count][i] * vectorRy[1][i];
}
}
else if ( PosCount == 2 )
{
for ( int i = 0; i <= 2; i++ )
{
YRotatePosition[count][PosCount] += PositionBase[count][i] * vectorRy[2][i];
}
}
//RotatePosition[count][PosCount] = PositionBase[count][PosCount];
}
}
//行列計算->X軸周りの計算
for ( int count = 0; count <= 24; count++ )
{
for ( int PosCount = 0; PosCount <= 2; PosCount++ )
{
if ( PosCount == 0 )
{
for ( int i = 0; i <= 2; i++ )
{
XRotatePosition[count][PosCount] += YRotatePosition[count][i] * vectorRx[0][i];
}
}
else if ( PosCount == 1 )
{
for ( int i = 0; i <= 2; i++ )
{
XRotatePosition[count][PosCount] += YRotatePosition[count][i] * vectorRx[1][i];
}
}
else if ( PosCount == 2 )
{
for ( int i = 0; i <= 2; i++ )
{
XRotatePosition[count][PosCount] += YRotatePosition[count][i] * vectorRx[2][i];
}
}
}
}
std::vector<double> shoulder( 3 );
shoulder[0] = /*PositionBase[4][0] - PositionBase[8][0];*/XRotatePosition[4][0] - XRotatePosition[8][0];
shoulder[1] = /*PositionBase[4][1] - PositionBase[8][1]; */XRotatePosition[4][1] - XRotatePosition[8][1];
shoulder[2] = /*PositionBase[4][2] - PositionBase[8][2];*/XRotatePosition[4][2] - XRotatePosition[8][2];
double cosXshoulder = shoulder[0] / sqrtf( ( shoulder[0] * shoulder[0] ) + ( shoulder[1] * shoulder[1] ) + ( shoulder[2] * shoulder[2] ) );
double sinXshoulder = shoulder[1] / sqrtf( ( shoulder[0] * shoulder[0] ) + ( shoulder[1] * shoulder[1] ) + ( shoulder[2] * shoulder[2] ) );
vectorRz[0][0] = cosXshoulder;//cosZ
vectorRz[0][1] = sinXshoulder;//-sinZ
vectorRz[0][2] = 0;
vectorRz[1][0] = -sinXshoulder;//sinZ
vectorRz[1][1] = cosXshoulder;//cosZ
vectorRz[1][2] = 0;
vectorRz[2][0] = 0;
vectorRz[2][1] = 0;
vectorRz[2][2] = 1;//Z軸周り回転行列
//行列計算->Z軸周りの計算
for ( int count = 0; count <= 24; count++ )
{
for ( int PosCount = 0; PosCount <= 2; PosCount++ )
{
if ( PosCount == 0 )
{
for ( int i = 0; i <= 2; i++ )
{
ZRotatePosition[count][PosCount] += XRotatePosition[count][i] * vectorRz[0][i];
}
}
else if ( PosCount == 1 )
{
for ( int i = 0; i <= 2; i++ )
{
ZRotatePosition[count][PosCount] += XRotatePosition[count][i] * vectorRz[1][i];
}
}
else if ( PosCount == 2 )
{
for ( int i = 0; i <= 2; i++ )
{
ZRotatePosition[count][PosCount] += XRotatePosition[count][i] * vectorRz[2][i];
}
}
}
}
//今回はz軸回転->y軸回転->z軸回転順に回転行列を計算した原点は骨格の腰中央点
RotatePosition = ZRotatePosition;
//どの回転結果を表示するか->以下RotatePositionを表示
//座標変換後のpostureをテキストファイルに保存
std::ofstream ofs1( "RotatePostureALL.txt", std::ios::out | std::ios::app );
for ( int count = 0; count <= 24; count++ )
{
for ( int PosCount = 0; PosCount <= 2; PosCount++ )
{
//cout << position[count][PosCount] << endl;
ofs1 << RotatePosition[count][PosCount] * 80000 << "\t";
}
if ( count == 24 )
{
ofs1 << "\n";
}
}
std::ofstream ofs3( "RotatePostureArm.txt", std::ios::out | std::ios::app );
for ( int count = 0; count <= 24; count++ )
{
if ( count == 8 || count == 9 || count == 11 )
{
ofs3 << RotatePosition[count][0] * 80000 << "\t" << -RotatePosition[count][1] * 80000 << "\t" << RotatePosition[count][2] * 80000 << "\t";
}
else
{
ofs3 << RotatePosition[count][0] * 80000 << "\t" << -RotatePosition[count][1] * 80000 << "\t" << RotatePosition[count][2] * 80000 << "\t";
}
if ( count == 24 )
{
ofs3 << "\n";
}
}
}
return 0;
}
int UHD_SAFE_MAIN(int argc, char *argv[]){
if (uhd::set_thread_priority_safe(1,true)) {
std::cout << "set priority went well " << std::endl;
};
//variables to be set by po
std::string args;
double seconds_in_future;
size_t total_num_samps;
double tx_rate, freq, LOoffset;
float gain;
bool demoMode, use_8bits;
bool use_external_10MHz;
std::string filename;
uhd::tx_streamer::sptr tx_stream;
uhd::device_addr_t dev_addr;
uhd::usrp::multi_usrp::sptr dev;
uhd::stream_args_t stream_args;
//setup the program options
po::options_description desc("Allowed options");
desc.add_options()
("help", "help message")
("args", po::value<std::string>(&args)->default_value(""), "simple uhd device address args")
("secs", po::value<double>(&seconds_in_future)->default_value(3), "number of seconds in the future to transmit")
("nsamps", po::value<size_t>(&total_num_samps)->default_value(37028), "total number of samples to transmit")//9428
("txrate", po::value<double>(&tx_rate)->default_value(100e6/4), "rate of outgoing samples")
("freq", po::value<double>(&freq)->default_value(70e6), "rf center frequency in Hz")
("LOoffset", po::value<double>(&LOoffset)->default_value(0), "Offset between main LO and center frequency")
("demoMode",po::value<bool>(&demoMode)->default_value(true), "demo mode")
("10MHz",po::value<bool>(&use_external_10MHz)->default_value(false),
"external 10MHz on 'REF CLOCK' connector (true=1=yes)")
("filename",po::value<std::string>(&filename)->default_value("codedData.dat"), "input filename")
("gain",po::value<float>(&gain)->default_value(0), "gain of transmitter(0-13) ")
("8bits",po::value<bool>(&use_8bits)->default_value(false), "Use eight bits/sample to increase bandwidth")
;
po::variables_map vm;
po::store(po::parse_command_line(argc, argv, desc), vm);
po::notify(vm);
//print the help message
if (vm.count("help")){
std::cout << boost::format("tx %s") % desc << std::endl;
return ~0;
}
///////////////////////////////////////////////////////////////// START PROCESSING /////////////////////////////////////////////////////////////////////
std::complex<int16_t> *buffer0;
buffer0 = new std::complex<int16_t>[total_num_samps];
std::complex<int16_t> *buffer1;
buffer1 = new std::complex<int16_t>[total_num_samps];
std::complex<int16_t> *buffer2;
buffer2 = new std::complex<int16_t>[total_num_samps];
std::complex<int16_t> *buffer3;
buffer3 = new std::complex<int16_t>[total_num_samps];
std::complex<int16_t> *buffer4;
buffer4 = new std::complex<int16_t>[total_num_samps];
int16_t *aux0;
aux0 = new int16_t[2*total_num_samps];
int16_t *aux1;
aux1 = new int16_t[2*total_num_samps];
int16_t *aux2;
aux2 = new int16_t[2*total_num_samps];
int16_t *aux3;
aux3 = new int16_t[2*total_num_samps];
int16_t *aux4;
aux4 = new int16_t[2*total_num_samps];
//generate the picture as grayscale
int r=system("octave image_transmition.m &");
if(r){
std::cout<<" loading picture - check!\n";
}
int nPicRaw = 16384;//size of the image in grayscale 128*128
double nBinPac = 27200;//size of binary data in one packet
//loading picture as grayscale
int16_t pictureRaw[nPicRaw];
std::ifstream ifs( "data_toSend.dat", std::ifstream::in );
ifs.read((char * )pictureRaw,nPicRaw*sizeof(int16_t));
ifs.close();
//converting grayscale to binary and XOR with pseudonoise
itpp::bvec picBinInter = prepairPic(pictureRaw,nPicRaw);//transforms grayscale in binary data
//cutting the large binary data into 5 packets
bvec dataBinTmp0;
dataBinTmp0.ins(dataBinTmp0.length(),picBinInter.get(0,(nBinPac-1)));
bvec dataBinTmp1;
dataBinTmp1.ins(dataBinTmp1.length(),picBinInter.get(nBinPac,(2*nBinPac-1)));
bvec dataBinTmp2;
dataBinTmp2.ins(dataBinTmp2.length(),picBinInter.get(2*nBinPac,(3*nBinPac-1)));
bvec dataBinTmp3;
dataBinTmp3.ins(dataBinTmp3.length(),picBinInter.get(3*nBinPac,(4*nBinPac-1)));
bvec dataBinTmp4;
dataBinTmp4.ins(dataBinTmp4.length(),picBinInter.get(4*nBinPac,picBinInter.length()));
dataBinTmp4.ins(dataBinTmp4.length(),randb(nBinPac-dataBinTmp4.length())); //filling the last packet with random data
//saving the binary picture
it_file my_file("binPicture.it");
my_file << Name("picBinInter") << picBinInter;
my_file.flush();
my_file.close();
//processing each packet
tx_funct(aux0,dataBinTmp0,dataBinTmp0.length());
tx_funct(aux1,dataBinTmp1,dataBinTmp1.length());
tx_funct(aux2,dataBinTmp2,dataBinTmp2.length());
tx_funct(aux3,dataBinTmp3,dataBinTmp3.length());
tx_funct(aux4,dataBinTmp4,dataBinTmp4.length());
//filling the output buffer
for(int i=0,count1=0;i<(int)(2*total_num_samps);i=i+2){
buffer0[count1]=std::complex<short>(aux0[i],aux0[i+1]);
buffer1[count1]=std::complex<short>(aux1[i],aux1[i+1]);
buffer2[count1]=std::complex<short>(aux2[i],aux2[i+1]);
buffer3[count1]=std::complex<short>(aux3[i],aux3[i+1]);
buffer4[count1]=std::complex<short>(aux4[i],aux4[i+1]);
count1++;
}
// Save data to file to check what was sent
std::ofstream ofs( "sent0.dat" , std::ifstream::out );
ofs.write((char * ) buffer0, 2*total_num_samps*sizeof(int16_t));
ofs.flush();
ofs.close();
// Save data to file to check what was sent
std::ofstream ofs1( "sent1.dat" , std::ifstream::out );
ofs1.write((char * ) buffer1, 2*total_num_samps*sizeof(int16_t));
ofs1.flush();
ofs1.close();
// Save data to file to check what was sent
std::ofstream ofs2( "sent2.dat" , std::ifstream::out );
ofs2.write((char * ) buffer2, 2*total_num_samps*sizeof(int16_t));
ofs2.flush();
ofs2.close();
// Save data to file to check what was sent
std::ofstream ofs3( "sent3.dat" , std::ifstream::out );
ofs3.write((char * ) buffer3, 2*total_num_samps*sizeof(int16_t));
ofs3.flush();
ofs3.close();
// Save data to file to check what was sent
std::ofstream ofs4( "sent4.dat" , std::ifstream::out );
ofs4.write((char * ) buffer4, 2*total_num_samps*sizeof(int16_t));
ofs4.flush();
ofs4.close();
//Conjugate!!!
for(int i=0; i<(int)(total_num_samps);i++){
buffer0[i]=std::conj(buffer0[i]);
buffer1[i]=std::conj(buffer1[i]);
buffer2[i]=std::conj(buffer2[i]);
buffer3[i]=std::conj(buffer3[i]);
buffer4[i]=std::conj(buffer4[i]);
}
std::cout << " ----------- " << std::endl;
std::cout<<" Conjugated! \n";
std::cout << " ----------- " << std::endl;
///////////////////////////////////////////////////////////////// END PROCESSING /////////////////////////////////////////////////////////////////////
//create a usrp device and streamer
dev_addr["addr0"]="192.168.10.2";
dev = uhd::usrp::multi_usrp::make(dev_addr);
// Internal variables
uhd::clock_config_t my_clock_config;
if (!demoMode) {
dev->set_time_source("external");
};
if (use_external_10MHz) {
dev->set_clock_source("external");
}
else {
dev->set_clock_source("internal");
};
uhd::usrp::dboard_iface::sptr db_iface;
db_iface=dev->get_tx_dboard_iface(0);
board_60GHz_TX my_60GHz_TX(db_iface); //60GHz
my_60GHz_TX.set_gain(gain); // 60GHz
uhd::tune_result_t tr;
uhd::tune_request_t trq(freq,LOoffset); //std::min(tx_rate,10e6));
tr=dev->set_tx_freq(trq,0);
//dev->set_tx_gain(gain);
std::cout << tr.to_pp_string() << "\n";
stream_args.cpu_format="sc16";
if (use_8bits)
stream_args.otw_format="sc8";
else
stream_args.otw_format="sc16";
tx_stream=dev->get_tx_stream(stream_args);
//set properties on the device
std::cout << boost::format("Setting TX Rate: %f Msps...") % (tx_rate/1e6) << std::endl;
dev->set_tx_rate(tx_rate);
std::cout << boost::format("Actual TX Rate: %f Msps...") % (dev->get_tx_rate()/1e6) << std::endl;
std::cout << boost::format("Setting device timestamp to 0...") << std::endl;
uhd::tx_metadata_t md;
if(demoMode){
dev->set_time_now(uhd::time_spec_t(0.0));
md.start_of_burst = true;
md.end_of_burst = false;
md.has_time_spec = false;
md.time_spec = uhd::time_spec_t(seconds_in_future);
tx_stream->send(buffer0,total_num_samps,md,60);
tx_stream->send(buffer1,total_num_samps,md,3);
tx_stream->send(buffer2,total_num_samps,md,3);
tx_stream->send(buffer3,total_num_samps,md,3);
tx_stream->send(buffer4,total_num_samps,md,3);
tx_stream->send(buffer4,total_num_samps,md,3);
md.start_of_burst = false;
std::cout << " " << std::endl;
std::cout<< "picture transmitted once!" << std::endl;
std::cout << " " << std::endl;
int f=system("octave toMatlab.m");
if(f){
std::cout << " Programm Paused - Press Any Key To leave! " << std::endl;
}
}
else
{
dev->set_time_now(uhd::time_spec_t(0.0));
md.start_of_burst = true;
md.end_of_burst = false;
md.has_time_spec = false;
md.time_spec = uhd::time_spec_t(seconds_in_future);
tx_stream->send(buffer0,total_num_samps,md,60);
tx_stream->send(buffer1,total_num_samps,md,3);
tx_stream->send(buffer2,total_num_samps,md,3);
tx_stream->send(buffer3,total_num_samps,md,3);
tx_stream->send(buffer4,total_num_samps,md,3);
tx_stream->send(buffer4,total_num_samps,md,3);
md.start_of_burst = false;
std::cout << " " << std::endl;
std::cout<< "picture transmitted once!" << std::endl;
std::cout << " " << std::endl;
};
//finished
std::cout << std::endl << "Done!" << std::endl << std::endl;
return 0;
}
void IDPNagato::sinc()
{
//char fname_energy[64]; sprintf(fname_energy, ftemp_energy, 0);
//ofstream ofs_energy(fname_energy);
//ofstream ofs_r1_st(fname_r1_sinc_st), ofs_r1_ev(fname_r1_sinc_ev);
//ofstream ofs_r2_st(fname_r2_sinc_st), ofs_r2_ev(fname_r2_sinc_ev);
//ofstream ofs_endeffector(fname_sinc_endeffector);
ofstream ofs_energy(fname_lst[eENERGY][eSinc][eName]);
ofstream ofs_r1_st(fname_lst[eGRAPH_ST_R1][eSinc][eName]);
ofstream ofs_r1_ev(fname_lst[eGRAPH_EV_R1][eSinc][eName]);
ofstream ofs_r2_st(fname_lst[eGRAPH_ST_R2][eSinc][eName]);
ofstream ofs_r2_ev(fname_lst[eGRAPH_EV_R2][eSinc][eName]);
ofstream ofs_endeffector(fname_lst[eGRAPH_ENDEFFECTOR][eSinc][eName]);
// FILE* fp_energy = fopen(fname_energy,"w");
//printf("line=%d %s\n", __LINE__, fname_r1_sinc_st);
//printf("line=%d %s\n", __LINE__, fname_r2_sinc_st);
//printf("line=%d %s\n", __LINE__, fname_r1_sinc_ev);
printf("line=%d %s\n", __LINE__
, fname_lst[eGRAPH_ST_R1][eSinc][eName]);
printf("line=%d %s\n", __LINE__
, fname_lst[eGRAPH_EV_R1][eSinc][eName]);
printf("line=%d %s\n", __LINE__
, fname_lst[eGRAPH_ST_R2][eSinc][eName]);
printf("line=%d %s\n", __LINE__
, fname_lst[eGRAPH_EV_R2][eSinc][eName]);
sleep(2);
double t = 0;
for(nt = 0; nt <= static_cast<int>(Ts/dt); ++nt )
{
graph_standard(ofs_r1_st, t,
r1.th, r1.thv, r1.tha,
e1.ev, e1.ia, e1.tau,
e1.energy, e1.ev*e1.ia);
graph_volt_constituent(ofs_r1_ev, t, e1.ev,
b1*r1.thv, b2*r1.tha, b3*e1.tau);
graph_standard(ofs_r2_st, t,
r2.th, r2.thv, r2.tha,
e2.ev, e2.ia, e2.tau,
e2.energy, e2.ev*e2.ia);
graph_volt_constituent(ofs_r2_ev, t, e2.ev,
b1*r2.thv, b2*r2.tha, b3*e2.tau);
graph_endeffector(ofs_endeffector, t, v1, r3);
t = t + dt;
v1.th = sc.sin_th1(t);
v1.thv = sc.sin_th1v(t);
v1.tha = sc.sin_th1a(t);
enemin2 = En();
// if (nt < nt11){ enemin2 = En1(); }
// if (nt >= nt11 && nt < nt22){ enemin2 = En2(); }
// if (nt >= nt22){ enemin2 = En3(); }
ofs_energy << t <<" "<< enemin2 << endl;
// fprintf(fp_energy, "%8.4lf %8.4lf\n", t, enemin2 );
if (nt % 10 == 0)
{
//char fname[4][64];
//sprintf(fname[1], ftemp_sinc_e[1], nt);
//sprintf(fname[2], ftemp_sinc_e[2], nt);
//sprintf(fname[3], ftemp_sinc_e[3], nt);
sprintf(fname_lst[eZU_E1][eSinc][eName],
fname_lst[eZU_E1][eSinc][eTemp], nt);
sprintf(fname_lst[eZU_E2][eSinc][eName],
fname_lst[eZU_E2][eSinc][eTemp], nt);
sprintf(fname_lst[eZU_E3][eSinc][eName],
fname_lst[eZU_E3][eSinc][eTemp], nt);
ofstream ofs1(fname_lst[eZU_E1][eSinc][eName]);
ofstream ofs2(fname_lst[eZU_E2][eSinc][eName]);
ofstream ofs3(fname_lst[eZU_E3][eSinc][eName]);
//zu( ofs1 ); all in
//zu( ofs1, ofs2 );
zu( ofs1, ofs2, ofs3 );// !not implement
}
// output volt ampere field
// plot("");
}
//assert(false);
}
