項(xiàng)目背景：
項(xiàng)目要做一個(gè)雙足機(jī)器人报嵌，先做一條腿的仿真玻熙，驗(yàn)證電機(jī)的輸出力矩是否能夠?qū)崿F(xiàn)跳躍功能叉庐。
在做pybullet的仿真之前裹匙，采用的是gazebo進(jìn)行仿真瑞凑，但是由于gazebo中urdf模型文件不支持閉鏈機(jī)構(gòu)，sdf文件雖然支持閉鏈機(jī)構(gòu)概页，但是只能通過編寫插件的方式進(jìn)行關(guān)節(jié)的控制籽御。
看到pybullet的樣例中四足機(jī)器人minitaur中存在閉鏈機(jī)構(gòu)，并且能夠?qū)崿F(xiàn)關(guān)節(jié)的控制惰匙，就決定使用pybullet進(jìn)行仿真技掏。
解決過程：
之前雖然看過pybullet的一些仿真項(xiàng)目的代碼，但是沒有真正的自己動(dòng)手寫過程序徽曲。
找到pybullet官方的使用文檔零截，看了一下get start的代碼Hello Pybullet World
我內(nèi)心竊喜，這可難不倒我ctrl-CV大法秃臣。
通過簡(jiǎn)單的分析代碼涧衙，大概知道先加載了地面，然后加載了機(jī)器人奥此，再進(jìn)入仿真循環(huán)
知道了大概流程弧哎，就開始我們自己的手術(shù)實(shí)驗(yàn)。
第一次-加載sdf文件
既然要仿真自己的機(jī)器人模型稚虎，那就要加載自己的機(jī)器人模型撤嫩。看到pybullet中有l(wèi)oadSDF函數(shù)蠢终，心中竊喜序攘，這不是白給。gazebo中早就吧sdf文件準(zhǔn)備好了寻拂。
robot = p.loadSDF("/home/craipy/PycharmProjects/single_leg_simulation/model/single_leg_v7/model copy.sdf")
程奠？？怎么回事祭钉？瞄沙？
怎么報(bào)錯(cuò)了，我sdf文件gazebo中是能夠加載的啊
嘗試把sdf中的閉鏈關(guān)節(jié)去掉
啊距境？
掏出pybullet說明文檔
為什么申尼？pybullet演我？要你有何用
第二次-構(gòu)建約束
不行垫桂，掏出我的百度大法
在知乎中看到了一個(gè)小伙伴和我遇到了同樣的問題
但遺憾的是這位朋友最后問題并沒有得到解決师幕。
然后我查閱了一下pybullet的說明文檔
首先createContraint方法，需要提供鏈接這個(gè)約束點(diǎn)的兩個(gè)linkId伪货，但是我在pybullet的說明文檔中怎么也沒有找到關(guān)于如何能夠獲取linkId的方法们衙。
通過getNumJoints方法與getJointInfo方法能夠獲得每個(gè)Joint的id與這joint的父Linkid钾怔，我進(jìn)行了一步推測(cè)碱呼，由于構(gòu)建模型文件的時(shí)候，每一個(gè)joint對(duì)應(yīng)著一個(gè)子link宗侦，子link的id應(yīng)該與jointid是一樣的愚臀。（后面閱讀minitaur的源碼文件，發(fā)現(xiàn)是這樣的道理）
勉強(qiáng)解決了2個(gè)參數(shù)矾利，還有兩個(gè)很重要的參數(shù)姑裂，parentFramePosition與childFramePosition。這兩個(gè)參數(shù)給我整的迷迷糊糊的男旗。為了定位到約束點(diǎn)舶斧，需要確定約束點(diǎn)在父Link中的位置與在子link中的位置。
這可不好整了察皇，我根本就不知道link的Position和orientation怎么確定這兩個(gè)參數(shù)呢茴厉？
繼續(xù)在說明文檔找說明
這讓我明白了pybullet包含joint坐標(biāo)系，和link的質(zhì)心坐標(biāo)系什荣。
我又不斷的嘗試矾缓，試圖找到link的質(zhì)心坐標(biāo)。在urdf文件生成過程中稻爬，我明白了urdf文件中 inertial->origin是決定link的質(zhì)心坐標(biāo)系的關(guān)鍵嗜闻。但具體是怎么決定的還需要進(jìn)一步嘗試。
<inertial>
<origin xyz="-1.49612837680224E-16 0.0680546106444214 0.450099514805464" rpy="0 0 0" />
<mass value="7.53402464620325" />
<inertia ixx="2.04472009475898" ixy="9.13747033457407E-16" ixz="-3.39571306621468E-16" iyy="2.14218400370622" iyz="0.111298129150483" izz="0.24400607242942" />
</inertial>
我在solidworks中把每個(gè)關(guān)節(jié)的坐標(biāo)點(diǎn)和每個(gè)link的質(zhì)心都以絕對(duì)位置的形式記錄下來桅锄。
在將urdf文件中的joint的origin與link的origin記錄下來琉雳。
很快就發(fā)現(xiàn)了其中的規(guī)律
joint的origin是相對(duì)前一個(gè)joint的origin建立的。
link的origin是相對(duì)joint的origin建立的
知道這個(gè)建系規(guī)則友瘤，就能夠確定指定link的質(zhì)心坐標(biāo)翠肘，進(jìn)而確定約束點(diǎn)在質(zhì)心坐標(biāo)系中的位置。
然后我發(fā)現(xiàn)有一個(gè)getLinkState的方法商佑，能夠直接獲得制定link的質(zhì)心坐標(biāo)系
知道這些锯茄，我想這回應(yīng)該能成功了吧！！肌幽！
然后
晚碾？？喂急？為什么
人最迷茫的時(shí)候不是遇到困難格嘁，而是遇到困難沒有方向解決。
我懷疑是我的坐標(biāo)位置弄錯(cuò)了廊移，但是我繪圖出來是沒有問題的糕簿，但是就是無法形成約束關(guān)系。驅(qū)動(dòng)關(guān)節(jié)的位置一改變狡孔，約束關(guān)節(jié)就裂開了懂诗。
我人也裂開了。苗膝。
我嘗試找到minitaur的源碼殃恒，看看他是怎么添加約束的，于是下載了bullet的倉庫辱揭，這里面有minitaur的例程
我還是覺得是我把關(guān)節(jié)位置弄錯(cuò)了
于是在minitaur機(jī)器人用同樣的思路把約束點(diǎn)畫出來离唐，發(fā)現(xiàn)沒有錯(cuò)。
說明約束點(diǎn)的位置沒有錯(cuò)问窃。
對(duì)比其他的例子亥鬓，我發(fā)現(xiàn)一個(gè)共同點(diǎn)，minitaur在創(chuàng)建了約束點(diǎn)后域庇，會(huì)將不是驅(qū)動(dòng)關(guān)節(jié)的控制方式修改為VELOCITY_CONTROL嵌戈。
我把我機(jī)器人中對(duì)應(yīng)的關(guān)節(jié)也做了同樣的修改
臥槽！较剃！皇天不負(fù)有心人
我終于成功了
牛批咕别！牛批
