見出し画像

ロボットアームを使ってみよう4　PyBullet

CODELIGHT

高田です。ROSを使わずロボットアームをリアルタイムに動かす方法を色々調べていると、xArmのフォーラムで以下の投稿を見つけました。

https://forum.ufactory.cc/t/smoother-linear-motion-with-xarm-7-leap-motion/2285

Bullet物理エンジンを使ってxArmの姿勢を計算するデモのようです。作者はBulletの開発者です。実機で試してみたところ、非常に高速に動かす事が出来ました。
肝はIKを自前で計算してset_servo_angle_jで関節位置を直接指定することのようです。

これを使って自由に動したいので、PyBulletを調べてみます。

PyBullet

xArm7のURDFを読み込む


PyBulletをダウンロードするとLib\site-packages\pybullet_data\xarmにxarm6のURDFが含まれていました。ただ弊社にあるxarm7のurdfは含まれていなかったので、先ほどのスレッドに掲示されていたgithubのURLにコミットされていたデータを使います。
https://github.com/erwincoumans/xArm-Python-SDK/tree/master/example/wrapper/xarm7

このxarm7_robot.urdfをPyBulletに含まれていたxarm6_robot.urdfに合わせて変更しました。このデータをLib\site-packages\pybullet_data\xarmに置きます。

そして、Lib\site-packages\pybullet_data\xarm\xarm_description\meshesにxarm7のメッシュフォルダをコピーします。

次に以下のようなpythonコードを実行するとロボットアームが表示されます。これはPyBullet Quickstart Guideの最初に載っていたコードを少しだけ変更したものです。
https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#heading=h.2ye70wns7io3

import pybullet as p
import time
import pybullet_data

physicsClient = p.connect(p.GUI)  # or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath())  # optionally
p.setGravity(0, 0, -9.8)

planeId = p.loadURDF("plane.urdf")
startPos = [0, 0, 0]
startOrientation = p.getQuaternionFromEuler([0, 0, 0])
p.loadURDF("xarm/xarm7_robot.urdf", startPos, startOrientation)

# set the center of mass frame (loadURDF sets base link frame) startPos/Ornp.resetBasePositionAndOrientation(boxId, startPos, startOrientation)
for i in range(10000):
    p.stepSimulation()
    time.sleep(1./240.)

p.disconnect()

先ほどのコードを見るとシミュレーション用のループがあります。ためしにマウスでアームのモデルをドラッグするとBulletがIKを解いてアームの姿勢を計算してくれました！

実機へ反映

関節角度を取得して実機に与えてやれば一応リアルタイムに動くはずです。
改めて、最初に出て来たスレッドで紹介されていたコードを確認します。
実行するとこのようにエンドエフェクターが円運動を行います。

メインのスクリプトはxarm_real_ik.pyで以下のように書かれています。
arm.set_servo_angle_jはxArmのAPIで各関節位置を直接指定します。この値はxarm_sim.pyのstep内で計算されます。

xarm = xarm_sim.XArm7Sim(p,[0,0,0])
while (p.isConnected()):
	xarm.step()
	arm.set_servo_angle_j(angles=xarm.jointPoses, speed=speed, is_radian=True)
	p.stepSimulation()
	time.sleep(timeStep)

xarm_sim.pyのstep関数はこうなっています。

def step(self):
        t = self.t
        self.t += 1./60.

        pos = [0.407+self.offset[0]+0.2 *
               math.sin(1.5 * t), 0.0+self.offset[1]+0.2 * math.cos(1.5 * t), 0.12+self.offset[2]]
        orn = [1, 0, 0, 0]

        restPoses = [0]*xarmNumDofs
        jointPoses = self.bullet_client.calculateInverseKinematics(self.xarm, 
                        xarmEndEffectorIndex, 
                        pos, 
                        orn, 
                        lowerLimits=ll,
                        upperLimits=ul, 
                        jointRanges=jr, 
                        restPoses=np.array(restPoses).tolist(), 
                        residualThreshold=1e-5, 
                        maxNumIterations=ikMaxNumIterations)

        self.jointPoses = jointPoses

        for i in range(xarmNumDofs):
            pose = self.jointPoses[i]
            self.bullet_client.setJointMotorControl2(
                self.xarm, 
                i+1, 
                self.bullet_client.POSITION_CONTROL, 
                pose, 
                force=5 * 240.)

calculateInverseKinematicsでエンドエフェクターの位置を指定してIKを計算しています。jointPoseに関節角度が返ってくるので、xArm Apiのset_servo_angle_jで指定すればその通りに動きます。

posの部分にセンサーからの値を入れてやればリアルタイムに動くようになりますが、全てIKで姿勢を計算させているため安全面で不安要素があり、姿勢に制限を入れる対応が重要だと思います。

ともあれ、リアルタイムに制御できそうなことが分かったので次回以降センサーと連動させていこうと思います。