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

Merge remote-tracking branch 'origin/dev-stool' into dev-stool

Browse Source
This commit is contained in:
TuxMonkey
2026-07-07 02:05:27 +08:00
parent 70ff2815f3 7525ecdffc
commit a4ebc4b452
5 changed files with 200 additions and 72 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] = {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,};
static float k[12][3] = {91.443258,-113.149301,-8.780431,
2.186397,-6.908826,-0.171192,
47.943203,-39.423913,-13.297140,
30.671413,-28.308799,-9.363969,
231.778706,-224.742110,68.974495,
18.910995,-20.079072,7.490403,
50.114072,-58.670210,23.856863,
3.273746,-3.321150,1.267603,
140.969395,-137.481822,42.507159,
93.859327,-90.754295,27.811839,
-283.581279,232.017305,87.865897,
-28.193619,23.802666,3.963794,};
float T[2] = {0}; // 0 T_wheel 1 T_hip
float l = p->leg_len;
float lsqr = l * l;