00001 /********************************************************************* 00002 * Copyright (c) 2015 Robosavvy Ltd. 00003 * Author: Vitor Matos 00004 * 00005 *********************************************************************/ 00006 #include "rsv_balance_gazebo_control/balance_gazebo_control.h" 00007 00008 namespace balance_control 00009 { 00010 00011 BalanceControl::BalanceControl() {} 00012 00018 void BalanceControl::resetControl() 00019 { 00020 t = 0; 00021 for (int i=0; i < 4; i++) 00022 { 00023 x_hat[i] = 0; 00024 x_adjust[i] = 0; 00025 x_r[i] = 0; 00026 } 00027 u_output[0] = 0.0; 00028 u_output[1] = 0.0; 00029 } 00030 00039 void BalanceControl::stepControl(double dt, const double (&x_desired)[4], const double (&y_fbk)[4]) 00040 { 00041 t += dt; 00042 //************************************************************** 00043 // Set reference state 00044 //************************************************************** 00045 x_reference[theta] = x_desired[theta] + x_adjust[theta]; 00046 x_reference[dx] = x_desired[dx] + x_adjust[dx]; 00047 x_reference[dphi] = x_desired[dphi] + x_adjust[dphi]; 00048 x_reference[dtheta] = x_desired[dtheta] + x_adjust[dtheta]; 00049 00050 //************************************************************** 00051 // State estimation 00052 //************************************************************** 00053 // x_hat - x_ref 00054 float x_hat_x_ref[4]; 00055 x_hat_x_ref[theta] = x_hat[theta] - x_reference[theta]; 00056 x_hat_x_ref[dx] = x_hat[dx] - x_reference[dx]; 00057 x_hat_x_ref[dphi] = x_hat[dphi] - x_reference[dphi]; 00058 x_hat_x_ref[dtheta] = x_hat[dtheta] - x_reference[dtheta]; 00059 00060 // A*(x_hat-x_ref) 00061 dx_hat[theta] = A[0][0]*x_hat_x_ref[theta] + A[0][1]*x_hat_x_ref[dx] 00062 + A[0][2]*x_hat_x_ref[dphi] + A[0][3]*x_hat_x_ref[dtheta]; 00063 dx_hat[dx] = A[1][0]*x_hat_x_ref[theta] + A[1][1]*x_hat_x_ref[dx] 00064 + A[1][2]*x_hat_x_ref[dphi] + A[1][3]*x_hat_x_ref[dtheta]; 00065 dx_hat[dphi] = A[2][0]*x_hat_x_ref[theta] + A[2][1]*x_hat_x_ref[dx] 00066 + A[2][2]*x_hat_x_ref[dphi] + A[2][3]*x_hat_x_ref[dtheta]; 00067 dx_hat[dtheta] = A[3][0]*x_hat_x_ref[theta] + A[3][1]*x_hat_x_ref[dx] 00068 + A[3][2]*x_hat_x_ref[dphi] + A[3][3]*x_hat_x_ref[dtheta]; 00069 00070 // + B*u_output 00071 dx_hat[theta] += B[0][0]*u_output[tauL] + B[0][1]*u_output[tauR]; 00072 dx_hat[dx] += B[1][0]*u_output[tauL] + B[1][1]*u_output[tauR]; 00073 dx_hat[dphi] += B[2][0]*u_output[tauL] + B[2][1]*u_output[tauR]; 00074 dx_hat[dtheta] += B[3][0]*u_output[tauL] + B[3][1]*u_output[tauR]; 00075 00076 // y - C*x_hat - x_desired 00077 float yC[4]; 00078 yC[0] = y_fbk[theta] - (C[0][0]*x_hat[theta] + C[0][1]*x_hat[dx] + C[0][2]*x_hat[dphi] + C[0][3]*x_hat[dtheta]); 00079 yC[1] = y_fbk[dx] - (C[1][0]*x_hat[theta] + C[1][1]*x_hat[dx] + C[1][2]*x_hat[dphi] + C[1][3]*x_hat[dtheta]); 00080 yC[2] = y_fbk[dphi] - (C[2][0]*x_hat[theta] + C[2][1]*x_hat[dx] + C[2][2]*x_hat[dphi] + C[2][3]*x_hat[dtheta]); 00081 yC[3] = y_fbk[dtheta] - (C[3][0]*x_hat[theta] + C[3][1]*x_hat[dx] + C[3][2]*x_hat[dphi] + C[3][3]*x_hat[dtheta]); 00082 00083 // L*(y-C*x_hat) 00084 dx_hat[theta] += L[0][0]*yC[0] + L[0][1]*yC[1] + L[0][2]*yC[2] + L[0][3]*yC[3]; 00085 dx_hat[dx] += L[1][0]*yC[0] + L[1][1]*yC[1] + L[1][2]*yC[2] + L[1][3]*yC[3]; 00086 dx_hat[dphi] += L[2][0]*yC[0] + L[2][1]*yC[1] + L[2][2]*yC[2] + L[2][3]*yC[3]; 00087 dx_hat[dtheta] += L[3][0]*yC[0] + L[3][1]*yC[1] + L[3][2]*yC[2] + L[3][3]*yC[3]; 00088 00089 // Integrate Observer, Euler method: 00090 x_hat[theta] += dx_hat[theta]*dt; 00091 x_hat[dx] += dx_hat[dx]*dt; 00092 x_hat[dphi] += dx_hat[dphi]*dt; 00093 x_hat[dtheta] += dx_hat[dtheta]*dt; 00094 00095 //************************************************************** 00096 // Reference model 00097 //************************************************************** 00098 float x_r_x_ref[4]; 00099 x_r_x_ref[theta] = x_r[theta] - x_reference[theta]; 00100 x_r_x_ref[dx] = x_r[dx] - x_reference[dx]; 00101 x_r_x_ref[dphi] = x_r[dphi] - x_reference[dphi]; 00102 x_r_x_ref[dtheta] = x_r[dtheta] - x_reference[dtheta]; 00103 x_r[theta] += (A_BK[0][0]*x_r_x_ref[theta] + A_BK[0][1]*x_r_x_ref[dx] 00104 + A_BK[0][2]*x_r_x_ref[dphi] + A_BK[0][3]*x_r_x_ref[dtheta])*dt; 00105 x_r[dx] += (A_BK[1][0]*x_r_x_ref[theta] + A_BK[1][1]*x_r_x_ref[dx] 00106 + A_BK[1][2]*x_r_x_ref[dphi] + A_BK[1][3]*x_r_x_ref[dtheta])*dt; 00107 x_r[dphi] += (A_BK[2][0]*x_r_x_ref[theta] + A_BK[2][1]*x_r_x_ref[dx] 00108 + A_BK[2][2]*x_r_x_ref[dphi] + A_BK[2][3]*x_r_x_ref[dtheta])*dt; 00109 x_r[dtheta] += (A_BK[3][0]*x_r_x_ref[theta] + A_BK[3][1]*x_r_x_ref[dx] 00110 + A_BK[3][2]*x_r_x_ref[dphi] + A_BK[3][3]*x_r_x_ref[dtheta])*dt; 00111 00112 // Adjust theta based on reference model 00113 float e_r_dx; 00114 // e_r_dx = x_r[dx] - x_hat[dx]; 00115 e_r_dx = x_r[dx] - y_fbk[dx]; 00116 x_adjust[theta] += (.025*e_r_dx)*dt; 00117 00118 //************************************************************** 00119 // Feedback control 00120 //************************************************************** 00121 u_output[tauL] = -(K[0][0]*(x_hat[theta]-x_reference[theta]) + K[0][1]*(x_hat[dx]-x_reference[dx]) 00122 + K[0][2]*(x_hat[dphi]-x_reference[dphi]) + K[0][3]*x_hat[dtheta]); 00123 u_output[tauR] = -(K[1][0]*(x_hat[theta]-x_reference[theta]) + K[1][1]*(x_hat[dx]-x_reference[dx]) 00124 + K[1][2]*(x_hat[dphi]-x_reference[dphi]) + K[1][3]*x_hat[dtheta]); 00125 } 00126 00132 double *BalanceControl::getControl() 00133 { 00134 return u_output; 00135 } 00136 } // namespace balance_control