// 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;
}
