mpc_python_learn/mpc_pybullet_demo/racecar/racecar.py

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