add example robots in pybullet_robots module

examples/pybullet/gym/pybullet_envs/examples/testBike.py

@@ -1,139 +0,0 @@
-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)
-
-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=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.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)