cuiklinks2joints.c

Go to the documentation of this file.

#include "box.h"
#include "random.h"
#include "defines.h"
#include "filename.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 fjoints;

      unsigned int i,j;

      unsigned int ndof;
      double *dof;
      unsigned int nv,nvs;
      boolean *sv;
      double *sample;

      FILE *fd,*fs;

      boolean end;
      int token;

      unsigned int r;

      /* 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("The system representation can not be JOINTS when converting to JOINTS");

      /* Read the problem file */
      InitWorldFromFile(&parameters,TRUE,arg[1],&world);

      /* Open the file from where to read the samples */
      CreateFileName(NULL,arg[1],NULL,LINKS_EXT,&flinks);
      fprintf(stderr,"Reading link poses from           : %s\n",GetFileFullName(&flinks));
      fs=fopen(GetFileFullName(&flinks),"r");
      if (!fs)
        Error("Could not open the file to read the link poses");
      
      /* Open the file where to store the dof */
      if (argc>2)
        CreateFileName(NULL,arg[2],NULL,JOINTS_EXT,&fjoints);
      else
        CreateFileName(NULL,arg[1],NULL,JOINTS_EXT,&fjoints);
      fprintf(stderr,"Writing dof to                    : %s\n",GetFileFullName(&fjoints));
      fd=fopen(GetFileFullName(&fjoints),"w");
      if (!fd)
        Error("Could not open the file to store the dof");

      /* Allocate space for the sample and dof */
      nv=GetWorldSystemVars(&sv,&world);
      nvs=0;
      for(i=0;i<nv;i++)
        {
          if (sv[i])
            nvs++;
        }
      NEW(sample,nvs,double);

      ndof=GetWorldNDOF(&world);
      NEW(dof,ndof,double);

      end=FALSE;
      do {
        for(i=0;((!end)&&(i<nvs));i++)
          {
            token=fscanf(fs,"%lf",&(sample[i]));
            if ((token==EOF)||(token==0))
              end=TRUE;
          }
        if (!end)
          {
            WorldSample2DOF(&parameters,sample,dof,&world);
            for(j=0;j<ndof;j++) 
              fprintf(fd,"%.16f ",dof[j]);
            fprintf(fd,"\n");
          }
      } while (!end);

      free(sv);
      free(dof);
      free(sample);

      fclose(fd);
      fclose(fs);
 
      DeleteWorld(&world);
      DeleteParameters(&parameters);

      DeleteFileName(&fparam);
      DeleteFileName(&fjoints);
      DeleteFileName(&flinks);    
    }
  else
    {
      fprintf(stderr,"Use:\n");
      fprintf(stderr,"  cuiklinks2joints <problem_name> <dof_name>\n");
      fprintf(stderr,"    <problem_name> the prefix to define the .world and.\n");
      fprintf(stderr,"        .links files.\n");
      fprintf(stderr,"    <dof_name> [optional] The prefis to define the .dof.\n");
      fprintf(stderr,"        file. If not given the <problem_name> is used.\n");
    }
 
  return(EXIT_SUCCESS);
}