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(); }