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