int arms_simple (int ninit, double *xl, double *xr,
double (*myfunc)(double x, void *mydata), void *mydata,
int dometrop, double *xprev, double *xsamp)
/* adaptive rejection metropolis sampling - simplified argument list */
/* ninit : number of starting values to be used */
/* *xl : left bound */
/* *xr : right bound */
/* *myfunc : function to evaluate log-density */
/* *mydata : data required by *myfunc */
/* dometrop : whether metropolis step is required */
/* *xprev : current value from markov chain */
/* *xsamp : to store sampled value */
{
double xinit[ninit], convex=1.0, qcent, xcent;
int err, i, npoint=100, nsamp=1, ncent=0, neval;
/* set up starting values */
for(i=0; i<ninit; i++){
xinit[i] = *xl + (i + 1.0) * (*xr - *xl)/(ninit + 1.0);
}
err = arms(xinit,ninit,xl,xr,myfunc,mydata,&convex,npoint,dometrop,xprev,xsamp,
nsamp,&qcent,&xcent,ncent,&neval);
return err;
}
void display(){
// Clear screen and Z-buffer
glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
glEnable(GL_COLOR_MATERIAL);
// Reset transformations
glLoadIdentity();
// Other Transformations
glTranslatef( 0.0, -0.8, 0.0 );
glRotatef( -10, 1.0, 0.0, 0.0 );
// Rotate when user changes rotate_x and rotate_y
glRotatef( rotate_x, 1.0, 0.0, 0.0 );
glRotatef( rotate_y, 0.0, 1.0, 0.0 );
// Other Transformations
// glScalef( 2.0, 2.0, 0.0 ); // Not included
helper();
legs();
hips();
torso();
arms(tangan_l);
head();
glFlush();
glutSwapBuffers();
}
bool goHome(bool go){
if(!go){
return true;
}
ros::Duration(0.1).sleep();
clopema_robot::ClopemaRobotCommander arms("arms");
arms.setNamedTarget("home_arms");
return arms.move();
}
int main(void)
{
FILE *f;
int err, ninit = 4, npoint = 100, nsamp = 1, ncent = 4 ;
int neval,i;
double xinit[10]={0.0,3.0,17.0,20.0}, xl = -100.0, xr = 100.0;
double xsamp[100], xcent[10], qcent[10] = {5., 30., 70., 95.};
unsigned seed = 44;
double convex = 1.;
int dometrop = 0;
double xprev = 0.0;
/* set up structures for each density function */
struct norm_parm norm_data;
struct normix_parm normix_data;
/* initialise data needed by normal density function */
norm_data.mean = 10.0;
norm_data.sd = 5.;
/* initialise data needed by normal mixture density function */
normix_data.p1 = .3;
normix_data.mean1 = 5.;
normix_data.sd1 = 1.;
normix_data.mean2 = 10.;
normix_data.sd2 = 2.5;
/* initialise random number generator */
srand(seed);
/* open a file */
f = fopen("arms.out01","w+");
for(i=0;i<100000;i++){
err = arms(xinit,ninit,&xl,&xr,norm,&norm_data,&convex,
npoint,dometrop,&xprev,xsamp,nsamp,qcent,xcent,ncent,&neval);
if(err>0){
fprintf(f,"error code = %d\n",err);
exit(1);
}
fprintf(f,"%d %11.5f %d\n",i,xsamp[0],neval);
}
return 0;
}
static int myarms_simple(int ninit, double *xl, double *xr,
double (*myfunc)(double x, void *mydata), void *mydata,
int dometrop, double *xprev, double *xsamp, int nsamp)
/* adaptive rejection metropolis sampling - simplified argument list */
/* ninit : number of starting values to be used */
/* *xl : left bound */
/* *xr : right bound */
/* *myfunc : function to evaluate log-density */
/* *mydata : data required by *myfunc */
/* dometrop : whether metropolis step is required */
/* *xprev : current value from markov chain */
/* *xsamp : to store sampled value */
/* nsamp : number of samples */
{
double xinit[ninit+1], convex=1.0, qcent, xcent;
int err, i, npoint=100, ncent=0, neval;
/* set up starting values */
for(i=0; i<ninit; i++){
xinit[i] = *xl + (i + 0.1) * (*xr - *xl)/(ninit - 1.0 + 0.2);
}
/* now insert *xprev */
if ( *xprev>*xl && *xprev<*xr ) {
/* now insert *xprev */
for(i=0; i<ninit && xinit[i]<*xprev; i++) ;
if ( i>=ninit )
xinit[ninit] = *xprev;
else {
int savei = i;
for(i=ninit; i>savei; i--) {
xinit[i] = xinit[i-1];
}
xinit[savei] = *xprev;
}
}
err = arms(xinit,ninit,xl,xr,myfunc,mydata,&convex,npoint,
dometrop,xprev,xsamp,
nsamp,&qcent,&xcent,ncent,&neval);
return err;
}
int phypp_main(int argc, char* argv[]) {
if (argc < 2) {
print("usage: make_shifts <ob_filter> [options]");
return 0;
}
vec1u exclude;
std::string helper;
read_args(argc-1, argv+1, arg_list(exclude, helper));
if (helper.empty()) {
error("please provide the name of (one of) the helper target you used to "
"calibrate the shifts (helper=...)");
return 1;
}
helper = tolower(helper);
std::string scis = argv[1];
vec1d cent_ra, cent_dec;
file::read_table("centroid_helper.txt", 0, cent_ra, cent_dec);
vec1u ids = uindgen(cent_ra.size())+1;
vec1u idex = where(is_any_of(ids, exclude));
inplace_remove(ids, idex);
inplace_remove(cent_ra, idex);
inplace_remove(cent_dec, idex);
std::ofstream cmb("combine.sof");
vec1d shx, shy;
vec1d x0, y0;
bool first_line = true;
uint_t nexp = 0;
for (uint_t i : range(ids)) {
std::string dir = scis+align_right(strn(ids[i]), 2, '0')+"/";
print(dir);
vec1s files = dir+file::list_files(dir+"sci_reconstructed*-sci.fits");
inplace_sort(files);
// Find out which exposures contain the helper target from which the
// shifts were calibrated
vec1u ignore;
for (uint_t k : range(files)) {
std::string f = files[k];
fits::generic_file fcubes(f);
vec1s arms(24);
bool badfile = false;
for (uint_t u : range(24)) {
if (!fcubes.read_keyword("ESO OCS ARM"+strn(u+1)+" NAME", arms[u])) {
note("ignoring invalid file '", f, "'");
note("missing keyword 'ESO OCS ARM"+strn(u+1)+" NAME'");
ignore.push_back(k);
badfile = true;
break;
}
}
if (badfile) continue;
arms = tolower(trim(arms));
vec1u ida = where(arms == helper);
if (ida.empty()) {
ignore.push_back(k);
} else if (x0.empty()) {
// Get astrometry of IFUs from first exposure
// NB: assumes the rotation is the same for all exposures,
// which is anyway what kmos_combine does later on.
fcubes.reach_hdu(ida[0]+1);
fits::wcs astro(fcubes.read_header());
fits::ad2xy(astro, cent_ra, cent_dec, x0, y0);
}
}
inplace_remove(files, ignore);
if (files.empty()) {
warning("folder ", dir, " does not contain any usable file");
continue;
}
for (std::string f : files) {
cmb << f << " COMMAND_LINE\n";
++nexp;
}
double dox = x0[0] - x0[i], doy = y0[0] - y0[i];
if (first_line) {
// Ommit first line which has, by definition, no offset
first_line = false;
} else {
shx.push_back(dox);
shy.push_back(doy);
}
if (file::exists(dir+"helpers/shifts.txt")) {
vec1d tx, ty;
file::read_table(dir+"helpers/shifts.txt", 0, tx, ty);
for (uint_t j : range(tx)) {
shx.push_back(dox+tx[j]);
shy.push_back(doy+ty[j]);
}
}
}
note("found ", nexp, " exposures");
cmb.close();
auto truncate_decimals = vectorize_lambda([](double v, uint_t nd) {
return long(v*e10(nd))/e10(nd);
});
shx = truncate_decimals(shx, 2);
shy = truncate_decimals(shy, 2);
file::write_table("shifts.txt", 10, shx, shy);
return 0;
}
//// Sample a chain conditional on absorbing at an observation, returning only sufficient statistics of the chain
// y (input)
// absorption time to be conditioned on
// pi (input)
// 1 x n vector of initial state probabilities (*must* sum to 1)
// S (input)
// n x n matrix of transition rates between non-absorbing states
// s (input)
// 1 x n vector of exit rates
// Q (input)
// precomputed n x n matrix of eigenvectors of the original S matrix
// evals (input)
// array size n of the n eigenvalues of the original S matrix
// Qinv_s (input)
// precomputed n x 1 matrix Qinv %*% s
// n (input)
// dimension
// PjQSjjLinv (input)
// matrix where rows are P_{j\cdot} Q (\mathbf{I}(-S_{jj})+\Lambda)^{-1} for each j
// P (input)
// n x n matrix of embedded transition probabilities, excluding absorbing moves
// res_z (output)
// n dimensional array, filled with times spent in each state
// res_B (output)
// integer, set to the state the sample chain started in
// res_N (output)
// n x n matrix, filled with number of transitions between states (i != j). The only non-zero diagonal was the exit-to-absorption state
// workD (output)
// ?? element workspace
//
// return: sufficient statistics of the sample (z, B, N)
void LJMA_samplechain_Aslett2(double *y, double *pi, double *S, double *s, double *Q, double *evals, double *Qinv_s, int *n, double *PjQSjjLinv, double *evals_Sjj, double *P, double *res_z, int *res_B, int *res_N, double *workD) {
LJMA_GetRNGstate();
// dotProd_pars pars;
// pars.PjQSjjLinv = PjQSjjLinv;
// pars.evals_Sjj = evals_Sjj;
// pars.diagConst = workD; workD += *n;
// pars.Ly_t = workD; workD += *n;
// pars.evals = evals;
// pars.Qinv_s = Qinv_s;
// pars.n = *n;
ECS_dens_pars pars2;
// Initialise output vars
*res_B = 0;
int i;
for(i=0; i<*n; i++) {
res_z[i] = 0;
for(int j=0; j<*n; j++) {
res_N[i + j * *n] = 0;
}
}
// Choose starting state ...
double sofar = 0.0, target;
target = runif(0.0, 1.0);
*res_B = 0;
while(sofar < target) {
sofar += pi[(*res_B)++];
}
(*res_B)--;
//Rprintf("%d - Start at: %d\n", *reverse, *res_B);
// Run through the chain
double t = 0.0, lastt = 0.0, d, y_t, Tol;
int j = *res_B, lastj, Maxit;
double *p;
p = workD; workD += *n;
while(TRUE) {
y_t = *y-t;
// Do we move from where we are, or is this the state from which we absorb?
if(s[j] > 0.0) { // can't absorb from here if no exit allowed
if(runif(0.0, 1.0) < LJMA_probAbsorb(y_t, j, S, Q, evals, Qinv_s, s, *n)) {
break;
}
}
lastt = t;
lastj = j;
/* D1 <- (1/(rep(-S[j,j],3)+L))*exp(L*(y-t))
Dd <- (1/(rep(-S[j,j],3)+L))*exp(L*(y-t)-x*(L+rep(-S[j,j],3)))
D2 <- (1/(rep(-S[j,j],3)+L))*exp(-rep((y-t)*(-S[j,j]),3))
V <- Q %*% diag((D1*z-D2*z),3,3) %*% solve(Q)
Ud <- Q %*% diag((D1-Dd),3,3) %*% solve(Q)
P[j,]%*%Q%*%diag(D1*(z-1)-D2*z+Dd,3)%*%solve(Q)%*%s # It should be this
*/
// Choose sojourn time
// pars.U = runif(0.0, 1.0);
// pars.j = j;
// pars.xi = -S[j + j * *n];
// for(i=0; i<*n; i++) {
// //pars.eLy_t[i] = exp(evals[i]*y_t); // Non-log scale
// pars.Ly_t[i] = evals[i]*y_t;
// //pars.diagConst[i] = (pars.U-1.0)*pars.eLy_t[i] - pars.U*exp(-y_t*pars.xi); // Non-log scale pre changing eLy_t
// //pars.diagConst[i] = (pars.U-1.0)*exp(pars.Ly_t[i]) - pars.U*exp(-y_t*pars.xi); // Non-log scale
// if( evals_Sjj[i + j * *n] != 0.0 ) {
// pars.diagConst[i] = -exp(log(1.0-pars.U)+pars.Ly_t[i]) - exp(log(pars.U)-y_t*pars.xi); // log scale
// } else {
// pars.diagConst[i] = -y_t*pars.U*exp(pars.Ly_t[i]);
// }
// //Rprintf("state %d, Ly_t = %e, diagConst = %e\n", i+1, pars.Ly_t[i], pars.diagConst[i]);
// }
// Tol = 0.0;
// Maxit = 10000;
// t += d = Find0(0.0, y_t, &LJMA_dotProd, &pars, &Tol, &Maxit);
//Rprintf("U=%lf, Tol=%e, Maxit=%d\n", pars.U, Tol, Maxit);
// ******************************************************************
for(i=0; i<*n; i++) {
p[i] = S[j + i * *n]/(-S[j + j * *n]);
}
p[j] = 0.0;
pars2.j = j;
pars2.y_t = y_t;
pars2.p = p;
pars2.S = S;
pars2.Q = Q;
pars2.evals = evals;
pars2.Qinv_s = Qinv_s;
pars2.n = *n;
pars2.workD = workD;
double xinit[4], xl, xr, convex, xprev, xsamp, qcent, xcent;
int info, ninit, npoint, dometrop, nsamp, ncent, neval;
//xinit[3] = *y;
//while(!isfinite(LJMA_condjumpdens_ars(xinit[3], &pars))) {
// xinit[3] = log(xinit[3]);
//}
//xinit[0] = xinit[3]/4.0;
xinit[0] = (y_t)/1e6;
xinit[1] = (y_t)/3.0;
xinit[2] = xinit[1]*2.0;
xinit[3] = y_t - xinit[0];
ninit = 4;
xl = 0.0;
xr = y_t;
convex = 1.0;
npoint = 100;
dometrop = 1;
xprev = 0.0;
xsamp = 0.0;
nsamp = 1;
qcent = 0.0;
xcent = 0.0;
ncent = 0;
neval = 0;
double (*myfunc)(double, void*);
myfunc = &LJMA_ECS_dens;
info = arms(xinit, ninit, &xl, &xr, myfunc, &pars2, &convex, npoint, dometrop, &xprev, &xsamp, nsamp, &qcent, &xcent, ncent, &neval);
if(info != 0) {
Rprintf("Error (LJMA_samplechain_Aslett2): ARMS failed, code=%d\n", info);
}
t += d = xsamp;
// ******************************************************************
// Make the state jump
LJMA_moveMass(p, y_t-d, j, P, Q, evals, Qinv_s, *n, workD);
target = runif(0.0, 1.0);
sofar = 0.0;
j = 0;
while(sofar < target) {
sofar += p[j++];
}
j--;
//Rprintf("Sojourn: %lf, move %d -> %d\n", d, lastj, j);
res_z[lastj] += d;
res_N[lastj + j * *n]++;
}
//Rprintf("Exit from %d\n", j);
res_N[j + j * *n]++;
res_z[j] += *y-t;
LJMA_GUI();
LJMA_PutRNGstate();
}
void robot(int t, int T)
{
#define robot_fall_speed 2048
#define robot_start 40
t *= 402;
// ast = |sin(t)|
int ast = sin(t);
if ( ast < 0 )
ast = -ast;
// act = |cos(t)|
int act = cos(t);
if ( act < 0 )
act = -act;
int fall;
// head
push();
// translate(0, 0.71+0.05*abs(sin(t)), 0.04);
fall = (robot_start+88)*robot_fall_speed - T*robot_fall_speed;
if ( fall < 0 )
fall = 0;
translate(0, 2908+((ast*204)>>12)+fall, 164);
// rotateX(-5°+10°*abs(cos(t)));
rotateX((-357+715*act)>>12);
// scale(0.2, 0.2, 0.2);
scale(819, 819, 819);
cube();
pop();
// body
push();
// translate(0, 0.05*abs(sin(t)), 0);
fall = (robot_start+48)*robot_fall_speed - T*robot_fall_speed;
if ( fall < 0 )
fall = 0;
translate(0, ((ast*204)>>12)+fall, 0);
// rotateY(2.5°-5°*cos(t));
rotateY(179-((357*cos(t))>>12));
// scale(0.3, 0.5, 0.17);
scale(1229, 2048, 696);
cube();
pop();
// left side
push();
fall = (robot_start+56)*robot_fall_speed - T*robot_fall_speed;
if ( fall < 0 )
fall = 0;
translate(0, fall, 0);
arms(t, -1);
pop();
push();
fall = (robot_start+8)*robot_fall_speed - T*robot_fall_speed;
if ( fall < 0 )
fall = 0;
translate(0, fall, 0);
legs(t, -1);
pop();
// right side
t += 12868;
push();
fall = (robot_start+64)*robot_fall_speed - T*robot_fall_speed;
if ( fall < 0 )
fall = 0;
translate(0, fall, 0);
arms(t, 1);
pop();
push();
fall = (robot_start+24)*robot_fall_speed - T*robot_fall_speed;
if ( fall < 0 )
fall = 0;
translate(0, fall, 0);
legs(t, 1);
pop();
}
