#print("planeId=",planeId)

pybullet_client.configureDebugVisualizer(pybullet_client.COV_ENABLE_Y_AXIS_UP,
                                         1)
pybullet_client.setGravity(0, -9.8, 0)

pybullet_client.setPhysicsEngineParameter(numSolverIterations=10)

mocapData = motion_capture_data.MotionCaptureData()
#motionPath = pybullet_data.getDataPath()+"/data/motions/humanoid3d_walk.txt"
motionPath = pybullet_data.getDataPath(
) + "/data/motions/humanoid3d_backflip.txt"
mocapData.Load(motionPath)
timeStep = 1. / 600
useFixedBase = False
humanoid = humanoid_stable_pd.HumanoidStablePD(pybullet_client, mocapData,
                                               timeStep, useFixedBase)
isInitialized = True

pybullet_client.setTimeStep(timeStep)
pybullet_client.setPhysicsEngineParameter(numSubSteps=2)
timeId = pybullet_client.addUserDebugParameter("time", 0, 10, 0)


def isKeyTriggered(keys, key):
    o = ord(key)
    if o in keys:
        return keys[ord(key)] & pybullet_client.KEY_WAS_TRIGGERED
    return False


animating = False
Пример #2
0
    def reset(self):
        self.t = 0
        if not self._isInitialized:
            if self.enable_draw:
                self._pybullet_client = bullet_client.BulletClient(
                    connection_mode=p1.GUI)
            else:
                self._pybullet_client = bullet_client.BulletClient()

            self._pybullet_client.setAdditionalSearchPath(
                pybullet_data.getDataPath())
            z2y = self._pybullet_client.getQuaternionFromEuler(
                [-math.pi * 0.5, 0, 0])
            self._planeId = self._pybullet_client.loadURDF(
                "plane.urdf", [0, 0, 0], z2y)
            #print("planeId=",self._planeId)
            self._pybullet_client.configureDebugVisualizer(
                self._pybullet_client.COV_ENABLE_Y_AXIS_UP, 1)
            self._pybullet_client.setGravity(0, -9.8, 0)

            self._pybullet_client.setPhysicsEngineParameter(
                numSolverIterations=10)
            self._pybullet_client.changeDynamics(self._planeId,
                                                 linkIndex=-1,
                                                 lateralFriction=0.9)

            self._mocapData = motion_capture_data.MotionCaptureData()
            motionPath = pybullet_data.getDataPath(
            ) + "/motions/humanoid3d_walk.txt"
            #motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
            self._mocapData.Load(motionPath)
            timeStep = 1. / 600
            useFixedBase = False
            self._humanoid = humanoid_stable_pd.HumanoidStablePD(
                self._pybullet_client, self._mocapData, timeStep, useFixedBase)
            self._isInitialized = True

            self._pybullet_client.setTimeStep(timeStep)
            self._pybullet_client.setPhysicsEngineParameter(numSubSteps=2)

            selfCheck = False
            if (selfCheck):
                curTime = 0
                while self._pybullet_client.isConnected():
                    self._humanoid.setSimTime(curTime)
                    state = self._humanoid.getState()
                    #print("state=",state)
                    pose = self._humanoid.computePose(
                        self._humanoid._frameFraction)
                    for i in range(10):
                        curTime += timeStep
                        #taus = self._humanoid.computePDForces(pose)
                        #self._humanoid.applyPDForces(taus)
                        #self._pybullet_client.stepSimulation()
                    time.sleep(timeStep)
        #print("numframes = ", self._humanoid._mocap_data.NumFrames())
        startTime = random.randint(0,
                                   self._humanoid._mocap_data.NumFrames() - 2)
        rnrange = 1000
        rn = random.randint(0, rnrange)
        startTime = float(rn) / rnrange * self._humanoid.getCycleTime()

        self._humanoid.setSimTime(startTime)
        self._humanoid.resetPose()
        #this clears the contact points. Todo: add API to explicitly clear all contact points?
        self._pybullet_client.stepSimulation()
Пример #3
0
    def reset(self):

        if not self._isInitialized:
            if self.enable_draw:
                self._pybullet_client = bullet_client.BulletClient(
                    connection_mode=p1.GUI)
                #disable 'GUI' since it slows down a lot on Mac OSX and some other platforms
                self._pybullet_client.configureDebugVisualizer(
                    self._pybullet_client.COV_ENABLE_GUI, 0)
            else:
                self._pybullet_client = bullet_client.BulletClient()

            self._pybullet_client.setAdditionalSearchPath(
                pybullet_data.getDataPath())
            z2y = self._pybullet_client.getQuaternionFromEuler(
                [-math.pi * 0.5, 0, 0])
            self._planeId = self._pybullet_client.loadURDF(
                "plane_implicit.urdf", [0, 0, 0],
                z2y,
                useMaximalCoordinates=True)
            #print("planeId=",self._planeId)
            self._pybullet_client.configureDebugVisualizer(
                self._pybullet_client.COV_ENABLE_Y_AXIS_UP, 1)
            self._pybullet_client.setGravity(0, -9.8, 0)

            self._pybullet_client.setPhysicsEngineParameter(
                numSolverIterations=10)
            self._pybullet_client.changeDynamics(self._planeId,
                                                 linkIndex=-1,
                                                 lateralFriction=0.9)

            self._mocapData = motion_capture_data.MotionCaptureData()

            motion_file = self._arg_parser.parse_strings('motion_file')
            print("motion_file=", motion_file[0])

            motionPath = pybullet_data.getDataPath() + "/" + motion_file[0]
            #motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
            self._mocapData.Load(motionPath)
            timeStep = self.timeStep
            useFixedBase = False
            self._humanoid = humanoid_stable_pd.HumanoidStablePD(
                self._pybullet_client,
                self._mocapData,
                timeStep,
                useFixedBase,
                self._arg_parser,
                mode=self.ppp)
            self._isInitialized = True

            self._pybullet_client.setTimeStep(timeStep)
            self._pybullet_client.setPhysicsEngineParameter(numSubSteps=1)

            selfCheck = False
            if (selfCheck):
                curTime = 0
                while self._pybullet_client.isConnected():
                    self._humanoid.setSimTime(curTime)
                    state = self._humanoid.getState()
                    #print("state=",state)
                    pose = self._humanoid.computePose(
                        self._humanoid._frameFraction)
                    for i in range(10):
                        curTime += timeStep
                        #taus = self._humanoid.computePDForces(pose)
                        #self._humanoid.applyPDForces(taus)
                        #self._pybullet_client.stepSimulation()
                    time.sleep(timeStep)
        #print("numframes = ", self._humanoid._mocap_data.NumFrames())
        #startTime = random.randint(0,self._humanoid._mocap_data.NumFrames()-2)

        if self._init_strategy == InitializationStrategy.RANDOM:
            rnrange = 1000
            rn = random.randint(0, rnrange)
            startTime = float(rn) / rnrange * self._humanoid.getCycleTime()
        elif self._init_strategy == InitializationStrategy.START:
            startTime = 0

        self.t = startTime

        self._humanoid.setSimTime(startTime)

        self._humanoid.resetPose()
        #this clears the contact points. Todo: add API to explicitly clear all contact points?
        #self._pybullet_client.stepSimulation()
        self._humanoid.resetPose()
        self.needs_update_time = self.t - 1  #force update