/* inRange() returns non-zero if the position lies within the joint
limits, or 0 if not */
static int inRange(EmcPose pos)
{
double joint_pos[EMCMOT_MAX_JOINTS];
int joint_num;
emcmot_joint_t *joint;
/* fill in all joints with 0 */
for (joint_num = 0; joint_num < num_joints; joint_num++) {
joint_pos[joint_num] = 0.0;
}
/* now fill in with real values, for joints that are used */
kinematicsInverse(&pos, joint_pos, &iflags, &fflags);
for (joint_num = 0; joint_num < num_joints; joint_num++) {
/* point to joint data */
joint = &joints[joint_num];
if (!GET_JOINT_ACTIVE_FLAG(joint)) {
/* if joint is not active, don't even look at its limits */
continue;
}
if ((joint_pos[joint_num] > joint->max_pos_limit) ||
(joint_pos[joint_num] < joint->min_pos_limit)) {
return 0; /* can't move further past limit */
}
}
/* okay to move */
return 1;
}
int kinematicsTest(int argc, char *argv[]) {
float x, y, L1, L2, S, E;
if (argc != 4) {
warning("usage: %s %s kinematics <X> <Y>\n", argv[0], argv[1]);
return EXIT_FAILURE;
}
x = atof(argv[2]);
y = atof(argv[3]);
L1 = L1_MM;
L2 = L1_MM;
printf("x = %.2f y = %.2f L1 = %.2f L2 = %.2f\n", x, y, L1, L2);
kinematicsInverse(x, y, L1, L2, &S, &E);
printf("raw - S = %.4f E = %.4f\n", S, E);
printf("deg - S = %.4f E = %.4f\n", kinematicsRadToDeg(S), kinematicsRadToDeg(E));
printf("rad - S = %.4f E = %.4f\n", kinematicsDegToRad(kinematicsRadToDeg(S)), kinematicsDegToRad(kinematicsRadToDeg(E)));
printf("step - S = %4d E = %4d\n", kinematicsRadToStep(S), kinematicsRadToStep(E));
kinematicsForward(&x, &y, L1, L2, S, E);
printf("x = %.4f y = %.4f\n", x, y);
return EXIT_SUCCESS;
}
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 }; // will need this for
// jacobian
double joints[6] = { 0.0 };
double jointvels[6] = { 0.0 };
KINEMATICS_INVERSE_FLAGS iflags = 0;
KINEMATICS_FORWARD_FLAGS fflags = 0;
int t;
int retval = 0, i;
double start, end;
// FIXME-AJ: implement ULAPI HAL version of the pins
haldata = malloc(sizeof(struct haldata));
KINS_PTR = malloc(sizeof(genser_struct));
haldata->pos = (go_pose *) malloc(sizeof(go_pose));
for (i = 0; i < GENSER_MAX_JOINTS ; i++) {
haldata->a[i] = malloc(sizeof(double));
haldata->alpha[i] = malloc(sizeof(double));
haldata->d[i] = malloc(sizeof(double));
}
A(0) = DEFAULT_A1;
A(1) = DEFAULT_A2;
A(2) = DEFAULT_A3;
A(3) = DEFAULT_A4;
A(4) = DEFAULT_A5;
A(5) = DEFAULT_A6;
ALPHA(0) = DEFAULT_ALPHA1;
ALPHA(1) = DEFAULT_ALPHA2;
ALPHA(2) = DEFAULT_ALPHA3;
ALPHA(3) = DEFAULT_ALPHA4;
ALPHA(4) = DEFAULT_ALPHA5;
ALPHA(5) = DEFAULT_ALPHA6;
D(0) = DEFAULT_D1;
D(1) = DEFAULT_D2;
D(2) = DEFAULT_D3;
D(3) = DEFAULT_D4;
D(4) = DEFAULT_D5;
D(5) = DEFAULT_D6;
/* 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();
if (inverse) {
retval = kinematicsInverse(&pos, joints, &iflags, &fflags);
if (0 != retval) {
printf("inv kins error %d\n", retval);
}
} else {
retval = kinematicsForward(joints, &pos, &fflags, &iflags);
if (0 != retval) {
printf("fwd kins error %d\n", retval);
}
}
end = timestamp();
printf("calculation time: %f secs\n", (end - start));
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 {
//FIXME-AJ
//disabled for now 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 {
//FIXME-AJ
//disabled for now 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 {
//FIXME-AJ
//disabled for now 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 {
//FIXME-AJ
//disabled for now 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
}
/*
Interactive testing of kins.
Syntax: a.out <Bx> <Cx> <Cy>
*/
int main(int argc, char *argv[])
{
#ifndef BUFFERLEN
#define BUFFERLEN 256
#endif
char buffer[BUFFERLEN];
char cmd[BUFFERLEN];
EmcPose pos, vel;
double joints[3], jointvels[3];
char inverse;
char flags;
KINEMATICS_FORWARD_FLAGS fflags;
inverse = 0; /* forwards, by default */
flags = 0; /* didn't provide flags */
fflags = 0; /* above xy plane, by default */
if (argc != 4 ||
1 != sscanf(argv[1], "%lf", &Bx) ||
1 != sscanf(argv[2], "%lf", &Cx) ||
1 != sscanf(argv[3], "%lf", &Cy)) {
fprintf(stderr, "syntax: %s Bx Cx Cy\n", argv[0]);
return 1;
}
while (! feof(stdin)) {
if (inverse) {
printf("inv> ");
}
else {
printf("fwd> ");
}
fflush(stdout);
if (NULL == fgets(buffer, BUFFERLEN, stdin)) {
break;
}
if (1 != sscanf(buffer, "%s", cmd)) {
continue;
}
if (! strcmp(cmd, "quit")) {
break;
}
if (! strcmp(cmd, "i")) {
inverse = 1;
continue;
}
if (! strcmp(cmd, "f")) {
inverse = 0;
continue;
}
if (! strcmp(cmd, "ff")) {
if (1 != sscanf(buffer, "%*s %d", &fflags)) {
printf("need forward flag\n");
}
continue;
}
if (inverse) { /* inverse kins */
if (3 != sscanf(buffer, "%lf %lf %lf",
&pos.tran.x,
&pos.tran.y,
&pos.tran.z)) {
printf("need X Y Z\n");
continue;
}
if (0 != kinematicsInverse(&pos, joints, NULL, &fflags)) {
printf("inverse kin error\n");
}
else {
printf("%f\t%f\t%f\n", joints[0], joints[1], joints[2]);
if (0 != kinematicsForward(joints, &pos, &fflags, NULL)) {
printf("forward kin error\n");
}
else {
printf("%f\t%f\t%f\n", pos.tran.x, pos.tran.y, pos.tran.z);
}
}
}
else { /* forward kins */
if (flags) {
if (4 != sscanf(buffer, "%lf %lf %lf %d",
&joints[0],
&joints[1],
&joints[2],
&fflags)) {
printf("need 3 strut values and flag\n");
continue;
}
}
else {
if (3 != sscanf(buffer, "%lf %lf %lf",
&joints[0],
&joints[1],
&joints[2])) {
printf("need 3 strut values\n");
continue;
}
}
if (0 != kinematicsForward(joints, &pos, &fflags, NULL)) {
printf("forward kin error\n");
}
else {
printf("%f\t%f\t%f\n", pos.tran.x, pos.tran.y, pos.tran.z);
if (0 != kinematicsInverse(&pos, joints, NULL, &fflags)) {
printf("inverse kin error\n");
}
else {
printf("%f\t%f\t%f\n", joints[0], joints[1], joints[2]);
}
}
}
} /* end while (! feof(stdin)) */
return 0;
}
