TuxMonkey/bf_original_balance_chassis
1
1
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-24 19:47:44 +08:00
Code Issues Packages Projects Releases Wiki Activity

changed ID

Browse Source
This commit is contained in:
TuxMonkey
2026-04-18 16:16:20 +08:00
parent d0b3771bf9
commit df12ca7256
2 changed files with 25 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
application/chassis/lqr_calc.h
View File

@@ -9,18 +9,18 @@
*/
static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis)
{
static float k[12][3] = {64.570926,-78.151551,-15.098827,
1.716153,-4.573289,-0.526836,
25.565621,-20.634320,-17.563574,
11.633020,-11.021911,-13.087704,
212.754066,-196.450645,56.227305,
12.051262,-11.941105,4.523285,
74.149148,-71.296364,23.673363,
4.449069,-4.214434,1.145555,
115.321993,-106.373546,29.626387,
82.459182,-74.915697,20.072512,
-163.793049,130.808796,114.419419,
-13.396998,11.357026,4.288949};
static float k[12][3] = {152.073959,-322.787113,4.602472,
-18.091924,-50.039341,3.160226,
124.677598,-113.871572,-13.279757,
86.314525,-98.184173,-8.140564,
162.907111,-185.705008,74.114015,
14.280240,-18.420891,8.593112,
-148.548074,99.259778,48.506697,
-37.972970,41.871624,3.715364,
192.255883,-224.641960,95.484838,
93.227296,-117.550338,60.336158,
-343.472953,311.036102,57.349362,
-41.609057,38.668058,-0.809331,};
float T[2] = {0}; // 0 T_wheel 1 T_hip
float l = p->leg_len;
float lsqr = l * l;