/******************************************************************
* Solve Inverse Kinematics
* @deltaX : delta for end-effector position < R 3
* @deltaP : delta for palm direction < R 3
* @restQ : rest joint angles
* @deltaQ : output joint angles
******************************************************************/
void sKinematics::solveIKPalm(double delta_x[], int axis, double delta_dir[], double q_rest[], double delta_q[])
{
int i;
double in[6];
double *q, *q_null;
q = svector(dof);
q_null = svector(dof);
getJacobianPalm(axis, OJ );
for( i=0; i<3; i++ ){
in[i ] = delta_x[i];
in[i+3] = delta_dir[i];
}
//matsvdinv(OJ, 6, dof, OJi );
matsvdinvDLS(OJ, 6, dof, 0.01, OJi );
matmul_vec(OJi, dof, 6, in, delta_q );
// pseudo null space motion
getJoints(q);
matsub_c1(dof, q_rest, q, q_null );
for(i=0; i<dof; i++ ) q_null[i] *= lamda;
matnull(6, dof, OJ, OJi, N);
matmul_vec( N, dof, dof, q_null, q );
matadd_c1(dof, q, delta_q );
}
int rd_coords( FILE *istream, double ***ptr_pcoords, int *ncoords )
{
int n,i,status=0;
double **p_coords;
char line[MAXLENGTH],**p_parse;
p_coords = dmatrix(2,MAX_N_COORDS);
n=0;
while (fgets( line, MAXLENGTH, istream ) != NULL) {
if (n>=MAX_N_COORDS) {
fprintf(stderr,"ERROR: Too many sources. Max=%d\n",MAX_N_COORDS);
free_dmatrix(p_coords);
return(ERRNO_INPUT_ERROR);
}
p_parse = svector(2,MAXLENGTH);
splitstr(line,p_parse,SPACES);
*(*(p_coords+0)+n) = atof(p_parse[0]);
*(*(p_coords+1)+n) = atof(p_parse[1]);
n++;
free_svector(p_parse);
}
*ncoords = n;
*ptr_pcoords = dmatrix(2,*ncoords);
for (i=0;i<*ncoords;i++) {
*(*(*ptr_pcoords + 0) + i) = *(*(p_coords+0)+i);
*(*(*ptr_pcoords + 1) + i) = *(*(p_coords+1)+i);
}
free_dmatrix(p_coords);
return(status);
}
void spolint (const float *xa, const float *ya, int_t n,
float x, float *y, float *dy)
{
register int_t ns = 1;
#if defined(__uxp__) || defined(_SX)
float dif = (float) fabs (x - xa[1]),
#else
float dif = fabsf(x - xa[1]),
#endif
*c = svector(1, n),
*d = svector(1, n);
register int_t i, m;
float den, dift, ho, hp, w;
for (i = 1; i <= n; i++) {
#if defined(__uxp__) || defined(_SX)
if ((dift = (float) fabs (x - xa[1])) < dif) {
#else
if ((dift = fabsf (x - xa[i])) < dif) {
#endif
ns = i;
dif = dift;
}
c[i] = ya[i];
d[i] = ya[i];
}
*y = ya[ns--];
for (m = 1; m < n; m++) {
for (i = 1; i <= n - m; i++) {
ho = xa[i] - x;
hp = xa[i+m] -x;
w = c[i+1] - d[i];
if( (den = ho-hp) == 0.0F)
message("spolint()", "two x values the same (within roundoff)", ERROR);
den = w / den;
d[i] = hp * den;
c[i] = ho * den;
}
*y += (*dy=((ns<<1) < (n-m) ? c[ns+1] : d[ns--]));
}
freeSvector (c, 1);
freeSvector (d, 1);
}
/******************************************************************
* Set Inverse points
*
******************************************************************/
void sKinematics::initIKpoints(int nPoints, int points_index[], double points_weight[])
{
int i;
int row = nPoints*3;
ik_points_index = (int *)malloc(nPoints*sizeof(int));
ik_points_weight = svector(nPoints);
vweight = svector(row);
this->nPoints = nPoints;
for(i=0; i<nPoints; i++){
ik_points_index[i] = points_index[i];
ik_points_weight[i] = points_weight[i];
vweight[i*3+0] = points_weight[i];
vweight[i*3+1] = points_weight[i];
vweight[i*3+2] = points_weight[i];
}
J = smatrix(3, dof);
FJ = smatrix(row, dof);
FJi = smatrix(dof, row);
}
SHORTVECTOR *new_shortvector(long nh)
{
SHORTVECTOR *m;
m=(SHORTVECTOR *)malloc(sizeof(SHORTVECTOR));
if (!m) t_error("allocation failure in SHORTVECTOR()");
m->isdynamic=isDynamic;
m->nl=NL;
m->nh=nh;
m->co=svector(1,nh);
return m;
}
GMMStates* readGMMStatesFromFile(const char* file, unsigned int& num_states, unsigned int& num_vars)
{
FILE *fid;
int res;
double dtemp;
fid = fopen(file, "r");
if(!fid)
{
cout<<"Error opening file \""<<file<<"\"\n";
throw;
}
res=fscanf(fid, "%lf\n", &dtemp);
num_states = (int)dtemp;
res=fscanf(fid, "%lf\n", &(dtemp));
num_vars = (int)dtemp;
GMMStates* GMMState = (GMMStates *)malloc(num_states*sizeof(GMMStates ) );
for( unsigned int s=0; s<num_states; s++ ){
GMMState[s].Mu = svector(num_vars);
GMMState[s].Sigma = smatrix(num_vars, num_vars );
}
// Read priors
for( unsigned int s=0; s<num_states; s++ )
res=fscanf(fid, "%lf", &(GMMState[s].Prio ) );
// Read Mu
for( unsigned int i=0; i<num_vars; i++ )
for( unsigned int s=0; s<num_states; s++ )
res=fscanf(fid, "%lf", &(GMMState[s].Mu[i]) );
// Read Sigmas
for( unsigned int s=0; s<num_states; s++ )
for( unsigned int i=0; i<num_vars; i++ )
for( unsigned int j=0; j<num_vars; j++ )
res=fscanf(fid, "%lf", &(GMMState[s].Sigma[i][j]));
fclose(fid);
return GMMState;
}
void sspline (integer n, float yp1, float ypn, const float* x, const float* y,
float* y2)
{
register integer i, k;
float h = x[1] - x[0],
*u = svector(0, n-2);
float p, qn, sig, un, hh;
if (yp1 > 0.99e30)
y2[0] = u[0] = 0.0F;
else {
y2[0] = -0.5F;
u [0] = (3.0F / h) * ((y[1]-y[0]) / h - yp1);
}
for (i = 1; i < n-1; i++) {
hh = 1. / (x[i+1] - x[i-1]);
sig = h * hh;
p = sig * y2[i-1] + 2.;
y2[i] = (sig - 1.) / p;
u [i] = (y[i] - y[i-1]) / h;
u [i] = (y[i+1] - y[i]) / (h = x[i+1]-x[i]) - u[i];
u [i] = (6.0F * u[i] * hh - sig * u[i-1])/p;
}
if (ypn > 0.99e30)
qn = un = 0.0F;
else {
qn = 0.5F;
un = (3.0F / h)*(ypn - (y[n-1]-y[n-2])/h);
}
y2[n-1] = (un - qn * u[n-2]) / (qn * y2[n-2] + 1.);
for (k = n-2; k; k--) y2[k] = y2[k] * y2[k+1] + u[k];
freeSvector(u, 0);
}
int main(int argc, char* argv[])
{
int i = 0;
int data = 0;
CVector<int>* dvector = new CVector<int>(10);
for(i = 0; i < 20; i++)
{
dvector->append(i);
assert(dvector->get(i, data) && data == i);
}
CVector<int> svector(*dvector);
CVector<int> avector = *dvector;
for(i = 0; i < 20; i++)
{
assert(svector.get(i, data) && data == i);
}
delete dvector;
return 0;
}
/******************************************************************
* Solve Inverse Kinematics
******************************************************************/
double sKinematics::solveIKPalmItr(double x_dest[], int axis, double dir_dest[], double q_rest[], double q_out[])
{
int i,j;
double *q_old, *q_curr, *q_delta, *q_null;
double x[3], dir[3];
double ox[6];
double error_old, error;
error_old = 1000.0; // set maximum error;
q_old = svector(dof);
q_curr = svector(dof);
q_delta= svector(dof);
q_null = svector(dof);
while(1){
getJacobianPalm(axis, OJ );
getEndPos(x);
getEndDirAxis(axis, dir);
getJoints(q_curr);
for( i=0; i<3; i++ ){
ox[i ] = x_dest[i]-x[i];
ox[i+3] = dir_dest[i] - dir[i];
}
error = vnorm(6, ox );
//printf("%f \n", error );
// stop when the error reach to a local minimum
if( error < IK_TOLERANCE ){
matcp_c1(dof, q_curr, q_out );
return error;
}
else if( abs(error_old - error) < IK_TOLERANCE ){
matcp_c1(dof, q_curr, q_out );
return error;
}
else if( error > error_old ){
matcp_c1(dof, q_old, q_out );
return error_old;
}
matcp_c1(dof, q_curr, q_old );
// solve inverse kinematics
matsvdinvDLS(OJ, 6, dof, 0.01, OJi );
// set weight
for( i=0; i<dof; i++ ){
for( j=0; j<6; j++ ){
OJi[i][j] *= sDH[active_index[i]].weight;
}
}
matnull(6, dof, OJ, OJi, N );
matsub_c1(dof, q_rest, q_curr, q_null );
for(i=0; i<dof; i++ ) q_null[i] *= lamda;
matmul_vec(OJi, dof, 6, ox, q_delta );
matadd_c1(dof, q_delta, q_curr );
matmul_vec( N, dof, dof, q_null, q_delta );
matadd_c1(dof, q_delta, q_curr );
checkVelocityLimit(q_old, q_curr, deltaT);
setJoints(q_curr);
getEndPos(x);
getEndDirAxis(axis, dir);
for( i=0; i<3; i++ ){
ox[i ] = x_dest[i]-x[i];
ox[i+3] = dir_dest[i] - dir[i];
}
error = vnorm(6, ox );
error_old = error;
}
}
double sKinematics::solveIKpointsItr(double x_dest[], double q_out[])
{
int i, j, k;
double *q_old, *q_curr, *q_delta, *q_init;
double x[3];
double *ox_delta;
double error_old, error;
int dim = 3*nPoints;
// set maximum error;
error_old = 100000.0;
q_old = svector(dof);
q_curr = svector(dof);
q_delta = svector(dof);
q_init = svector(dof);
ox_delta= svector(dim);
getJoints(q_old);
matcp_c1(dof, q_old, q_init );
while(1){
// get current joint
getJoints(q_curr);
// get jacobian and delta x
for( i=0; i<nPoints; i++){
getJacobianPos(ik_points_index[i], J);
for( j=0; j<3; j++ ){
for( k=0; k<dof; k++ ){
FJ[i*3+j][k] = J[j][k];
}
}
getEndPos(ik_points_index[i], x);
for( j=0; j<3; j++ ) {
ox_delta[i*3+j] = x_dest[i*3+j]-x[j];
}
}
// calculate error
error = 0;
for( i=0; i<dim; i++ ){
error += ox_delta[i]*ox_delta[i]*vweight[i];
}
error = sqrt(error);
//printf("%f \n", error );
// exit when the error reach a local minimum
if( error < IK_TOLERANCE ){
matcp_c1(dof, q_curr, q_out );
return error;
}
else if( abs(error_old - error) < IK_TOLERANCE*0.1 ){
matcp_c1(dof, q_curr, q_out );
return error;
}
else if( error > error_old ){
matcp_c1(dof, q_old, q_out );
return error_old;
}
// solve IK
matsvdinvDLS(FJ, dim, dof, 0.5, FJi );
matmul_vec(FJi, dof, dim, ox_delta, q_delta );
matcp_c1(dof, q_curr, q_old ); // backup joint angles
matadd_c1(dof, q_delta, q_curr ); // set current joint
checkVelocityLimit(q_init, q_curr, deltaT);
setJoints(q_curr);
error_old = error;
}
return error;
}
int artimg_snr(FILE *fstream, ARTIMGPARS pars, double **ppix,
KLFITS_HEADER *headers, int *nheaders)
{
int ncol, status=0;
long int xcenter,ycenter,ii,jj;
double gcd,gcl,diameter,sb;
double pc2pix,lum2flux,gcdpix,radius,sbinst,r;
char line[2*MAXLENGTH],**p_parse;
KLFITS_HEADER *h;
pc2pix = (pars.artimg_distance * pars.artimg_pixscale) / SECPERRAD;
lum2flux = 1./(4*PI*pow(pars.artimg_distance*MPERPC,2.));
p_parse=svector(MKSNRPOP_OUTPUT_NCOL,MAXLENGTH);
while (fgets( line, 2*MAXLENGTH, fstream ) != NULL) {
if (!strncmp(line,"#",1)) {
h = create_list_klfits_header();
if (status = parse_popheader(line, h)) {
fprintf(stderr,"ERROR: Parsing header in artimg_snr.\n");
fprintf(stderr,ERRMSG_INPUT_ERROR,line);
return(status);
}
if ( h->datatype != 0 ) {
add_klfits_header( h, &headers );
(*nheaders)++;
}
h = NULL;
continue;
}
ncol = splitstr(line,p_parse,SPACES);
if (ncol != MKSNRPOP_OUTPUT_NCOL) {
fprintf(stderr,ERRMSG_INPUT_ERROR,"");
return(ERRNO_INPUT_ERROR);
}
gcd = atof(p_parse[0]);
gcl = atof(p_parse[1]);
diameter = atof(p_parse[2]);
sb = atof(p_parse[3]);
gcdpix = gcd / pc2pix;
xcenter = (long int)(pars.artimg_x0 + cos(gcl*PI/180.)*gcdpix);
ycenter = (long int)(pars.artimg_y0 + sin(gcl*PI/180.)*gcdpix);
radius = 0.5 * diameter / pc2pix;
if ( (xcenter+radius < 1) || (xcenter-radius > pars.artimg_naxes[0]) ||
(ycenter+radius < 1) || (ycenter-radius > pars.artimg_naxes[1]) ) {
continue;
}
sbinst = sb * pow(pc2pix,2) * lum2flux / pars.artimg_fluxcalib;
for (jj=ycenter-radius-1; jj<ycenter+radius; jj++) {
if ( (jj>=0) && (jj<pars.artimg_naxes[1]) ) {
for (ii=xcenter-radius-1; ii<xcenter+radius; ii++) {
if ( (ii>=0) && (ii<pars.artimg_naxes[0]) ) {
r = sqrt( pow(ii-xcenter,2.) + pow(jj-ycenter,2.) );
if ( r <= radius ) { *(*(ppix+jj)+ii) += sbinst; }
}
}
}
}
}
free_svector(p_parse);
return(status);
}
