genhexkins.c

CNC 的开放码,EMC2 V2.2.8版

    q_trans.x -= delta[0];
    q_trans.y -= delta[1];
    q_trans.z -= delta[2];
    q_RPY.r   -= delta[3];
    q_RPY.p   -= delta[4];
    q_RPY.y   -= delta[5];

    /* determine value of conv_error (used to determine if no convergence) */
    conv_err = 0.0;
    for (i = 0; i < NUM_STRUTS; i++) {
      conv_err += fabs(StrutLengthDiff[i]);
    }

    /* enter loop to determine if a strut needs another iteration */
    iterate = 0;			/*assume iteration is done */
    for (i = 0; i < NUM_STRUTS; i++) {
      if (fabs(StrutLengthDiff[i]) > conv_criterion) {
	iterate = 1;
      }
    }
  } /* exit Newton-Raphson Iterative loop */

  /* assign r,p,w to a,b,c */
  pos->a = q_RPY.r * 180.0 / PM_PI;  
  pos->b = q_RPY.p * 180.0 / PM_PI;  
  pos->c = q_RPY.y * 180.0 / PM_PI;
  
  /* assign q_trans to pos */
  pos->tran.x = q_trans.x;
  pos->tran.y = q_trans.y;
  pos->tran.z = q_trans.z;

  return retval;
}

int genhexKinematicsForwardIterations(void)
{
  return iteration;
}

/************************ kinematicsInverse() ********************************/

int kinematicsInverse(const EmcPose * pos,
                      double * joints,
                      const KINEMATICS_INVERSE_FLAGS * iflags,
                      KINEMATICS_FORWARD_FLAGS * fflags)
{
  PmCartesian aw, temp;
  PmRotationMatrix RMatrix;
  PmRpy rpy;
  int i;

  /* define Rotation Matrix */
  rpy.r = pos->a * PM_PI / 180.0;
  rpy.p = pos->b * PM_PI / 180.0;
  rpy.y = pos->c * PM_PI / 180.0;
  pmRpyMatConvert(rpy, &RMatrix);

  /* enter for loop to calculate joints (strut lengths) */
  for (i = 0; i < NUM_STRUTS; i++) {
    /* convert location of platform strut end from platform
       to world coordinates */
    pmMatCartMult(RMatrix, a[i], &temp);
    pmCartCartAdd(pos->tran, temp, &aw);

    /* define strut lengths */
    pmCartCartSub(aw, b[i], &temp);
    pmCartMag(temp, &joints[i]);
  }

  return 0;
}

/*
  kinematicsHome() is implemented by taking the desired world coordinates,
  which are passed as an arg, and running the inverse kinematics to get
  the resulting joints. The flags are set to zero.
*/
int kinematicsHome(EmcPose * world,
                   double * joint,
                   KINEMATICS_FORWARD_FLAGS * fflags,
                   KINEMATICS_INVERSE_FLAGS * iflags)
{
  *fflags = 0;
  *iflags = 0;

  return kinematicsInverse(world, joint, iflags, fflags);
}

KINEMATICS_TYPE kinematicsType()
{
#if 1
  return KINEMATICS_BOTH;
#else
  return KINEMATICS_INVERSE_ONLY;
#endif
}

#ifdef MAIN

#include <stdio.h>
/* FIXME-- this works for Unix only */
#include <sys/time.h>		/* struct timeval */
#include <unistd.h>		/* gettimeofday() */

static double timestamp()
{
  struct timeval tp;

  if (0 != gettimeofday(&tp, NULL)) {
    return 0.0;
  }
  return ((double) tp.tv_sec) + ((double) tp.tv_usec) / 1000000.0;
}

