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));
}
/*!*****************************************************************************
*******************************************************************************
\note compute_kinematics
\date June 99
\remarks
computes kinematic variables
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
static void
compute_kinematics(void)
{
int i,j,r,o;
static int firsttime = TRUE;
static SL_DJstate *temp;
static Matrix last_J;
static Matrix last_Jbase;
if (firsttime) {
temp=(SL_DJstate *)my_calloc((unsigned long)(n_dofs+1),sizeof(SL_DJstate),MY_STOP);
last_J = my_matrix(1,6*n_endeffs,1,n_dofs);
last_Jbase = my_matrix(1,6*n_endeffs,1,2*N_CART);
}
/* compute the desired link positions */
linkInformationDes(joint_des_state,&base_state,&base_orient,endeff,
joint_cog_mpos_des,joint_axis_pos_des,joint_origin_pos_des,
link_pos_des,Alink_des,Adof_des);
/* the desired endeffector information */
for (i=1; i<=N_CART; ++i) {
for (j=1; j<=n_endeffs; ++j) {
cart_des_state[j].x[i] = link_pos_des[link2endeffmap[j]][i];
}
}
/* the desired quaternian of the endeffector */
for (j=1; j<=n_endeffs; ++j) {
linkQuat(Alink_des[link2endeffmap[j]],&(cart_des_orient[j]));
}
/* addititional link information */
linkInformation(joint_state,&base_state,&base_orient,endeff,
joint_cog_mpos,joint_axis_pos,joint_origin_pos,
link_pos,Alink,Adof);
/* create the endeffector information */
for (i=1; i<=N_CART; ++i) {
for (j=1; j<=n_endeffs; ++j) {
cart_state[j].x[i] = link_pos[link2endeffmap[j]][i];
}
}
/* the quaternian of the endeffector */
for (j=1; j<=n_endeffs; ++j) {
linkQuat(Alink[link2endeffmap[j]],&(cart_orient[j]));
}
/* the COG position */
compute_cog();
/* the jacobian */
jacobian(link_pos,joint_origin_pos,joint_axis_pos,J);
baseJacobian(link_pos,joint_origin_pos,joint_axis_pos,Jbase);
jacobian(link_pos_des,joint_origin_pos_des,joint_axis_pos_des,Jdes);
baseJacobian(link_pos_des,joint_origin_pos_des,joint_axis_pos_des,Jbasedes);
/* numerical time derivative of Jacobian */
if (!firsttime) {
mat_sub(J,last_J,dJdt);
mat_mult_scalar(dJdt,(double)ros_servo_rate,dJdt);
mat_sub(Jbase,last_Jbase,dJbasedt);
mat_mult_scalar(dJbasedt,(double)ros_servo_rate,dJbasedt);
}
mat_equal(J,last_J);
mat_equal(Jbase,last_Jbase);
/* compute the cartesian velocities and accelerations */
for (j=1; j<=n_endeffs; ++j) {
for (i=1; i<=N_CART; ++i) {
cart_state[j].xd[i] = 0.0;
cart_state[j].xdd[i] = 0.0;
cart_orient[j].ad[i] = 0.0;
cart_orient[j].add[i] = 0.0;
cart_des_state[j].xd[i] = 0.0;
cart_des_orient[j].ad[i] = 0.0;
/* contributations from the joints */
for (r=1; r<=n_dofs; ++r) {
cart_state[j].xd[i] += J[(j-1)*6+i][r] * joint_state[r].thd;
cart_orient[j].ad[i] += J[(j-1)*6+i+3][r] * joint_state[r].thd;
cart_des_state[j].xd[i] += Jdes[(j-1)*6+i][r] *joint_des_state[r].thd;
cart_des_orient[j].ad[i]+= Jdes[(j-1)*6+i+3][r] * joint_des_state[r].thd;
cart_state[j].xdd[i] += J[(j-1)*6+i][r] * joint_state[r].thdd +
dJdt[(j-1)*6+i][r] * joint_state[r].thd;
cart_orient[j].add[i] += J[(j-1)*6+i+3][r] * joint_state[r].thdd +
dJdt[(j-1)*6+i+3][r] * joint_state[r].thd;
}
/* contributations from the base */
for (r=1; r<=N_CART; ++r) {
cart_state[j].xd[i] += Jbase[(j-1)*6+i][r] * base_state.xd[r];
cart_orient[j].ad[i] += Jbase[(j-1)*6+i+3][r] * base_state.xd[r];
cart_state[j].xd[i] += Jbase[(j-1)*6+i][3+r] * base_orient.ad[r];
cart_orient[j].ad[i] += Jbase[(j-1)*6+i+3][3+r] * base_orient.ad[r];
cart_des_state[j].xd[i] += Jbasedes[(j-1)*6+i][r] * base_state.xd[r];
cart_des_orient[j].ad[i] += Jbasedes[(j-1)*6+i+3][r] * base_state.xd[r];
cart_des_state[j].xd[i] += Jbasedes[(j-1)*6+i][3+r] * base_orient.ad[r];
cart_des_orient[j].ad[i] += Jbasedes[(j-1)*6+i+3][3+r] * base_orient.ad[r];
cart_state[j].xdd[i] += Jbase[(j-1)*6+i][r] * base_state.xdd[r] +
dJbasedt[(j-1)*6+i][r] * base_state.xd[r];
cart_orient[j].add[i] += Jbase[(j-1)*6+i+3][r] * base_state.xdd[r] +
dJbasedt[(j-1)*6+i+3][r] * base_state.xd[r];
cart_state[j].xdd[i] += Jbase[(j-1)*6+i][3+r] * base_orient.add[r] +
dJbasedt[(j-1)*6+i][3+r] * base_orient.ad[r];
cart_orient[j].add[i] += Jbase[(j-1)*6+i+3][3+r] * base_orient.add[r] +
dJbasedt[(j-1)*6+i+3][3+r] * base_orient.ad[r];
}
}
/* compute quaternion derivatives */
quatDerivatives(&(cart_orient[j]));
quatDerivatives(&(cart_des_orient[j]));
for (r=1; r<=N_QUAT; ++r)
cart_des_orient[j].qdd[r] = 0.0; // we don't have dJdes_dt so far
}
/* reset first time flag */
firsttime = FALSE;
}
/*!*****************************************************************************
*******************************************************************************
\note drawFootSensor
\date April 2013
\remarks
draws force/torque and acceleration at the foot sensor
*******************************************************************************
Function Parameters: [in]=input,[out]=output
******************************************************************************/
static void
drawFootSensor(void)
{
int i,j,n;
GLfloat color_point1[4]={(float)1.0,(float)1.0,(float)0.5,(float)opacity};
GLfloat color_point2[4]={(float)0.0,(float)1.0,(float)0.5,(float)opacity};
GLfloat color_point3[4]={(float)0.0,(float)1.0,(float)1.0,(float)opacity};
GLfloat color_point4[4]={(float)0.5,(float)0.5,(float)1.0,(float)opacity};
double arrow_width = 0.01;
double s[N_CART+1];
double e[N_CART+1];
double acc_scale = 0.02;
double vel_scale = 1;
double force_scale = fscale*0.1;
double torque_scale = fscale*2.0;
glPushMatrix();
// translate to L_FOOT
glTranslated(link_pos_sim[L_FOOT][_X_],link_pos_sim[L_FOOT][_Y_],link_pos_sim[L_FOOT][_Z_]);
// rotate into L_FOOT coordinates
linkQuat(Alink_sim[L_FOOT],&(cart_orient[LEFT_FOOT]));
glRotated((GLdouble)2.*acos(cart_orient[LEFT_FOOT].q[_Q0_])/PI*180.,
(GLdouble)cart_orient[LEFT_FOOT].q[_Q1_],
(GLdouble)cart_orient[LEFT_FOOT].q[_Q2_],
(GLdouble)cart_orient[LEFT_FOOT].q[_Q3_]);
// the start and end point of the acceleration vector
s[_X_] = 0.0;
s[_Y_] = 0.0;
s[_Z_] = 0.0;
e[_X_] = s[_X_] + misc_sim_sensor[L_FOOT_XACC]*acc_scale;
e[_Y_] = s[_Y_] + misc_sim_sensor[L_FOOT_YACC]*acc_scale;
e[_Z_] = s[_Z_] + misc_sim_sensor[L_FOOT_ZACC]*acc_scale;
glColor4fv(color_point1);
drawArrow(s,e,arrow_width);
// the start and end point of the force vector
s[_X_] = -ZTOE+FTA_Z_OFF;
s[_Y_] = FTA_X_OFF;
s[_Z_] = -FTA_Y_OFF;
e[_X_] = s[_X_] + misc_sim_sensor[L_CFx]*force_scale;
e[_Y_] = s[_Y_] + misc_sim_sensor[L_CFy]*force_scale;
e[_Z_] = s[_Z_] + misc_sim_sensor[L_CFz]*force_scale;
glColor4fv(color_point3);
drawArrow(s,e,arrow_width);
// the end point of the torque vector
e[_X_] = s[_X_] + misc_sim_sensor[L_CTa]*torque_scale;
e[_Y_] = s[_Y_] + misc_sim_sensor[L_CTb]*torque_scale;
e[_Z_] = s[_Z_] + misc_sim_sensor[L_CTg]*torque_scale;
glColor4fv(color_point4);
drawArrow(s,e,arrow_width);
glPopMatrix();
glPushMatrix();
// translate to R_FOOT
glTranslated(link_pos_sim[R_FOOT][_X_],link_pos_sim[R_FOOT][_Y_],link_pos_sim[R_FOOT][_Z_]);
// rotate into L_FOOT coordinates
linkQuat(Alink_sim[R_FOOT],&(cart_orient[RIGHT_FOOT]));
glRotated((GLdouble)2.*acos(cart_orient[RIGHT_FOOT].q[_Q0_])/PI*180.,
(GLdouble)cart_orient[RIGHT_FOOT].q[_Q1_],
(GLdouble)cart_orient[RIGHT_FOOT].q[_Q2_],
(GLdouble)cart_orient[RIGHT_FOOT].q[_Q3_]);
// the start and end point of the acceleration vector
s[_X_] = 0.0;
s[_Y_] = 0.0;
s[_Z_] = 0.0;
e[_X_] = s[_X_] + misc_sim_sensor[R_FOOT_XACC]*acc_scale;
e[_Y_] = s[_Y_] + misc_sim_sensor[R_FOOT_YACC]*acc_scale;
e[_Z_] = s[_Z_] + misc_sim_sensor[R_FOOT_ZACC]*acc_scale;
glColor4fv(color_point1);
drawArrow(s,e,arrow_width);
// the start and end point of the force vector
s[_X_] = -ZTOE+FTA_Z_OFF;
s[_Y_] = FTA_X_OFF;
s[_Z_] = FTA_Y_OFF;
e[_X_] = s[_X_] + misc_sim_sensor[R_CFx]*force_scale;
e[_Y_] = s[_Y_] + misc_sim_sensor[R_CFy]*force_scale;
e[_Z_] = s[_Z_] + misc_sim_sensor[R_CFz]*force_scale;
glColor4fv(color_point3);
drawArrow(s,e,arrow_width);
// the end point of the torque vector
e[_X_] = s[_X_] + misc_sim_sensor[R_CTa]*torque_scale;
e[_Y_] = s[_Y_] + misc_sim_sensor[R_CTb]*torque_scale;
e[_Z_] = s[_Z_] + misc_sim_sensor[R_CTg]*torque_scale;
glColor4fv(color_point4);
drawArrow(s,e,arrow_width);
glPopMatrix();
}
