add bicycle resources and testBike.py script (use python -m pybullet_envs.examples.testBike after pip install pybullet)

center shadow around camera target
2018-01-31 22:17:15 -08:00
parent 40482e0925
commit 13d67e8fc3
14 changed files with 692 additions and 1 deletions
--- a/examples/pybullet/gym/pybullet_envs/examples/testBike.py
+++ b/examples/pybullet/gym/pybullet_envs/examples/testBike.py
@@ -0,0 +1,137 @@
+import pybullet as p
+import math
+import time
+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.
+
+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)
+
+	p.changeDynamics(plane,-1, mass=20,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.setRealTimeSimulation(0)
+#coordPos	=	[0,0,0]
+#coordOrnEuler = [0,0,0]
+
+#coordPos= [0.7000000000000004, 0, 0.22000000000000006]
+#coordOrnEuler= [0, -0.2617993877991496, 0]
+
+coordPos= [0.07, 0, -0.6900000000000004]
+coordOrnEuler= [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))
+eul = p.getEulerFromQuaternion(mOrn)
+print("rpy=\"",eul[0],eul[1],eul[2],"\" xyz=\"", mPos[0],mPos[1], mPos[2])
+
+
+shift=0
+gui = 1
+
+
+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):
+
+	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);
+
+