mpc_python_learn/mpc_pybullet_demo/racecar/racecar.py

196 lines
7.1 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./120.)
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,.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>.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./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