home
***
CD-ROM
|
disk
|
FTP
|
other
***
search
/
Between Heaven & Hell 2
/
BetweenHeavenHell.cdr
/
500
/
470
/
rccl035
< prev
next >
Wrap
Text File
|
1987-03-02
|
11KB
|
514 lines
/*
* RCCL Version 1.0 Author : Vincent Hayward
* School of Electrical Engineering
* Purdue University
* Dir : src
* File : solut.c
* Remarks : kinematic functions (solve handpos).
* Usage : part of the library
*/
/*
* jns_to_tr is handpos, it also computes the configuration (in a string )
* solveconf just compute the configuration
* tr_to_jns is solve, it has static conf flags and get the changes
*/
#include "../h/which.h"
#include "../h/rccl.h"
#include "../h/manip.h"
#include "../h/kine.h"
#include "../h/umac.h"
/* performs mod 2 pi */
static double range(a) /*##*/
real a;
{
if (a > 15. || a < -15.)
return(10.);
while(a >= pit2_m)
a -= pit2_m;
while(a < 0.)
a += pit2_m;
if (a > pit2_m)
return(0.);
return(a);
}
#ifdef PUMA
jns_to_tr_n(t, j, u) /*::*/
register TRSF_PTR t;
register JNTS_PTR j;
bool u;
{
real c1, s1, c2, s2, c23, s23, c4, s4, c5, s5, c6, s6,
k1, k2, k3, k4, k5, k6, k7, x;
SINCOS(s1, c1, j->th1 + jmin_c.th1);
SINCOS(s2, c2, j->th2 + jmin_c.th2);
SINCOS(s23, c23, j->th2 + jmin_c.th2 + j->th3 + jmin_c.th3);
SINCOS(s4, c4, j->th4 + jmin_c.th4);
SINCOS(s5, c5, j->th5 + jmin_c.th5);
SINCOS(s6, c6, j->th6 + jmin_c.th6);
k1 = c23 * s5;
k2 = c4 * c5 * s6 + s4 * c6;
k3 = - c23 * k2 + s23 * s5 * s6;
k4 = - s4 * c5 * s6 + c4 * c6;
k5 = s4 * s5;
k6 = c23 * c4 * s5 + s23 * c5;
k7 = armk_c.d4 * s23 + armk_c.a3 * c23 + armk_c.a2 * c2;
t->o.x = c1 * k3 - s1 * k4;
t->o.y = s1 * k3 + c1 * k4;
t->o.z = s23 * k2 + k1 * s6;
t->a.x = c1 * k6 - s1 * k5;
t->a.y = s1 * k6 + c1 * k5;
t->a.z = - s23 * c4 * s5 + c23 * c5;
t->p.x = c1 * k7 - s1 * armk_c.d3;
t->p.y = s1 * k7 + c1 * armk_c.d3;
t->p.z = armk_c.d4 * c23 - armk_c.a3 * s23 - armk_c.a2 * s2;
/* n = o X a */
t->n.x = t->o.y * t->a.z - t->o.z * t->a.y;
t->n.y = t->o.z * t->a.x - t->o.x * t->a.z;
t->n.z = t->o.x * t->a.y - t->o.y * t->a.x;
x = range(atan2(t->p.y, t->p.x) - j->th1 - jmin_c.th1);
(j->conf)[0] = (x < pib2_m || x > pi_m + pib2_m) ? 'l' : 'r';
x = range(armk_c.aa3d4 - j->th3 - jmin_c.th3);
(j->conf)[1] = (x < pib2_m || x > pi_m + pib2_m) ? 'u' : 'd';
(j->conf)[2] = (range(j->th5 + jmin_c.th5) > pi_m) ? 'f' : 'n';
(j->conf)[3] = '\0';
if (u) {
sncs_d.s1 = s1;
sncs_d.c1 = c1;
sncs_d.s2 = s2;
sncs_d.c2 = c2;
sncs_d.s23 = s23;
sncs_d.c23 = c23;
SINCOS(sncs_d.s3, sncs_d.c3, j->th3 + jmin_c.th3);
sncs_d.s4 = s4;
sncs_d.c4 = c4;
sncs_d.s5 = s5;
sncs_d.c5 = c5;
sncs_d.s6 = s6;
sncs_d.c6 = c6;
}
}
tr_to_jns_n(j, t, u) /*::*/
register JNTS_PTR j;
register TRSF_PTR t;
{
int code = 0;
real t1d, f11p, d, t3d, f13a, f11a, f31a, f13o, f11o, w1, w2, wd,
s1, c1, s23, c23, s3, c3, s4, c4, s5, c5, s6, c6,
px, py, pz, ax, ay, az, ox, oy, oz,
t1, t2, t3, t4, t5, t6;
static real t4o;
static lefty = NO, up = YES, flip = NO;
char *c = j->conf;
while (*c) {
switch (*c++) {
case 'l' :
lefty = YES;
break;
case 'r' :
lefty = NO;
break;
case 'u' :
up = YES;
break;
case 'd' :
up = NO;
break;
case 'f' :
flip = YES;
break;
case 'n' :
flip = NO;
break;
default :
break;
}
}
px = t->p.x;
py = t->p.y;
pz = t->p.z;
ax = t->a.x;
ay = t->a.y;
az = t->a.z;
ox = t->o.x;
oy = t->o.y;
oz = t->o.z;
/* theta1 */
t1d = sqrt(px * px + py * py - armk_c.d32);
t1 = atan2(py, px) - atan2(armk_c.d3, (lefty) ? t1d : - t1d);
if ((j->th1 = range (t1 - jmin_c.th1)) > jrng_c.th1)
code |= 01;
SINCOS(s1, c1, t1);
/* theta3 */
f11p = c1 * px + s1 * py;
d = f11p * f11p + pz * pz - armk_c.e432;
t3d = sqrt(armk_c.e4aa4ad - d * d);
t3 = armk_c.aa3d4 - atan2(d, (up) ? t3d : - t3d);
if ((j->th3 = range (t3 - jmin_c.th3)) > jrng_c.th3)
code |= 04;
SINCOS(s3, c3, t3);
/* theta2 */
w1 = armk_c.a2 * c3 + armk_c.a3;
w2 = armk_c.d4 + armk_c.a2 * s3;
wd = f11p * f11p + pz * pz;
if (FABS(wd) < SMALL) {
return(01000);
}
s23 = (w2 * f11p - w1 * pz) / wd;
c23 = (w1 * f11p + w2 * pz) / wd;
t2 = atan2(s23, c23) - t3;
if ((j->th2 = range (t2 - jmin_c.th2)) > jrng_c.th2)
code |= 02;
/* theta4 */
f13a = - s1 * ax + c1 * ay;
f11a = c1 * ax + s1 * ay;
f31a = c23 * f11a - s23 * az;
t4 = (flip) ? atan2(- f13a, - f31a) : atan2( f13a, f31a);
if ((j->th4 = range (t4 - jmin_c.th4)) > jrng_c.th4)
code |= 010;
SINCOS(s4, c4, t4);
/* theta5 */
s5 = c4 * f31a + s4 * f13a ;
c5 = s23 * f11a + c23 * az;
t5 = atan2(s5, c5);
if (FABS(t5) < SMALL) {
j->th4 = range(t4o - jmin_c.th4);
SINCOS(s4, c4, t4o);
s5 = 0.;
c5 = 1.;
code |= 0400;
}
else {
t4o = t4;
}
if ((j->th5 = range (t5 - jmin_c.th5)) > jrng_c.th5)
code |= 020;
/* theta6 */
f13o = - s1 * ox + c1 * oy;
f11o = c1 * ox + s1 * oy;
s6 = - c5 * (c4 * (c23 * f11o - s23 * oz) + s4 * f13o)
+ s5 * (s23 * f11o + c23 * oz);
c6 = - s4 * (c23 * f11o - s23 * oz) + c4 * f13o;
t6 = atan2(s6, c6);
if ((j->th6 = range (t6 - jmin_c.th6)) > jrng_c.th6)
code |= 040;
if (u) {
sncs_d.s1 = s1;
sncs_d.c1 = c1;
SINCOS(sncs_d.s2, sncs_d.c2, t2);
sncs_d.s23 = s23;
sncs_d.c23 = c23;
sncs_d.s3 = s3;
sncs_d.c3 = c3;
sncs_d.s4 = s4;
sncs_d.c4 = c4;
sncs_d.s5 = s5;
sncs_d.c5 = c5;
sncs_d.s6 = s6;
sncs_d.c6 = c6;
}
return(code);
}
solveconf_n(j) /*::*/
register JNTS_PTR j;
{
real s1, c1, s23, c23, c2, px, py, k, x;
SINCOS(s1, c1, j->th1 + jmin_c.th1);
SINCOS(s23, c23, j->th2 + jmin_c.th2 + j->th3 + jmin_c.th3);
c2 = cos(j->th2 + jmin_c.th2);
k = armk_c.d4 * s23 + armk_c.a3 * c23 + armk_c.a2 * c2;
px = c1 * k - s1 * armk_c.d3;
py = s1 * k + c1 * armk_c.d3;
x = range(atan2(py, px) - j->th1 - jmin_c.th1);
(j->conf)[0] = (x < pib2_m || x > pi_m + pib2_m) ? 'l' : 'r';
x = range(armk_c.aa3d4 - j->th3 - jmin_c.th3);
(j->conf)[1] = (x < pib2_m || x > pi_m + pib2_m) ? 'u' : 'd';
(j->conf)[2] = (range(j->th5 + jmin_c.th5) > pi_m) ? 'f' : 'n';
(j->conf)[3] = '\0';
}
#endif
#ifdef STAN
jns_to_tr_n(t, j, u) /*::*/
register TRSF_PTR t;
register JNTS_PTR j;
bool u;
{
real c1, s1, c2, s2, d3, c4, s4, c5, s5, c6, s6,
k1, k2, k3, k4, k5, k6, k7, x;
SINCOS(s1, c1, j->th1 + jmin_c.th1);
SINCOS(s2, c2, j->th2 + jmin_c.th2);
SINCOS(s4, c4, j->th4 + jmin_c.th4);
SINCOS(s5, c5, j->th5 + jmin_c.th5);
SINCOS(s6, c6, j->th6 + jmin_c.th6);
d3 = j->th3 + jmin_c.th3;
k1 = c4 * s5;
k2 = s4 * s5;
k3 = c4 * c5 * s6 + s4 * c6;
k4 = - c2 * k3 + s2 * s5 * s6;
k5 = - s4 * c5 * s6 + c4 * c6;
k6 = c2 * k1 + s2 * c5;
k7 = s2 * d3;
t->o.x = c1 * k4 - s1 * k5;
t->o.y = s1 * k4 + c1 * k5;
t->o.z = s2 * k3 + c2 * s5 * s6;
t->a.x = c1 * k6 - s1 * k2;
t->a.y = s1 * k6 + c1 * k2;
t->a.z = - s2 * k1 + c2 * c5;
t->p.x = c1 * k7 - s1 * armk_c.d2;
t->p.y = s1 * k7 + c1 * armk_c.d2;
t->p.z = c2 * d3;
/* n = o X a */
t->n.x = t->o.y * t->a.z - t->o.z * t->a.y;
t->n.y = t->o.z * t->a.x - t->o.x * t->a.z;
t->n.z = t->o.x * t->a.y - t->o.y * t->a.x;
x = range(atan2(t->p.y, t->p.x) - j->th1 - jmin_c.th1);
(j->conf)[0] = (x < pib2_m || x > pi_m + pib2_m) ? 'l' : 'r';
(j->conf)[1] = (range(j->th5 + jmin_c.th5) > pi_m) ? 'f' : 'n';
(j->conf)[2] = '\0';
if (u) {
sncs_d.s1 = s1;
sncs_d.c1 = c1;
sncs_d.s2 = s2;
sncs_d.c2 = c2;
sncs_d.d3 = d3;
sncs_d.s4 = s4;
sncs_d.c4 = c4;
sncs_d.s5 = s5;
sncs_d.c5 = c5;
sncs_d.s6 = s6;
sncs_d.c6 = c6;
}
}
tr_to_jns_n(j, t, u) /*::*/
register JNTS_PTR j;
register TRSF_PTR t;
bool u;
{
int code = 0;
real s1, c1, s2, c2, s4, c4, s5, c5, s6, c6,
f11p, f13a, f11a, f21a, f11o, f21o, f22o,
px, py, pz, ax, ay, az, ox, oy, oz,
t1d, t1, t2, d3, t4, t5, t6;
static real t4o;
static int lefty = YES, flip = NO;
char *c = j->conf;
while (*c) {
switch (*c++) {
case 'l' :
lefty = YES;
break;
case 'r' :
lefty = NO;
break;
case 'f' :
flip = YES;
break;
case 'n' :
flip = NO;
break;
default :
break;
}
}
px = t->p.x;
py = t->p.y;
pz = t->p.z;
ax = t->a.x;
ay = t->a.y;
az = t->a.z;
ox = t->o.x;
oy = t->o.y;
oz = t->o.z;
/* theta1 */
t1d = sqrt(px * px + py * py - armk_c.d22);
t1 = atan2(py, px) - atan2(armk_c.d2, (lefty) ? t1d : - t1d);
if ((j->th1 = range (t1 - jmin_c.th1)) > jrng_c.th1)
code |= 01;
SINCOS(s1, c1, t1);
/* theta2 */
t2 = atan2(f11p = c1 * px + s1 * py, pz);
if ((j->th2 = range (t2 - jmin_c.th2)) > jrng_c.th2)
code |= 02;
SINCOS(s2, c2, t2);
/* dist3 */
d3 = s2 * f11p + c2 * pz;
if ((j->th3 = d3 - jmin_c.th3) > jrng_c.th3 || j->th3 < 0.)
code |= 04;
/* theta4 */
f13a = - s1 * ax + c1 * ay;
f11a = c1 * ax + s1 * ay;
f21a = c2 * f11a - s2 * az;
t4 = (flip) ? atan2(- f13a, - f21a) : atan2(f13a, f21a);
if ((j->th4 = range (t4 - jmin_c.th4)) > jrng_c.th4)
code |= 010;
SINCOS(s4, c4, t4);
/* theta5 */
s5 = c4 * f21a + s4 * f13a ;
c5 = s2 * f11a + c2 * az;
t5 = atan2(s5, c5);
if (FABS(t5) < SMALL) {
j->th4 = range(t4o - jmin_c.th4);
SINCOS(s4, c4, t4o);
s5 = 0.;
c5 = 1.;
code |= 0400;
}
else {
t4o = t4;
}
if ((j->th5 = range (t5 - jmin_c.th5)) > jrng_c.th5)
code |= 020;
/* theta6 */
f11o = c1 * ox + s1 * oy;
f21o = c2 * f11o - s2 * oz;
f22o = - s1 * ox + c1 * oy;
s6 = - c5 * (c4 * f21o + s4 * f22o)
+ s5 * (s2 * f11o + c2 * oz);
c6 = - s4 * f21o + c4 * f22o;
t6 = atan2(s6, c6);
if ((j->th6 = range (t6 - jmin_c.th6)) > jrng_c.th6)
code |= 040;
if (u) {
sncs_d.s1 = s1;
sncs_d.c1 = c1;
sncs_d.s2 = s2;
sncs_d.c2 = c2;
sncs_d.d3 = d3;
sncs_d.s4 = s4;
sncs_d.c4 = c4;
sncs_d.s5 = s5;
sncs_d.c5 = c5;
sncs_d.s6 = s6;
sncs_d.c6 = c6;
}
return(code);
}
solveconf_n(j) /*::*/
register JNTS_PTR j;
{
real s1, c1, s2, px, py, x;
SINCOS(s1, c1, j->th1 + jmin_c.th1);
s2 = sin(j->th2 + jmin_c.th2);
x = s2 * (j->th3 + jmin_c.th3);
px = c1 * x - s1 * armk_c.d2;
py = s1 * x + c1 * armk_c.d2;
x = range(atan2(py, px) - j->th1 - jmin_c.th1);
(j->conf)[0] = (x < pib2_m || x > pi_m + pib2_m) ? 'l' : 'r';
(j->conf)[1] = (range(j->th5 + jmin_c.th5) > pi_m) ? 'f' : 'n';
(j->conf)[2] = '\0';
}
#endif