00001
00002
00003
00004
00005
00006 #include "rsv_balance_gazebo_control/control.h"
00007
00008 const float g_fWheelRadius = 0.19;
00009 const float g_fBaseWidth = 0.5;
00010 const float g_fI3 = .85;
00011
00012 const float A[4][4] = {
00013 { 0, 0, 0, 1 },
00014 { -1.2540568855640422, 0, 0, 0 },
00015 { 0, 0, 0, 0 },
00016 { 6.059985836147613, 0, 0, 0 }};
00017
00018 const float B[4][2] = {
00019 { 0.0, 0.0 },
00020 { 0.1635280961687897, 0.1635280961687897 },
00021 { -0.09719802837298575, 0.09719802837298575 },
00022 { -0.15573631660299134, -0.15573631660299134 }};
00023
00024 const float C[4][4] = {
00025 { 1.0, 0.0, 0.0, 0.0 },
00026 { 0.0, 1.0, 0.0, 0.0 },
00027 { 0.0, 0.0, 1.0, 0.0 },
00028 { 0.0, 0.0, 0.0, 1.0 }};
00029
00030 const float L[4][4] = {
00031 { 28.04011263, -2.25919745, -0. , 10.92301101 },
00032 { -2.25919745, 28.93520042, -0. , 5.07012978 },
00033 { 0., 0., 20., 0. },
00034 { 10.92301101, 5.07012978, -0. , 5.48639076 }};
00035
00036 const float K[2][4] = {
00037 {-117.29162704, -14.14213562, -7.07106781, -53.23791934},
00038 {-117.29162704, -14.14213562, 7.07106781, -53.23791934}};
00039
00040 const float A_BK[4][4] = {
00041 { 0. , 0. , 0. , 1. },
00042 { 37.10689605, 4.62527303, 0. , 17.41179119},
00043 { 0. , 0. , -1.3745877 , 0. },
00044 {-30.47314609, -4.40488822, -0. , -16.58215492 }};