【Pybullet】接触を検知する

pybulletを使っていて，ロボットが何かと接触したことを検知したかったです．

ロボットの足が地面と接地しているかは，よく強化学習における観測データとして利用されます．

命令としては，p.getContactPoints()を使えば出来ます．

docs.google.com

こんな感じで使います．
robotは対象のbodyのid番号で，right_footが対象linkのidです．

result = p.getContactPoints(robot, -1, right_foot, -1)

この場合，robotのright_footが接触している座標を返します．

値があるとこんな感じです．

print(result)
-> ((0, 1, 0, 5, -1, (0.306818108432366, -0.20726071519498707, -2.237793284010081e-16), (0.30681810843236623, -0.20726071519498718, 0.0004359660735957366), (-5.697036571337818e-13, 2.3927553599618834e-13, -1.0), 0.00043596607359596036, 14.825712481037616),)

何とも接触していなければ，空のタプルを返します．

print(result)
-> ()