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