int main(int argc, char *argv[])
{
#define BUFFERLEN 256
  char buffer[BUFFERLEN];
  int inverse = 1;
  int jacobian = 0;
  EmcPose pos = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
  EmcPose vel = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
  double joints[6] = {0.0};
  double jointvels[6] = {0.0};
  KINEMATICS_INVERSE_FLAGS iflags = 0;
  KINEMATICS_FORWARD_FLAGS fflags = 0;
  int t;
  int retval;
#define ITERATIONS 100000
  double start, end;

  /* syntax is a.out {i|f # # # # # #} */
  if (argc == 8) {
    if (argv[1][0] == 'f') {
      /* joints passed, so do interations on forward kins for timing */
      for (t = 0; t < 6; t++) {
	if (1 != sscanf(argv[t + 2], "%lf", &joints[t])) {
	  fprintf(stderr, "bad value: %s\n", argv[t + 2]);
	  return 1;
	}
      }
      inverse = 0;
    }
    else if (argv[1][0] == 'i') {
      /* world coords passed, so do iterations on inverse kins for timing */
      if (1 != sscanf(argv[2], "%lf", &pos.tran.x)) {
	fprintf(stderr, "bad value: %s\n", argv[2]);
	return 1;
      }
      if (1 != sscanf(argv[3], "%lf", &pos.tran.y)) {
	fprintf(stderr, "bad value: %s\n", argv[3]);
	return 1;
      }
      if (1 != sscanf(argv[4], "%lf", &pos.tran.z)) {
	fprintf(stderr, "bad value: %s\n", argv[4]);
	return 1;
      }
      if (1 != sscanf(argv[5], "%lf", &pos.a)) {
	fprintf(stderr, "bad value: %s\n", argv[5]);
	return 1;
      }
      if (1 != sscanf(argv[6], "%lf", &pos.b)) {
	fprintf(stderr, "bad value: %s\n", argv[6]);
	return 1;
      }
      if (1 != sscanf(argv[7], "%lf", &pos.c)) {
	fprintf(stderr, "bad value: %s\n", argv[7]);
	return 1;
      }
      inverse = 1;
    }
    else {
      fprintf(stderr, "syntax: %s {i|f # # # # # #}\n", argv[0]);
      return 1;
    }

    /* need an initial estimate for the forward kins, so ask for it */
    if (inverse == 0) {
      do {
	printf("initial estimate for Cartesian position, xyzrpw: ");
	fflush(stdout);
	if (NULL == fgets(buffer, BUFFERLEN, stdin)) {
	  return 0;
	}
      } while (6 != sscanf(buffer, "%lf %lf %lf %lf %lf %lf",
			   &pos.tran.x,
			   &pos.tran.y,
			   &pos.tran.z,
			   &pos.a,
			   &pos.b,
			   &pos.c));
    }

    start = timestamp();
    for (t = 0; t < ITERATIONS; t++) {
      if (inverse) {
	retval = kinematicsInverse(&pos, joints, &iflags, &fflags);
	if (0 != retval) {
	  printf("inv kins error %d\n", retval);
	  break;
	}
      }
      else {
	retval = kinematicsForward(joints, &pos, &fflags, &iflags);
	if (0 != retval) {
	  printf("fwd kins error %d\n", retval);
	  break;
	}
      }
    }
    end = timestamp();

    printf("calculation time: %f secs\n",
	   (end - start) / ((double) ITERATIONS));
    return 0;
  } /* end of if args for timestamping */

  /* else we're interactive */
  while (! feof(stdin)) {
    if (inverse) {
      if (jacobian) {
	printf("jinv> ");
      }
      else {
	printf("inv> ");
      }
    }
    else {
      if (jacobian) {
	printf("jfwd> ");
      }
      else {
	printf("fwd> ");
      }
    }
    fflush(stdout);

    if (NULL == fgets(buffer, BUFFERLEN, stdin)) {
      break;
    }

    if (buffer[0] == 'i') {
      inverse = 1;
      continue;
    }
    else if (buffer[0] == 'f') {
      inverse = 0;
      continue;
    }
    else if (buffer[0] == 'j') {
      jacobian = ! jacobian;
      continue;
    }
    else if (buffer[0] == 'q') {
      break;
    }

    if (inverse) {
      if (jacobian) {
	if (12 != sscanf(buffer, "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
			&pos.tran.x,
			&pos.tran.y,
			&pos.tran.z,
			&pos.a,
			&pos.b,
			&pos.c,
			&vel.tran.x,
			&vel.tran.y,
			&vel.tran.z,
			&vel.a,
			&vel.b,
			&vel.c)) {
	  printf("?\n");
	}
	else {
	  retval = jacobianInverse(&pos, &vel, joints, jointvels);
	  printf("%f %f %f %f %f %f\n",
		 jointvels[0],
		 jointvels[1],
		 jointvels[2],
		 jointvels[3],
		 jointvels[4],
		 jointvels[5]);
	  if (0 != retval) {
	    printf("inv Jacobian error %d\n", retval);
	  }
	  else {
	    retval = jacobianForward(joints, jointvels, &pos, &vel);
	    printf("%f %f %f %f %f %f\n",
		   vel.tran.x,
		   vel.tran.y,
		   vel.tran.z,
		   vel.a,
		   vel.b,
		   vel.c);
	    if (0 != retval) {
	      printf("fwd kins error %d\n", retval);
	    }
	  }
	}
      }
      else {
	if (6 != sscanf(buffer, "%lf %lf %lf %lf %lf %lf",
			&pos.tran.x,
			&pos.tran.y,
			&pos.tran.z,
			&pos.a,
			&pos.b,
			&pos.c)) {
	  printf("?\n");
	}
	else {
	  retval = kinematicsInverse(&pos, joints, &iflags, &fflags);
	  printf("%f %f %f %f %f %f\n",
		 joints[0],
		 joints[1],
		 joints[2],
		 joints[3],
		 joints[4],
		 joints[5]);
	  if (0 != retval) {
	    printf("inv kins error %d\n", retval);
	  }
	  else {
	    retval = kinematicsForward(joints, &pos, &fflags, &iflags);
	    printf("%f %f %f %f %f %f\n",
		   pos.tran.x,
		   pos.tran.y,
		   pos.tran.z,
		   pos.a,
		   pos.b,
		   pos.c);
	    if (0 != retval) {
	      printf("fwd kins error %d\n", retval);
	    }
	  }
	}
      }
    }
    else {
      if (jacobian) {
	if (12 != sscanf(buffer, "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
			&joints[0],
			&joints[1],
			&joints[2],
			&joints[3],
			&joints[4],
			&joints[5],
			&jointvels[0],
			&jointvels[1],
			&jointvels[2],
			&jointvels[3],
			&jointvels[4],
			&jointvels[5])) {
	  printf("?\n");
	}
	else {
	  retval = jacobianForward(joints, jointvels, &pos, &vel);
	  printf("%f %f %f %f %f %f\n",
		 vel.tran.x,
		 vel.tran.y,
		 vel.tran.z,
		 vel.a,
		 vel.b,
		 vel.c);
	  if (0 != retval) {
	    printf("fwd kins error %d\n", retval);
	  }
	  else {
	    retval = jacobianInverse(&pos, &vel, joints, jointvels);
	    printf("%f %f %f %f %f %f\n",
		   jointvels[0],
		   jointvels[1],
		   jointvels[2],
		   jointvels[3],
		   jointvels[4],
		   jointvels[5]);
	    if (0 != retval) {
	      printf("inv kins error %d\n", retval);
	    }
	  }
	}
      }
      else {
	if (6 != sscanf(buffer, "%lf %lf %lf %lf %lf %lf",
			&joints[0],
			&joints[1],
			&joints[2],
			&joints[3],
			&joints[4],
			&joints[5])) {
	  printf("?\n");
	}
	else {
	  retval = kinematicsForward(joints, &pos, &fflags, &iflags);
	  printf("%f %f %f %f %f %f\n",
		 pos.tran.x,
		 pos.tran.y,
		 pos.tran.z,
		 pos.a,
		 pos.b,
		 pos.c);
	  if (0 != retval) {
	    printf("fwd kins error %d\n", retval);
	  }
	  else {
	    retval = kinematicsInverse(&pos, joints, &iflags, &fflags);
	    printf("%f %f %f %f %f %f\n",
		   joints[0],
		   joints[1],
		   joints[2],
		   joints[3],
		   joints[4],
		   joints[5]);
	    if (0 != retval) {
	      printf("inv kins error %d\n", retval);
	    }
	  }
	}
      }
    }
  }

  return 0;

#undef ITERATIONS
#undef BUFFERLEN
}

#endif /* MAIN */

#ifdef RTAPI
#include "rtapi.h"		/* RTAPI realtime OS API */
#include "rtapi_app.h"		/* RTAPI realtime module decls */
#include "hal.h"

EXPORT_SYMBOL(kinematicsType);
EXPORT_SYMBOL(kinematicsForward);
EXPORT_SYMBOL(kinematicsInverse);

MODULE_LICENSE("GPL");



int comp_id;
int rtapi_app_main(void) {
    comp_id = hal_init("genhexkins");
    if(comp_id > 0) {
	hal_ready(comp_id);
	return 0;
    }
    return comp_id;
}

void rtapi_app_exit(void) { hal_exit(comp_id); }
#endif