12345qiupeng
/
mpc_python_learn
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

updated pybullet mpc demo to latest version

master
mcarfagno 2020-10-28 15:08:58 +00:00
parent cac053410c
commit ef175816d8
5 changed files with 90 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
mpc_pybullet_demo/cvxpy_mpc.py
View File

@ -5,7 +5,7 @@ from scipy.integrate import odeint
from scipy.interpolate import interp1d
import cvxpy as cp
from utils import road_curve, f, df
from utils import *
from mpc_config import Params
P=Params()
@ -50,16 +50,17 @@ def optimize(state,u_bar,track,ref_vel=1.):
:returns:
'''
MAX_SPEED = ref_vel*1.5
MAX_STEER = np.pi/4
MAX_ACC = 1.0
MAX_SPEED = 1.5 #m/s
MAX_ACC = 1.0 #m/ss
MAX_D_ACC = 1.0 #m/sss
MAX_STEER = np.radians(30) #rad
MAX_D_STEER = np.radians(30) #rad/s
# compute polynomial coefficients of the track
K=road_curve(state,track)
REF_VEL = ref_vel #m/s
# dynamics starting state w.r.t vehicle frame
x_bar=np.zeros((P.N,P.T+1))
x_bar[2,0]=state[2]
# dynamics starting state
x_bar = np.zeros((P.N,P.T+1))
x_bar[:,0] = state
#prediction for linearization of costrains
for t in range (1,P.T+1):
@ -75,29 +76,40 @@ def optimize(state,u_bar,track,ref_vel=1.):
x = cp.Variable((P.N, P.T+1))
u = cp.Variable((P.M, P.T))
# Cost Matrices
Q = np.diag([20,20,10,0]) #state error cost
Qf = np.diag([10,10,10,10]) #state final error cost
R = np.diag([10,10]) #input cost
R_ = np.diag([10,10]) #input rate of change cost
#Get Reference_traj
x_ref, d_ref = get_ref_trajectory(x_bar[:,0] ,track, REF_VEL)
#Prediction Horizon
for t in range(P.T):
cost += 20*cp.sum_squares(x[3,t]-np.clip(np.arctan(df(x_bar[0,t],K)),-np.pi,np.pi) ) # psi
cost += 40*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte
cost += 20*cp.sum_squares(ref_vel-x[2,t]) # desired v
# Tracking Error
cost += cp.quad_form(x[:,t] - x_ref[:,t], Q)
# Actuation effort
cost += cp.quad_form(u[:,t], R)
# Actuation rate of change
if t < (P.T - 1):
cost += cp.quad_form(u[:, t + 1] - u[:, t], 10*np.eye(P.M))
# Actuation effort
cost += cp.quad_form( u[:, t],10*np.eye(P.M))
cost += cp.quad_form(u[:,t+1] - u[:,t], R_)
constr+= [cp.abs(u[0, t + 1] - u[0, t])/P.dt <= MAX_D_ACC] #max acc rate of change
constr += [cp.abs(u[1, t + 1] - u[1, t])/P.dt <= MAX_D_STEER] #max steer rate of change
# Kinrmatics Constrains (Linearized model)
A,B,C=get_linear_model(x_bar[:,t],u_bar[:,t])
A,B,C = get_linear_model(x_bar[:,t], u_bar[:,t])
constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]
# sums problem objectives and concatenates constraints.
constr += [x[:,0] == x_bar[:,0]] #<--watch out the start condition
constr += [x[2, :] <= MAX_SPEED]
constr += [x[2, :] >= 0.0]
constr += [cp.abs(u[0, :]) <= MAX_ACC]
constr += [cp.abs(u[1, :]) <= MAX_STEER]
constr += [x[:,0] == x_bar[:,0]] #starting condition
constr += [x[2,:] <= MAX_SPEED] #max speed
constr += [x[2,:] >= 0.0] #min_speed (not really needed)
constr += [cp.abs(u[0,:]) <= MAX_ACC] #max acc
constr += [cp.abs(u[1,:]) <= MAX_STEER] #max steer
# Solve
prob = cp.Problem(cp.Minimize(cost), constr)

1
mpc_pybullet_demo/mpc_config.py
View File

@ -4,3 +4,4 @@ class Params():
self.M = 2 #number of control variables
self.T = 10 #Prediction Horizon
self.dt = 0.25 #discretization step
self.path_tick = 0.05

2
mpc_pybullet_demo/mpc_demo_nosim.py
View File

@ -35,7 +35,7 @@ class MPC():
# Interpolated Path to follow given waypoints
#self.path = compute_path_from_wp([0,10,12,2,4,14],[0,0,2,10,12,12])
self.path = compute_path_from_wp([0,3,4,6,10,12,13,13,6,1,0],
[0,0,2,4,3,3,-1,-2,-6,-2,-2],0.5)
[0,0,2,4,3,3,-1,-2,-6,-2,-2],P.path_tick)
# Sim help vars
self.sim_time=0

4
mpc_pybullet_demo/mpc_demo_pybullet.py
View File

@ -20,7 +20,7 @@ def get_state(robotId):
robPos, robOrn = p.getBasePositionAndOrientation(robotId)
linVel,angVel = p.getBaseVelocity(robotId)
return[robPos[0], robPos[1], linVel[0], p.getEulerFromQuaternion(robOrn)[2]]
return[robPos[0], robPos[1], np.sqrt(linVel[0]**2 + linVel[1]**2), p.getEulerFromQuaternion(robOrn)[2]]
def set_ctrl(robotId,currVel,acceleration,steeringAngle):
@ -105,7 +105,7 @@ def run_sim():
# 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],0.5)
[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])

90
mpc_pybullet_demo/utils.py
View File

@ -1,16 +1,22 @@
import numpy as np
from scipy.interpolate import interp1d
from mpc_config import Params
P=Params()
def compute_path_from_wp(start_xp, start_yp, step = 0.1):
"""
Computes a reference path given a set of waypoints
"""
final_xp=[]
final_yp=[]
delta = step #[m]
for idx in range(len(start_xp)-1):
section_len = np.sqrt(np.sum(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))
interp_range = np.linspace(0,1, int(1+section_len/delta))
section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))
interp_range = np.linspace(0,1,np.floor(section_len/delta).astype(int))
fx=interp1d(np.linspace(0,1,2),start_xp[idx:idx+2],kind=1)
fy=interp1d(np.linspace(0,1,2),start_yp[idx:idx+2],kind=1)
@ -18,14 +24,21 @@ def compute_path_from_wp(start_xp, start_yp, step = 0.1):
final_xp=np.append(final_xp,fx(interp_range))
final_yp=np.append(final_yp,fy(interp_range))
return np.vstack((final_xp,final_yp))
dx = np.append(0, np.diff(final_xp))
dy = np.append(0, np.diff(final_yp))
theta = np.arctan2(dy, dx)
return np.vstack((final_xp,final_yp,theta))
def get_nn_idx(state,path):
"""
Computes the index of the waypoint closest to vehicle
"""
dx = state[0]-path[0,:]
dy = state[1]-path[1,:]
dist = np.sqrt(dx**2 + dy**2)
dist = np.hypot(dx,dy)
nn_idx = np.argmin(dist)
try:
@ -46,53 +59,42 @@ def get_nn_idx(state,path):
return target_idx
def road_curve(state,track):
def get_ref_trajectory(state, path, target_v):
"""
"""
xref = np.zeros((P.N, P.T + 1))
dref = np.zeros((1, P.T + 1))
POLY_RANK = 3
#sp = np.ones((1,T +1))*target_v #speed profile
#given vehicle pos find lookahead waypoints
nn_idx=get_nn_idx(state,track)
LOOKAHED = POLY_RANK*2
lk_wp=track[:,max(0,nn_idx-1):nn_idx+LOOKAHED]
ncourse = path.shape[1]
#trasform lookahead waypoints to vehicle ref frame
dx = lk_wp[0,:] - state[0]
dy = lk_wp[1,:] - state[1]
ind = get_nn_idx(state, path)
wp_vehicle_frame = np.vstack(( dx * np.cos(-state[3]) - dy * np.sin(-state[3]),
dy * np.cos(-state[3]) + dx * np.sin(-state[3]) ))
xref[0, 0] = path[0,ind] #X
xref[1, 0] = path[1,ind] #Y
xref[2, 0] = target_v #sp[ind] #V
xref[3, 0] = path[2,ind] #Theta
dref[0, 0] = 0.0 # steer operational point should be 0
#fit poly
return np.polyfit(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:], POLY_RANK, rcond=None, full=False, w=None, cov=False)
dl = P.path_tick # Waypoints spacing [m]
travel = 0.0
# def f(x,coeff):
# """
# """
# return round(coeff[0]*x**3 + coeff[1]*x**2 + coeff[2]*x**1 + coeff[3]*x**0,6)
for i in range(P.T + 1):
travel += abs(target_v) * P.dt #current V or target V?
dind = int(round(travel / dl))
# def f(x,coeff):
# return round(coeff[0]*x**5+coeff[1]*x**4+coeff[2]*x**3+coeff[3]*x**2+coeff[4]*x**1+coeff[5]*x**0,6)
if (ind + dind) < ncourse:
xref[0, i] = path[0,ind + dind]
xref[1, i] = path[1,ind + dind]
xref[2, i] = target_v #sp[ind + dind]
xref[3, i] = path[2,ind + dind]
dref[0, i] = 0.0
else:
xref[0, i] = path[0,ncourse - 1]
xref[1, i] = path[1,ncourse - 1]
xref[2, i] = 0.0 #stop? #sp[ncourse - 1]
xref[3, i] = path[2,ncourse - 1]
dref[0, i] = 0.0
def f(x,coeff):
y=0
j=len(coeff)
for k in range(j):
y += coeff[k]*x**(j-k-1)
return round(y,6)
# def df(x,coeff):
# """
# """
# return round(3*coeff[0]*x**2 + 2*coeff[1]*x**1 + coeff[2]*x**0,6)
# def df(x,coeff):
# return round(5*coeff[0]*x**4 + 4*coeff[1]*x**3 +3*coeff[2]*x**2 + 2*coeff[3]*x**1 + coeff[4]*x**0,6)
def df(x,coeff):
y=0
j=len(coeff)
for k in range(j-1):
y += (j-k-1)*coeff[k]*x**(j-k-2)
return round(y,6)
return xref, dref