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