cuikjoints2links.c

Go to the documentation of this file.

#include "box.h"
#include "random.h"
#include "defines.h"
#include "filename.h"
#include "geom.h"
#include "world.h"
 
#include <stdlib.h>
 
int main(int argc, char **arg)
{ 
  if (argc>1)
    {
      Tparameters parameters;
      Tworld world;
 
      Tfilename fparam;
      Tfilename flinks;
      Tfilename fboxes;
      Tfilename fjoints;
      Tfilename fvmask;
      Tfilename fvname;
 
      unsigned int r;
      unsigned int i,j;
 
      unsigned int ndof;
      double *dof;
      unsigned int nv;
      boolean *sv,*mask;
      double *sample;
      Tbox bsample;
      char **varNames;
 
      FILE *fd,*fs,*fb;
 
      boolean end;
      int token;
 
      char **vn;
 
      /* Init parameters */
      CreateFileName(NULL,arg[1],NULL,PARAM_EXT,&fparam);      
      fprintf(stderr,"Reading parameters                : %s\n",GetFileFullName(&fparam));
      InitParametersFromFile(GetFileFullName(&fparam),&parameters);
      r=(unsigned int)(GetParameter(CT_REPRESENTATION,&parameters));
 
      if (r==REP_JOINTS)
        Error("Please, set a REPRESENTATION different from JOINTS in the parameter file");
 
      /* Read the problem file */
      InitWorldFromFile(&parameters,FALSE,TRUE,arg[1],&world);
      
      /* Open the the dof file */
      if (argc>2)
        CreateFileName(NULL,arg[2],NULL,JOINTS_EXT,&fjoints);
      else
        CreateFileName(NULL,arg[1],NULL,JOINTS_EXT,&fjoints);
      fprintf(stderr,"Reading dof from                  : %s\n",GetFileFullName(&fjoints));
      fd=fopen(GetFileFullName(&fjoints),"r");
      if (!fd)
        Error("Could not open the file with the dof");
 
      /* Open the file to store the link poses */
      if (argc>2)
        CreateFileName(NULL,arg[2],NULL,LINKS_EXT,&flinks);
      else
        CreateFileName(NULL,arg[1],NULL,LINKS_EXT,&flinks);
      fprintf(stderr,"Writing link poses to             : %s\n",GetFileFullName(&flinks));
      fs=fopen(GetFileFullName(&flinks),"w");
      if (!fs)
        Error("Could not open the file to store the link poses");
 
      if (argc>2)
        CreateFileName(NULL,arg[2],NULL,SOL_EXT,&fboxes);
      else
        CreateFileName(NULL,arg[1],NULL,SOL_EXT,&fboxes);
      fprintf(stderr,"Writing boxes to                  : %s\n",GetFileFullName(&fboxes));
      fb=fopen(GetFileFullName(&fboxes),"w");
      if (!fb)
        Error("Could not open the file to store the boxes");
      
      /* Allocate space to read the dof */
      nv=GetWorldSystemVars(&sv,&world);
 
      ndof=GetWorldNDOF(&world);
      NEW(dof,ndof,double);
 
      end=FALSE;
      do {
        for(i=0;((!end)&&(i<ndof));i++)
          {
            token=fscanf(fd,"%lf",&(dof[i]));
            if ((token==EOF)||(token==0))
              end=TRUE;
          }
        if (!end)
          {
            if (WorldDOF2Sol(&parameters,dof,&sample,&bsample,&world)!=nv)
              Error("Missmatch variable number in cuikjoints2links");
            for(j=0;j<nv;j++)
              {
                if (sv[j])
                  { PrintReal(fs,sample[j]);fprintf(fs," "); }
              }
            fprintf(fs,"\n");
            free(sample);
            NEW(vn,nv,char *);
            GetWorldVarNames(vn,&world);
            PrintBoxSubset(fb,sv,vn,&bsample);
            free(vn);
            DeleteBox(&bsample);
          }
      } while (!end);
 
      fclose(fd);
      fclose(fs);
      fclose(fb);
 
      free(dof);
 
      /* Now write a file with the array of booleans indicating the good
         variables: the ones refering to link poses that are keept in
         the simplified cuiksystem. This is very adhoc for generating
         eigengrasps.... We dot it that way to have an automatic
         way to generate the eigengrasp without resorting to a manual
         procedure prone to errors. */
      
      nv=GetWorldSimpVariableMask(&parameters,&mask,&world);
 
      if (argc>2)
        CreateFileName(NULL,arg[2],NULL,"vname",&fvname);
      else
        CreateFileName(NULL,arg[1],NULL,"vname",&fvname);
      fprintf(stderr,"Writting variable names to        : %s\n",GetFileFullName(&fvname));
      fs=fopen(GetFileFullName(&fvname),"w");
      if (!fs)
        Error("Could not open the file to store the variable names");
      
      NEW(varNames,nv,char*);
      GetWorldVarNames(varNames,&world);
 
      for(j=0;j<nv;j++)
        {
          if (sv[j])
            {
              PRINT_VARIABLE_NAME(fs,varNames[j]);
              fprintf(fs,"\n");
            }
        }
      fclose(fs);
      free(varNames);
 
 
      if (argc>2)
        CreateFileName(NULL,arg[2],NULL,"vmask",&fvmask);
      else
        CreateFileName(NULL,arg[1],NULL,"vmask",&fvmask);
      fprintf(stderr,"Writting variable mask to         : %s\n",GetFileFullName(&fvmask));
      fs=fopen(GetFileFullName(&fvmask),"w");
      if (!fs)
        Error("Could not open the file to store the variable mask");
 
      for(j=0;j<nv;j++)
        {
          if (sv[j])
            {
              if (mask[j])
                fprintf(fs,"1 ");
              else
                fprintf(fs,"0 ");
            }
        }
      fprintf(fs,"\n");
 
      free(sv);
      free(mask);
      fclose(fs);
 
      DeleteWorld(&world);
      DeleteParameters(&parameters);
 
      DeleteFileName(&fparam);
      DeleteFileName(&fjoints);
      DeleteFileName(&flinks);
      DeleteFileName(&fboxes);
      DeleteFileName(&fvmask);
      DeleteFileName(&fvname);
    }
  else
    {
      fprintf(stderr,"Use:\n");
      fprintf(stderr,"  cuikjoints2links <problem_name> <output_name>\n");
      fprintf(stderr,"    <problem_name> the file with the .world description.\n");
      fprintf(stderr,"    <output_name> [optional] Prefix for the .joints file and to generate the .links.\n");
      fprintf(stderr,"        .sol, .vname, and .vmask files. If not given the <problem_name> is used.\n");
    }
 
  return(EXIT_SUCCESS);
}