Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

add yapf style and apply yapf to format all Python files

Browse Source 
This recreates pull request #2192
This commit is contained in:
Erwin Coumans
2019-04-27 07:31:15 -07:00
parent c591735042
commit ef9570c315
347 changed files with 70304 additions and 22752 deletions
Download Patch File Download Diff File Expand all files Collapse all files

194
examples/pybullet/gym/pybullet_envs/examples/testBike.py
View File

@@ -2,7 +2,7 @@ import os
import inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0,parentdir)
os.sys.path.insert(0, parentdir)
import pybullet as p
import math
@@ -12,31 +12,31 @@ import pybullet_data
p.connect(p.GUI)
#p.loadURDF("wheel.urdf",[0,0,3])
p.setAdditionalSearchPath(pybullet_data.getDataPath())
plane=p.loadURDF("plane100.urdf",[0,0,0])
timestep = 1./240.
plane = p.loadURDF("plane100.urdf", [0, 0, 0])
timestep = 1. / 240.
bike=-1
for i in range (1):
bike = -1
for i in range(1):
bike=p.loadURDF("bicycle/bike.urdf",[0,0+3*i,1.5], [0,0,0,1], useFixedBase=False)
p.setJointMotorControl2(bike,0,p.VELOCITY_CONTROL,targetVelocity=0,force=0.05)
#p.setJointMotorControl2(bike,1,p.VELOCITY_CONTROL,targetVelocity=5, force=1000)
p.setJointMotorControl2(bike,1,p.VELOCITY_CONTROL,targetVelocity=5, force=0)
p.setJointMotorControl2(bike,2,p.VELOCITY_CONTROL,targetVelocity=15, force=20)
bike = p.loadURDF("bicycle/bike.urdf", [0, 0 + 3 * i, 1.5], [0, 0, 0, 1], useFixedBase=False)
p.setJointMotorControl2(bike, 0, p.VELOCITY_CONTROL, targetVelocity=0, force=0.05)
#p.setJointMotorControl2(bike,1,p.VELOCITY_CONTROL,targetVelocity=5, force=1000)
p.setJointMotorControl2(bike, 1, p.VELOCITY_CONTROL, targetVelocity=5, force=0)
p.setJointMotorControl2(bike, 2, p.VELOCITY_CONTROL, targetVelocity=15, force=20)
p.changeDynamics(plane,-1, mass=0,lateralFriction=1, linearDamping=0, angularDamping=0)
p.changeDynamics(bike,1,lateralFriction=1,linearDamping=0, angularDamping=0)
p.changeDynamics(bike,2,lateralFriction=1,linearDamping=0, angularDamping=0)
#p.resetJointState(bike,1,0,100)
#p.resetJointState(bike,2,0,100)
#p.resetBaseVelocity(bike,[0,0,0],[0,0,0])
p.changeDynamics(plane, -1, mass=0, lateralFriction=1, linearDamping=0, angularDamping=0)
p.changeDynamics(bike, 1, lateralFriction=1, linearDamping=0, angularDamping=0)
p.changeDynamics(bike, 2, lateralFriction=1, linearDamping=0, angularDamping=0)
#p.resetJointState(bike,1,0,100)
#p.resetJointState(bike,2,0,100)
#p.resetBaseVelocity(bike,[0,0,0],[0,0,0])
#p.setPhysicsEngineParameter(numSubSteps=0)
#bike=p.loadURDF("frame.urdf",useFixedBase=True)
#bike = p.loadURDF("handlebar.urdf", useFixedBase=True)
#p.loadURDF("handlebar.urdf",[0,2,1])
#coord = p.loadURDF("handlebar.urdf", [0.7700000000000005, 0, 0.22000000000000006],useFixedBase=True)# p.loadURDF("coordinateframe.urdf",[-2,0,1],useFixedBase=True)
#coord = p.loadURDF("coordinateframe.urdf",[-2,0,1],useFixedBase=True)
p.setGravity(0,0,-10)
p.setGravity(0, 0, -10)
p.setRealTimeSimulation(0)
#coordPos = [0,0,0]
#coordOrnEuler = [0,0,0]
@@ -44,100 +44,96 @@ p.setRealTimeSimulation(0)
#coordPos= [0.7000000000000004, 0, 0.22000000000000006]
#coordOrnEuler= [0, -0.2617993877991496, 0]
coordPos= [0.07, 0, -0.6900000000000004]
coordOrnEuler= [0, 0, 0]
coordPos = [0.07, 0, -0.6900000000000004]
coordOrnEuler = [0, 0, 0]
coordPos2= [0, 0, 0 ]
coordOrnEuler2= [0, 0, 0]
coordPos2 = [0, 0, 0]
coordOrnEuler2 = [0, 0, 0]
invPos,invOrn=p.invertTransform(coordPos,p.getQuaternionFromEuler(coordOrnEuler))
mPos,mOrn = p.multiplyTransforms(invPos,invOrn, coordPos2,p.getQuaternionFromEuler(coordOrnEuler2))
invPos, invOrn = p.invertTransform(coordPos, p.getQuaternionFromEuler(coordOrnEuler))
mPos, mOrn = p.multiplyTransforms(invPos, invOrn, coordPos2,
p.getQuaternionFromEuler(coordOrnEuler2))
eul = p.getEulerFromQuaternion(mOrn)
print("rpy=\"",eul[0],eul[1],eul[2],"\" xyz=\"", mPos[0],mPos[1], mPos[2])
print("rpy=\"", eul[0], eul[1], eul[2], "\" xyz=\"", mPos[0], mPos[1], mPos[2])
shift=0
shift = 0
gui = 1
frame=0
states=[]
frame = 0
states = []
states.append(p.saveState())
#observations=[]
#observations.append(obs)
running = True
reverting = False
p.getCameraImage(320,200)#,renderer=p.ER_BULLET_HARDWARE_OPENGL )
while (1):
p.getCameraImage(320, 200) #,renderer=p.ER_BULLET_HARDWARE_OPENGL )
updateCam = 0
keys = p.getKeyboardEvents()
for k,v in keys.items():
if (reverting or (k == p.B3G_LEFT_ARROW and (v&p.KEY_WAS_TRIGGERED))):
reverting=True
stateIndex = len(states)-1
#print("prestateIndex=",stateIndex)
time.sleep(timestep)
updateCam=1
if (stateIndex>0):
stateIndex-=1
states=states[:stateIndex+1]
#observations=observations[:stateIndex+1]
#print("states=",states)
#print("stateIndex =",stateIndex )
p.restoreState(states[stateIndex])
#obs=observations[stateIndex]
#obs, r, done, _ = env.step(a)
if (k == p.B3G_LEFT_ARROW and (v&p.KEY_WAS_RELEASED)):
reverting = False
while (1):
if (k == ord('1') and (v&p.KEY_WAS_TRIGGERED)):
gui = not gui
if (k == p.B3G_RIGHT_ARROW and (v&p.KEY_WAS_RELEASED)):
running=False
if (running or (k == p.B3G_RIGHT_ARROW and (v&p.KEY_WAS_TRIGGERED))):
running = True
if (running):
p.stepSimulation()
updateCam=1
time.sleep(timestep)
pts = p.getContactPoints()
#print("numPoints=",len(pts))
#for point in pts:
# print("Point:bodyA=", point[1],"bodyB=",point[2],"linkA=",point[3],"linkB=",point[4],"dist=",point[8],"force=",point[9])
states.append(p.saveState())
#observations.append(obs)
stateIndex = len(states)
#print("stateIndex =",stateIndex )
frame += 1
if (updateCam):
distance=5
yaw = 0
humanPos, humanOrn = p.getBasePositionAndOrientation(bike)
humanBaseVel = p.getBaseVelocity(bike)
#print("frame",frame, "humanPos=",humanPos, "humanVel=",humanBaseVel)
if (gui):
camInfo = p.getDebugVisualizerCamera()
curTargetPos = camInfo[11]
distance=camInfo[10]
yaw = camInfo[8]
pitch=camInfo[9]
targetPos = [0.95*curTargetPos[0]+0.05*humanPos[0],0.95*curTargetPos[1]+0.05*humanPos[1],curTargetPos[2]]
p.resetDebugVisualizerCamera(distance,yaw,pitch,targetPos);
updateCam = 0
keys = p.getKeyboardEvents()
for k, v in keys.items():
if (reverting or (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_TRIGGERED))):
reverting = True
stateIndex = len(states) - 1
#print("prestateIndex=",stateIndex)
time.sleep(timestep)
updateCam = 1
if (stateIndex > 0):
stateIndex -= 1
states = states[:stateIndex + 1]
#observations=observations[:stateIndex+1]
#print("states=",states)
#print("stateIndex =",stateIndex )
p.restoreState(states[stateIndex])
#obs=observations[stateIndex]
#obs, r, done, _ = env.step(a)
if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_RELEASED)):
reverting = False
if (k == ord('1') and (v & p.KEY_WAS_TRIGGERED)):
gui = not gui
if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_RELEASED)):
running = False
if (running or (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_TRIGGERED))):
running = True
if (running):
p.stepSimulation()
updateCam = 1
time.sleep(timestep)
pts = p.getContactPoints()
#print("numPoints=",len(pts))
#for point in pts:
# print("Point:bodyA=", point[1],"bodyB=",point[2],"linkA=",point[3],"linkB=",point[4],"dist=",point[8],"force=",point[9])
states.append(p.saveState())
#observations.append(obs)
stateIndex = len(states)
#print("stateIndex =",stateIndex )
frame += 1
if (updateCam):
distance = 5
yaw = 0
humanPos, humanOrn = p.getBasePositionAndOrientation(bike)
humanBaseVel = p.getBaseVelocity(bike)
#print("frame",frame, "humanPos=",humanPos, "humanVel=",humanBaseVel)
if (gui):
camInfo = p.getDebugVisualizerCamera()
curTargetPos = camInfo[11]
distance = camInfo[10]
yaw = camInfo[8]
pitch = camInfo[9]
targetPos = [
0.95 * curTargetPos[0] + 0.05 * humanPos[0], 0.95 * curTargetPos[1] + 0.05 * humanPos[1],
curTargetPos[2]
]
p.resetDebugVisualizerCamera(distance, yaw, pitch, targetPos)