277 lines
8.0 KiB
Python
277 lines
8.0 KiB
Python
import pybullet as p
|
|
import time
|
|
import math
|
|
import os
|
|
import pybullet_data
|
|
|
|
p.connect(p.GUI)
|
|
|
|
p.resetSimulation()
|
|
|
|
p.setGravity(0, 0, -10)
|
|
useRealTimeSim = 0
|
|
|
|
p.setTimeStep(1.0 / 120.0)
|
|
p.setRealTimeSimulation(useRealTimeSim) # either this
|
|
|
|
track = p.loadURDF("plane.urdf")
|
|
# track = p.loadSDF("f10_racecar/meshes/barca_track.sdf", globalScaling=1)
|
|
# otherCar = p.loadURDF("f10_racecar/racecar_differential.urdf", [0,1,.3])
|
|
car = p.loadURDF("f10_racecar/racecar_differential.urdf", [0, 0, 0.3])
|
|
|
|
for wheel in range(p.getNumJoints(car)):
|
|
print("joint[", wheel, "]=", p.getJointInfo(car, wheel))
|
|
p.setJointMotorControl2(car, wheel, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
|
|
p.getJointInfo(car, wheel)
|
|
|
|
wheels = [8, 15]
|
|
print("----------------")
|
|
|
|
# p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10)
|
|
c = p.createConstraint(
|
|
car,
|
|
9,
|
|
car,
|
|
11,
|
|
jointType=p.JOINT_GEAR,
|
|
jointAxis=[0, 1, 0],
|
|
parentFramePosition=[0, 0, 0],
|
|
childFramePosition=[0, 0, 0],
|
|
)
|
|
p.changeConstraint(c, gearRatio=1, maxForce=10000)
|
|
|
|
c = p.createConstraint(
|
|
car,
|
|
10,
|
|
car,
|
|
13,
|
|
jointType=p.JOINT_GEAR,
|
|
jointAxis=[0, 1, 0],
|
|
parentFramePosition=[0, 0, 0],
|
|
childFramePosition=[0, 0, 0],
|
|
)
|
|
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
|
|
|
|
c = p.createConstraint(
|
|
car,
|
|
9,
|
|
car,
|
|
13,
|
|
jointType=p.JOINT_GEAR,
|
|
jointAxis=[0, 1, 0],
|
|
parentFramePosition=[0, 0, 0],
|
|
childFramePosition=[0, 0, 0],
|
|
)
|
|
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
|
|
|
|
c = p.createConstraint(
|
|
car,
|
|
16,
|
|
car,
|
|
18,
|
|
jointType=p.JOINT_GEAR,
|
|
jointAxis=[0, 1, 0],
|
|
parentFramePosition=[0, 0, 0],
|
|
childFramePosition=[0, 0, 0],
|
|
)
|
|
p.changeConstraint(c, gearRatio=1, maxForce=10000)
|
|
|
|
|
|
c = p.createConstraint(
|
|
car,
|
|
16,
|
|
car,
|
|
19,
|
|
jointType=p.JOINT_GEAR,
|
|
jointAxis=[0, 1, 0],
|
|
parentFramePosition=[0, 0, 0],
|
|
childFramePosition=[0, 0, 0],
|
|
)
|
|
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
|
|
|
|
c = p.createConstraint(
|
|
car,
|
|
17,
|
|
car,
|
|
19,
|
|
jointType=p.JOINT_GEAR,
|
|
jointAxis=[0, 1, 0],
|
|
parentFramePosition=[0, 0, 0],
|
|
childFramePosition=[0, 0, 0],
|
|
)
|
|
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
|
|
|
|
c = p.createConstraint(
|
|
car,
|
|
1,
|
|
car,
|
|
18,
|
|
jointType=p.JOINT_GEAR,
|
|
jointAxis=[0, 1, 0],
|
|
parentFramePosition=[0, 0, 0],
|
|
childFramePosition=[0, 0, 0],
|
|
)
|
|
p.changeConstraint(c, gearRatio=-1, gearAuxLink=15, maxForce=10000)
|
|
c = p.createConstraint(
|
|
car,
|
|
3,
|
|
car,
|
|
19,
|
|
jointType=p.JOINT_GEAR,
|
|
jointAxis=[0, 1, 0],
|
|
parentFramePosition=[0, 0, 0],
|
|
childFramePosition=[0, 0, 0],
|
|
)
|
|
p.changeConstraint(c, gearRatio=-1, gearAuxLink=15, maxForce=10000)
|
|
|
|
|
|
steering = [0, 2]
|
|
|
|
hokuyo_joint = 4
|
|
zed_camera_joint = 5
|
|
|
|
targetVelocitySlider = p.addUserDebugParameter("wheelVelocity", -2, 2, 0)
|
|
maxForceSlider = p.addUserDebugParameter("maxForce", 0, 50, 20)
|
|
steeringSlider = p.addUserDebugParameter("steering", -1, 1, 0)
|
|
|
|
# replaceLines=True
|
|
# numRays=100
|
|
# rayFrom=[]
|
|
# rayTo=[]
|
|
# rayIds=[]
|
|
# rayHitColor = [1,0,0]
|
|
# rayMissColor = [0,1,0]
|
|
# rayLen = 8
|
|
# rayStartLen=0.25
|
|
# for i in range (numRays):
|
|
# #rayFrom.append([0,0,0])
|
|
# rayFrom.append([rayStartLen*math.sin(-0.5*0.25*2.*math.pi+0.75*2.*math.pi*float(i)/numRays), rayStartLen*math.cos(-0.5*0.25*2.*math.pi+0.75*2.*math.pi*float(i)/numRays),0])
|
|
# rayTo.append([rayLen*math.sin(-0.5*0.25*2.*math.pi+0.75*2.*math.pi*float(i)/numRays), rayLen*math.cos(-0.5*0.25*2.*math.pi+0.75*2.*math.pi*float(i)/numRays),0])
|
|
# if (replaceLines):
|
|
# rayIds.append(p.addUserDebugLine(rayFrom[i], rayTo[i], rayMissColor,parentObjectUniqueId=car, parentLinkIndex=hokuyo_joint ))
|
|
# else:
|
|
# rayIds.append(-1)
|
|
|
|
|
|
# def getCarYaw(car):
|
|
# carPos,carOrn = p.getBasePositionAndOrientation(car)
|
|
# carEuler = p.getEulerFromQuaternion(carOrn)
|
|
# carYaw = carEuler[2]*360/(2.*math.pi)-90
|
|
# return carYaw
|
|
|
|
# prevCarYaw = getCarYaw(car)
|
|
frame = 0
|
|
|
|
lineId = p.addUserDebugLine([0, 0, 0], [0, 0, 1], [1, 0, 0])
|
|
lineId2 = p.addUserDebugLine([0, 0, 0], [0, 0, 1], [1, 0, 0])
|
|
lineId3 = p.addUserDebugLine([0, 0, 0], [0, 0, 1], [1, 0, 0])
|
|
print("lineId=", lineId)
|
|
|
|
# camInfo = p.getDebugVisualizerCamera()
|
|
lastTime = time.time()
|
|
lastControlTime = time.time()
|
|
# lastLidarTime = time.time()
|
|
|
|
|
|
def getCarVel(car):
|
|
linVel, angVel = p.getBaseVelocity(car)
|
|
return linVel[0]
|
|
|
|
|
|
# frame=0
|
|
while True:
|
|
# print (getCarVel(car))
|
|
nowTime = time.time()
|
|
# render Camera at 10Hertz
|
|
# if (nowTime-lastTime>.1):
|
|
# ls = p.getLinkState(car,zed_camera_joint, computeForwardKinematics=True)
|
|
# camPos = ls[0]
|
|
# camOrn = ls[1]
|
|
# camMat = p.getMatrixFromQuaternion(camOrn)
|
|
# upVector = [0,0,1]
|
|
# forwardVec = [camMat[0],camMat[3],camMat[6]]
|
|
# #sideVec = [camMat[1],camMat[4],camMat[7]]
|
|
# camUpVec = [camMat[2],camMat[5],camMat[8]]
|
|
# camTarget = [camPos[0]+forwardVec[0]*10,camPos[1]+forwardVec[1]*10,camPos[2]+forwardVec[2]*10]
|
|
# camUpTarget = [camPos[0]+camUpVec[0],camPos[1]+camUpVec[1],camPos[2]+camUpVec[2]]
|
|
# viewMat = p.computeViewMatrix(camPos, camTarget, camUpVec)
|
|
# projMat = camInfo[3]
|
|
# #p.getCameraImage(320,200,viewMatrix=viewMat,projectionMatrix=projMat, flags=p.ER_NO_SEGMENTATION_MASK, renderer=p.ER_BULLET_HARDWARE_OPENGL)
|
|
# p.getCameraImage(320,200,viewMatrix=viewMat,projectionMatrix=projMat, renderer=p.ER_BULLET_HARDWARE_OPENGL)
|
|
# lastTime=nowTime
|
|
|
|
nowControlTime = time.time()
|
|
|
|
nowLidarTime = time.time()
|
|
# lidar at 20Hz
|
|
# if (nowLidarTime-lastLidarTime>.3):
|
|
# #print("Lidar!")
|
|
# numThreads=0
|
|
# results = p.rayTestBatch(rayFrom,rayTo,numThreads, parentObjectUniqueId=car, parentLinkIndex=hokuyo_joint)
|
|
# for i in range (numRays):
|
|
# hitObjectUid=results[i][0]
|
|
# hitFraction = results[i][2]
|
|
# hitPosition = results[i][3]
|
|
# if (hitFraction==1.):
|
|
# p.addUserDebugLine(rayFrom[i],rayTo[i], rayMissColor,replaceItemUniqueId=rayIds[i],parentObjectUniqueId=car, parentLinkIndex=hokuyo_joint)
|
|
# else:
|
|
# localHitTo = [rayFrom[i][0]+hitFraction*(rayTo[i][0]-rayFrom[i][0]),
|
|
# rayFrom[i][1]+hitFraction*(rayTo[i][1]-rayFrom[i][1]),
|
|
# rayFrom[i][2]+hitFraction*(rayTo[i][2]-rayFrom[i][2])]
|
|
# p.addUserDebugLine(rayFrom[i],localHitTo, rayHitColor,replaceItemUniqueId=rayIds[i],parentObjectUniqueId=car, parentLinkIndex=hokuyo_joint)
|
|
# lastLidarTime = nowLidarTime
|
|
|
|
# control at 100Hz
|
|
if nowControlTime - lastControlTime > 0.01:
|
|
carPos, carOrn = p.getBasePositionAndOrientation(car)
|
|
|
|
# Keep the previous orientation of the camera set by the user.
|
|
|
|
# yaw = camInfo[8]
|
|
# pitch = camInfo[9]
|
|
# distance = camInfo[10]
|
|
# targetPos = camInfo[11]
|
|
# camFwd = camInfo[5]
|
|
# carYaw = getCarYaw(car)
|
|
#
|
|
# #the car yaw is clamped between -90 and 270, make sure to deal with angles that wrap around
|
|
# if (carYaw-prevCarYaw>45):
|
|
# yaw+=360
|
|
# if (carYaw-prevCarYaw<-45):
|
|
# yaw-=360
|
|
# prevCarYaw = carYaw
|
|
|
|
# print("carYaw=", carYaw)
|
|
# print("camYaw=", yaw)
|
|
|
|
# slowly rotate the camera behind the car
|
|
# diffYaw = (carYaw-yaw)*0.03
|
|
|
|
# track the position of the car as target
|
|
# p.resetDebugVisualizerCamera(distance, yaw+diffYaw, pitch, carPos)
|
|
|
|
maxForce = p.readUserDebugParameter(maxForceSlider)
|
|
targetVelocity = p.readUserDebugParameter(targetVelocitySlider)
|
|
steeringAngle = p.readUserDebugParameter(steeringSlider)
|
|
# print(targetVelocity)
|
|
gearRatio = 1.0 / 21
|
|
|
|
for wheel in wheels:
|
|
p.setJointMotorControl2(
|
|
car,
|
|
wheel,
|
|
p.VELOCITY_CONTROL,
|
|
targetVelocity=targetVelocity / gearRatio,
|
|
force=maxForce,
|
|
)
|
|
|
|
for steer in steering:
|
|
p.setJointMotorControl2(
|
|
car, steer, p.POSITION_CONTROL, targetPosition=steeringAngle
|
|
)
|
|
|
|
if useRealTimeSim == 0:
|
|
frame += 1
|
|
p.stepSimulation()
|
|
lastControlTime = nowControlTime
|