140 lines
4.5 KiB
Python
140 lines
4.5 KiB
Python
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)
|