Как начать физическую симуляцию в pybullet с GUI в wxpython - PullRequest
Новая
       92

Как начать физическую симуляцию в pybullet с GUI в wxpython

0 голосов
/ 09 февраля 2020

Я пытаюсь открыть проект Python с GUI в wx python через событие кнопки. Проект Python представляет собой физический симулятор, в котором используется pybullet. Следующий код показывает минимальный пример, чтобы показать вам мою проблему. Первый код является примером для приложения wx python. Я импортирую имитационный проект и использую функцию sim.start(), чтобы инициализировать pybullet-симуляцию через событие кнопки. 

import wx
import Sim as sim #This is the physics simulation


class MainFrame(wx.Frame):
    def __init__(self):
        wx.Frame.__init__(self, None, wx.ID_ANY, "Digitaler Zwilling", size=(800, 600))

        self.panel = Panel(self)
        self.sizer = wx.BoxSizer(wx.VERTICAL)
        self.sizer.Add(self.panel, 0, wx.EXPAND, 0)
        self.SetSizer(self.sizer)


class Panel(wx.Panel):
    def __init__(self, parent):
        wx.Panel.__init__(self, parent = parent)

        # ____Buttons_____#
        start_btn = wx.Button(self, -1, label = "Start", pos = (635, 450), size = (95, 55))
        start_btn.Bind(wx.EVT_BUTTON, self.start)

    def start(self, event):
       sim.start   


if __name__ == '__main__':
    app = wx.App(False)
    frame = MainFrame()
    frame.Show()
    app.MainLoop()

В качестве проекта pybullet я выбираю пример из целого rnet: 

import pybullet as p
import pybullet_data as pd
import time

def start():
    p.connect(p.GUI)
    p.setAdditionalSearchPath(pd.getDataPath())

    plane = p.loadURDF("plane.urdf")
    p.setGravity(0, 0, -9.8)
    p.setTimeStep(1./500)
    # p.setDefaultContactERP(0)
    # urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
    urdfFlags = p.URDF_USE_SELF_COLLISION
    quadruped = p.loadURDF("laikago/laikago_toes.urdf", [0, 0, .5], [0, 0.5, 0.5, 0],
                           flags = urdfFlags,
                           useFixedBase = False)

    # enable collision between lower legs

    for j in range(p.getNumJoints(quadruped)):
        print(p.getJointInfo(quadruped, j))

    # 2,5,8 and 11 are the lower legs
    lower_legs = [2, 5, 8, 11]
    for l0 in lower_legs:
        for l1 in lower_legs:
            if (l1 > l0):
                enableCollision = 1
                print("collision for pair", l0, l1,
                      p.getJointInfo(quadruped, l0) [12],
                      p.getJointInfo(quadruped, l1) [12], "enabled=", enableCollision)
                p.setCollisionFilterPair(quadruped, quadruped, 2, 5, enableCollision)

    jointIds = []
    paramIds = []
    jointOffsets = []
    jointDirections = [-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1]
    jointAngles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]

    for i in range(4):
        jointOffsets.append(0)
        jointOffsets.append(-0.7)
        jointOffsets.append(0.7)

    maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20)

    for j in range(p.getNumJoints(quadruped)):
        p.changeDynamics(quadruped, j, linearDamping = 0, angularDamping = 0)
        info = p.getJointInfo(quadruped, j)
        # print(info)
        jointName = info [1]
        jointType = info [2]
        if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
            jointIds.append(j)

    p.getCameraImage(480, 320)
    p.setRealTimeSimulation(0)

    joints = []

    with open(pd.getDataPath() + "/laikago/data1.txt", "r") as filestream:
        for line in filestream:
            print("line=", line)
            maxForce = p.readUserDebugParameter(maxForceId)
            currentline = line.split(",")
            # print (currentline)
            # print("-----")
            frame = currentline [0]
            t = currentline [1]
            # print("frame[",frame,"]")
            joints = currentline [2:14]
            # print("joints=",joints)
            for j in range(12):
                targetPos = float(joints [j])
                p.setJointMotorControl2(quadruped,
                                        jointIds [j],
                                        p.POSITION_CONTROL,
                                        jointDirections [j]*targetPos + jointOffsets [j],
                                        force = maxForce)
            p.stepSimulation()
            for lower_leg in lower_legs:
                # print("points for ", quadruped, " link: ", lower_leg)
                pts = p.getContactPoints(quadruped, -1, lower_leg)
                # print("num points=",len(pts))
                # for pt in pts:
                # print(pt[9])
            time.sleep(1./500.)

    index = 0
    for j in range(p.getNumJoints(quadruped)):
        p.changeDynamics(quadruped, j, linearDamping = 0, angularDamping = 0)
        info = p.getJointInfo(quadruped, j)
        js = p.getJointState(quadruped, j)
        # print(info)
        jointName = info [1]
        jointType = info [2]
        if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
            paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4,
                                                    (js [0] - jointOffsets [index])/jointDirections [index]))
            index = index + 1

    p.setRealTimeSimulation(1)

    while (1):

        for i in range(len(paramIds)):
            c = paramIds [i]
            targetPos = p.readUserDebugParameter(c)
            maxForce = p.readUserDebugParameter(maxForceId)
            p.setJointMotorControl2(quadruped,
                                    jointIds [i],
                                    p.POSITION_CONTROL,
                                    jointDirections [i]*targetPos + jointOffsets [i],
                                    force = maxForce)

Открывается pybullet GUI, и начинается симуляция, но затем она застревает ( Посмотреть скриншот) Screenshot of the pybullet GUI

Думаю, проблема может заключаться в том, что я запустил симуляцию pybullet с событием кнопки из wx python, которое может быть вызвано только с app.MainLoop(). Но на самом деле я не уверен.

Я пытался:

  • , чтобы выйти из Mainl oop до начала симуляции
  • , чтобы начать симуляцию с новым нить типа: 
def start(self, event):
    self.create_thread(sim.start)

def create_thread(self, target):
    thread = threading.Thread(target=target)
    thread.daemon = True
    thread.start()

Кто-нибудь знает, как запустить симуляцию pybullet с wx python GUI, без каких-либо ограничений симуляции? Или я могу сказать кому-нибудь, что я делаю не так?

...