53 lines
1.5 KiB
Python
53 lines
1.5 KiB
Python
|
import pybullet as p
|
||
|
import pybullet_data
|
||
|
|
||
|
import time
|
||
|
import os
|
||
|
|
||
|
p.connect(p.GUI)
|
||
|
|
||
|
p.resetSimulation()
|
||
|
p.setGravity(0, 0, -10)
|
||
|
|
||
|
useRealTimeSim = 1
|
||
|
|
||
|
#for video recording (works best on Mac and Linux, not well on Windows)
|
||
|
#p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "racecar.mp4")
|
||
|
p.setRealTimeSimulation(useRealTimeSim) # either this
|
||
|
p.loadURDF(os.path.join(pybullet_data.getDataPath(), "plane.urdf"))
|
||
|
|
||
|
car = p.loadURDF(os.path.join(pybullet_data.getDataPath(), "racecar/racecar.urdf"))
|
||
|
for i in range(p.getNumJoints(car)):
|
||
|
print(p.getJointInfo(car, i))
|
||
|
|
||
|
inactive_wheels = [3, 5, 7]
|
||
|
wheels = [2]
|
||
|
|
||
|
for wheel in inactive_wheels:
|
||
|
p.setJointMotorControl2(car, wheel, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
|
||
|
|
||
|
steering = [4, 6]
|
||
|
|
||
|
targetVelocitySlider = p.addUserDebugParameter("wheelVelocity", -10, 10, 0)
|
||
|
maxForceSlider = p.addUserDebugParameter("maxForce", 0, 10, 10)
|
||
|
steeringSlider = p.addUserDebugParameter("steering", -0.5, 0.5, 0)
|
||
|
while (True):
|
||
|
maxForce = p.readUserDebugParameter(maxForceSlider)
|
||
|
targetVelocity = p.readUserDebugParameter(targetVelocitySlider)
|
||
|
steeringAngle = p.readUserDebugParameter(steeringSlider)
|
||
|
|
||
|
for wheel in wheels:
|
||
|
p.setJointMotorControl2(car,
|
||
|
wheel,
|
||
|
p.VELOCITY_CONTROL,
|
||
|
targetVelocity=targetVelocity,
|
||
|
force=maxForce)
|
||
|
|
||
|
for steer in steering:
|
||
|
p.setJointMotorControl2(car, steer, p.POSITION_CONTROL, targetPosition=steeringAngle)
|
||
|
|
||
|
if (useRealTimeSim == 0):
|
||
|
p.stepSimulation()
|
||
|
|
||
|
time.sleep(0.01)
|