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