194 lines
6.3 KiB
Python
194 lines
6.3 KiB
Python
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()
|