TuxMonkey/bf_original_balance_chassis
1
0
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

添加增益矩阵k

Browse Source
This commit is contained in:
kai
2024-03-23 21:40:23 +08:00
parent 19bdac0863
commit 60ed50d438
2 changed files with 44 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
application/chassis/lqr_calc.h
View File

@@ -9,7 +9,18 @@
*/
static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis)
{
float k[12][3] = {0};
float k[12][3] = {85.842511,-168.176021,-4.383994,
-8.011354,-27.544550,0.946791,
40.658824,-34.245857,-13.443024,
33.057883,-37.645803,-8.872245,
186.414265,-182.258244,59.110130,
12.371459,-12.979501,4.575691,
-2.044517,-13.973090,28.443243,
-8.073717,8.553946,2.899594,
109.977348,-109.218563,37.018016,
67.432105,-68.632617,25.883284,
-210.173934,175.310058,96.350481,
-15.827060,13.395116,3.049615};
float T[2] = {0}; // 0 T_wheel 1 T_hip
float l = p->leg_len;
float lsqr = l * l;