// fills the INFILE struct with all the useful information
int
get_input_data( struct infile_data *INFILE ,
const char *file_name )
{
// open the input file in here and free it at the bottom
FILE *infile = fopen( file_name , "r" ) ;
if( infile == NULL ) {
fprintf( stderr , "[IO] input file %s cannot be read ... Leaving\n" ,
file_name ) ;
return GLU_FAILURE ;
}
// if we can open the file we push it into a big structure
pack_inputs( infile ) ;
int INPUT_FAILS = 0 ; // counter for the number of failures
// fill in the input data into a struct
if( get_mode( &( INFILE -> mode ) ) == GLU_FAILURE ) INPUT_FAILS++ ;
if( read_cuts_struct( &( INFILE -> CUTINFO ) ) == GLU_FAILURE ) INPUT_FAILS++ ;
if( read_gf_struct ( &( INFILE -> GFINFO ) ) == GLU_FAILURE ) INPUT_FAILS++ ;
if( read_hb_struct( &( INFILE -> HBINFO ) ) == GLU_FAILURE ) INPUT_FAILS++ ;
if( smearing_info( &( INFILE -> SMINFO ) ) == GLU_FAILURE ) INPUT_FAILS++ ;
if( read_suNC_x_U1( &( INFILE -> U1INFO ) ) == GLU_FAILURE ) INPUT_FAILS++ ;
// are we performing a random transform
INFILE -> rtrans = rtrans( ) ;
// get the header type
if( header_type( &( INFILE -> head ) ) == GLU_FAILURE ) {
INPUT_FAILS++ ;
} else {
Latt.head = INFILE -> head ; // set the header type
}
// check out_details
if( out_details( &( INFILE -> storage ) , INFILE -> mode ) == GLU_FAILURE ) {
INPUT_FAILS++ ;
}
// put the config info
config_information( INFILE -> output_details ) ;
// close the file and deallocate the buffer
fclose( infile ) ;
unpack_inputs( ) ;
// if we have hit ANY problem we return GLU_FAILURE this causes it to exit
return ( INPUT_FAILS != 0 ) ? GLU_FAILURE : GLU_SUCCESS ;
}
int main(int argc, char *argv[])
{
int i, j;
int ntrans = 0;
int npts = 0;
double fwhm = .5;
double trans[TAB_SIZE][CRD_SIZE] = {{0.}};
double xmin = 0.;
double xmax = 5000.;
double xstp = 1.;
double pts[5000][CRD_SIZE] = {{0.}};
char inpfile[LGN_SIZE] = "";
char outfile[LGN_SIZE] = "";
rcmdl(argc, argv, inpfile, outfile, &fwhm);
rgauss(inpfile, &ntrans, trans, fwhm);
if (ntrans == 0)
{
rtrans(inpfile, &ntrans, trans, fwhm);
}
printf(" %s: %d transitions\n", argv[1], ntrans);
for (i=0; i<ntrans; i++)
{
printf("%12.6lf%12.6lf%12.6lf\n", trans[i][0], trans[i][1], trans[i][2]);
}
spectrum(&npts, pts, xmin, xmax, xstp, ntrans, trans);
for (i=0; i<npts; i++)
{
printf("%12.6lf%12.6lf\n", pts[i][0], pts[i][1]);
}
wspec(outfile, npts, pts);
return 0;
}
int main( int argc, char **argv )
{
Hubo_Control hubo("proto-manip-daemon");
ach_channel_t chan_manip_cmd;
int r = ach_open( &chan_manip_cmd, CHAN_HUBO_MANIP, NULL );
daemon_assert( r==ACH_OK, __LINE__ );
hubo_manip_cmd manip;
memset( &manip, 0, sizeof(manip) );
hubo.update();
Eigen::Isometry3d Br, Bl;
Vector6d right, left, zeros; zeros.setZero();
Vector3d rtrans, ltrans, langles, rangles;
hubo.getRightArmAngles(right);
hubo.getLeftArmAngles(left);
hubo.huboArmFK( Br, right, RIGHT );
hubo.huboArmFK( Bl, left, LEFT );
std::cout << "Performed initial FK" << std::endl;
for(int i=0; i<3; i++)
{
manip.translation[RIGHT][i] = Br(i,3);
manip.translation[LEFT][i] = Bl(i,3);
}
std::cout << "Putting first transformation" << std::endl;
ach_put( &chan_manip_cmd, &manip, sizeof(manip) );
size_t fs;
std::cout << "About to start loop" << std::endl;
while( !daemon_sig_quit )
{
hubo.update();
ach_get( &chan_manip_cmd, &manip, sizeof(manip), &fs, NULL, ACH_O_LAST );
for(int i=0; i<3; i++)
{
rtrans(i) = manip.translation[RIGHT][i];
ltrans(i) = manip.translation[LEFT][i];
rangles(i) = manip.eulerAngles[RIGHT][i];
langles(i) = manip.eulerAngles[LEFT][i];
}
// Handle the right arm
Br = Eigen::Matrix4d::Identity();
Br.translate(rtrans);
Br.rotate( Eigen::AngleAxisd(rangles(0), Vector3d(1,0,0)) );
Br.rotate( Eigen::AngleAxisd(rangles(1), Vector3d(0,1,0)) );
Br.rotate( Eigen::AngleAxisd(rangles(2), Vector3d(0,0,1)) );
hubo.huboArmIK( right, Br, zeros, RIGHT );
hubo.setRightArmAngles( right );
// Handle the left arm
Bl = Eigen::Matrix4d::Identity();
Bl.translate(ltrans);
Bl.rotate( Eigen::AngleAxisd(langles(0), Vector3d(1,0,0)) );
Bl.rotate( Eigen::AngleAxisd(langles(1), Vector3d(0,1,0)) );
Bl.rotate( Eigen::AngleAxisd(langles(2), Vector3d(0,0,1)) );
hubo.huboArmIK( left, Bl, zeros, LEFT );
hubo.setLeftArmAngles( left );
// Send commands off to the control daemon
hubo.sendControls();
}
ach_close( &chan_manip_cmd );
}
