Pybullet上のxarmをcolaboratry上で動かしてみる

Pythonでリアルタイム衝突シミュレーションをやるライブラリPyBullet - Bye Bye Moore
の続き

表示するだけでは旨みが少ないので、xarmを動かしてみます

実際のところ

前回と同様、xArmのURDFを導入

%%time
!pip install pybullet

import pybullet as p
import pybullet_data as pd
import numpy as np
import matplotlib.pyplot as plt
from matplotlib import pylab
%matplotlib inline
p.connect(p.DIRECT)
#allow to find the assets (URDF, obj, textures etc)
p.setAdditionalSearchPath(pd.getDataPath())


p.resetSimulation()
p.configureDebugVisualizer(p.COV_ENABLE_GUI)
useFixedBase = True
flags = p.URDF_INITIALIZE_SAT_FEATURES

plane_pos = [0,0,-0.625]
plane = p.loadURDF("plane.urdf", plane_pos, flags = flags, useFixedBase=useFixedBase)
table_pos = [0,0,-0.625]
table = p.loadURDF("table/table.urdf", table_pos, flags = flags, useFixedBase=useFixedBase)
xarm = p.loadURDF("xarm/xarm6_robot.urdf", flags = flags, useFixedBase=useFixedBase)

アニメーション画像を生成するnumpngwを導入

!pip install numpngw

numpngwをIPython.displayのImageモジュールを使って描画

角度情報は”setJointMotorControl2”で指定し、stepSimulationでレンダリング

from numpngw import write_apng
from IPython.display import Image


jointIds = []
paramIds = []

for j in range(p.getNumJoints(xarm)):
  p.changeDynamics(xarm, j, linearDamping=0, angularDamping=0)
  info = p.getJointInfo(xarm, j)
  #print(info)
  jointName = info[1]
  jointType = info[2]
  if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
    jointIds.append(j)
    paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4, 0))

for i in range(len(paramIds)):
  targetPos = 0.0
  p.setJointMotorControl2(xarm, jointIds[i], p.POSITION_CONTROL, targetPos, force=5 * 240.)
  p.stepSimulation()

targetPos1 = 0
targetPos2 = 0


frames=[] #frames to create animated png
for r in range(60):
    #Create xArm Motion
    for i in range(len(paramIds)):
      targetPos1 = 0.1 *r
      targetPos2 -= 0.002
      p.setJointMotorControl2(xarm, 1, p.POSITION_CONTROL, targetPos1, force=5 * 240.)
      p.setJointMotorControl2(xarm, 2, p.POSITION_CONTROL, targetPos2, force=5 * 240.)
      p.stepSimulation()


    yaw = 1
    pitch = -10.0
    roll = 0
    upAxisIndex = 2
    camDistance = 1.5
    pixelWidth = 320
    pixelHeight = 200
    nearPlane = 0.01
    farPlane = 100
    fov = 60
    viewMatrix = p.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch,
                                                                roll, upAxisIndex)
    aspect = pixelWidth / pixelHeight
    projectionMatrix = p.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane)
        
    img_arr = p.getCameraImage(pixelWidth,pixelHeight,viewMatrix,projectionMatrix)
    w = img_arr[0]  #width of the image, in pixels
    h = img_arr[1]  #height of the image, in pixels
    rgb = img_arr[2]  #color data RGB
    dep = img_arr[3]  #depth data
    #print("w=",w,"h=",h)
    np_img_arr = np.reshape(rgb, (h, w, 4))
    frame = np_img_arr[:, :, :3]
    frames.append(frame)
print("creating animated png, please about 5 seconds")
%time write_apng("example6.png", frames, delay=100)
%time Image(filename="example6.png")

こんな感じで、くるくる回る画像がでてくる