/*!***************************************************************************** ******************************************************************************* \note my_inv_ludcmp_solve \date March 93 \remarks uses the LUDCMP to invert a square matrix and solve it for a given vector A x = b ******************************************************************************* Function Parameters: [in]=input,[out]=output \param[in] mat : matrix to be inverted \param[in] b_vec : vector for which system is to be solved \param[in] size : mat is a size x size matrix \param[out] x_vec : the result note: - return code FALSE is given if not invertable - mat and inv_mat can be the same matrices; the program takes care of this - matrices are according to numerical recipes convention ******************************************************************************/ int my_inv_ludcmp_solve(double **mat, double *b_vec, int size, double *x_vec) { int i,j,n; int rc = 1; double info; MY_MATRIX(aux, 1, size, 1, size); MY_IVECTOR(indx, 1, size); for (i=1; i<=size; ++i) for (j=1; j<=size; ++j) aux[i][j]=mat[i][j]; if (!my_ludcmp(aux,size,indx,&info)) { rc = 0; } else { /* make zuu_inv the identity matrix */ for (i=1; i<=size; ++i) { x_vec[i] = b_vec[i]; } /* now generate the solution */ my_lubksb(aux, size, indx, x_vec); } return rc; }
/*!***************************************************************************** ******************************************************************************* \note run_user_task \date Nov. 2007 \remarks this function is clocked out of the task servo ******************************************************************************* Function Parameters: [in]=input,[out]=output none ******************************************************************************/ int run_user_task(void) { int i,j; MY_MATRIX(M,1,N_CART,1,N_CART); MY_VECTOR(v,1,N_CART); // compute the contact forces in world coordinates at the feet from the // force 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 */ // transform forces v[_X_] = misc_sensor[L_CFx]; v[_Y_] = misc_sensor[L_CFy]; v[_Z_] = misc_sensor[L_CFz]; mat_vec_mult_size(Alink[L_IN_HEEL],N_CART,N_CART,v,N_CART,endeff[LEFT_FOOT].cf); // transform torques v[_A_] = misc_sensor[L_CTa]; v[_B_] = misc_sensor[L_CTb]; v[_G_] = misc_sensor[L_CTg]; mat_vec_mult_size(Alink[L_IN_HEEL],N_CART,N_CART,v,N_CART,endeff[LEFT_FOOT].ct); /* 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 */ // transform forces // transform forces v[_X_] = misc_sensor[R_CFx]; v[_Y_] = misc_sensor[R_CFy]; v[_Z_] = misc_sensor[R_CFz]; mat_vec_mult_size(Alink[R_IN_HEEL],N_CART,N_CART,v,N_CART,endeff[RIGHT_FOOT].cf); // transform torques v[_A_] = misc_sensor[R_CTa]; v[_B_] = misc_sensor[R_CTb]; v[_G_] = misc_sensor[R_CTg]; mat_vec_mult_size(Alink[R_IN_HEEL],N_CART,N_CART,v,N_CART,endeff[RIGHT_FOOT].ct); // use the simulated base state if required if (use_simulated_base_state) read_simulated_base(); /*//sending char buf[BUFLEN]; sprintf(buf, 'to je paket %d\n', 5); mainSend(buf); printf("send\n");*/ return TRUE; }
/*!***************************************************************************** ******************************************************************************* \note my_inv_ludcmp_gen \date Nov 2008 (Modified by Mrinal) \remarks uses the LUDCMP to invert a square matrix. Also calculates the determinant if needed ******************************************************************************* Function Parameters: [in]=input,[out]=output \param[in] mat : matrix to be inverted \param[in] size : mat is a size x size matrix \param[out] inv_mat : inverse of this matrix \param[in] calculate_determinant : one of CALC_NO_DET, CALC_DET, CALC_DET_ONLY \param[out] det : determinant of the matrix (if requested) note: - return code FALSE is given if not invertable - mat and inv_mat can be the same matrices; the program takes care of this - matrices are according to numerical recipes convention ******************************************************************************/ int my_inv_ludcmp_gen(double **mat, int size, double **inv_mat, int calculate_determinant, double* det) { int i,j,n; int rc = 1; double info; MY_MATRIX(aux, 1, size, 1, size); MY_IVECTOR(indx, 1, size); for (i=1; i<=size; ++i) for (j=1; j<=size; ++j) aux[i][j]=mat[i][j]; if (!my_ludcmp(aux,size,indx,&info)) { rc = 0; } else { if (calculate_determinant != CALC_DET_ONLY) { /* make zuu_inv the identity matrix */ for (i=1; i<=size; ++i) { for (j=1; j<=size; ++j) { inv_mat[i][j] = 0.0; if (i==j) inv_mat[i][j] = 1.0; } } /* now generate the inverse. Due to the setup of matrices, I will actuall create the transpose of the inverse */ for (i=1; i<=size; ++i) { my_lubksb(aux, size, indx, inv_mat[i]); } /* now take the transpose of this inverse to make it what I want */ mat_trans(inv_mat, inv_mat); } if (calculate_determinant && det!=NULL) { calculate_determinant = CALC_NO_DET; *det = info; for (i=1; i<=size; ++i) *det *= aux[i][i]; } } return rc; }
/*!***************************************************************************** ******************************************************************************* \note my_ludcmp_det \date March 93 \remarks uses the LUDCMP to calculate the determinant of the given matrix ******************************************************************************* Function Parameters: [in]=input,[out]=output \param[in] mat : matrix \param[in] size : mat is a size x size matrix \param[out] deter : the determinant note: - return code FALSE is given if not invertable - matrices are according to numerical recipes convention ******************************************************************************/ int my_ludcmp_det(double **mat, int size, double *deter) { int rc,i; MY_MATRIX(aux, 1, size, 1, size); rc = my_inv_ludcmp_gen(mat, size, aux, CALC_DET_ONLY, deter); return rc; }
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_); }
/*!***************************************************************************** ******************************************************************************* \note baseToWorldAcc \date April 2006 \remarks converts a 3D acceleration in base coordinates to world coordinates ******************************************************************************* Function Parameters: [in]=input,[out]=output \param[in] xl : the vector in local coordinates \param[in] cbase : the base position \param[in] obase : the base orientation \param[out] xw : the vector in world coordinates ******************************************************************************/ void baseToWorldAcc(double *xl, SL_Cstate *cbase, SL_quat *obase, double *xw) { int i,j; static int firsttime=TRUE; double tmp[N_CART+1]; MY_MATRIX(R,1,N_CART,1,N_CART); quatToRotMatInv(obase,R); mat_vec_mult_size(R,N_CART,N_CART,xl,N_CART,tmp); for (i=1; i<=N_CART; ++i) xw[i] = tmp[i]; }
/*!***************************************************************************** ******************************************************************************* \note worldToBaseAcc \date April 2006 \remarks converts a 3D acceleration in world coordinates to base coordinates ******************************************************************************* Function Parameters: [in]=input,[out]=output \param[in] xw : the vector in world coordinates \param[in] cbase : the base position \param[in] obase : the base orientation \param[out] xl : the vector in local coordinates ******************************************************************************/ void worldToBaseAcc(double *xw, SL_Cstate *cbase, SL_quat *obase, double *xl) { int i,j; static int firsttime=TRUE; double temp[N_CART+1]; MY_MATRIX(R,1,N_CART,1,N_CART); for (i=1; i<=N_CART; ++i) temp[i] = xw[i]; quatToRotMat(obase,R); mat_vec_mult_size(R,N_CART,N_CART,temp,N_CART,xl); }
/*!***************************************************************************** ******************************************************************************* \note drawCOMs \date April 2013 \remarks draws the position of the center of gravity of every link ******************************************************************************* Function Parameters: [in]=input,[out]=output ******************************************************************************/ static void drawCOMs(SL_Jstate *state, SL_endeff *eff, SL_Cstate *basec, SL_quat *baseo) { int i, j, n; GLfloat color_point[4] = { (float) 1.0, (float) 1.0, (float) 0.5, (float) opacity }; double radius = 0.01; double sum; MY_MATRIX(Xmcog,0,N_DOFS,1,3); MY_MATRIX(Xaxis,0,N_DOFS,1,3); MY_MATRIX(Xorigin,0,N_DOFS,1,3); MY_MATRIX(Xlink,0,N_LINKS,1,3); MY_MATRIX_ARRAY(Ahmat,1,4,1,4,N_LINKS); MY_MATRIX_ARRAY(Ahmatdof,1,4,1,4,N_DOFS); MY_MATRIX(Ec,1,(N_CART*2),1,(N_CART*2)); MY_MATRIX(Jac,1,(N_ENDEFFS*2*N_CART),1,N_DOFS); mat_eye(Ec); // compute the link information for this state linkInformation(state, basec, baseo, eff, Xmcog, Xaxis, Xorigin, Xlink, Ahmat,Ahmatdof); for (i = 0; i <= N_DOFS; i++) { // draw a blob at each center of mass of each link glPushMatrix(); glTranslated((GLdouble) (Xmcog[i][_X_] / links[i].m), (GLdouble) (Xmcog[i][_Y_] / links[i].m), (GLdouble) (Xmcog[i][_Z_] / links[i].m)); glColor4fv(color_point); glutSolidSphere(radius, 10, 10); glPopMatrix(); } }
/** * NO MODE SWITCHES, NO RT REQUIREMENTS */ void imuDataToSLQuat(double* accel, double* angrate, double* orientation, SL_quat& sl_orient, SL_Cstate& sl_pos, double* unstab_accel, SL_Cstate& sl_pos_unstab) { MY_MATRIX(sl_rot_mat,1,3,1,3); for(unsigned int i=0; i<9;i++) { // raw data is given in M1,1; M1,2; M1,3; M2,1; ... // we transpose to get it in world frame const unsigned int c = i/3; const unsigned int r = i%3; sl_rot_mat[r+1][c+1] = orientation[i]; } // imu world needs to be transformed into SL world double rot_scales[4] = {0.0, -1.0, 1.0, -1.0}; for(int r=1; r<=3; r++) for(int c=1; c<=3; c++) sl_rot_mat[r][c] *= rot_scales[r]; // avoid using linkQuat { Eigen::Matrix3d eig_rot_mat; for(int r=1; r<=3; r++) for(int c=1; c<=3; c++) eig_rot_mat(r-1,c-1) = sl_rot_mat[r][c]; Eigen::Quaterniond eig_orient(eig_rot_mat); sl_orient.q[_QW_] = eig_orient.w(); sl_orient.q[_QX_] = eig_orient.x(); sl_orient.q[_QY_] = eig_orient.y(); sl_orient.q[_QZ_] = eig_orient.z(); } //linkQuat(sl_rot_mat, &sl_orient); //transfrom angular rate and acceleration into world frame MY_VECTOR(sl_angrate, 1,3); MY_VECTOR(sl_accel, 1,3); MY_VECTOR(sl_accel_unstab, 1,3); // MY_VECTOR(sl_linvel, 1,3); for(unsigned int i=1; i<=3; i++) { sl_angrate[i] = sl_rot_mat[i][1]*angrate[0] + sl_rot_mat[i][2]*angrate[1] + sl_rot_mat[i][3]*angrate[2]; sl_accel[i] = sl_rot_mat[i][1]*accel[0] + sl_rot_mat[i][2]*accel[1] + sl_rot_mat[i][3]*accel[2]; sl_accel_unstab[i] = sl_rot_mat[i][0]*unstab_accel[0] + sl_rot_mat[i][1]*unstab_accel[1] + sl_rot_mat[i][2]*unstab_accel[2]; // sl_linvel[i] = sl_rot_mat[i][1]*lin_vel[0] + // sl_rot_mat[i][2]*lin_vel[1] + sl_rot_mat[i][3]*lin_vel[2]; } for(unsigned int i=1; i<=3; i++) { sl_orient.ad[i] = sl_angrate[i]; sl_pos.xdd[i] = sl_accel[i]; sl_pos_unstab.xdd[i] = sl_accel_unstab[i]; // sl_pos.xd[i] = sl_linvel[i]; } // quatDerivatives(&sl_orient); }
/*!***************************************************************************** ******************************************************************************* \note run_user_task \date Nov. 2007 \remarks this function is clocked out of the task servo ******************************************************************************* Function Parameters: [in]=input,[out]=output none ******************************************************************************/ int run_user_task(void) { int i,j; MY_MATRIX(M,1,N_CART,1,N_CART); MY_VECTOR(v,1,N_CART); // compute the contact forces in world coordinates at the feet from the // force 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 */ #ifdef HAS_LOWER_BODY // transform forces v[_X_] = misc_sensor[L_CFx]; v[_Y_] = misc_sensor[L_CFy]; v[_Z_] = misc_sensor[L_CFz]; mat_vec_mult_size(Alink[L_IN_HEEL],N_CART,N_CART,v,N_CART,endeff[LEFT_FOOT].cf); // transform torques v[_A_] = misc_sensor[L_CTa]; v[_B_] = misc_sensor[L_CTb]; v[_G_] = misc_sensor[L_CTg]; mat_vec_mult_size(Alink[L_IN_HEEL],N_CART,N_CART,v,N_CART,endeff[LEFT_FOOT].ct); /* 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 */ // transform forces // transform forces v[_X_] = misc_sensor[R_CFx]; v[_Y_] = misc_sensor[R_CFy]; v[_Z_] = misc_sensor[R_CFz]; mat_vec_mult_size(Alink[R_IN_HEEL],N_CART,N_CART,v,N_CART,endeff[RIGHT_FOOT].cf); // transform torques v[_A_] = misc_sensor[R_CTa]; v[_B_] = misc_sensor[R_CTb]; v[_G_] = misc_sensor[R_CTg]; mat_vec_mult_size(Alink[R_IN_HEEL],N_CART,N_CART,v,N_CART,endeff[RIGHT_FOOT].ct); #endif differentiate_cog(&cog); test_fall(&cog); // use the simulated base state if required if (use_simulated_base_state) read_simulated_base(); else { // do base state estimation // update_base_state_estimation(&base_orient, &base_state); // run_state_est_lin_task(); // getPelv(&base_orient, &base_state); // base state if (semTake(sm_base_state_sem,ns2ticks(NO_WAIT)) == ERROR) { //printf("sm_base_state_sem take error\n"); return TRUE; } cSL_Cstate((&base_state)-1, sm_base_state_data, 1, DOUBLE2FLOAT); sm_base_state->state[1] = sm_base_state_data[1]; semGive(sm_base_state_sem); // base orient if (semTake(sm_base_orient_sem,ns2ticks(NO_WAIT)) == ERROR) { //printf("sm_base_orien_sem take error\n"); return TRUE; } cSL_quat(&base_orient-1, sm_base_orient_data, 1, DOUBLE2FLOAT); sm_base_orient->orient[1] = sm_base_orient_data[1]; semGive(sm_base_orient_sem); } return TRUE; }
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)); }