f10 pybullet demo

master
mcarfagno 2020-07-06 16:54:22 +01:00
parent 49549a5f7b
commit f3a985940e
11 changed files with 274 additions and 91 deletions

View File

@ -4,30 +4,35 @@ Python implementation of a mpc controller for path tracking using **[CVXPY](http
## About ## About
The MPC is a model predictive path following controller which does follow a predefined reference path Xref and Yref by solving an optimization problem. The resulting optimization problem is shown in the following equation: The MPC is a model predictive path following controller which does follow a predefined reference by solving an optimization problem. The resulting optimization problem is shown in the following equation:
![](img/quicklatex1.png) ![](img/quicklatex1.png)
The vehicle dynamics are described by the differential drive model: The terns of the cost function are the sum of the **cross-track error**, **heading error**, **velocity error** and **actuaction effort**.
Where R,P,K,Q are the cost matrices used to tune the response.
The vehicle model is described by the bicycle kinematics model using the state space matrices A and B:
![](img/quicklatex2.png) ![](img/quicklatex2.png)
The state variables of the model are: The state variables **(x)** of the model are:
* **x** coordinate of the robot * **x** coordinate of the robot
* **y** coordinate of the robot * **y** coordinate of the robot
* **v** velocuty of the robot
* **theta** heading of the robot * **theta** heading of the robot
The inputs of the model are: The inputs **(u)** of the model are:
* **v** linear velocity of the robot * **a** linear acceleration of the robot
* **w** angular velocity of the robot * **delta** steering angle of the robot
## Demo ## Demo
The MPC implementation is tested using **[bullet](https://pybullet.org/wordpress/)** physics simulator. Turtlebot model is from: *https://github.com/erwincoumans/pybullet_robots*. The MPC implementation is tested using **[bullet](https://pybullet.org/wordpress/)** physics simulator. Racing car model is from: *https://github.com/erwincoumans/pybullet_robots*.
![](img/Turtlebot.png) ![](img/f10.png)
Results: Results:

Binary file not shown.

Before

Width:  |  Height:  |  Size: 1.6 MiB

After

Width:  |  Height:  |  Size: 3.3 MiB

BIN
img/demo.gif.old Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 1.6 MiB

BIN
img/f10.png Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 54 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 4.1 KiB

After

Width:  |  Height:  |  Size: 14 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 1.2 KiB

After

Width:  |  Height:  |  Size: 2.5 KiB

View File

@ -50,8 +50,8 @@ def optimize(state,u_bar,track,ref_vel=1.):
:returns: :returns:
''' '''
MAX_SPEED = 1.25 MAX_SPEED = ref_vel*1.5
MAX_STEER = 1.57/2 MAX_STEER = np.pi/4
MAX_ACC = 1.0 MAX_ACC = 1.0
# compute polynomial coefficients of the track # compute polynomial coefficients of the track
@ -77,9 +77,9 @@ def optimize(state,u_bar,track,ref_vel=1.):
for t in range(P.T): for t in range(P.T):
cost += 30*cp.sum_squares(x[3,t]-np.arctan(df(x_bar[0,t],K))) # psi cost += 20*cp.sum_squares(x[3,t]-np.clip(np.arctan(df(x_bar[0,t],K)),-np.pi,np.pi) ) # psi
cost += 20*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte cost += 40*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte
cost += 10*cp.sum_squares(ref_vel-x[2,t]) # desired v cost += 20*cp.sum_squares(ref_vel-x[2,t]) # desired v
# Actuation rate of change # Actuation rate of change
if t < (P.T - 1): if t < (P.T - 1):
@ -101,10 +101,14 @@ def optimize(state,u_bar,track,ref_vel=1.):
# Solve # Solve
prob = cp.Problem(cp.Minimize(cost), constr) prob = cp.Problem(cp.Minimize(cost), constr)
solution = prob.solve(solver=cp.OSQP, verbose=False) prob.solve(solver=cp.OSQP, verbose=False)
if "optimal" not in prob.status:
print("WARN: No optimal solution")
return u_bar
#retrieved optimized U and assign to u_bar to linearize in next step #retrieved optimized U and assign to u_bar to linearize in next step
u_bar=np.vstack((np.array(u.value[0, :]).flatten(), u_opt=np.vstack((np.array(u.value[0, :]).flatten(),
(np.array(u.value[1, :]).flatten()))) (np.array(u.value[1, :]).flatten())))
return u_bar return u_opt

View File

@ -34,8 +34,8 @@ class MPC():
# Interpolated Path to follow given waypoints # 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,10,12,2,4,14],[0,0,2,10,12,12])
self.path = compute_path_from_wp([0,3,4,6,10,12,14,6,1,0], self.path = compute_path_from_wp([0,3,4,6,10,12,13,13,6,1,0],
[0,0,2,4,3,3,-2,-6,-2,-2],1) [0,0,2,4,3,3,-1,-2,-6,-2,-2],0.5)
# Sim help vars # Sim help vars
self.sim_time=0 self.sim_time=0
@ -174,7 +174,7 @@ class MPC():
plt.subplot(grid[1, 2]) plt.subplot(grid[1, 2])
#plt.title("Angular Velocity {} m/s".format(self.w_history[-1])) #plt.title("Angular Velocity {} m/s".format(self.w_history[-1]))
plt.plot(np.degrees(self.d_history),c='tab:orange') plt.plot(np.degrees(self.d_history),c='tab:orange')
plt.ylabel('w(t) [deg]') plt.ylabel('gamma(t) [deg]')
locs, _ = plt.xticks() locs, _ = plt.xticks()
plt.xticks(locs[1:], locs[1:]*P.dt) plt.xticks(locs[1:], locs[1:]*P.dt)
plt.xlabel('t [s]') plt.xlabel('t [s]')
@ -186,8 +186,8 @@ class MPC():
def plot_car(x, y, yaw): def plot_car(x, y, yaw):
LENGTH = 0.3 # [m] LENGTH = 0.35 # [m]
WIDTH = 0.1 # [m] WIDTH = 0.2 # [m]
OFFSET = LENGTH # [m] OFFSET = LENGTH # [m]
outline = np.array([[-OFFSET, (LENGTH - OFFSET), (LENGTH - OFFSET), -OFFSET, -OFFSET], outline = np.array([[-OFFSET, (LENGTH - OFFSET), (LENGTH - OFFSET), -OFFSET, -OFFSET],

View File

@ -20,21 +20,26 @@ def get_state(robotId):
robPos, robOrn = p.getBasePositionAndOrientation(robotId) robPos, robOrn = p.getBasePositionAndOrientation(robotId)
linVel,angVel = p.getBaseVelocity(robotId) linVel,angVel = p.getBaseVelocity(robotId)
return[robPos[0], robPos[1], p.getEulerFromQuaternion(robOrn)[2]] return[robPos[0], robPos[1], linVel[0], p.getEulerFromQuaternion(robOrn)[2]]
def set_ctrl(robotId,v,w): def set_ctrl(robotId,currVel,acceleration,steeringAngle):
"""
"""
L= 0.354
R= 0.076/2
rightWheelVelocity= (2*v+w*L)/(2*R) gearRatio=1./21
leftWheelVelocity = (2*v-w*L)/(2*R) steering = [0,2]
wheels = [8,15]
maxForce = 50
p.setJointMotorControl2(robotId,0,p.VELOCITY_CONTROL,targetVelocity=leftWheelVelocity,force=1000) targetVelocity = currVel + acceleration*P.dt
p.setJointMotorControl2(robotId,1,p.VELOCITY_CONTROL,targetVelocity=rightWheelVelocity,force=1000) #targetVelocity=lastVel
#print(targetVelocity)
def plot(path,x_history,y_history): 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.style.use("ggplot")
@ -52,24 +57,54 @@ def run_sim():
""" """
p.connect(p.GUI) p.connect(p.GUI)
start_offset = [0,2,0] p.resetSimulation()
start_orientation = p.getQuaternionFromEuler([0,0,0])
turtle = p.loadURDF("turtlebot.urdf",start_offset, start_orientation)
plane = p.loadURDF("plane.urdf")
p.setRealTimeSimulation(1)
p.setGravity(0,0,-10) p.setGravity(0,0,-10)
useRealTimeSim = 1
# MPC time step p.setTimeStep(1./120.)
P.dt = 0.25 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])
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)
opt_u = np.zeros((P.M,P.T)) opt_u = np.zeros((P.M,P.T))
opt_u[0,:] = 1 #m/s opt_u[0,:] = 1 #m/ss
opt_u[1,:] = np.radians(0) #rad/s opt_u[1,:] = np.radians(0) #rad/
# Interpolated Path to follow given waypoints # Interpolated Path to follow given waypoints
path = compute_path_from_wp([0,3,4,6,10,13], path = compute_path_from_wp([0,3,4,6,10,11,12,6,1,0],
[0,0,2,4,3,3],1) [0,0,2,4,3,3,-1,-6,-2,-2],0.5)
for x_,y_ in zip(path[0,:],path[1,:]): for x_,y_ in zip(path[0,:],path[1,:]):
p.addUserDebugLine([x_,y_,0],[x_,y_,0.33],[0,0,1]) p.addUserDebugLine([x_,y_,0],[x_,y_,0.33],[0,0,1])
@ -77,9 +112,10 @@ def run_sim():
x_history=[] x_history=[]
y_history=[] y_history=[]
time.sleep(0.5)
while (1): while (1):
state = get_state(turtle) state = get_state(car)
x_history.append(state[0]) x_history.append(state[0])
y_history.append(state[1]) y_history.append(state[1])
@ -88,18 +124,18 @@ def run_sim():
if np.sqrt((state[0]-path[0,-1])**2+(state[1]-path[1,-1])**2)<1: if np.sqrt((state[0]-path[0,-1])**2+(state[1]-path[1,-1])**2)<1:
print("Success! Goal Reached") print("Success! Goal Reached")
set_ctrl(turtle,0,0) set_ctrl(car,0,0,0)
plot(path,x_history,y_history) plot_results(path,x_history,y_history)
p.disconnect() p.disconnect()
return return
#optimization loop #optimization loop
start=time.time() start=time.time()
opt_u = optimize(state,opt_u,path) opt_u = optimize(state,opt_u,path,ref_vel=1.0)
elapsed=time.time()-start elapsed=time.time()-start
print("CVXPY Optimization Time: {:.4f}s".format(elapsed)) print("CVXPY Optimization Time: {:.4f}s".format(elapsed))
set_ctrl(turtle,opt_u[0,1],opt_u[1,1]) set_ctrl(car,state[2],opt_u[0,1],opt_u[1,1])
if P.dt-elapsed>0: if P.dt-elapsed>0:
time.sleep(P.dt-elapsed) time.sleep(P.dt-elapsed)

View File

@ -9,9 +9,8 @@ def compute_path_from_wp(start_xp, start_yp, step = 0.1):
delta = step #[m] delta = step #[m]
for idx in range(len(start_xp)-1): for idx in range(len(start_xp)-1):
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))) 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))
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) 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) fy=interp1d(np.linspace(0,1,2),start_yp[idx:idx+2],kind=1)
@ -54,9 +53,9 @@ def road_curve(state,track):
POLY_RANK = 3 POLY_RANK = 3
#given vehicle pos find lookahead waypoints #given vehicle pos find lookahead waypoints
nn_idx=get_nn_idx(state,track)-1 nn_idx=get_nn_idx(state,track)
LOOKAHED = POLY_RANK + 1 LOOKAHED = POLY_RANK*2
lk_wp=track[:,nn_idx:nn_idx+LOOKAHED] lk_wp=track[:,max(0,nn_idx-1):nn_idx+LOOKAHED]
#trasform lookahead waypoints to vehicle ref frame #trasform lookahead waypoints to vehicle ref frame
dx = lk_wp[0,:] - state[0] dx = lk_wp[0,:] - state[0]
@ -68,17 +67,32 @@ def road_curve(state,track):
#fit poly #fit poly
return np.polyfit(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:], POLY_RANK, rcond=None, full=False, w=None, cov=False) return np.polyfit(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:], POLY_RANK, rcond=None, full=False, w=None, cov=False)
def f(x,coeff): # def f(x,coeff):
""" # """
""" # """
return round(coeff[0]*x**3 + coeff[1]*x**2 + coeff[2]*x**1 + coeff[3]*x**0,6) # return round(coeff[0]*x**3 + coeff[1]*x**2 + coeff[2]*x**1 + coeff[3]*x**0,6)
# def f(x,coeff): # 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) # 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)
def df(x,coeff): def f(x,coeff):
""" y=0
""" j=len(coeff)
return round(3*coeff[0]*x**2 + 2*coeff[1]*x**1 + coeff[2]*x**0,6) 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): # 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) # 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)

File diff suppressed because one or more lines are too long