def save_world(self, dir, pre_f, inf_f, ep): wn = "{}.video.{}.video{:06}.world.world".format(pre_f, inf_f, ep) sn = "{}.video.{}.video{:06}.setpoints.yaml".format(pre_f, inf_f, ep) world_path = os.path.join(dir, wn) sp_path = os.path.join(dir, sn) pb.saveWorld(world_path, self.clientId) with open(sp_path, 'w') as f: yaml.dump(self.sp_history, f)
def saveStatus(self, save_state=False, on_disk=False, filename=None): """Save the current status of the simulation. Either stores - an initial configuration (save_state = False ,on_disk = False) with a sufficient filename - current state in memory (save_state = True, on_disk = False) - current state on disk (save_state = True, on_disk = True) with a sufficient filename Is a wrapper for pybullet.saveWorld, saveState, saveBullet Returns: state_id, filename """ if save_state and on_disk: if not filename: print("Supply a valid filename!") return pybullet.saveBullet(filename) return filename if save_state and not on_disk: if not hasattr(self, "stored_states"): self.stored_states = [] self.stored_states.append(pybullet.saveState()) return self.stored_states[-1] if not save_state and not on_disk: if not filename: print("Supply a valid filename!") return if not hasattr(self, "stored_states"): self.stored_states = [] self.stored_states.append( pybullet.saveWorld(fileName=filename, clientServerId=self.client_id) ) return self.stored_states[-1], filename
wheels = [2, 3, 4, 5] #(2, b'front_left_wheel', 0, 7, 6, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'front_left_wheel_link') #(3, b'front_right_wheel', 0, 8, 7, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'front_right_wheel_link') #(4, b'rear_left_wheel', 0, 9, 8, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'rear_left_wheel_link') #(5, b'rear_right_wheel', 0, 10, 9, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'rear_right_wheel_link') wheelVelocities = [0, 0, 0, 0] wheelDeltasTurn = [1, -1, 1, -1] wheelDeltasFwd = [1, 1, 1, 1] while 1: keys = p.getKeyboardEvents() shift = 0.01 wheelVelocities = [0, 0, 0, 0] speed = 1.0 for k in keys: if ord('s') in keys: p.saveWorld("state.py") if ord('a') in keys: basepos = basepos = [basepos[0], basepos[1] - shift, basepos[2]] if ord('d') in keys: basepos = basepos = [basepos[0], basepos[1] + shift, basepos[2]] if p.B3G_LEFT_ARROW in keys: for i in range(len(wheels)): wheelVelocities[ i] = wheelVelocities[i] - speed * wheelDeltasTurn[i] if p.B3G_RIGHT_ARROW in keys: for i in range(len(wheels)): wheelVelocities[ i] = wheelVelocities[i] + speed * wheelDeltasTurn[i] if p.B3G_UP_ARROW in keys: for i in range(len(wheels)):
t = 0.0 t_end = t + 15 ref_time = time.time() while (t < t_end): if (useRealTime): t = time.time() - ref_time else: t = t + fixedTimeStep if (useRealTime == 0): p.stepSimulation() time.sleep(fixedTimeStep) print("quadruped Id = ") print(quadruped) p.saveWorld("quadru.py") logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR, "quadrupedLog.bin", [quadruped]) #jump t = 0.0 t_end = t + 100 i = 0 ref_time = time.time() while (1): if (useRealTime): t = time.time() - ref_time else: t = t + fixedTimeStep if (True):
ser = None portindex = 10 while (ser is None and portindex < 30): portname = 'COM'+str(portindex) print(portname) ser = getSerialOrNone(portname) if (ser is None): portname = "/dev/cu.usbmodem14"+str(portindex) print(portname) ser = getSerialOrNone(portname) if (ser is not None): print("COnnected!") portindex = portindex+1 p.saveWorld("setupTrackerWorld.py") if (ser.isOpen()): while True: events = p.getVREvents(deviceTypeFilter=p.VR_DEVICE_GENERIC_TRACKER) for e in (events): p.changeConstraint(hand_cid,e[POSITION],e[ORIENTATION], maxForce=50) serialSteps = 0 while ser.inWaiting() > 0: serialSteps=serialSteps+1 if (serialSteps>serialStepsUntilCheckVREvents): ser.flushInput() break
p.setGravity(0,0,-9.8) jointIds = [] paramIds = [] jointPositions=[0,0,0,-1.4,0,1.4,0.71,0.014,0.016] index = 0 print("numJoints = ",p.getNumJoints(panda)) for j in range(p.getNumJoints(panda)): p.changeDynamics(panda, j, linearDamping=0, angularDamping=0) info = p.getJointInfo(panda, j) #print(info) jointName = info[1] jointType = info[2] if (jointType == p.JOINT_PRISMATIC): jointIds.append(j) paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"), -0.1, 0.1, jointPositions[index])) index=index+1 if (jointType == p.JOINT_REVOLUTE): jointIds.append(j) paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4, jointPositions[index])) index=index+1 while (1): for i in range(len(paramIds)): c = paramIds[i] targetPos = p.readUserDebugParameter(c) p.setJointMotorControl2(panda, jointIds[i], p.POSITION_CONTROL, targetPos, force=5 * 240.) p.stepSimulation() p.saveWorld("bla")
ser = None portindex = 10 while (ser is None and portindex < 30): portname = 'COM' + str(portindex) print(portname) ser = getSerialOrNone(portname) if (ser is None): portname = "/dev/cu.usbmodem14" + str(portindex) print(portname) ser = getSerialOrNone(portname) if (ser is not None): print("COnnected!") portindex = portindex + 1 p.saveWorld("setupTrackerWorld.py") if (ser.isOpen()): while True: events = p.getVREvents(deviceTypeFilter=p.VR_DEVICE_GENERIC_TRACKER) for e in (events): p.changeConstraint(hand_cid, e[POSITION], e[ORIENTATION], maxForce=50) serialSteps = 0 while ser.inWaiting() > 0: serialSteps = serialSteps + 1 if (serialSteps > serialStepsUntilCheckVREvents):
wheels=[2,3,4,5] #(2, b'front_left_wheel', 0, 7, 6, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'front_left_wheel_link') #(3, b'front_right_wheel', 0, 8, 7, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'front_right_wheel_link') #(4, b'rear_left_wheel', 0, 9, 8, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'rear_left_wheel_link') #(5, b'rear_right_wheel', 0, 10, 9, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'rear_right_wheel_link') wheelVelocities=[0,0,0,0] wheelDeltasTurn=[1,-1,1,-1] wheelDeltasFwd=[1,1,1,1] while 1: keys = p.getKeyboardEvents() shift = 0.01 wheelVelocities=[0,0,0,0] speed = 1.0 for k in keys: if ord('s') in keys: p.saveWorld("state.py") if ord('a') in keys: basepos = basepos=[basepos[0],basepos[1]-shift,basepos[2]] if ord('d') in keys: basepos = basepos=[basepos[0],basepos[1]+shift,basepos[2]] if p.B3G_LEFT_ARROW in keys: for i in range(len(wheels)): wheelVelocities[i] = wheelVelocities[i] - speed*wheelDeltasTurn[i] if p.B3G_RIGHT_ARROW in keys: for i in range(len(wheels)): wheelVelocities[i] = wheelVelocities[i] +speed*wheelDeltasTurn[i] if p.B3G_UP_ARROW in keys: for i in range(len(wheels)): wheelVelocities[i] = wheelVelocities[i] + speed*wheelDeltasFwd[i] if p.B3G_DOWN_ARROW in keys:
import pybullet as p import time p.connect(p.SHARED_MEMORY) timestr = time.strftime("%Y%m%d-%H%M%S") filename = "saveWorld" + timestr + ".py" p.saveWorld(filename) p.disconnect()
t = 0.0 t_end = t + 15 ref_time = time.time() while (t < t_end): p.setGravity(0, 0, -10) if (useRealTime): t = time.time() - ref_time else: t = t + fixedTimeStep if (useRealTime == 0): p.stepSimulation() time.sleep(fixedTimeStep) print("quadruped Id = ") print(quadruped) p.saveWorld("quadru.py") logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR, "quadrupedLog.bin", [quadruped]) #jump t = 0.0 t_end = t + 100 i = 0 ref_time = time.time() while (1): if (useRealTime): t = time.time() - ref_time else: t = t + fixedTimeStep if (True):
paramIds = [] p.setPhysicsEngineParameter(numSolverIterations=10) p.changeDynamics(humanoid, -1, linearDamping=0, angularDamping=0) for j in range(p.getNumJoints(humanoid)): p.changeDynamics(humanoid, j, linearDamping=0, angularDamping=0) info = p.getJointInfo(humanoid, 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, jointPositions[j])) p.setRealTimeSimulation(1) while (1): p.setGravity(0, 0, p.readUserDebugParameter(gravId)) for i in range(len(paramIds)): c = paramIds[i] targetPos = p.readUserDebugParameter(c) p.setJointMotorControl2(humanoid, jointIds[i], p.POSITION_CONTROL, targetPos, force=5 * 240.) p.saveWorld("latest.py") time.sleep(0.01)
rootPos[0] = rootPos[0] + vr_shift[0] rootPos[1] = rootPos[1] + vr_shift[1] rootPos[2] = rootPos[2] + vr_shift[2] p.setVRCameraState(rootPosition=rootPos) #uiPts = p.getClosestPoints(bodyA=uiCube,bodyB=pr2_gripper,distance=0.01, linkIndexA=0) uiPts = p.getContactPoints(bodyA=uiCube, bodyB=pr2_gripper, linkIndexA=0) t4 = mult * time.clock() if (len(uiPts)): if (buttonA != 1): buttonA = 1 p.changeVisualShape(uiCube, 0, rgbaColor=[1, 1, 0, 1]) demos[currentDemo][2].reset() p.saveWorld("demoObjects.py") else: if (buttonA != 0): p.changeVisualShape(uiCube, 0, rgbaColor=[0, 0, 0, 1]) buttonA = 0 t5 = mult * time.clock() #if ((frameNr % 32) == 0): # p.removeUserDebugItem(lines[i]) # for i in range (numLines): # spacing = 0.01 # textPos = [.1-(i+1)*spacing,.1,0.011] # text = "Demo:"+demos[currentDemo][0]+" Frame:"+str(frameNr)+"\nObject UID:"+objectInfo # textUid = p.addUserDebugText(text,textColorRGB=[0,0,0], textSize = 0.02, textPosition = textPos,textOrientation = textOrn,parentObjectUniqueId=uiCube, parentLinkIndex = -1)