/*!***************************************************************************** ******************************************************************************* \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 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 }
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)); }