mpc_python_learn/mpc_pybullet_demo/racecar/racecar.py

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