Webots建模
我們首先要過一遍官方指導，瞭解node的相關概念:下圖爲一個solid node
一個有物理效果的實體（重力，碰撞）就必須有boundingObject ,shape,physics節點組成
多個solid可以在children目錄通過自身相對上級節點的座標變換串接起來
一個實體主要要由boundingObject ,shape,physics 即物理屬性 碰撞綁定形狀，形狀組成
箭頭從上到下依次是：
座標變換：參考座標系爲上一級座標系（如果是再樹中建立，就是以世界座標系爲參考，如果再children目錄中的solid及其派生節點，則是以children的座標原點變換(點擊children目錄就可以看到其對應的座標系)）
rotation：四個參數，前三個定義一根旋轉軸，第四個爲角度
children:子節點用來連接其他東西，
contactMaterial：接觸材質，在樹最開始的worldInfo中設置
參數相關含義即英文含義，這裏主要定義接觸材質名稱，和摩擦係數
boundingObject：綁定碰撞形狀，參考官方例程。
physics：物理參數，參考官方例程：如果其爲空（NULL),其所在節點將不會移動，參考示例中的場地牆的案例
機器人建模需要的Robot節點和solid節點基本相同這裏就不再說明
開始修改模型
我們直接修改官方示例：
需要更改的項目將用其他顏色標出
我們將其另存爲新後就可以開始更改
藍綠色的字表示值是非默認值
contactProperties：定義接觸面的相關參數
material：接觸面名稱
coulombFriction：摩擦係數
我們可以看到官方例程裏的碰撞綁定爲圖中紅色箭頭指示的紅色框；我們要將它和下面的物理屬性刪掉；
因爲不知道什麼原因，這裏設置的質量會影響LQR控制器的控制;比如本來小車只有1KG的質量，但是如果你要在計算狀態空間方程時小車質量 設爲1KG 控制器控制的小車就會飛掉。
修改小車材質，使其透明 方便後續
在其中選擇 show joint axis 顯示關節軸，可以看到其中會有兩條條黑線：
其他顯示參數自行調整
藍色箭頭方向的爲小車導軌，紅色箭頭方向的就是擺杆的轉軸。白色線框爲小車碰撞邊界
SliderJoint :線性驅動器，參數名稱就是英文本意 ，從上到下爲
關節參數：關於初始位置，方向 ，限位，彈簧參數，阻尼係數
驅動器:線性馬達，位置傳感器，要記住驅動器名稱 後期的控制器需要它來調用API
現在可以看到：
箭頭所指的有一個顏色較暗的小座標系，這個是一個solid 節點質心的標誌，它的位置明顯不對勁
我們可以看到線性驅動器的endPoint的物理屬性,形狀和碰撞邊界表現爲小車，centerofmass爲默認，沒有偏移，所以這個節點的座標系原點應該是擺杆的質心，其質量爲1KG
我們將轉動關節HIP 的關節參數的position(初始位置)改爲0，方便讀取後續座標值，
anchor：轉軸原點，改不改對本例沒有影響。
可以看到 圖中大號座標系爲轉動關節endpoint 的座標系，其參考座標系爲上級，（即滑動關節endpoint），有興趣的可以自己改下參數觀察。
我們看下面物理參數部分，centerOFMass有值，y=-0.27 就是以endpoint爲參考，我們將其刪掉，或回覆默認值
這樣質心就會和endpoint solid的座標系重合，質心離轉軸等於擺杆的座標系Y軸數值
如果要修改擺杆長度，需要先修改endpoint的座標變換，在修改其children中的shape相關。
倒立擺控制率及屬性模型參考https://zhuanlan.zhihu.com/p/54071212
這裏僅給出狀態空間方程：
其中q:
控制器輸入U=-[K1 K2 K3 K4] X
X=[X,X_DOT,TH,TH_DOT],
K矩陣由matlab的LQR函數算出 代碼參照上述鏈接
控制器主要通過傳感器獲取位置和角度信息，計算狀態變量，傳給控制率來控制小車實現平衡。
這是控制效果，位置設置爲1