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)
