import numpy as np import matplotlib.pyplot as plt from matplotlib import animation from mpcpy.utils import compute_path_from_wp import mpcpy P = mpcpy.Params() import sys import time import pybullet as p import time def get_state(robotId): """ """ robPos, robOrn = p.getBasePositionAndOrientation(robotId) linVel,angVel = p.getBaseVelocity(robotId) return np.array([robPos[0], robPos[1], np.sqrt(linVel[0]**2 + linVel[1]**2), p.getEulerFromQuaternion(robOrn)[2]]) def set_ctrl(robotId,currVel,acceleration,steeringAngle): gearRatio=1./21 steering = [0,2] wheels = [8,15] maxForce = 50 targetVelocity = currVel + acceleration*P.DT #targetVelocity=lastVel #print(targetVelocity) for wheel in wheels: p.setJointMotorControl2(robotId, wheel, p.VELOCITY_CONTROL, targetVelocity=targetVelocity/gearRatio, force=maxForce) for steer in steering: p.setJointMotorControl2(robotId, steer, p.POSITION_CONTROL, targetPosition=steeringAngle) def plot_results(path,x_history,y_history): """ """ plt.style.use("ggplot") plt.figure() plt.title("MPC Tracking Results") plt.plot(path[0,:],path[1,:], c='tab:orange',marker=".",label="reference track") plt.plot(x_history, y_history, c='tab:blue',marker=".",alpha=0.5,label="vehicle trajectory") plt.axis("equal") plt.legend() plt.show() def run_sim(): """ """ p.connect(p.GUI) p.resetDebugVisualizerCamera(cameraDistance=1.0, cameraYaw=-90, cameraPitch=-45, cameraTargetPosition=[-0.1,-0.0,0.65]) p.resetSimulation() p.setGravity(0,0,-10) useRealTimeSim = 1 p.setTimeStep(1./120.) p.setRealTimeSimulation(useRealTimeSim) # either this plane = p.loadURDF("racecar/plane.urdf") #track = p.loadSDF("racecar/f10_racecar/meshes/barca_track.sdf", globalScaling=1) car = p.loadURDF("racecar/f10_racecar/racecar_differential.urdf", [0,0.3,.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) 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) # Interpolated Path to follow given waypoints path = compute_path_from_wp([0,3,4,6,10,11,12,6,1,0], [0,0,2,4,3,3,-1,-6,-2,-2],P.path_tick) for x_,y_ in zip(path[0,:],path[1,:]): p.addUserDebugLine([x_,y_,0],[x_,y_,0.33],[0,0,1]) #starting guess action = np.zeros(P.M) action[0] = P.MAX_ACC/2 #a action[1] = 0.0 #delta # Cost Matrices Q = np.diag([20,20,10,20]) #state error cost Qf = np.diag([30,30,30,30]) #state final error cost R = np.diag([10,10]) #input cost R_ = np.diag([10,10]) #input rate of change cost mpc = mpcpy.MPC(P.N,P.M,Q,R) x_history=[] y_history=[] time.sleep(0.5) input("Press Enter to continue...") while (1): state = get_state(car) x_history.append(state[0]) y_history.append(state[1]) #track path in bullet p.addUserDebugLine([state[0],state[1],0],[state[0],state[1],0.5],[1,0,0]) if np.sqrt((state[0]-path[0,-1])**2+(state[1]-path[1,-1])**2)<0.2: print("Success! Goal Reached") set_ctrl(car,0,0,0) plot_results(path,x_history,y_history) input("Press Enter to continue...") p.disconnect() return #for MPC car ref frame is used state[0:2] = 0.0 state[3] = 0.0 #add 1 timestep delay to input state[0]=state[0]+state[2]*np.cos(state[3])*P.DT state[1]=state[1]+state[2]*np.sin(state[3])*P.DT state[2]=state[2]+action[0]*P.DT state[3]=state[3]+action[0]*np.tan(action[1])/P.L*P.DT #optimization loop start=time.time() # State Matrices A,B,C = mpcpy.get_linear_model_matrices(state, action) #Get Reference_traj -> inputs are in worldframe target, _ = mpcpy.get_ref_trajectory(get_state(car), path, 1.0) x_mpc, u_mpc = mpc.optimize_linearized_model(A, B, C, state, target, time_horizon=P.T, verbose=False) #action = np.vstack((np.array(u_mpc.value[0,:]).flatten(), # (np.array(u_mpc.value[1,:]).flatten()))) action[:] = [u_mpc.value[0,1],u_mpc.value[1,1]] elapsed=time.time()-start print("CVXPY Optimization Time: {:.4f}s".format(elapsed)) set_ctrl(car,state[2],action[0],action[1]) if P.DT-elapsed>0: time.sleep(P.DT-elapsed) if __name__ == '__main__': run_sim()