f10 pybullet demo

README.md

@@ -4,30 +4,35 @@ Python implementation of a mpc controller for path tracking using **[CVXPY](http
 
 ## 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)
 
-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)
 
-The state variables of the model are:
+The state variables **(x)** of the model are:
 
 * **x** coordinate of the robot
 * **y** coordinate of the robot
+* **v** velocuty 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
-* **w** angular velocity of the robot
+* **a** linear acceleration of the robot
+* **delta** steering angle of the robot
 
 ## 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:
 

mpc_demo/cvxpy_mpc.py

@@ -50,8 +50,8 @@ def optimize(state,u_bar,track,ref_vel=1.):
     :returns:
     '''
 
-    MAX_SPEED = 1.25
-    MAX_STEER = 1.57/2
+    MAX_SPEED = ref_vel*1.5
+    MAX_STEER = np.pi/4
     MAX_ACC = 1.0
 
     # 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):
 
-        cost += 30*cp.sum_squares(x[3,t]-np.arctan(df(x_bar[0,t],K))) # psi
-        cost += 20*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(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
 
         # Actuation rate of change
         if t < (P.T - 1):
@@ -101,10 +101,14 @@ def optimize(state,u_bar,track,ref_vel=1.):
 
     # Solve
     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
-    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())))
 
-    return u_bar
+    return u_opt

mpc_demo/mpc_demo_nosim.py

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

mpc_demo/mpc_demo_pybullet.py

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

mpc_demo/utils.py

@@ -9,9 +9,8 @@ def compute_path_from_wp(start_xp, start_yp, step = 0.1):
     delta = step #[m]
 
     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)))
-
-        interp_range = np.linspace(0,1,np.floor(section_len/delta).astype(int))
+        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))
 
         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)
@@ -54,9 +53,9 @@ def road_curve(state,track):
     POLY_RANK = 3
 
     #given vehicle pos find lookahead waypoints
-    nn_idx=get_nn_idx(state,track)-1
-    LOOKAHED = POLY_RANK + 1
-    lk_wp=track[:,nn_idx:nn_idx+LOOKAHED]
+    nn_idx=get_nn_idx(state,track)
+    LOOKAHED = POLY_RANK*2
+    lk_wp=track[:,max(0,nn_idx-1):nn_idx+LOOKAHED]
 
     #trasform lookahead waypoints to vehicle ref frame
     dx = lk_wp[0,:] - state[0]
@@ -68,17 +67,32 @@ def road_curve(state,track):
     #fit poly
     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):
-    """
-    """
-    return round(coeff[0]*x**3 + coeff[1]*x**2 + coeff[2]*x**1 + coeff[3]*x**0,6)
+# def f(x,coeff):
+#     """
+#     """
+#     return round(coeff[0]*x**3 + coeff[1]*x**2 + coeff[2]*x**1 + coeff[3]*x**0,6)
 
 # 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)
 
-def df(x,coeff):
-    """
-    """
-    return round(3*coeff[0]*x**2 + 2*coeff[1]*x**1 + coeff[2]*x**0,6)
+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)