add example robots in pybullet_robots module
This commit is contained in:
Erwin Coumans
@@ -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)
|
||||
Reference in New Issue
Block a user