// this is the method used in starlight aurora vsx, quite handy actually void beginBlobs(vsx_module_engine_info* engine) { glEnable(GL_TEXTURE_2D); GLfloat tmpMat[16]; glGetFloatv(GL_MODELVIEW_MATRIX, blobMat); glMatrixMode(GL_PROJECTION); glPushMatrix(); glGetFloatv(GL_PROJECTION_MATRIX, tmpMat); tmpMat[3] = 0; tmpMat[7] = 0; tmpMat[11] = 0; tmpMat[12] = 0; tmpMat[13] = 0; tmpMat[14] = 0; tmpMat[15] = 1; v_norm(tmpMat); v_norm(tmpMat + 4); v_norm(tmpMat + 8); glLoadIdentity(); glMultMatrixf(tmpMat); blobMat[3] = 0; blobMat[7] = 0; blobMat[11] = 0; blobMat[12] = 0; blobMat[13] = 0; blobMat[14] = 0; blobMat[15] = 1; v_norm(blobMat); v_norm(blobMat + 4); v_norm(blobMat + 8); glMultMatrixf(blobMat); glGetFloatv(GL_PROJECTION_MATRIX, blobMat); glPopMatrix(); glMatrixMode(GL_MODELVIEW); //inv_mat(blobMat, blobMatInv); GLfloat upLeft[] = {-0.5f, 0.5f, 0.0f, 1.0f}; GLfloat upRight[] = {0.5f, 0.5f, 0.0f, 1.0f}; mat_vec_mult(blobMat, upLeft, blobVec0); mat_vec_mult(blobMat, upRight, blobVec1); }
/*!***************************************************************************** ******************************************************************************* \note w2i \date Jul 2000, Tom \remarks transfer coordinate from the world to the images ******************************************************************************* Function Parameters: [in]=input,[out]=output \param[in] pos : 3D position of an object \param[out] xy_l : projected 2D position of the object for the left eye \param[out] xy_r : projected 2D position of the object for the right eye ******************************************************************************/ int w2i (Vector pos, Vector xy_l, Vector xy_r) { Vector tmp; tmp = my_vector (1, 3); mat_vec_mult (Pl, pos, tmp); if (tmp[3] > 1.0e-4) { xy_l[1] = tmp[1]/tmp[3]; xy_l[2] = tmp[2]/tmp[3]; } else { printf ("numerical problem during calculation of a optical projection\n"); return FALSE; } mat_vec_mult (Pr, pos, tmp); if (tmp[3] > 1.0e-4) { xy_r[1] = tmp[1]/tmp[3]; xy_r[2] = tmp[2]/tmp[3]; } else { printf ("numerical problem during calculation of a optical projection\n"); return FALSE; } my_free_vector (tmp, 1,3); return TRUE; }
static double* inv_mul_full(double* a, double* x, int n, mat u, mat v, mat q_t, mat r) { int i; double* a_x = inv_mul_ax(a,x,n); //A^{-1}x double* tmp1 = mat_vec_mult(v,a_x); //VA^{-1}x double* tmp2 = mat_vec_mult(q_t,tmp1); //Q^TVA^{-1]x upper_tri_solve(r,tmp2); //R^{-1}Q^TVA^{-1}x free(tmp1); tmp1 = mat_vec_mult(u,tmp2); //UR^{-1}Q^TVA^{-1}x free(tmp2); tmp2 = inv_mul_ax(a,tmp1,n); //A^{-1}UR^{-1}Q^TVA^{-1}x for(i = 0; i < n; i++) { //A^{-1}x-A^{-1}UR^{-1}Q^TVA^{-1}x a_x[i] = a_x[i] - tmp2[i]; } free(tmp1); free(tmp2); return a_x; }
void BaseStateEstimation::transformImuStateToBase() { MY_MATRIX(S_angrate, 1,3,1,3); vectorToSkewMatrix(O_angrate_I_, S_angrate); MY_MATRIX(S_angacc, 1,3,1,3); vectorToSkewMatrix(O_angacc_I_, S_angacc); MY_MATRIX(S_helper, 1,3,1,3); mat_mult(S_angrate, S_angrate, S_helper); mat_add(S_angacc, S_helper, S_helper); mat_mult(O_rot_I_, I_rot_B_, O_rot_B_); mat_vec_mult(O_rot_I_, I_linpos_B_, O_linpos_B_); vec_add(O_linpos_I_, O_linpos_B_, O_linpos_B_); mat_vec_mult(O_rot_I_, I_linpos_B_, O_linvel_B_); mat_vec_mult(S_angrate, O_linvel_B_, O_linvel_B_); vec_add(O_linvel_I_, O_linvel_B_, O_linvel_B_); mat_vec_mult(O_rot_I_, I_linpos_B_, O_linacc_B_); mat_vec_mult(S_helper, O_linacc_B_, O_linacc_B_); vec_add(O_linacc_I_, O_linacc_B_, O_linacc_B_); }
/* Adds correlated noise to a vector. Supplied with a square root of covariance matrix * TODO: supply covariance only * */ void add_noise(Vector vec, Matrix std) { int nr = std[0][0]; //number of rows //printf("Vector length: %d\n",nr); Vector noise = my_vector(1,nr); int i; for (i = 1; i <= nr; i++) { noise[i] = gaussian(noise[i],1.0); } mat_vec_mult(std,noise,noise); for (i = 1; i <= nr; i++) { vec[i] += noise[i]; } return; }
std::vector<double> MATHNS::rotate_vector(std::vector<double> X, std::string axis, double angle) { std::vector<double> rotated; double **rotMatrix; rotMatrix = mmemory->new_2d<double>(3,3); if (axis=="x") { rotMatrix[0][0] = 1; rotMatrix[0][1] = 0; rotMatrix[0][2] = 0; rotMatrix[1][0] = 0; rotMatrix[1][1] = cos(angle*D2R); rotMatrix[1][2] = -sin(angle*D2R); rotMatrix[2][0] = 0; rotMatrix[2][1] = sin(angle*D2R); rotMatrix[2][2] = cos(angle*D2R); } else if (axis=="y") { rotMatrix[0][0] = cos(angle*D2R); rotMatrix[0][1] = 0; rotMatrix[0][2] = sin(angle*D2R); rotMatrix[1][0] = 0; rotMatrix[1][1] = 1; rotMatrix[1][2] = 0; rotMatrix[2][0] = -sin(angle*D2R); rotMatrix[2][1] = 0; rotMatrix[2][2] = cos(angle*D2R); } else if (axis=="z") { rotMatrix[0][0] = cos(angle*D2R); rotMatrix[0][1] = -sin(angle*D2R); rotMatrix[0][2] = 0; rotMatrix[1][0] = sin(angle*D2R); rotMatrix[1][1] = cos(angle*D2R); rotMatrix[1][2] = 0; rotMatrix[2][0] = 0; rotMatrix[2][1] = 0; rotMatrix[2][2] = 1; } rotated = mat_vec_mult(3,rotMatrix,X); mmemory->destroy(rotMatrix); return rotated; }
int steepest_solver ( int n, double ** A, double * b, double * x ) { int row, col; double **AT, *ATb, **ATA; double *residual; AT = (double**) malloc (n*sizeof(double*)); ATb = (double*) malloc (n*sizeof(double)); ATA = (double**) malloc (n*sizeof(double*)); residual = (double*) malloc (n*sizeof(double)); // allocate for (row=0; row<n; row++) { AT[row] = (double*) malloc(n*sizeof(double)); ATA[row] = (double*) malloc(n*sizeof(double)); } // aux variables transpose (n, A, AT); mat_mat_mult (n, AT, A, ATA); mat_vec_mult (n, AT, b, ATb); // x is the vector that minimizes f(x) = 0.5(x^T)ATAx - ((ATb)^T)x // initialize int step = 0; for (row=0; row<n; row++) { residual[row] = ATb[row]; for (col=0; col<n; col++) residual[row] -= ATA[row][col]*x[col]; // in HCDC, x elements are non constant all but row number of terms are constant } // delta = rTr; double delta = vec_vec_mult (n,residual,residual); double delta_0 = delta; do { double alpha = delta / vec_mat_vec_mult (n, residual, ATA);// + DBL_EPSILON; for (row=0; row<n; row++) x[row] += alpha * residual[row]; for (row=0; row<n; row++) { residual[row] = ATb[row]; for (col=0; col<n; col++) residual[row] -= ATA[row][col]*x[col]; // in HCDC, x elements are non constant all but row number of terms are constant } delta = vec_vec_mult (n,residual,residual); step++; } while ( delta>DBL_EPSILON*delta_0 && step<(1<<20) ); for (row=0; row<n; row++) { free(AT[row]); free(ATA[row]); } free (AT); free (ATA); free (ATb); free (residual); return step; }
/*!***************************************************************************** ******************************************************************************* \note compute_force_sensors \date March 2003 \remarks This function computes simulated force sensor information for both feet. The results are force and torque values for in link coordinates of the last ankle joint. This informatin can be extracted from the contact force computation, which has world coordinates. Thus, only a simple coordinate transformatino from world to local link coordinates is needed. A small correction is need to transform the force/torque information to the exact position of the load cell center. ******************************************************************************* Function Parameters: [in]=input,[out]=output All results can be computed based on global variables. Results are assigned to the misc_sensor structures. ******************************************************************************/ static void compute_force_sensors(void) { int i,j; static int firsttime = TRUE; static Matrix M; static Vector v; static Vector vv; static Vector r; if (firsttime) { firsttime = FALSE; M = my_matrix(1,N_CART,1,N_CART); v = my_vector(1,N_CART); vv= my_vector(1,N_CART); r = my_vector(1,N_CART); } // rotation matrix from world to L_AAA coordinates mat_trans_size(Adof_sim[L_AAA],N_CART,N_CART,M); // transform forces for (i=1; i<=N_CART; ++i) v[i] = ucontact[L_AAA].f[i] - uext_sim[L_AAA].f[i]; mat_vec_mult(M,v,vv); misc_sim_sensor[L_CFx] = vv[_X_]; misc_sim_sensor[L_CFy] = vv[_Y_]; misc_sim_sensor[L_CFz] = vv[_Z_]; // transform torques for (i=1; i<=N_CART; ++i) v[i] = ucontact[L_AAA].t[i] - uext_sim[L_AAA].t[i]; mat_vec_mult(M,v,v); // correct for load cell offset (need to use local coordinates) r[_X_] = FTA_Z_OFF; r[_Y_] = FTA_X_OFF; r[_Z_] = -FTA_Y_OFF; vec_mult_outer(vv,r,vv); misc_sim_sensor[L_CTa] = v[_A_] + vv[_A_]; misc_sim_sensor[L_CTb] = v[_B_] + vv[_B_]; misc_sim_sensor[L_CTg] = v[_G_] + vv[_G_]; // rotation matrix from world to R_AAA coordinates mat_trans_size(Adof_sim[R_AAA],N_CART,N_CART,M); // transform forces for (i=1; i<=N_CART; ++i) v[i] = ucontact[R_AAA].f[i] - uext_sim[R_AAA].f[i]; mat_vec_mult(M,v,vv); misc_sim_sensor[R_CFx] = vv[_X_]; misc_sim_sensor[R_CFy] = vv[_Y_]; misc_sim_sensor[R_CFz] = vv[_Z_]; // transform torques for (i=1; i<=N_CART; ++i) v[i] = ucontact[R_AAA].t[i] - uext_sim[R_AAA].t[i]; mat_vec_mult(M,v,v); // correct for load cell offset (need to use local coordinates) r[_X_] = FTA_Z_OFF; r[_Y_] = FTA_X_OFF; r[_Z_] = FTA_Y_OFF; vec_mult_outer(vv,r,vv); misc_sim_sensor[R_CTa] = v[_A_] + vv[_X_]; misc_sim_sensor[R_CTb] = v[_B_] + vv[_Y_]; misc_sim_sensor[R_CTg] = v[_G_] + vv[_Z_]; }
/*!***************************************************************************** ******************************************************************************* \note compute_inertial \date Oct 2005 \remarks This functions computes simulated inertial signals, i.e., body quaternion, angular velocity, and linear acceleration, and linear acceleration at the feet ******************************************************************************* Function Parameters: [in]=input,[out]=output All results can be computed based on global variables. Results are assigned to the misc_sensor structures. ******************************************************************************/ static void compute_inertial(void) { int i,j; static int firsttime = TRUE; static Matrix R; static Matrix RT; static Vector ad; static Vector xdd; static Vector r; static Vector r0; static Vector t1; static Vector t2; static Vector t3; static Vector lv; static Vector rv; static Vector lv_last; static Vector rv_last; static Vector lacc; static Vector racc; if (firsttime) { R = my_matrix(1,N_CART,1,N_CART); RT = my_matrix(1,N_CART,1,N_CART); ad = my_vector(1,N_CART); xdd = my_vector(1,N_CART); r = my_vector(1,N_CART); r0 = my_vector(1,N_CART); t1 = my_vector(1,N_CART); t2 = my_vector(1,N_CART); t3 = my_vector(1,N_CART); lv = my_vector(1,N_CART); rv = my_vector(1,N_CART); lacc= my_vector(1,N_CART); racc= my_vector(1,N_CART); lv_last = my_vector(1,N_CART); rv_last = my_vector(1,N_CART); firsttime = FALSE; } // copy quaternion from base coordinates (perfect data) misc_sim_sensor[B_Q0_IMU] = base_orient.q[_Q0_]; misc_sim_sensor[B_Q1_IMU] = base_orient.q[_Q1_]; misc_sim_sensor[B_Q2_IMU] = base_orient.q[_Q2_]; misc_sim_sensor[B_Q3_IMU] = base_orient.q[_Q3_]; // the angular vel. and translatory acc are first computed in world coordinates // and then transformed to base coordinates. // need the base coordinate rotatin matrix, which transforms world to base quatToRotMat(&base_orient,R); mat_trans(R,RT); // offset is IMU_X_OFFSET and -IMU_Y_OFFSET r[_X_] = IMU_X_OFFSET; r[_Y_] = -IMU_Y_OFFSET; r[_Z_] = 0.0; // get offset vector in global coordinates mat_vec_mult(RT,r,r0); // w x r0 vec_mult_outer_size(base_orient.ad,r0,N_CART,t1); // w x (w x r0) = w x t1 (which is one term of xdd) vec_mult_outer_size(base_orient.ad,t1,N_CART,t2); // wd x r0 vec_mult_outer_size(base_orient.add,r0,N_CART,t1); // add to xdd vec_add(t1,t2,t1); // and add the base acceleration vec_add_size(t1,base_state.xdd,N_CART,t1); // add gravity to t1 t1[_Z_] -= gravity; // rotate all into base coordinates mat_vec_mult_size(R,N_CART,N_CART,base_orient.ad,N_CART,t3); mat_vec_mult_size(R,N_CART,N_CART,t1,N_CART,t2); // and rotate this information into IMU coordinates // IMU_X = -Z_BASE; IMU_Y = -X_BASE; IMU_Z = Y_BASE t1[_A_] = -PI/2.; t1[_B_] = 0.0; t1[_G_] = PI/2.; eulerToRotMat(t1,R); mat_vec_mult(R,t2,xdd); mat_vec_mult(R,t3,ad); // the final result misc_sim_sensor[B_AD_A_IMU] = ad[_A_]; misc_sim_sensor[B_AD_B_IMU] = ad[_B_]; misc_sim_sensor[B_AD_G_IMU] = ad[_G_]; misc_sim_sensor[B_XACC_IMU] = xdd[_X_]; misc_sim_sensor[B_YACC_IMU] = xdd[_Y_]; misc_sim_sensor[B_ZACC_IMU] = xdd[_Z_]; // now get the foot accelerations in WORLD coordinate, computed at the L_FOOT position, // which is not exactly the same as the load cell offset, but very close. computeLinkVelocity(L_FOOT, link_pos_sim, joint_origin_pos_sim, joint_axis_pos_sim, joint_sim_state, lv); computeLinkVelocity(R_FOOT, link_pos_sim, joint_origin_pos_sim, joint_axis_pos_sim, joint_sim_state, rv); for (i=1; i<=N_CART; ++i) { lacc[i] = (lv[i]-lv_last[i])*(double)simulation_servo_rate; lv_last[i] = lv[i]; racc[i] = (rv[i]-rv_last[i])*(double)simulation_servo_rate; rv_last[i] = rv[i]; } // transform to local foot coordinates after adding gravity lacc[_Z_] -= gravity; racc[_Z_] -= gravity; // rotation matrix from world to L_AAA coordinates mat_trans_size(Alink_sim[L_FOOT],N_CART,N_CART,R); mat_vec_mult(R,lacc,t1); // rotation matrix from world to R_AAA coordinates mat_trans_size(Alink_sim[R_FOOT],N_CART,N_CART,R); mat_vec_mult(R,racc,t2); #ifdef HAS_LOWER_BODY // the final result misc_sim_sensor[L_FOOT_XACC] = t1[_X_]; misc_sim_sensor[L_FOOT_YACC] = t1[_Y_]; misc_sim_sensor[L_FOOT_ZACC] = t1[_Z_]; misc_sim_sensor[R_FOOT_XACC] = t2[_X_]; misc_sim_sensor[R_FOOT_YACC] = t2[_Y_]; misc_sim_sensor[R_FOOT_ZACC] = t2[_Z_]; #endif }
/*!***************************************************************************** ******************************************************************************* \note parm_opt \date 10/20/91 \remarks this is the major optimzation program ******************************************************************************* Function Parameters: [in]=input,[out]=output \param[in] tol : error tolernance to be achieved \param[in] n_parm : number of parameters to be optimzed \param[in] n_con : number of contraints to be taken into account \param[in] f_dLda : function which calculates the derivative of the optimziation criterion with respect to the parameters; must return vector \param[in] f_dMda : function which calculates the derivate of the constraints with respect to parameters must return matrix \param[in] f_M : constraints function, must always be formulted to return 0 for properly fulfilled constraints \param[in] f_L : function to calculate simple cost (i.e., constraint cost NOT included), the constraint costs are added by this program automatically, the function returns a double scalar \param[in] f_dLdada : second derivative of L with respect to the parameters, must be a matrix of dim n_parm x n_parm \param[in] f_dMdada : second derivative of M with respect to parameters, must be a matrix n_con*n_parm x n_parm \param[in] use_newton: TRUE or FALSE to indicate that second derivatives are given and can be used for Newton algorithm \param[in,out] a : initial setting of parameters and also return of optimal value (must be a vector, even if scalar) \param[out] final_cost: the final cost \param[out] err : the sqrt of the squared error of all constraints NOTE: - program returns TRUE if everything correct, otherwise FALSE - always minimizes the cost!!! - algorithms come from Dyer McReynolds NOTE: besides the possiblity of a bug, the Newton method seems to sacrifice the validity of the constraint a little up to quite a bit and should be used prudently ******************************************************************************/ int parm_opt(double *a,int n_parm, int n_con, double *tol, void (*f_dLda)(), void (*f_dMda)(), void (*f_M)(), double (*f_L)(), void (*f_dMdada)(), void (*f_dLdada)(), int use_newton, double *final_cost, double *err) { register int i,j,n; double cost= 999.e30; double last_cost = 0.0; double *mult=NULL, *new_mult=NULL; /* this is the vector of Lagrange mulitplier */ double **dMda=NULL, **dMda_t=NULL; double *dLda; double *K=NULL; /* the error in the constraints */ double eps = 0.025; /* the learning rate */ double **aux_mat=NULL; /* needed for inversion of matrix */ double *aux_vec=NULL; double *new_a; double **dMdada=NULL; double **dLdada=NULL; double **A=NULL; /* big matrix, a combination of several other matrices */ double *B=NULL; /* a big vector */ double **A_inv=NULL; int rc=TRUE; long count = 0; int last_sign = 1; int pending1 = FALSE, pending2 = FALSE; int firsttime = TRUE; int newton_active = FALSE; dLda = my_vector(1,n_parm); new_a = my_vector(1,n_parm); if (n_con > 0) { mult = my_vector(1,n_con); dMda = my_matrix(1,n_con,1,n_parm); dMda_t = my_matrix(1,n_parm,1,n_con); K = my_vector(1,n_con); aux_mat = my_matrix(1,n_con,1,n_con); aux_vec = my_vector(1,n_con); } if (use_newton) { dLdada = my_matrix(1,n_parm,1,n_parm); A = my_matrix(1,n_parm+n_con,1,n_parm+n_con); A_inv = my_matrix(1,n_parm+n_con,1,n_parm+n_con); B = my_vector(1,n_parm+n_con); if (n_con > 0) { dMdada = my_matrix(1,n_con*n_parm,1,n_parm); new_mult = my_vector(1,n_con); } for (i=1+n_parm; i<=n_con+n_parm; ++i) { for (j=1+n_parm; j<=n_con+n_parm; ++j) { A[i][j] = 0.0; } } } while (fabs(cost-last_cost) > *tol) { ++count; pending1 = FALSE; pending2 = FALSE; AGAIN: /* calculate the current Lagrange multipliers */ if (n_con > 0) { (*f_M)(a,K); /* takes the parameters, returns residuals */ (*f_dMda)(a,dMda); /* takes the parameters, returns the Jacobian */ } (*f_dLda)(a,dLda); /* takes the parameters, returns the gradient */ if (n_con > 0) { mat_trans(dMda,dMda_t); } if (newton_active) { if (n_con > 0) { (*f_dMdada)(a,dMdada); } (*f_dLdada)(a,dLdada); } /* the first step is always a gradient step */ if (newton_active) { if (firsttime) { firsttime = FALSE; eps = 0.1; } /* build the A matrix */ for (i=1; i<=n_parm; ++i) { for (j=1; j<=n_parm; ++j) { A[i][j] = dLdada[i][j]; for (n=1; n<=n_con; ++n) { A[i][j] += mult[n]*dMdada[n+(i-1)*n_con][j]; } } } for (i=1+n_parm; i<=n_con+n_parm; ++i) { for (j=1; j<=n_parm; ++j) { A[j][i] = A[i][j] = dMda[i-n_parm][j]; } } /* build the B vector */ if (n_con > 0) { mat_vec_mult(dMda_t,mult,B); } for (i=1; i<=n_con; ++i) { B[i+n_parm] = K[i]; } /* invert the A matrix */ if (!my_inv_ludcmp(A, n_con+n_parm, A_inv)) { rc = FALSE; break; } mat_vec_mult(A_inv,B,B); vec_mult_scalar(B,eps,B); for (i=1; i<=n_parm; ++i) { new_a[i] = a[i] + B[i]; } for (i=1; i<=n_con; ++i) { new_mult[i] = mult[i] + B[n_parm+i]; } } else { if (n_con > 0) { /* the mulitpliers are updated according: mult = (dMda dMda_t)^(-1) (K/esp - dMda dLda_t) */ mat_mult(dMda,dMda_t,aux_mat); if (!my_inv_ludcmp(aux_mat, n_con, aux_mat)) { rc = FALSE; break; } mat_vec_mult(dMda,dLda,aux_vec); vec_mult_scalar(K,1./eps,K); vec_sub(K,aux_vec,aux_vec); mat_vec_mult(aux_mat,aux_vec,mult); } /* the update step looks the following: a_new = a - eps * (dLda + mult_t * dMda)_t */ if (n_con > 0) { vec_mat_mult(mult,dMda,new_a); vec_add(dLda,new_a,new_a); } else { vec_equal(dLda,new_a); } vec_mult_scalar(new_a,eps,new_a); vec_sub(a,new_a,new_a); } if (count == 1 && !pending1) { last_cost = (*f_L)(a); if (n_con > 0) { (*f_M)(a,K); last_cost += vec_mult_inner(K,mult); } } else { last_cost = cost; } /* calculate the updated cost */ cost = (*f_L)(new_a); /*printf(" %f\n",cost);*/ if (n_con > 0) { (*f_M)(new_a,K); if (newton_active) { cost += vec_mult_inner(K,new_mult); } else { cost += vec_mult_inner(K,mult); } } /* printf("last=%f new=%f\n",last_cost,cost); */ /* check out whether we reduced the cost */ if (cost > last_cost && fabs(cost-last_cost) > *tol) { /* reduce the gradient climbing rate: sometimes a reduction of eps causes an increase in cost, thus leave an option to increase eps */ cost = last_cost; /* reset last_cost */ if (pending1 && pending2) { /* this means that either increase nor decrease of eps helps, ==> leave the program */ rc = TRUE; break; } else if (pending1) { eps *= 4.0; /* the last cutting by half did not help, thus multiply by 2 to get to previous value, and one more time by 2 to get new value */ pending2 = TRUE; } else { eps /= 2.0; pending1 = TRUE; } goto AGAIN; } else { vec_equal(new_a,a); if (newton_active && n_con > 0) { vec_equal(new_mult,mult); } if (use_newton && fabs(cost-last_cost) < NEWTON_THRESHOLD) newton_active = TRUE; } } my_free_vector(dLda,1,n_parm); my_free_vector(new_a,1,n_parm); if (n_con > 0) { my_free_vector(mult,1,n_con); my_free_matrix(dMda,1,n_con,1,n_parm); my_free_matrix(dMda_t,1,n_parm,1,n_con); my_free_vector(K,1,n_con); my_free_matrix(aux_mat,1,n_con,1,n_con); my_free_vector(aux_vec,1,n_con); } if (use_newton) { my_free_matrix(dLdada,1,n_parm,1,n_parm); my_free_matrix(A,1,n_parm+n_con,1,n_parm+n_con); my_free_matrix(A_inv,1,n_parm+n_con,1,n_parm+n_con); my_free_vector(B,1,n_parm+n_con); if (n_con > 0) { my_free_matrix(dMdada,1,n_con*n_parm,1,n_parm); my_free_vector(new_mult,1,n_con); } } *final_cost = cost; *tol = fabs(cost-last_cost); if (n_con > 0) { *err = sqrt(vec_mult_inner(K,K)); } else { *err = 0.0; } /* printf("count=%ld rc=%d\n",count,rc); */ return rc; }
int lob_scaat(SCAATState *out, STORAGE_TYPE z, STORAGE_TYPE s_x, STORAGE_TYPE s_y, STORAGE_TYPE dt, SCAATState *in, STORAGE_TYPE q, STORAGE_TYPE R){ STORAGE_TYPE **A; STORAGE_TYPE **Q; STORAGE_TYPE **Q1; // temporary matrix STORAGE_TYPE opt_z=0.0; STORAGE_TYPE *H; STORAGE_TYPE *K; //Kalman gain STORAGE_TYPE **I; //unit matrix STORAGE_TYPE **v; // for the eigenvectors int *nrot; // for the Jacobi method of eigenvalue computation STORAGE_TYPE *d; SCAATState pred; // prediction int i, j; int add_noise = 0; // section for temporary variables STORAGE_TYPE *t_v1; // vector STORAGE_TYPE **t_m1; // matrix STORAGE_TYPE **t_m2; // matrix STORAGE_TYPE **t_m3; // matrix STORAGE_TYPE t_s; //temporary scalar value STORAGE_TYPE temp_sum = 0.0; STORAGE_TYPE min_d = 0.0; // program start //A=[1 0 dt 0 dt*dt/2 0 // 0 1 0 dt 0 dt*dt/2 // 0 0 1 0 dt 0 // 0 0 0 1 0 dt // 0 0 0 0 1 0 // 0 0 0 0 0 1]; pred.x = VECTOR(1, STATE_NUM); //pred.P = MATRIX(1, STATE_NUM, 1, STATE_NUM); pred.P = dmatrix_alloc(STATE_NUM,STATE_NUM); t_v1 = VECTOR(1, STATE_NUM); //t_m1 = MATRIX(1, STATE_NUM, 1, STATE_NUM); //t_m2 = MATRIX(1, STATE_NUM, 1, STATE_NUM); //t_m3 = MATRIX(1, STATE_NUM, 1, STATE_NUM); t_m1 = dmatrix_alloc(STATE_NUM,STATE_NUM); t_m2 = dmatrix_alloc(STATE_NUM,STATE_NUM); t_m3 = dmatrix_alloc(STATE_NUM,STATE_NUM); //v = MATRIX(1, STATE_NUM, 1, STATE_NUM); v = dmatrix_alloc(STATE_NUM,STATE_NUM); nrot = ivector(1, STATE_NUM); d = VECTOR(1, STATE_NUM); //A = MATRIX(1, STATE_NUM, 1, STATE_NUM); //Q = MATRIX(1, STATE_NUM, 1, STATE_NUM); //Q1 = MATRIX(1, STATE_NUM, 1, STATE_NUM); A = dmatrix_alloc(STATE_NUM,STATE_NUM); Q = dmatrix_alloc(STATE_NUM,STATE_NUM); Q1 = dmatrix_alloc(STATE_NUM,STATE_NUM); H = VECTOR(1, STATE_NUM); K = VECTOR(1, STATE_NUM); //I = MATRIX(1, STATE_NUM, 1, STATE_NUM); I = dmatrix_alloc(STATE_NUM,STATE_NUM); // Initialize matrices A and Q for (i=1;i<=STATE_NUM;i++){ K[i] = 0.0; for (j=1;j<=STATE_NUM;j++){ A[i][i] = 0; Q[i][i] = 0; Q1[i][i] = 0; if (i==j) I[i][j]= 1.0; else I[i][j]= 0.0; } } for (i=1;i<=STATE_NUM;i++){ A[i][i] = 1; if (i+2<=STATE_NUM) A[i][i+2] = dt; if (i+4<=STATE_NUM) A[i][i+4] = dt*dt/2.0; } //% The state error covariance matrix is a function of the driving //% error which is assumed to only hit the accelaration directly. //% Indirectly this noise affects the velocity and position estimate //% through the dynamics model. //Q=q*[(dt^5)/20 0 (dt^4)/8 0 (dt^3)/6 0 // 0 (dt^5)/20 0 (dt^4)/8 0 (dt^3)/6 // (dt^4)/8 0 (dt^3)/3 0 (dt^2)/2 0 // 0 (dt^4)/8 0 (dt^3)/3 0 (dt^2)/2 // (dt^3)/6 0 (dt^2)/2 0 dt 0 // 0 (dt^3)/6 0 (dt^2)/2 0 dt]; Q[1][1] = q*pow(dt, 5)/20.0; Q[1][3] = q*pow(dt, 4)/8.0; Q[1][5] = q*pow(dt, 3)/6.0; Q[2][2] = q*pow(dt, 5)/20.0; Q[2][4] = q*pow(dt, 4)/8.0; Q[2][6] = q*pow(dt, 3)/6.0; Q[3][1] = q*pow(dt, 4)/8.0; Q[3][3] = q*pow(dt, 3)/3.0; Q[3][5] = q*pow(dt, 2)/2.0; Q[4][2] = q*pow(dt, 4)/8.0; Q[4][4] = q*pow(dt, 3)/3.0; Q[4][6] = q*pow(dt, 2)/2.0; Q[5][1] = q*pow(dt, 3)/6.0; Q[5][3] = q*pow(dt, 2)/2.0; Q[5][5] = q*dt; Q[6][2] = q*pow(dt, 3)/6.0; Q[6][4] = q*pow(dt, 2)/2.0; Q[6][6] = q*dt; /*for (i=1;i<=STATE_NUM;i++) { for (j=1;j<=STATE_NUM;j++) printf("%f ",Q[i][j]); printf("\n"); }*/ //%step b //pred_x=A*pre_x; %6 x 1 mat_vec_mult(pred.x, A, in->x, STATE_NUM, 0); //pred_P=A*pre_P*(A')+Q; %6 x 6 // A is transpose mat_mat_mult(t_m1, in->P, A, STATE_NUM, 0, 1); mat_mat_mult(t_m2, A, t_m1, STATE_NUM, 0, 0); mat_add(pred.P, t_m2, Q, STATE_NUM, 0); // %step c // % assume that 0<=z<2*pi // opt_z = 2*pi + atan2((pred_x(2)-s_y), (pred_x(1)-s_x)); %1 x 1 // if (opt_z>2*pi) // opt_z = opt_z - 2*pi; // end // %H is 1 x 6 // % measurement function is nonlinear // % // % z = atan(x(2) - s_y, x(1) - s_x); // % In order to linearize the problem we find the jacobian // % noticing that // % // % d(atan(x))/dx = 1/(1+x^2) // % // H=[ -(pred_x(2)-s_y)/(( (pred_x(1)-s_x)^2+(pred_x(2)-s_y)^2 )), (pred_x(1)-s_x)/(( (pred_x(1)-s_x)^2+(pred_x(2)-s_y)^2 )), 0, 0, 0, 0]; opt_z = 2*PI + atan2((pred.x[2]-s_y), (pred.x[1]-s_x)); H[1] = -(pred.x[2]-s_y)/ ( (pred.x[1]-s_x)*(pred.x[1]-s_x)+(pred.x[2]-s_y)*(pred.x[2]-s_y) ); H[2] = (pred.x[1]-s_x) / ( (pred.x[1]-s_x)*(pred.x[1]-s_x)+(pred.x[2]-s_y)*(pred.x[2]-s_y) ); H[3] = 0.0; H[4] = 0.0; H[5] = 0.0; H[6] = 0.0; //test=find(isnan(H)==1); //if (~isempty(test)) // disp('H goes to infinity due to node position is the same as the predicted value') // return; //end if (isnan(H[1]) || isnan(H[1])){ fprintf(stderr, "H goes to infinity due to node position is the same as the predicted value\n"); return(-1); } //%step d //%also test if P is still a positive definite matrix. If not, add some small noise to it to //%make it still positive. //n1=1; //Q1=n1*eye(6); //Q1(1,1)=0.1; //Q1(2,2)=0.1; //Q1(3,3)=5; //Q1(4,4)=5; Q1[1][1] = 0.1; Q1[2][2] = 0.1; Q1[3][3] = 5.0; Q1[4][4] = 5.0; Q1[5][5] = 1.0; Q1[6][6] = 1.0; //if isempty(find(eig(pred_P)<0.001)) // K=pred_P*H'*inv(H*pred_P*H' + R); %6 x 1 //else // pred_P=pred_P+Q1; // K=pred_P*H'*inv(H*pred_P*H' + R); %6 x 1 //end add_noise=0; // jacobi destroys the input vector mat_copy(t_m2, pred.P, STATE_NUM, 0); jacobi(t_m2, STATE_NUM, d, v, nrot); for(i=1;i<=STATE_NUM;i++){ if (d[i]< 0.001) add_noise=1; } if (add_noise){ mat_add(t_m1, pred.P, Q1, STATE_NUM, 0); mat_copy(pred.P, t_m1, STATE_NUM, 0); } mat_vec_mult(t_v1, pred.P, H, STATE_NUM, 1); inner_prod(&t_s, t_v1, H, STATE_NUM); t_s+= R; scal_vec_mult(K, 1.0/t_s, t_v1, STATE_NUM); //%step e and f //I=eye(6); //opt_x=pred_x+K*(z-opt_z); %6 x 1 scal_vec_mult(t_v1, z - opt_z, K, STATE_NUM); vec_add(out->x, pred.x, t_v1, STATE_NUM); // H: 1 x 6, K: 6 x 1 //%Joseph form to ward off round off problem //opt_P=(I-K*H)*pred_P*(I-K*H)'+K*R*K'; %6 x 6 scal_vec_mult(t_v1, R, K, STATE_NUM); // R*K' outer_prod(t_m1, K, t_v1, STATE_NUM); // K*(R*K') outer_prod(t_m2, K, H, STATE_NUM); // K*H mat_add(t_m3, I, t_m2, STATE_NUM, 1); // I-K*H mat_mat_mult(t_m2, pred.P, t_m3, STATE_NUM, 0, 1); mat_mat_mult(out->P, t_m3, t_m2, STATE_NUM, 0, 0); mat_add(t_m3, out->P, t_m1, STATE_NUM, 0); mat_copy(out->P, t_m3, STATE_NUM, 0); //% PRECAUTIONARY APPROACH. EVEN THOUGH IT HASN'T HAPPEN IN THE SIMULATION SO FAR //% also test if opt_P is still a positive definite matrix. If not, add some small noise to it to //% make it still positive. //% d: a diagonal matrix of eigenvalues //% v: matrix whose vectors are the eigenvectors //% X*V = V*D //if isempty(find(eig(opt_P)<0.001)) //else //[v d]=eig(opt_P); //c=find(d==min(diag(d))); //d(c)=0.001; //opt_P=v*d*v'; //end add_noise=0; // jacobi destroys the input vector mat_copy(t_m1, out->P, STATE_NUM, 0); jacobi(t_m1, STATE_NUM, d, v, nrot); min_d = d[1] ; for(i=1;i<=STATE_NUM;i++){ if ( min_d > d[i]) min_d = d[i]; if (d[i]< 0.001) add_noise=1; } // CAUTION - maybe the 2nd minimum is quite far from 0.001 if (add_noise){ // change diagonal matrix d for (i=1;i<STATE_NUM;i++){ if (d[i] == min_d) d[i] = 0.001; } // opt_P=v*d*v'; mat_copy(t_m1, v, STATE_NUM, 1); convert_vec_diag(t_m2, d, STATE_NUM); mat_mat_mult(t_m3, t_m2, t_m1, STATE_NUM, 0, 0); mat_mat_mult(out->P, v, t_m3, STATE_NUM, 0, 0); } //printf("The pred are x = %f y = %f\n",pred.x[1],pred.x[2]); // Free all MALLOCed matrices FREE_VECTOR_FUNCTION(pred.x, 1, STATE_NUM); FREE_MATRIX_FUNCTION(pred.P, 1, STATE_NUM, 1, STATE_NUM); FREE_VECTOR_FUNCTION(t_v1, 1, STATE_NUM); FREE_MATRIX_FUNCTION(t_m1, 1, STATE_NUM, 1, STATE_NUM); FREE_MATRIX_FUNCTION(t_m2, 1, STATE_NUM, 1, STATE_NUM); FREE_MATRIX_FUNCTION(t_m3, 1, STATE_NUM, 1, STATE_NUM); FREE_MATRIX_FUNCTION(v, 1, STATE_NUM, 1, STATE_NUM); free_ivector(nrot, 1, STATE_NUM); FREE_VECTOR_FUNCTION(d, 1, STATE_NUM); FREE_MATRIX_FUNCTION(A, 1, STATE_NUM, 1, STATE_NUM); FREE_MATRIX_FUNCTION(Q, 1, STATE_NUM, 1, STATE_NUM); FREE_MATRIX_FUNCTION(Q1, 1, STATE_NUM, 1, STATE_NUM); FREE_VECTOR_FUNCTION(H, 1, STATE_NUM); FREE_VECTOR_FUNCTION(K, 1, STATE_NUM); FREE_MATRIX_FUNCTION(I, 1, STATE_NUM, 1, STATE_NUM); return(0); }
/* * This is the constraint that makes sure we hit the ball */ void kinematics_eq_constr(unsigned m, double *result, unsigned n, const double *x, double *grad, void *data) { static double ballPred[CART]; static double racketDesVel[CART]; static double racketDesNormal[CART]; static Matrix link_pos_des; static Matrix joint_cog_mpos_des; static Matrix joint_origin_pos_des; static Matrix joint_axis_pos_des; static Matrix Alink_des[N_LINKS+1]; static Matrix racketTransform; static Matrix Jacobi; static Vector qfdot; static Vector xfdot; static Vector normal; static int firsttime = TRUE; double T = x[2*DOF]; int N = T/TSTEP; int i; /* initialization of static variables */ if (firsttime) { firsttime = FALSE; link_pos_des = my_matrix(1,N_LINKS,1,3); joint_cog_mpos_des = my_matrix(1,N_DOFS,1,3); joint_origin_pos_des = my_matrix(1,N_DOFS,1,3); joint_axis_pos_des = my_matrix(1,N_DOFS,1,3); Jacobi = my_matrix(1,2*CART,1,N_DOFS); racketTransform = my_matrix(1,4,1,4); qfdot = my_vector(1,DOF); xfdot = my_vector(1,2*CART); normal = my_vector(1,CART); for (i = 0; i <= N_LINKS; ++i) Alink_des[i] = my_matrix(1,4,1,4); // initialize the base variables //taken from ParameterPool.cf bzero((void *) &base_state, sizeof(base_state)); bzero((void *) &base_orient, sizeof(base_orient)); base_orient.q[_Q1_] = 1.0; // the the default endeffector parameters setDefaultEndeffector(); // homogeneous transform instead of using quaternions racketTransform[1][1] = 1; racketTransform[2][3] = 1; racketTransform[3][2] = -1; racketTransform[4][4] = 1; } if (grad) { // compute gradient of kinematics = jacobian //TODO: grad[0] = 0.0; grad[1] = 0.0; grad[2] = 0.0; } // interpolate at time T to get the desired racket parameters first_order_hold(ballPred,racketDesVel,racketDesNormal,T); // extract state information from array to joint_des_state structure for (i = 1; i <= DOF; i++) { joint_des_state[i].th = x[i-1]; joint_des_state[i].thd = qfdot[i] = x[i-1+DOF]; } /* compute the desired link positions */ kinematics(joint_des_state, &base_state, &base_orient, endeff, joint_cog_mpos_des, joint_axis_pos_des, joint_origin_pos_des, link_pos_des, Alink_des); /* compute the racket normal */ mat_mult(Alink_des[6],racketTransform,Alink_des[6]); for (i = 1; i <= CART; i++) { normal[i] = Alink_des[6][i][3]; } /* compute the jacobian */ jacobian(link_pos_des, joint_origin_pos_des, joint_axis_pos_des, Jacobi); mat_vec_mult(Jacobi, qfdot, xfdot); /* deviations from the desired racket frame */ for (i = 1; i <= CART; i++) { //printf("xfdot[%d] = %.4f, racketDesVel[%d] = %.4f\n",i,xfdot[i],i,racketDesVel[i-1]); //printf("normal[%d] = %.4f, racketDesNormal[%d] = %.4f\n",i,normal[i],i,racketDesNormal[i-1]); result[i-1] = link_pos_des[6][i] - ballPred[i-1]; result[i-1 + CART] = xfdot[i] - racketDesVel[i-1]; result[i-1 + 2*CART] = normal[i] - racketDesNormal[i-1]; } }
void BaseStateEstimation::update(SL_quat& base_orientation, SL_Cstate& base_position) { SL_quat orient; memcpy(&(orient.q[1]), imu_quaternion_, 4*sizeof(double)); MY_MATRIX(quat_to_rot_helper, 1,3,1,3); quatToRotMat(&orient, quat_to_rot_helper); for(int r=1; r<=3; r++) for(int c=1; c<=3; c++) O_rot_I_[r][c] = quat_to_rot_helper[c][r]; memcpy(&(O_angrate_I_[1]), imu_angrate_, 3*sizeof(double)); // memcpy(&(O_linvel_I_[1]), imu_linvel_, 3*sizeof(double)); memcpy(&(O_linacc_I_[1]), imu_linacc_, 3*sizeof(double)); // compensate for gravity // for(int i=1; i<=3; i++) // O_linacc_I_[i] -= gravity_[i]; // do numerical integration for(int i=1; i<=3; i++) { //integrate O_angacc_I_[i] = (double)update_rate_*(O_angrate_I_[i] - prev_O_angrate_I_[i]); // O_linpos_I_[i] += (1.0/(double)update_rate_)*O_linvel_I_[i]; //apply leakage term // O_linvel_I_[i] *= leakage_factor_; // O_linvel_I_[i] += (1.0/(double)update_rate_)*O_linacc_I_[i]; } // do coordinate transformation MY_MATRIX(S_angrate, 1,3,1,3); vectorToSkewMatrix(O_angrate_I_, S_angrate); MY_MATRIX(S_angacc, 1,3,1,3); vectorToSkewMatrix(O_angacc_I_, S_angacc); MY_MATRIX(S_helper, 1,3,1,3); mat_mult(S_angrate, S_angrate, S_helper); mat_add(S_angacc, S_helper, S_helper); mat_mult(O_rot_I_, I_rot_B_, O_rot_B_); // mat_vec_mult(O_rot_I_, I_linpos_B_, O_linpos_B_); // vec_add(O_linpos_I_, O_linpos_B_, O_linpos_B_); // // mat_vec_mult(O_rot_I_, I_linpos_B_, O_linvel_B_); // mat_vec_mult(S_angrate, O_linvel_B_, O_linvel_B_); // vec_add(O_linvel_I_, O_linvel_B_, O_linvel_B_); mat_vec_mult(O_rot_I_, I_linpos_B_, O_linacc_B_); mat_vec_mult(S_helper, O_linacc_B_, O_linacc_B_); vec_add(O_linacc_I_, O_linacc_B_, O_linacc_B_); // integrate base state for(int i=1; i<=3; i++) { //integrate O_linpos_B_[i] += (1.0/(double)update_rate_)*O_linvel_B_[i]; //apply leakage term O_linvel_B_[i] *= leakage_factor_; O_linvel_B_[i] += (1.0/(double)update_rate_)*O_linacc_B_[i]; } // write data back linkQuat(O_rot_B_, &O_orient_B_); memcpy(&(O_orient_B_.ad[1]), &(O_angrate_I_[1]), 3*sizeof(double)); memcpy(&(O_orient_B_.add[1]), &(O_angacc_I_[1]), 3*sizeof(double)); memcpy(&(O_trans_B_.x[1]), &(O_linpos_B_[1]),3*sizeof(double)); memcpy(&(O_trans_B_.xd[1]), &(O_linvel_B_[1]),3*sizeof(double)); memcpy(&(O_trans_B_.xdd[1]), &(O_linacc_B_[1]),3*sizeof(double)); // linkQuat(O_rot_I_, &O_orient_B_); // memcpy(&(O_orient_B_.ad[1]), &(O_angrate_I_[1]), 3*sizeof(double)); // memcpy(&(O_orient_B_.add[1]), &(O_angacc_I_[1]), 3*sizeof(double)); // memcpy(&(O_trans_B_.x[1]), &(O_linpos_I_[1]),3*sizeof(double)); // memcpy(&(O_trans_B_.xd[1]), &(O_linvel_I_[1]),3*sizeof(double)); // memcpy(&(O_trans_B_.xdd[1]), &(O_linacc_I_[1]),3*sizeof(double)); // quatDerivatives(&O_orient_B_); base_orientation = O_orient_B_; base_position = O_trans_B_; // update buffers memcpy(&(prev_O_angrate_I_[1]), &(O_angrate_I_[1]), 3*sizeof(double)); }
void test_constraint(void) { int i,j,n,m; static Matrix Jbig; static Matrix dJbigdt; static Vector dsbigdt; static Matrix JbigMinvJbigt; static Matrix JbigMinvJbigtinv; static Matrix Minv; static Matrix MinvJbigt; static Matrix Jbigt; static Vector dJbigdtdsbigdt; static Matrix Jbart; static Matrix Jbar; static Vector lv; static Vector lambda; static Vector f; static Matrix R; static Vector v; static int firsttime = TRUE; int debug_print = TRUE; if (firsttime) { firsttime = FALSE; Jbig = my_matrix(1,N_ENDEFFS*6,1,N_DOFS+6); Jbigt = my_matrix(1,N_DOFS+6,1,N_ENDEFFS*6); dJbigdt = my_matrix(1,N_ENDEFFS*6,1,N_DOFS+6); JbigMinvJbigt = my_matrix(1,N_ENDEFFS*6,1,N_ENDEFFS*6); JbigMinvJbigtinv = my_matrix(1,N_ENDEFFS*6,1,N_ENDEFFS*6); Minv = my_matrix(1,N_DOFS+6,1,N_DOFS+6); MinvJbigt = my_matrix(1,N_DOFS+6,1,N_ENDEFFS*6); dsbigdt = my_vector(1,N_DOFS+6); dJbigdtdsbigdt = my_vector(1,N_ENDEFFS*6); Jbar = my_matrix(1,N_DOFS+6,1,N_ENDEFFS*6); Jbart = my_matrix(1,N_ENDEFFS*6,1,N_DOFS+6); lv = my_vector(1,N_ENDEFFS*6); lambda = my_vector(1,N_ENDEFFS*6); f = my_vector(1,N_DOFS+6); R = my_matrix(1,N_CART,1,N_CART); v = my_vector(1,N_CART); // the base part of Jbig is just the identity matrix for both constraints for (i=1; i<=6; ++i) Jbig[i][N_DOFS+i] = 1.0; for (i=1; i<=6; ++i) Jbig[i+6][N_DOFS+i] = 1.0; } // build the Jacobian including the base -- just copy J into Jbig mat_equal_size(J,N_ENDEFFS*6,N_DOFS,Jbig); mat_trans(Jbig,Jbigt); // build the Jacobian derivative mat_equal_size(dJdt,N_ENDEFFS*6,N_DOFS,dJbigdt); // build the augmented state derivate vector for (i=1; i<=N_DOFS; ++i) dsbigdt[i] = joint_state[i].thd; for (i=1; i<=N_CART; ++i) dsbigdt[N_DOFS+i] = base_state.xd[i]; for (i=1; i<=N_CART; ++i) dsbigdt[N_DOFS+N_CART+i] = base_orient.ad[i]; // compute J-bar transpose my_inv_ludcmp(rbdInertiaMatrix,N_DOFS+6, Minv); mat_mult(Minv,Jbigt,MinvJbigt); mat_mult(Jbig,MinvJbigt,JbigMinvJbigt); my_inv_ludcmp(JbigMinvJbigt,N_ENDEFFS*6,JbigMinvJbigtinv); mat_mult(MinvJbigt,JbigMinvJbigtinv,Jbar); mat_trans(Jbar,Jbart); // compute C+G-u for (i=1; i<=N_DOFS; ++i) f[i] = -joint_state[i].u; for (i=1; i<=6; ++i) f[N_DOFS+i] = 0.0; vec_add(f,rbdCplusGVector,f); // compute first part of lambda mat_vec_mult(Jbart,f,lambda); // compute second part of lambda mat_vec_mult(dJbigdt,dsbigdt,dJbigdtdsbigdt); mat_vec_mult(JbigMinvJbigtinv,dJbigdtdsbigdt,lv); // compute final lambda vec_sub(lambda,lv,lambda); // ---------------------------------------------------------------------- // express lambda in the same coordinate at the foot sensors /* rotation matrix from world to L_AAA coordinates: we can borrow this matrix from the toes, which have the same rotation, but just a different offset vector, which is not needed here */ mat_trans_size(Alink[L_IN_HEEL],N_CART,N_CART,R); // transform forces for (i=1; i<=N_CART; ++i) v[i] = lambda[N_CART*2+i]; mat_vec_mult(R,v,v); if (debug_print) { printf("LEFT Force: lx=% 7.5f sx=% 7.5f\n",v[1],misc_sim_sensor[L_CFx]); printf("LEFT Force: ly=% 7.5f sy=% 7.5f\n",v[2],misc_sim_sensor[L_CFy]); printf("LEFT Force: lz=% 7.5f sz=% 7.5f\n",v[3],misc_sim_sensor[L_CFz]); } misc_sensor[L_CFx] = v[1]; misc_sensor[L_CFy] = v[2]; misc_sensor[L_CFz] = v[3]; // transform torques for (i=1; i<=N_CART; ++i) v[i] = lambda[N_CART*2+N_CART+i]; mat_vec_mult(R,v,v); if (debug_print) { printf("LEFT Torque: lx=% 7.5f sx=% 7.5f\n",v[1],misc_sim_sensor[L_CTa]); printf("LEFT Torque: ly=% 7.5f sy=% 7.5f\n",v[2],misc_sim_sensor[L_CTb]); printf("LEFT Torque: lz=% 7.5f sz=% 7.5f\n",v[3],misc_sim_sensor[L_CTg]); } misc_sensor[L_CTa] = v[1]; misc_sensor[L_CTb] = v[2]; misc_sensor[L_CTg] = v[3]; /* rotation matrix from world to R_AAA coordinates : we can borrow this matrix from the toes, which have the same rotation, but just a different offset vector, which is not needed here */ mat_trans_size(Alink[R_IN_HEEL],N_CART,N_CART,R); // transform forces for (i=1; i<=N_CART; ++i) v[i] = lambda[i]; mat_vec_mult(R,v,v); if (debug_print) { printf("RIGHT Force: lx=% 7.5f sx=% 7.5f\n",v[1],misc_sim_sensor[R_CFx]); printf("RIGHT Force: ly=% 7.5f sy=% 7.5f\n",v[2],misc_sim_sensor[R_CFy]); printf("RIGHT Force: lz=% 7.5f sz=% 7.5f\n",v[3],misc_sim_sensor[R_CFz]); } misc_sensor[R_CFx] = v[1]; misc_sensor[R_CFy] = v[2]; misc_sensor[R_CFz] = v[3]; // transform torques for (i=1; i<=N_CART; ++i) v[i] = lambda[N_CART+i]; mat_vec_mult(R,v,v); if (debug_print) { printf("RIGHT Torque: lx=% 7.5f sx=% 7.5f\n",v[1],misc_sim_sensor[R_CTa]); printf("RIGHT Torque: ly=% 7.5f sy=% 7.5f\n",v[2],misc_sim_sensor[R_CTb]); printf("RIGHT Torque: lz=% 7.5f sz=% 7.5f\n",v[3],misc_sim_sensor[R_CTg]); } misc_sensor[R_CTa] = v[1]; misc_sensor[R_CTb] = v[2]; misc_sensor[R_CTg] = v[3]; if (debug_print) getchar(); }
/***************************************************************************** ****************************************************************************** Function Name : run_gravityComp_task Date : Dec. 1997 Remarks: run the task from the task servo: REAL TIME requirements! ****************************************************************************** Paramters: (i/o = input/output) none *****************************************************************************/ static int run_gravityComp_task(void) { int j,i; // Damping Gains (index starts at 1) //double Kdamp[N_DOFS+1] = {0.0, 0.4, 0.4, 0.4, 0.4, 0.4, 0.4, 0.4, 0.1, 0.1, 0.1}; double Kdamp[N_DOFS+1] = {0.0, 0.07, 0.07, 0.07, 0.07, 0.07, 0.07, 0.07, 0.1, 0.1, 0.1}; // double Kdamp[N_DOFS+1] = {0.0, 0.2, 0.2, 0.5, 0.5, 1.0, 0.5, 0.5, 1, 1, 1}; // Coriolus Compensation Gains double Kcori[N_DOFS+1] = {0.0, 1.00, 1.00, 1.00, 1.00, 1.00, 1.00, 1.00, 0.00, 0.00, 0.00}; // double Kcori[N_DOFS+1] = {0.0, 0.30, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.00, 0.00, 0.00}; // double Kcori[N_DOFS+1] = {0.0, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00}; // Inertia Compensation Gains double Kiner[N_DOFS+1] = {0.0, 0.40, 0.40, 0.30, 0.35, 0.00, 0.4, 0.4, 0.00, 0.00, 0.00}; // double Kiner[N_DOFS+1] = {0.0, 1.00, 1.00, 1.00, 1.00, 0.50, 1.00, 1.00, 0.00, 0.00, 0.00}; //double Kiner[N_DOFS+1] = {0.0, 0.20, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.00, 0.00, 0.00}; // double Kiner[N_DOFS+1] = {0.0, 0.50, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.00, 0.00, 0.00}; // double Kiner[N_DOFS+1] = {0.0, 0.00, 0.00, 0.00, 0.00, 0.00, 0.0, 0.0, 0.00, 0.00, 0.00}; // Integer gains (how fast desired state becomes actual state) //double Kint[N_DOFS+1] = {0.0, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7, 0.001, 0.001, 0.001}; double Kint[N_DOFS+1] = {0.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.001, 0.001, 0.001}; //double Kint[N_DOFS+1] = {0.0, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.001, 0.001, 0.001}; // Dead Zone Threshold double thres[N_DOFS+1] = {0.0, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.05, 0.05, 0.05}; // To shut off Inertia,Coriolus Compensation //double Kcori[N_DOFS+1] = {0.0, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00}; //double Kiner[N_DOFS+1] = {0.0, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00}; for (i=1; i<=N_DOFS; ++i) { joint_filt_state[i] = filter(i); } if (joint_state[8].th < 0.08) { // Hold down thumb to trigger Grav. Comp. // Gravity Compensation for (i=1; i<=N_DOFS; ++i) { if (fabs(joint_state[i].th-joint_des_state[i].th) > thres[i]){ joint_des_state[i].th += (joint_state[i].th-joint_des_state[i].th)*Kint[i]; } } // add compensation relative to the current state of the robot for (i=1; i<=N_DOFS; ++i) { // Gravity Compensation target[i].th = joint_state[i].th; // Coriolus Compensation target[i].thd = Kcori[i]*joint_filt_state[i].thd; // Inertia Compensation target[i].thdd = Kiner[i]*joint_filt_state[i].thdd; // Damping (to be premultiplied by inertia matrix) // if (i != 5 ) { // target[i].thdd += (0.0 - joint_state[i].thd ) * Kdamp[i] * DampScale * controller_gain_thd[i]; // } target[i].uff = 0.0; } SL_InvDynNE(NULL,target,endeff,&base_state,&base_orient); // assign compensation torques for (i=1; i<=N_DOFS; ++i) { if (i==1 || i==2 || i==3 || i==4 || i==6 || i==7 || i==5) { // Use my own damping (not premultiplied by the inertia matrix) joint_des_state[i].uff = target[i].uff + (0.0 - joint_filt_state[i].thd) * Kdamp[i] * DampScale * controller_gain_thd[i]; // Cancel damping in the motor servo joint_des_state[i].thd = joint_state[i].thd; } else { // Do not use my own damping joint_des_state[i].uff = target[i].uff; } } fieldtorque = 0.0; /* FORCE FIELDS GO HERE */ switch (fieldon) { case 1: joint_des_state[4].uff += joint_filt_state[2].thd * fieldGain + joint_filt_state[1].thd * fieldGain; break; case 2: if (joint_state[1].thd > 0.0) { joint_des_state[5].uff += joint_state[1].thd * -0.5; } break; case 3: joint_des_state[2].uff += joint_state[3].thd * 1.0; break; case 4: joint_des_state[1].uff += joint_state[4].thd * 1.0; break; case 5: joint_des_state[4].uff += joint_state[3].thd * -1.0; break; case 6: if (joint_state[1].thd > 0.0) { joint_des_state[3].uff += joint_state[1].thd * 1.0; } break; case 7: //end-effector: for (i=1;i<=7;i++) { for (j=1;j<=3;j++) { J7T[i][j] = J[j][i]; } } for (i=1;i<=3;i++) { xd[i] = cart_state[1].xd[i]; } if (!mat_mult(J7T,B, J7T)) return FALSE; if (!mat_vec_mult(J7T, xd, u_field)) return FALSE; for (i=1;i<=7;i++) { joint_des_state[i].uff += fieldGain*u_field[i]; } fieldtorque = fieldGain*u_field[1]; break; } } return TRUE; }