├── README.md
├── dynamic
├── __pycache__
│ └── draw.cpython-38.pyc
├── draw.py
├── sim2_mpc_mul_shooting_opt.py
├── sim3_mpc_obs_avoid_mul_shooting_opt.py
├── sim4_mpc_tracking_mul_shooting_opt.py
└── sim5_mpc_tracking_obs_avoid_mul_shooting_opt.py
├── kinematic
├── __pycache__
│ └── draw.cpython-38.pyc
├── draw.py
├── sim2_mpc_mul_shooting_opt.py
├── sim3_mpc_obs_avoid_mul_shooting_opt.py
├── sim4_mpc_tracking_mul_shooting_opt.py
└── sim5_mpc_tracking_obs_avoid_mul_shooting_opt.py
├── lnmpc_smc.py
└── results
├── obstacle.gif
├── sim2.png
├── sim3.png
├── sim4.png
├── sim5.png
└── tracking_obs_avoid.gif
/README.md:
--------------------------------------------------------------------------------
1 | # Nonlinear MPC using CasADi and Python
2 |
3 | Applying Nonlinear MPC for Mobile Robot in simulation using Python3
4 |
5 | ## Robot model
6 | Model: Three Omni-wheeled directional Mobile robot
7 | State: $ [x, y, \theta]^T $
8 | Control: $ [v_x, v_y, \omega]^T $
9 | Constraints:
10 | $$ -1.0 \leq v_x, v_y \leq 1.0 $$
11 | $$ -\frac{\pi}{3} \leq \omega \leq \frac{\pi}{3} $$
12 |
13 | ## Tasks:
14 | * Sim 2 - Moving to target position
15 | * Sim 3 - Moving to target position and avoid static obstacles
16 | * Sim 4 - Tracking the desired path
17 | * Sim 5 - Tracking the desired path and avoid static obstacles
18 |
19 | ## Results
20 | ### Sim 2
21 | 
22 | ### Sim 3
23 | 
24 | 
25 | ### Sim 4
26 | 
27 | ### Sim 5
28 | 
29 | 
--------------------------------------------------------------------------------
/dynamic/__pycache__/draw.cpython-38.pyc:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/duynamrcv/nmpc_casadi_python/8255b724008e3fd1f223dce398c5c6ab2cbaef60/dynamic/__pycache__/draw.cpython-38.pyc
--------------------------------------------------------------------------------
/dynamic/draw.py:
--------------------------------------------------------------------------------
1 | #!/usr/bin/env python
2 | # coding=utf-8
3 |
4 | import numpy as np
5 | from matplotlib import pyplot as plt
6 | import matplotlib.animation as animation
7 | import matplotlib.patches as mpatches
8 |
9 |
10 | class Draw_MPC_point_stabilization_v1(object):
11 | def __init__(self, robot_states: np.array, init_state: np.array, target_state: np.array, rob_diam=0.3,
12 | export_fig=False):
13 | self.robot_states = robot_states
14 | self.init_state = init_state
15 | self.target_state = target_state
16 | self.rob_radius = rob_diam / 2.0
17 | self.fig = plt.figure()
18 | self.ax = plt.axes(xlim=(-0.8, 5), ylim=(-0.8, 5.))
19 | # self.fig.set_dpi(400)
20 | self.fig.set_size_inches(7, 6.5)
21 | # init for plot
22 | self.animation_init()
23 |
24 | self.ani = animation.FuncAnimation(self.fig, self.animation_loop, range(len(self.robot_states)),
25 | init_func=self.animation_init, interval=100, repeat=False)
26 |
27 | plt.grid('--')
28 | if export_fig:
29 | self.ani.save('./v1.gif', writer='imagemagick', fps=100)
30 | plt.show()
31 |
32 | def animation_init(self):
33 | # plot target state
34 | self.target_circle = plt.Circle(self.target_state[:2], self.rob_radius, color='b', fill=False)
35 | self.ax.add_artist(self.target_circle)
36 | self.target_arr = mpatches.Arrow(self.target_state[0], self.target_state[1],
37 | self.rob_radius * np.cos(self.target_state[2]),
38 | self.rob_radius * np.sin(self.target_state[2]), width=0.2)
39 | self.ax.add_patch(self.target_arr)
40 | self.robot_body = plt.Circle(self.init_state[:2], self.rob_radius, color='r', fill=False)
41 | self.ax.add_artist(self.robot_body)
42 | self.robot_arr = mpatches.Arrow(self.init_state[0], self.init_state[1],
43 | self.rob_radius * np.cos(self.init_state[2]),
44 | self.rob_radius * np.sin(self.init_state[2]), width=0.2, color='r')
45 | self.ax.add_patch(self.robot_arr)
46 | return self.target_circle, self.target_arr, self.robot_body, self.robot_arr
47 |
48 | def animation_loop(self, indx):
49 | position = self.robot_states[indx][:2]
50 | orientation = self.robot_states[indx][2]
51 | self.robot_body.center = position
52 | # self.ax.add_artist(self.robot_body)
53 | self.robot_arr.remove()
54 | self.robot_arr = mpatches.Arrow(position[0], position[1], self.rob_radius * np.cos(orientation),
55 | self.rob_radius * np.sin(orientation), width=0.2, color='r')
56 | self.ax.add_patch(self.robot_arr)
57 | self.ax.plot(self.robot_states[:indx, 0], self.robot_states[:indx, 1], color='r', linewidth=1.5)
58 | return self.robot_arr, self.robot_body
59 |
60 |
61 | class Draw_MPC_Obstacle(object):
62 | def __init__(self, robot_states: np.array, init_state: np.array, target_state: np.array, obstacle: list,
63 | rob_diam=0.3, export_fig=False):
64 | self.robot_states = robot_states
65 | self.init_state = init_state
66 | self.target_state = target_state
67 | self.rob_radius = rob_diam / 2.0
68 | self.fig = plt.figure()
69 | self.ax = plt.axes(xlim=(-0.8, 5), ylim=(-0.8, 5.))
70 | if obstacle is not None:
71 | self.obstacle = obstacle
72 | else:
73 | print('no obstacle given, break')
74 | self.fig.set_size_inches(7, 6.5)
75 | # init for plot
76 | self.animation_init()
77 |
78 | self.ani = animation.FuncAnimation(self.fig, self.animation_loop, range(len(self.robot_states)),
79 | init_func=self.animation_init, interval=100, repeat=False)
80 |
81 | plt.grid('--')
82 | if export_fig:
83 | self.ani.save('obstacle.gif', writer='imagemagick', fps=100)
84 | plt.show()
85 |
86 | def animation_init(self):
87 | # plot target state
88 | self.target_circle = plt.Circle(self.target_state[:2], self.rob_radius, color='b', fill=False)
89 | self.ax.add_artist(self.target_circle)
90 | self.target_arr = mpatches.Arrow(self.target_state[0], self.target_state[1],
91 | self.rob_radius * np.cos(self.target_state[2]),
92 | self.rob_radius * np.sin(self.target_state[2]), width=0.2)
93 | self.ax.add_patch(self.target_arr)
94 | self.robot_body = plt.Circle(self.init_state[:2], self.rob_radius, color='r', fill=False)
95 | self.ax.add_artist(self.robot_body)
96 | self.robot_arr = mpatches.Arrow(self.init_state[0], self.init_state[1],
97 | self.rob_radius * np.cos(self.init_state[2]),
98 | self.rob_radius * np.sin(self.init_state[2]), width=0.2, color='r')
99 | self.ax.add_patch(self.robot_arr)
100 | self.obstacle_circle = []
101 | for i in range(len(self.obstacle[0])):
102 | center = (self.obstacle[0][i], self.obstacle[1][i])
103 | obs = plt.Circle(center, self.obstacle[2], color='g', fill=True)
104 | self.obstacle_circle.append(obs)
105 | self.ax.add_artist(obs)
106 |
107 | return self.target_circle, self.target_arr, self.robot_body, self.robot_arr, self.obstacle_circle
108 |
109 | def animation_loop(self, indx):
110 | position = self.robot_states[indx][:2]
111 | orientation = self.robot_states[indx][2]
112 | self.robot_body.center = position
113 | self.robot_arr.remove()
114 | self.robot_arr = mpatches.Arrow(position[0], position[1], self.rob_radius * np.cos(orientation),
115 | self.rob_radius * np.sin(orientation), width=0.2, color='r')
116 | self.ax.plot(self.robot_states[:indx, 0], self.robot_states[:indx, 1], color='r', linewidth=1.5)
117 |
118 | self.ax.add_patch(self.robot_arr)
119 | return self.robot_arr, self.robot_body
120 |
121 |
122 | class Draw_MPC_tracking(object):
123 | def __init__(self, robot_states: np.array, init_state: np.array, rob_diam=0.3, export_fig=False):
124 | self.init_state = init_state
125 | self.robot_states = robot_states
126 | self.rob_radius = rob_diam
127 | self.fig = plt.figure()
128 | self.ax = plt.axes(xlim=(-6.0, 6.0), ylim=(-6.0, 6.0))
129 | # self.fig.set_size_inches(7, 6.5)
130 | # init for plot
131 | self.animation_init()
132 |
133 | self.ani = animation.FuncAnimation(self.fig, self.animation_loop, range(len(self.robot_states)),
134 | init_func=self.animation_init, interval=100, repeat=False)
135 |
136 | plt.grid('--')
137 | if export_fig:
138 | self.ani.save('tracking.gif', writer='imagemagick', fps=100)
139 | plt.show()
140 |
141 | def animation_init(self, ):
142 | # draw target line
143 | step = np.arange(0,12.1,np.pi/50)
144 | x_ref = 4*np.cos(step)
145 | y_ref = 4*np.sin(step)
146 | self.target_line = plt.plot(x_ref, y_ref, '-b')
147 | # draw the initial position of the robot
148 | self.init_robot_position = plt.Circle(self.init_state[:2], self.rob_radius, color='r', fill=False)
149 | self.ax.add_artist(self.init_robot_position)
150 | self.robot_body = plt.Circle(self.init_state[:2], self.rob_radius, color='r', fill=False)
151 | self.ax.add_artist(self.robot_body)
152 | self.robot_arr = mpatches.Arrow(self.init_state[0], self.init_state[1],
153 | self.rob_radius * np.cos(self.init_state[2]),
154 | self.rob_radius * np.sin(self.init_state[2]), width=0.2, color='r')
155 | self.ax.add_patch(self.robot_arr)
156 | return self.target_line, self.init_robot_position, self.robot_body, self.robot_arr
157 |
158 | def animation_loop(self, indx):
159 | position = self.robot_states[indx,:2]
160 | orientation = self.robot_states[indx,2]
161 | self.robot_body.center = position
162 | self.robot_arr.remove()
163 | self.robot_arr = mpatches.Arrow(position[0], position[1], self.rob_radius * np.cos(orientation),
164 | self.rob_radius * np.sin(orientation), width=0.2, color='r')
165 | self.ax.add_patch(self.robot_arr)
166 | self.ax.plot(self.robot_states[:indx, 0], self.robot_states[:indx, 1], color='r', linewidth=1.5)
167 | return self.robot_arr, self.robot_body
168 |
169 | class Draw_MPC_tracking_Obstacle(object):
170 | def __init__(self, robot_states: np.array, init_state: np.array, obstacle: list, rob_diam=0.3, export_fig=False):
171 | self.init_state = init_state
172 | self.robot_states = robot_states
173 | self.rob_radius = rob_diam
174 | self.fig = plt.figure()
175 | self.ax = plt.axes(xlim=(-6.0, 6.0), ylim=(-6.0, 6.0))
176 | if obstacle is not None:
177 | self.obstacle = obstacle
178 | else:
179 | print('no obstacle given, break')
180 |
181 | self.animation_init()
182 |
183 | self.ani = animation.FuncAnimation(self.fig, self.animation_loop, range(len(self.robot_states)),
184 | init_func=self.animation_init, interval=100, repeat=False)
185 |
186 | plt.grid('--')
187 | if export_fig:
188 | self.ani.save('tracking_obs_avoid.gif', writer='imagemagick', fps=100)
189 | plt.show()
190 |
191 | def animation_init(self, ):
192 | # draw target line
193 | step = np.arange(0,12.1,np.pi/50)
194 | x_ref = 4*np.cos(step)
195 | y_ref = 4*np.sin(step)
196 | self.target_line = plt.plot(x_ref, y_ref, '-b')
197 | # draw the initial position of the robot
198 | self.init_robot_position = plt.Circle(self.init_state[:2], self.rob_radius, color='r', fill=False)
199 | self.ax.add_artist(self.init_robot_position)
200 | self.robot_body = plt.Circle(self.init_state[:2], self.rob_radius, color='r', fill=False)
201 | self.ax.add_artist(self.robot_body)
202 | self.robot_arr = mpatches.Arrow(self.init_state[0], self.init_state[1],
203 | self.rob_radius * np.cos(self.init_state[2]),
204 | self.rob_radius * np.sin(self.init_state[2]), width=0.2, color='r')
205 | self.ax.add_patch(self.robot_arr)
206 | self.obstacle_circle = []
207 | for i in range(len(self.obstacle[0])):
208 | center = (self.obstacle[0][i], self.obstacle[1][i])
209 | obs = plt.Circle(center, self.obstacle[2], color='g', fill=True)
210 | self.obstacle_circle.append(obs)
211 | self.ax.add_artist(obs)
212 | return self.target_line, self.init_robot_position, self.robot_body, self.robot_arr, self.obstacle_circle
213 |
214 | def animation_loop(self, indx):
215 | position = self.robot_states[indx,:2]
216 | orientation = self.robot_states[indx,2]
217 | self.robot_body.center = position
218 | self.robot_arr.remove()
219 | self.robot_arr = mpatches.Arrow(position[0], position[1], self.rob_radius * np.cos(orientation),
220 | self.rob_radius * np.sin(orientation), width=0.2, color='r')
221 | self.ax.add_patch(self.robot_arr)
222 | self.ax.plot(self.robot_states[:indx, 0], self.robot_states[:indx, 1], color='r', linewidth=1.5)
223 | return self.robot_arr, self.robot_body
--------------------------------------------------------------------------------
/dynamic/sim2_mpc_mul_shooting_opt.py:
--------------------------------------------------------------------------------
1 | import casadi as ca
2 | import numpy as np
3 | import time
4 |
5 | from draw import Draw_MPC_point_stabilization_v1
6 |
7 | def shift(T, t0, x0, u, x_n, f):
8 | f_value = f(x0, u[0])
9 | st = x0 + T*f_value
10 | t = t0 + T
11 | u_end = np.concatenate((u[1:], u[-1:]))
12 | x_n = np.concatenate((x_n[1:], x_n[-1:]))
13 | return t, st, u_end, x_n
14 |
15 | def predict_state(x0, u, T, N):
16 | # Parameter
17 | d = 0.2
18 | M = 10; J = 2
19 | Bx = 0.05; By = 0.05; Bw = 0.06
20 | # Cx = 0.5; Cy = 0.5; Cw = 0.6
21 | # define prediction horizon function
22 | states = np.zeros((N+1, 6))
23 | states[0,:] = x0
24 | # euler method
25 | for i in range(N):
26 | states[i+1, 0] = states[i, 0] + (states[i, 3]*np.cos(states[i, 2]) - states[i, 4]*np.sin(states[i, 2]))*T
27 | states[i+1, 1] = states[i, 1] + (states[i, 3]*np.sin(states[i, 2]) + states[i, 4]*np.cos(states[i, 2]))*T
28 | states[i+1, 2] = states[i, 2] + states[i, 5]*T
29 | states[i+1, 3] = (0*u[i, 0] + np.sqrt(3)/2*u[i, 1] - np.sqrt(3)/2*u[i, 2] - Bx*states[3])/M
30 | states[i+1, 4] = (-1*u[i, 0] + 1/2*u[i, 1] + 1/2*u[i, 2] - By*states[4])/M
31 | states[i+1, 5] = (d*u[i, 0] + d*u[i, 1] + d*u[i, 2] - Bw*states[5])/J
32 | return states
33 |
34 | if __name__ == "__main__":
35 | # Parameter
36 | d = 0.2
37 | M = 10; J = 2
38 | Bx = 0.05; By = 0.05; Bw = 0.06
39 | # Cx = 0.5; Cy = 0.5; Cw = 0.6
40 |
41 | T = 0.2 # time step
42 | N = 30 # horizon length
43 | v_max = 1.0 # linear velocity max
44 | omega_max = np.pi/3.0 # angular velocity max
45 | u_max = 5 # force max of each direction
46 |
47 | opti = ca.Opti()
48 | # control variables, toruqe of each wheel
49 | opt_controls = opti.variable(N, 3)
50 | u1 = opt_controls[:, 0]
51 | u2 = opt_controls[:, 1]
52 | u3 = opt_controls[:, 2]
53 |
54 | # state variable: position and velocity
55 | opt_states = opti.variable(N+1, 6)
56 | x = opt_states[:, 0]
57 | y = opt_states[:, 1]
58 | theta = opt_states[:, 2]
59 | vx = opt_states[:, 3]
60 | vy = opt_states[:, 4]
61 | omega = opt_states[:, 5]
62 |
63 | # parameters
64 | opt_x0 = opti.parameter(6)
65 | opt_xs = opti.parameter(6)
66 |
67 | # create model
68 | f = lambda x_, u_: ca.vertcat(*[x_[3]*ca.cos(x_[2]) - x_[4]*ca.sin(x_[2]),
69 | x_[3]*ca.sin(x_[2]) + x_[4]*ca.cos(x_[2]),
70 | x_[5],
71 | (0*u_[0] + ca.sqrt(3)/2*u_[1] - ca.sqrt(3)/2*u_[2] - Bx*x_[3])/M,
72 | (-1*u_[0] + 1/2*u_[1] + 1/2*u_[2] - By*x_[4])/M,
73 | (d*u_[0] + d*u_[1] + d*u_[2] - Bw*x_[5])/J])
74 | f_np = lambda x_, u_: np.array([x_[3]*ca.cos(x_[2]) - x_[4]*ca.sin(x_[2]),
75 | x_[3]*ca.sin(x_[2]) + x_[4]*ca.cos(x_[2]),
76 | x_[5],
77 | (0*u_[0] + ca.sqrt(3)/2*u_[1] - ca.sqrt(3)/2*u_[2] - Bx*x_[3])/M,
78 | (-1*u_[0] + 1/2*u_[1] + 1/2*u_[2] - By*x_[4])/M,
79 | (d*u_[0] + d*u_[1] + d*u_[2] - Bw*x_[5])/J])
80 |
81 | # initial condition
82 | opti.subject_to(opt_states[0, :] == opt_x0.T)
83 | for i in range(N):
84 | x_next = opt_states[i, :] + f(opt_states[i, :], opt_controls[i, :]).T*T
85 | opti.subject_to(opt_states[i+1, :] == x_next)
86 |
87 | # weight matrix
88 | Q = np.diag([5.0, 5.0, 5.0, 1.0, 1.0, 1.0])
89 | R = np.diag([0.01, 0.01, 0.01])
90 |
91 | # cost function
92 | obj = 0
93 | for i in range(N):
94 | obj = obj + ca.mtimes([(opt_states[i, :] - opt_xs.T), Q, (opt_states[i, :] - opt_xs.T).T]) \
95 | + ca.mtimes([opt_controls[i, :], R, opt_controls[i, :].T])
96 | opti.minimize(obj)
97 |
98 | # boundary and control conditions
99 | opti.subject_to(opti.bounded(-10.0, x, 10.0))
100 | opti.subject_to(opti.bounded(-10.0, y, 10.0))
101 | opti.subject_to(opti.bounded(-v_max, vx, v_max))
102 | opti.subject_to(opti.bounded(-v_max, vy, v_max))
103 | opti.subject_to(opti.bounded(-omega_max, omega, omega_max))
104 | opti.subject_to(opti.bounded(-u_max, u1, u_max))
105 | opti.subject_to(opti.bounded(-u_max, u2, u_max))
106 | opti.subject_to(opti.bounded(-u_max, u3, u_max))
107 |
108 | opts_setting = {'ipopt.max_iter': 200,
109 | 'ipopt.print_level': 0,
110 | 'print_time': 0,
111 | 'ipopt.acceptable_tol': 1e-8,
112 | 'ipopt.acceptable_obj_change_tol': 1e-6}
113 | opti.solver('ipopt', opts_setting)
114 |
115 | # The final state
116 | final_state = np.array([4.5, 4.5, 0.0, 0.0, 0.0, 0.0])
117 | opti.set_value(opt_xs, final_state)
118 |
119 | # The initial state
120 | t0 = 0
121 | init_state = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
122 | u0 = np.zeros((N, 3))
123 | current_state = init_state.copy()
124 | next_states = np.zeros((N+1, 6))
125 | x_c = [] # contains for the history of the state
126 | u_c = []
127 | t_c = [t0] # for the time
128 | xx = []
129 | sim_time = 100.0
130 |
131 | ## start MPC
132 | mpciter = 0
133 | start_time = time.time()
134 | index_t = []
135 | while(np.linalg.norm(current_state - final_state) > 1e-2 and mpciter - sim_time/T < 0.0 ):
136 | # set parameter, here only update initial state of x (x0)
137 | opti.set_value(opt_x0, current_state)
138 | # print(mpciter)
139 |
140 | # set optimizing target withe init guess
141 | opti.set_initial(opt_controls, u0)# (N, 3)
142 | opti.set_initial(opt_states, next_states) # (N+1, 6)
143 |
144 | # solve the problem once again
145 | t_ = time.time()
146 | sol = opti.solve()
147 | index_t.append(time.time()- t_)
148 | # opti.set_initial(opti.lam_g, sol.value(opti.lam_g))
149 |
150 | # obtain the control input
151 | u_res = sol.value(opt_controls)
152 | u_c.append(u_res[0, :])
153 | t_c.append(t0)
154 | next_states_pred = sol.value(opt_states)# prediction_state(x0=current_state, u=u_res, N=N, T=T)
155 |
156 | # next_states_pred = prediction_state(x0=current_state, u=u_res, N=N, T=T)
157 | x_c.append(next_states_pred)
158 |
159 | # for next loop
160 | t0, current_state, u0, next_states = shift(T, t0, current_state, u_res, next_states_pred, f_np)
161 | mpciter = mpciter + 1
162 | xx.append(current_state)
163 |
164 | t_v = np.array(index_t)
165 | print(t_v.mean())
166 | print((time.time() - start_time)/(mpciter))
167 |
168 | # after loop
169 | print(mpciter)
170 | print('final error {}'.format(np.linalg.norm(final_state-current_state)))
171 |
172 | # draw function
173 | draw_result = Draw_MPC_point_stabilization_v1(rob_diam=0.3,
174 | init_state=init_state,
175 | target_state=final_state,
176 | robot_states=np.array(xx))
177 |
--------------------------------------------------------------------------------
/dynamic/sim3_mpc_obs_avoid_mul_shooting_opt.py:
--------------------------------------------------------------------------------
1 | import casadi as ca
2 | import numpy as np
3 | import time
4 |
5 | from draw import Draw_MPC_Obstacle
6 |
7 | def shift(T, t0, x0, u, x_n, f):
8 | f_value = f(x0, u[0])
9 | st = x0 + T*f_value
10 | t = t0 + T
11 | u_end = np.concatenate((u[1:], u[-1:]))
12 | x_n = np.concatenate((x_n[1:], x_n[-1:]))
13 | return t, st, u_end, x_n
14 |
15 | def predict_state(x0, u, T, N):
16 | # Parameter
17 | d = 0.2
18 | M = 10; J = 2
19 | Bx = 0.05; By = 0.05; Bw = 0.06
20 | # Cx = 0.5; Cy = 0.5; Cw = 0.6
21 | # define prediction horizon function
22 | states = np.zeros((N+1, 6))
23 | states[0,:] = x0
24 | # euler method
25 | for i in range(N):
26 | states[i+1, 0] = states[i, 0] + (states[i, 3]*np.cos(states[i, 2]) - states[i, 4]*np.sin(states[i, 2]))*T
27 | states[i+1, 1] = states[i, 1] + (states[i, 3]*np.sin(states[i, 2]) + states[i, 4]*np.cos(states[i, 2]))*T
28 | states[i+1, 2] = states[i, 2] + states[i, 5]*T
29 | states[i+1, 3] = (0*u[i, 0] + np.sqrt(3)/2*u[i, 1] - np.sqrt(3)/2*u[i, 2] - Bx*states[3])/M
30 | states[i+1, 4] = (-1*u[i, 0] + 1/2*u[i, 1] + 1/2*u[i, 2] - By*states[4])/M
31 | states[i+1, 5] = (d*u[i, 0] + d*u[i, 1] + d*u[i, 2] - Bw*states[5])/J
32 | return states
33 |
34 | if __name__ == "__main__":
35 | # Parameter
36 | d = 0.2
37 | M = 10; J = 2
38 | Bx = 0.05; By = 0.05; Bw = 0.06
39 | # Cx = 0.5; Cy = 0.5; Cw = 0.6
40 |
41 | T = 0.2 # time step
42 | N = 30 # horizon length
43 | rob_diam = 0.3 # [m]
44 | v_max = 1.0 # linear velocity max
45 | omega_max = np.pi/3.0 # angular velocity max
46 | u_max = 5 # force max of each direction
47 |
48 | opti = ca.Opti()
49 | # control variables, toruqe of each wheel
50 | opt_controls = opti.variable(N, 3)
51 | u1 = opt_controls[:, 0]
52 | u2 = opt_controls[:, 1]
53 | u3 = opt_controls[:, 2]
54 |
55 | # state variable: position and velocity
56 | opt_states = opti.variable(N+1, 6)
57 | x = opt_states[:, 0]
58 | y = opt_states[:, 1]
59 | theta = opt_states[:, 2]
60 | vx = opt_states[:, 3]
61 | vy = opt_states[:, 4]
62 | omega = opt_states[:, 5]
63 |
64 | # parameters
65 | opt_x0 = opti.parameter(6)
66 | opt_xs = opti.parameter(6)
67 |
68 | # create model
69 | f = lambda x_, u_: ca.vertcat(*[x_[3]*ca.cos(x_[2]) - x_[4]*ca.sin(x_[2]),
70 | x_[3]*ca.sin(x_[2]) + x_[4]*ca.cos(x_[2]),
71 | x_[5],
72 | (0*u_[0] + ca.sqrt(3)/2*u_[1] - ca.sqrt(3)/2*u_[2] - Bx*x_[3])/M,
73 | (-1*u_[0] + 1/2*u_[1] + 1/2*u_[2] - By*x_[4])/M,
74 | (d*u_[0] + d*u_[1] + d*u_[2] - Bw*x_[5])/J])
75 | f_np = lambda x_, u_: np.array([x_[3]*ca.cos(x_[2]) - x_[4]*ca.sin(x_[2]),
76 | x_[3]*ca.sin(x_[2]) + x_[4]*ca.cos(x_[2]),
77 | x_[5],
78 | (0*u_[0] + ca.sqrt(3)/2*u_[1] - ca.sqrt(3)/2*u_[2] - Bx*x_[3])/M,
79 | (-1*u_[0] + 1/2*u_[1] + 1/2*u_[2] - By*x_[4])/M,
80 | (d*u_[0] + d*u_[1] + d*u_[2] - Bw*x_[5])/J])
81 |
82 | # initial condition
83 | opti.subject_to(opt_states[0, :] == opt_x0.T)
84 | for i in range(N):
85 | x_next = opt_states[i, :] + f(opt_states[i, :], opt_controls[i, :]).T*T
86 | opti.subject_to(opt_states[i+1, :] == x_next)
87 |
88 | # obstacle
89 | obs_x = [1.5, 0.5, 3.0, 3.5, 4.5]
90 | obs_y = [1.0, 1.5, 2.7, 4.0, 4.0]
91 | obs_diam = 0.5
92 | bias = 0.02
93 |
94 | # add constraints to obstacle
95 | for i in range(N+1):
96 | for j in range(len(obs_x)):
97 | temp_constraints_ = ca.sqrt((opt_states[i, 0]-obs_x[j]-bias)**2+(opt_states[i, 1]-obs_y[j])**2-bias)-rob_diam/2.0-obs_diam/2.0
98 | opti.subject_to(opti.bounded(0.0, temp_constraints_, 10.0))
99 |
100 |
101 | # weight matrix
102 | Q = np.diag([5.0, 5.0, 5.0, 1.0, 1.0, 1.0])
103 | R = np.diag([0.01, 0.01, 0.01])
104 |
105 | # cost function
106 | obj = 0
107 | for i in range(N):
108 | obj = obj + ca.mtimes([(opt_states[i, :] - opt_xs.T), Q, (opt_states[i, :] - opt_xs.T).T]) \
109 | + ca.mtimes([opt_controls[i, :], R, opt_controls[i, :].T])
110 | opti.minimize(obj)
111 |
112 | # boundary and control conditions
113 | opti.subject_to(opti.bounded(-10.0, x, 10.0))
114 | opti.subject_to(opti.bounded(-10.0, y, 10.0))
115 | opti.subject_to(opti.bounded(-v_max, vx, v_max))
116 | opti.subject_to(opti.bounded(-v_max, vy, v_max))
117 | opti.subject_to(opti.bounded(-omega_max, omega, omega_max))
118 | opti.subject_to(opti.bounded(-u_max, u1, u_max))
119 | opti.subject_to(opti.bounded(-u_max, u2, u_max))
120 | opti.subject_to(opti.bounded(-u_max, u3, u_max))
121 |
122 | opts_setting = {'ipopt.max_iter': 100,
123 | 'ipopt.print_level': 0,
124 | 'print_time': 0,
125 | 'ipopt.acceptable_tol': 1e-8,
126 | 'ipopt.acceptable_obj_change_tol': 1e-6}
127 | opti.solver('ipopt', opts_setting)
128 |
129 | # The final state
130 | final_state = np.array([4.5, 4.5, 0.0, 0.0, 0.0, 0.0])
131 | opti.set_value(opt_xs, final_state)
132 |
133 | # The initial state
134 | t0 = 0
135 | init_state = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
136 | u0 = np.zeros((N, 3))
137 | current_state = init_state.copy()
138 | next_states = np.zeros((N+1, 6))
139 | x_c = [] # contains for the history of the state
140 | u_c = []
141 | t_c = [t0] # for the time
142 | xx = []
143 | sim_time = 30.0
144 |
145 | ## start MPC
146 | mpciter = 0
147 | start_time = time.time()
148 | index_t = []
149 | while(np.linalg.norm(current_state - final_state) > 1e-2 and mpciter - sim_time/T < 0.0 ):
150 | # set parameter, here only update initial state of x (x0)
151 | opti.set_value(opt_x0, current_state)
152 |
153 | # set optimizing target withe init guess
154 | opti.set_initial(opt_controls, u0)# (N, 3)
155 | opti.set_initial(opt_states, next_states) # (N+1, 6)
156 |
157 | # solve the problem once again
158 | t_ = time.time()
159 | sol = opti.solve()
160 | index_t.append(time.time()- t_)
161 | # opti.set_initial(opti.lam_g, sol.value(opti.lam_g))
162 |
163 | # obtain the control input
164 | u_res = sol.value(opt_controls)
165 | u_c.append(u_res[0, :])
166 | t_c.append(t0)
167 | next_states_pred = sol.value(opt_states)# prediction_state(x0=current_state, u=u_res, N=N, T=T)
168 |
169 | # next_states_pred = prediction_state(x0=current_state, u=u_res, N=N, T=T)
170 | x_c.append(next_states_pred)
171 |
172 | # for next loop
173 | t0, current_state, u0, next_states = shift(T, t0, current_state, u_res, next_states_pred, f_np)
174 | mpciter = mpciter + 1
175 | xx.append(current_state)
176 |
177 | t_v = np.array(index_t)
178 | print(t_v.mean())
179 | print((time.time() - start_time)/(mpciter))
180 |
181 | # after loop
182 | print(mpciter)
183 | print('final error {}'.format(np.linalg.norm(final_state-current_state)))
184 |
185 | # draw function
186 | draw_result = Draw_MPC_Obstacle(rob_diam=rob_diam,
187 | init_state=init_state,
188 | target_state=final_state,
189 | robot_states=np.array(xx),
190 | obstacle=[obs_x, obs_y, obs_diam/2.])
191 |
192 |
--------------------------------------------------------------------------------
/dynamic/sim4_mpc_tracking_mul_shooting_opt.py:
--------------------------------------------------------------------------------
1 | import casadi as ca
2 | import numpy as np
3 | import math
4 | import time
5 |
6 | from draw import Draw_MPC_tracking
7 |
8 | def shift(T, t0, x0, u, x_n, f):
9 | f_value = f(x0, u[0])
10 | st = x0 + T*f_value
11 | t = t0 + T
12 | u_end = np.concatenate((u[1:], u[-1:]))
13 | x_n = np.concatenate((x_n[1:], x_n[-1:]))
14 | return t, st, u_end, x_n
15 |
16 | def predict_state(x0, u, T, N):
17 | # Parameter
18 | d = 0.2
19 | M = 10; J = 2
20 | Bx = 0.05; By = 0.05; Bw = 0.06
21 | # Cx = 0.5; Cy = 0.5; Cw = 0.6
22 | # define prediction horizon function
23 | states = np.zeros((N+1, 6))
24 | states[0,:] = x0
25 | # euler method
26 | for i in range(N):
27 | states[i+1, 0] = states[i, 0] + (states[i, 3]*np.cos(states[i, 2]) - states[i, 4]*np.sin(states[i, 2]))*T
28 | states[i+1, 1] = states[i, 1] + (states[i, 3]*np.sin(states[i, 2]) + states[i, 4]*np.cos(states[i, 2]))*T
29 | states[i+1, 2] = states[i, 2] + states[i, 5]*T
30 | states[i+1, 3] = (0*u[i, 0] + np.sqrt(3)/2*u[i, 1] - np.sqrt(3)/2*u[i, 2] - Bx*states[3])/M
31 | states[i+1, 4] = (-1*u[i, 0] + 1/2*u[i, 1] + 1/2*u[i, 2] - By*states[4])/M
32 | states[i+1, 5] = (d*u[i, 0] + d*u[i, 1] + d*u[i, 2] - Bw*states[5])/J
33 | return states
34 |
35 |
36 | def desired_command_and_trajectory(t, T, x0_:np.array, N_):
37 | # initial state / last state
38 | x_ = np.zeros((N_+1, 6))
39 | x_[0] = x0_
40 | u_ = np.zeros((N_, 3))
41 | # states for the next N_ trajectories
42 | for i in range(N_):
43 | t_predict = t + T*i
44 | x_ref_ = 4*math.cos(2*math.pi/12*t_predict)
45 | y_ref_ = 4*math.sin(2*math.pi/12*t_predict)
46 | theta_ref_ = 2*math.pi/12*t_predict + math.pi/2
47 |
48 | dotx_ref_ = -2*math.pi/12*y_ref_
49 | doty_ref_ = 2*math.pi/12*x_ref_
50 | dotq_ref_ = 2*math.pi/12
51 |
52 | # vx_ref_ = dotx_ref_*math.cos(theta_ref_) + doty_ref_*math.sin(theta_ref_)
53 | # vy_ref_ = -dotx_ref_*math.sin(theta_ref_) + doty_ref_*math.cos(theta_ref_)
54 | vx_ref_ = 4*2*math.pi/12
55 | vy_ref_ = 0
56 | omega_ref_ = dotq_ref_
57 |
58 | x_[i+1] = np.array([x_ref_, y_ref_, theta_ref_, vx_ref_, vy_ref_, omega_ref_])
59 | u_[i] = np.array([0.05*vx_ref_, 0.05*vy_ref_, 0.06*omega_ref_])
60 | # return pose and command
61 | return x_, u_
62 |
63 | if __name__ == "__main__":
64 | # Parameter
65 | d = 0.2
66 | M = 10; J = 2
67 | Bx = 0.05; By = 0.05; Bw = 0.06
68 | # Cx = 0.5; Cy = 0.5; Cw = 0.6
69 |
70 | T = 0.1 # time step
71 | N = 50 # horizon length
72 | rob_diam = 0.3 # [m]
73 | v_max = 3.0 # linear velocity max
74 | omega_max = np.pi/3.0 # angular velocity max
75 | u_max = 5 # force max of each direction
76 |
77 | opti = ca.Opti()
78 | # control variables, toruqe of each wheel
79 | opt_controls = opti.variable(N, 3)
80 | u1 = opt_controls[:, 0]
81 | u2 = opt_controls[:, 1]
82 | u3 = opt_controls[:, 2]
83 |
84 | # state variable: position and velocity
85 | opt_states = opti.variable(N+1, 6)
86 | x = opt_states[:, 0]
87 | y = opt_states[:, 1]
88 | theta = opt_states[:, 2]
89 | vx = opt_states[:, 3]
90 | vy = opt_states[:, 4]
91 | omega = opt_states[:, 5]
92 |
93 | # parameters
94 | opt_x0 = opti.parameter(6)
95 | opt_xs = opti.parameter(6)
96 |
97 | # create model
98 | f = lambda x_, u_: ca.vertcat(*[x_[3]*ca.cos(x_[2]) - x_[4]*ca.sin(x_[2]),
99 | x_[3]*ca.sin(x_[2]) + x_[4]*ca.cos(x_[2]),
100 | x_[5],
101 | (0*u_[0] + ca.sqrt(3)/2*u_[1] - ca.sqrt(3)/2*u_[2] - Bx*x_[3])/M,
102 | (-1*u_[0] + 1/2*u_[1] + 1/2*u_[2] - By*x_[4])/M,
103 | (d*u_[0] + d*u_[1] + d*u_[2] - Bw*x_[5])/J])
104 | f_np = lambda x_, u_: np.array([x_[3]*ca.cos(x_[2]) - x_[4]*ca.sin(x_[2]),
105 | x_[3]*ca.sin(x_[2]) + x_[4]*ca.cos(x_[2]),
106 | x_[5],
107 | (0*u_[0] + ca.sqrt(3)/2*u_[1] - ca.sqrt(3)/2*u_[2] - Bx*x_[3])/M,
108 | (-1*u_[0] + 1/2*u_[1] + 1/2*u_[2] - By*x_[4])/M,
109 | (d*u_[0] + d*u_[1] + d*u_[2] - Bw*x_[5])/J])
110 | # parameters, these parameters are the reference trajectories of the pose and inputs
111 | opt_u_ref = opti.parameter(N, 3)
112 | opt_x_ref = opti.parameter(N+1, 6)
113 |
114 | # initial condition
115 | opti.subject_to(opt_states[0, :] == opt_x_ref[0, :])
116 | for i in range(N):
117 | x_next = opt_states[i, :] + f(opt_states[i, :], opt_controls[i, :]).T*T
118 | opti.subject_to(opt_states[i+1, :] == x_next)
119 |
120 | # weight matrix
121 | Q = np.diag([30.0, 30.0, 5.0, 5.0, 5.0, 1.0])
122 | R = np.diag([1.0, 1.0, 1.0])
123 |
124 | # cost function
125 | obj = 0
126 | for i in range(N):
127 | state_error_ = opt_states[i, :] - opt_x_ref[i+1, :]
128 | control_error_ = opt_controls[i, :] - opt_u_ref[i, :]
129 | obj = obj + ca.mtimes([state_error_, Q, state_error_.T]) + ca.mtimes([control_error_, R, control_error_.T])
130 | opti.minimize(obj)
131 |
132 | #### boundrary and control conditions
133 | # boundary and control conditions
134 | opti.subject_to(opti.bounded(-math.inf, x, math.inf))
135 | opti.subject_to(opti.bounded(-math.inf, y, math.inf))
136 | opti.subject_to(opti.bounded(-math.inf, theta, math.inf))
137 | opti.subject_to(opti.bounded(-v_max, vx, v_max))
138 | opti.subject_to(opti.bounded(-v_max, vy, v_max))
139 | opti.subject_to(opti.bounded(-omega_max, omega, omega_max))
140 | opti.subject_to(opti.bounded(-u_max, u1, u_max))
141 | opti.subject_to(opti.bounded(-u_max, u2, u_max))
142 | opti.subject_to(opti.bounded(-u_max, u3, u_max))
143 |
144 | opts_setting = {'ipopt.max_iter':2000,
145 | 'ipopt.print_level':0,
146 | 'print_time':0,
147 | 'ipopt.acceptable_tol':1e-8,
148 | 'ipopt.acceptable_obj_change_tol':1e-6}
149 |
150 | opti.solver('ipopt', opts_setting)
151 |
152 | t0 = 0
153 | init_state = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
154 | current_state = init_state.copy()
155 | u0 = np.zeros((N, 3))
156 | next_trajectories = np.tile(init_state, N+1).reshape(N+1, -1) # set the initial state as the first trajectories for the robot
157 | next_controls = np.zeros((N, 3))
158 | next_states = np.zeros((N+1, 6))
159 | x_c = [] # contains for the history of the state
160 | u_c = []
161 | t_c = [t0] # for the time
162 | xx = []
163 | sim_time = 12.0
164 |
165 | ## start MPC
166 | mpciter = 0
167 | start_time = time.time()
168 | index_t = []
169 | while(mpciter-sim_time/T<0.0):
170 | ## set parameter, here only update initial state of x (x0)
171 | opti.set_value(opt_x_ref, next_trajectories)
172 | opti.set_value(opt_u_ref, next_controls)
173 | ## provide the initial guess of the optimization targets
174 | opti.set_initial(opt_controls, u0.reshape(N, 3))# (N, 3)
175 | opti.set_initial(opt_states, next_states) # (N+1, 6)
176 | ## solve the problem once again
177 | t_ = time.time()
178 | sol = opti.solve()
179 | index_t.append(time.time()- t_)
180 | ## obtain the control input
181 | u_res = sol.value(opt_controls)
182 | x_m = sol.value(opt_states)
183 | # print(x_m[:3])
184 | u_c.append(u_res[0, :])
185 | t_c.append(t0)
186 | x_c.append(x_m)
187 | t0, current_state, u0, next_states = shift(T, t0, current_state, u_res, x_m, f_np)
188 | xx.append(current_state)
189 | ## estimate the new desired trajectories and controls
190 | next_trajectories, next_controls = desired_command_and_trajectory(t0, T, current_state, N)
191 | mpciter = mpciter + 1
192 |
193 |
194 | ## after loop
195 | print(mpciter)
196 | t_v = np.array(index_t)
197 | print(t_v.mean())
198 | print((time.time() - start_time)/(mpciter))
199 | ## draw function
200 | draw_result = Draw_MPC_tracking(rob_diam=0.3,
201 | init_state=init_state,
202 | robot_states=np.array(xx))
--------------------------------------------------------------------------------
/dynamic/sim5_mpc_tracking_obs_avoid_mul_shooting_opt.py:
--------------------------------------------------------------------------------
1 | import casadi as ca
2 | import numpy as np
3 | import math
4 | import time
5 |
6 | from draw import Draw_MPC_tracking_Obstacle
7 |
8 | def shift(T, t0, x0, u, x_n, f):
9 | f_value = f(x0, u[0])
10 | st = x0 + T*f_value
11 | t = t0 + T
12 | u_end = np.concatenate((u[1:], u[-1:]))
13 | x_n = np.concatenate((x_n[1:], x_n[-1:]))
14 | return t, st, u_end, x_n
15 |
16 | def predict_state(x0, u, T, N):
17 | # Parameter
18 | d = 0.2
19 | M = 10; J = 2
20 | Bx = 0.05; By = 0.05; Bw = 0.06
21 | # Cx = 0.5; Cy = 0.5; Cw = 0.6
22 | # define prediction horizon function
23 | states = np.zeros((N+1, 6))
24 | states[0,:] = x0
25 | # euler method
26 | for i in range(N):
27 | states[i+1, 0] = states[i, 0] + (states[i, 3]*np.cos(states[i, 2]) - states[i, 4]*np.sin(states[i, 2]))*T
28 | states[i+1, 1] = states[i, 1] + (states[i, 3]*np.sin(states[i, 2]) + states[i, 4]*np.cos(states[i, 2]))*T
29 | states[i+1, 2] = states[i, 2] + states[i, 5]*T
30 | states[i+1, 3] = (0*u[i, 0] + np.sqrt(3)/2*u[i, 1] - np.sqrt(3)/2*u[i, 2] - Bx*states[3])/M
31 | states[i+1, 4] = (-1*u[i, 0] + 1/2*u[i, 1] + 1/2*u[i, 2] - By*states[4])/M
32 | states[i+1, 5] = (d*u[i, 0] + d*u[i, 1] + d*u[i, 2] - Bw*states[5])/J
33 | return states
34 |
35 |
36 | def desired_command_and_trajectory(t, T, x0_:np.array, N_):
37 | # initial state / last state
38 | x_ = np.zeros((N_+1, 6))
39 | x_[0] = x0_
40 | u_ = np.zeros((N_, 3))
41 | # states for the next N_ trajectories
42 | for i in range(N_):
43 | t_predict = t + T*i
44 | x_ref_ = 4*math.cos(2*math.pi/12*t_predict)
45 | y_ref_ = 4*math.sin(2*math.pi/12*t_predict)
46 | theta_ref_ = 2*math.pi/12*t_predict + math.pi/2
47 |
48 | dotx_ref_ = -2*math.pi/12*y_ref_
49 | doty_ref_ = 2*math.pi/12*x_ref_
50 | dotq_ref_ = 2*math.pi/12
51 |
52 | # vx_ref_ = dotx_ref_*math.cos(theta_ref_) + doty_ref_*math.sin(theta_ref_)
53 | # vy_ref_ = -dotx_ref_*math.sin(theta_ref_) + doty_ref_*math.cos(theta_ref_)
54 | vx_ref_ = 4*2*math.pi/12
55 | vy_ref_ = 0
56 | omega_ref_ = dotq_ref_
57 |
58 | x_[i+1] = np.array([x_ref_, y_ref_, theta_ref_, vx_ref_, vy_ref_, omega_ref_])
59 | u_[i] = np.array([0.05*vx_ref_, 0.05*vy_ref_, 0.06*omega_ref_])
60 | # return pose and command
61 | return x_, u_
62 |
63 | if __name__ == "__main__":
64 | # Parameter
65 | d = 0.2
66 | M = 10; J = 2
67 | Bx = 0.05; By = 0.05; Bw = 0.06
68 | # Cx = 0.5; Cy = 0.5; Cw = 0.6
69 |
70 | T = 0.1 # time step
71 | N = 50 # horizon length
72 | rob_diam = 0.3 # [m]
73 | v_max = 3.0 # linear velocity max
74 | omega_max = np.pi/3.0 # angular velocity max
75 | u_max = 5 # force max of each direction
76 |
77 | opti = ca.Opti()
78 | # control variables, toruqe of each wheel
79 | opt_controls = opti.variable(N, 3)
80 | u1 = opt_controls[:, 0]
81 | u2 = opt_controls[:, 1]
82 | u3 = opt_controls[:, 2]
83 |
84 | # state variable: position and velocity
85 | opt_states = opti.variable(N+1, 6)
86 | x = opt_states[:, 0]
87 | y = opt_states[:, 1]
88 | theta = opt_states[:, 2]
89 | vx = opt_states[:, 3]
90 | vy = opt_states[:, 4]
91 | omega = opt_states[:, 5]
92 |
93 | # create model
94 | f = lambda x_, u_: ca.vertcat(*[x_[3]*ca.cos(x_[2]) - x_[4]*ca.sin(x_[2]),
95 | x_[3]*ca.sin(x_[2]) + x_[4]*ca.cos(x_[2]),
96 | x_[5],
97 | (0*u_[0] + ca.sqrt(3)/2*u_[1] - ca.sqrt(3)/2*u_[2] - Bx*x_[3])/M,
98 | (-1*u_[0] + 1/2*u_[1] + 1/2*u_[2] - By*x_[4])/M,
99 | (d*u_[0] + d*u_[1] + d*u_[2] - Bw*x_[5])/J])
100 | f_np = lambda x_, u_: np.array([x_[3]*ca.cos(x_[2]) - x_[4]*ca.sin(x_[2]),
101 | x_[3]*ca.sin(x_[2]) + x_[4]*ca.cos(x_[2]),
102 | x_[5],
103 | (0*u_[0] + ca.sqrt(3)/2*u_[1] - ca.sqrt(3)/2*u_[2] - Bx*x_[3])/M,
104 | (-1*u_[0] + 1/2*u_[1] + 1/2*u_[2] - By*x_[4])/M,
105 | (d*u_[0] + d*u_[1] + d*u_[2] - Bw*x_[5])/J])
106 |
107 | # parameters, these parameters are the reference trajectories of the pose and inputs
108 | opt_u_ref = opti.parameter(N, 3)
109 | opt_x_ref = opti.parameter(N+1, 6)
110 |
111 | # initial condition
112 | opti.subject_to(opt_states[0, :] == opt_x_ref[0, :])
113 | for i in range(N):
114 | x_next = opt_states[i, :] + f(opt_states[i, :], opt_controls[i, :]).T*T
115 | opti.subject_to(opt_states[i+1, :] == x_next)
116 |
117 | # obstacle
118 | obs_x = [0.0, 0.0, -1.0, -4.0, -3.2]
119 | obs_y = [2.0, -4.0, 1.0, 2.0, 0.0]
120 | obs_diam = 0.5
121 | bias = 0.05
122 |
123 | # add constraints to obstacle
124 | for i in range(N+1):
125 | for j in range(len(obs_x)):
126 | temp_constraints_ = ca.sqrt((opt_states[i, 0]-obs_x[j]-bias)**2+(opt_states[i, 1]-obs_y[j])**2-bias) - rob_diam - obs_diam/2
127 | opti.subject_to(opti.bounded(0.0, temp_constraints_, 10.0))
128 |
129 | # weight matrix
130 | Q = np.diag([30.0, 30.0, 5.0, 5.0, 5.0, 1.0])
131 | R = np.diag([1.0, 1.0, 1.0])
132 |
133 | # cost function
134 | obj = 0
135 | for i in range(N):
136 | state_error_ = opt_states[i, :] - opt_x_ref[i+1, :]
137 | control_error_ = opt_controls[i, :] - opt_u_ref[i, :]
138 | obj = obj + ca.mtimes([state_error_, Q, state_error_.T]) + ca.mtimes([control_error_, R, control_error_.T])
139 | opti.minimize(obj)
140 |
141 | #### boundrary and control conditions
142 | opti.subject_to(opti.bounded(-math.inf, x, math.inf))
143 | opti.subject_to(opti.bounded(-math.inf, y, math.inf))
144 | opti.subject_to(opti.bounded(-math.inf, theta, math.inf))
145 | opti.subject_to(opti.bounded(-v_max, vx, v_max))
146 | opti.subject_to(opti.bounded(-v_max, vy, v_max))
147 | opti.subject_to(opti.bounded(-omega_max, omega, omega_max))
148 | opti.subject_to(opti.bounded(-u_max, u1, u_max))
149 | opti.subject_to(opti.bounded(-u_max, u2, u_max))
150 | opti.subject_to(opti.bounded(-u_max, u3, u_max))
151 |
152 | opts_setting = {'ipopt.max_iter':2000,
153 | 'ipopt.print_level':0,
154 | 'print_time':0,
155 | 'ipopt.acceptable_tol':1e-8,
156 | 'ipopt.acceptable_obj_change_tol':1e-6}
157 |
158 | opti.solver('ipopt', opts_setting)
159 |
160 | t0 = 0
161 | init_state = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
162 | current_state = init_state.copy()
163 | u0 = np.zeros((N, 3))
164 | next_trajectories = np.tile(init_state, N+1).reshape(N+1, -1) # set the initial state as the first trajectories for the robot
165 | next_controls = np.zeros((N, 3))
166 | next_states = np.zeros((N+1, 6))
167 | x_c = [] # contains for the history of the state
168 | u_c = []
169 | t_c = [t0] # for the time
170 | xx = []
171 | sim_time = 12.0
172 |
173 | ## start MPC
174 | mpciter = 0
175 | start_time = time.time()
176 | index_t = []
177 | while(mpciter-sim_time/T<0.0):
178 | ## set parameter, here only update initial state of x (x0)
179 | opti.set_value(opt_x_ref, next_trajectories)
180 | opti.set_value(opt_u_ref, next_controls)
181 | ## provide the initial guess of the optimization targets
182 | opti.set_initial(opt_controls, u0.reshape(N, 3))# (N, 3)
183 | opti.set_initial(opt_states, next_states) # (N+1, 6)
184 | ## solve the problem once again
185 | t_ = time.time()
186 | sol = opti.solve()
187 | index_t.append(time.time()- t_)
188 | ## obtain the control input
189 | u_res = sol.value(opt_controls)
190 | x_m = sol.value(opt_states)
191 | # print(x_m[:3])
192 | u_c.append(u_res[0, :])
193 | t_c.append(t0)
194 | x_c.append(x_m)
195 | t0, current_state, u0, next_states = shift(T, t0, current_state, u_res, x_m, f_np)
196 | xx.append(current_state)
197 | ## estimate the new desired trajectories and controls
198 | next_trajectories, next_controls = desired_command_and_trajectory(t0, T, current_state, N)
199 | mpciter = mpciter + 1
200 |
201 |
202 | ## after loop
203 | print(mpciter)
204 | t_v = np.array(index_t)
205 | print(t_v.mean())
206 | print((time.time() - start_time)/(mpciter))
207 | ## draw function
208 | draw_result = Draw_MPC_tracking_Obstacle(rob_diam=0.3,
209 | init_state=init_state,
210 | robot_states=np.array(xx),
211 | obstacle=[obs_x, obs_y, obs_diam/2.],
212 | export_fig=False)
--------------------------------------------------------------------------------
/kinematic/__pycache__/draw.cpython-38.pyc:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/duynamrcv/nmpc_casadi_python/8255b724008e3fd1f223dce398c5c6ab2cbaef60/kinematic/__pycache__/draw.cpython-38.pyc
--------------------------------------------------------------------------------
/kinematic/draw.py:
--------------------------------------------------------------------------------
1 | #!/usr/bin/env python
2 | # coding=utf-8
3 |
4 | import numpy as np
5 | from matplotlib import pyplot as plt
6 | import matplotlib.animation as animation
7 | import matplotlib.patches as mpatches
8 |
9 |
10 | class Draw_MPC_point_stabilization_v1(object):
11 | def __init__(self, robot_states: np.array, init_state: np.array, target_state: np.array, rob_diam=0.3,
12 | export_fig=False):
13 | self.robot_states = robot_states
14 | self.init_state = init_state
15 | self.target_state = target_state
16 | self.rob_radius = rob_diam / 2.0
17 | self.fig = plt.figure()
18 | self.ax = plt.axes(xlim=(-0.8, 5), ylim=(-0.8, 5.))
19 | # self.fig.set_dpi(400)
20 | self.fig.set_size_inches(7, 6.5)
21 | # init for plot
22 | self.animation_init()
23 |
24 | self.ani = animation.FuncAnimation(self.fig, self.animation_loop, range(len(self.robot_states)),
25 | init_func=self.animation_init, interval=100, repeat=False)
26 |
27 | plt.grid('--')
28 | if export_fig:
29 | self.ani.save('./v1.gif', writer='imagemagick', fps=100)
30 | plt.show()
31 |
32 | def animation_init(self):
33 | # plot target state
34 | self.target_circle = plt.Circle(self.target_state[:2], self.rob_radius, color='b', fill=False)
35 | self.ax.add_artist(self.target_circle)
36 | self.target_arr = mpatches.Arrow(self.target_state[0], self.target_state[1],
37 | self.rob_radius * np.cos(self.target_state[2]),
38 | self.rob_radius * np.sin(self.target_state[2]), width=0.2)
39 | self.ax.add_patch(self.target_arr)
40 | self.robot_body = plt.Circle(self.init_state[:2], self.rob_radius, color='r', fill=False)
41 | self.ax.add_artist(self.robot_body)
42 | self.robot_arr = mpatches.Arrow(self.init_state[0], self.init_state[1],
43 | self.rob_radius * np.cos(self.init_state[2]),
44 | self.rob_radius * np.sin(self.init_state[2]), width=0.2, color='r')
45 | self.ax.add_patch(self.robot_arr)
46 | return self.target_circle, self.target_arr, self.robot_body, self.robot_arr
47 |
48 | def animation_loop(self, indx):
49 | position = self.robot_states[indx][:2]
50 | orientation = self.robot_states[indx][2]
51 | self.robot_body.center = position
52 | # self.ax.add_artist(self.robot_body)
53 | self.robot_arr.remove()
54 | self.robot_arr = mpatches.Arrow(position[0], position[1], self.rob_radius * np.cos(orientation),
55 | self.rob_radius * np.sin(orientation), width=0.2, color='r')
56 | self.ax.add_patch(self.robot_arr)
57 | self.ax.plot(self.robot_states[:indx, 0], self.robot_states[:indx, 1], color='r', linewidth=1.5)
58 | return self.robot_arr, self.robot_body
59 |
60 |
61 | class Draw_MPC_Obstacle(object):
62 | def __init__(self, robot_states: np.array, init_state: np.array, target_state: np.array, obstacle: list,
63 | rob_diam=0.3, export_fig=False):
64 | self.robot_states = robot_states
65 | self.init_state = init_state
66 | self.target_state = target_state
67 | self.rob_radius = rob_diam / 2.0
68 | self.fig = plt.figure()
69 | self.ax = plt.axes(xlim=(-0.8, 5), ylim=(-0.8, 5.))
70 | if obstacle is not None:
71 | self.obstacle = obstacle
72 | else:
73 | print('no obstacle given, break')
74 | self.fig.set_size_inches(7, 6.5)
75 | # init for plot
76 | self.animation_init()
77 |
78 | self.ani = animation.FuncAnimation(self.fig, self.animation_loop, range(len(self.robot_states)),
79 | init_func=self.animation_init, interval=100, repeat=False)
80 |
81 | plt.grid('--')
82 | if export_fig:
83 | self.ani.save('obstacle.gif', writer='imagemagick', fps=100)
84 | plt.show()
85 |
86 | def animation_init(self):
87 | # plot target state
88 | self.target_circle = plt.Circle(self.target_state[:2], self.rob_radius, color='b', fill=False)
89 | self.ax.add_artist(self.target_circle)
90 | self.target_arr = mpatches.Arrow(self.target_state[0], self.target_state[1],
91 | self.rob_radius * np.cos(self.target_state[2]),
92 | self.rob_radius * np.sin(self.target_state[2]), width=0.2)
93 | self.ax.add_patch(self.target_arr)
94 | self.robot_body = plt.Circle(self.init_state[:2], self.rob_radius, color='r', fill=False)
95 | self.ax.add_artist(self.robot_body)
96 | self.robot_arr = mpatches.Arrow(self.init_state[0], self.init_state[1],
97 | self.rob_radius * np.cos(self.init_state[2]),
98 | self.rob_radius * np.sin(self.init_state[2]), width=0.2, color='r')
99 | self.ax.add_patch(self.robot_arr)
100 | self.obstacle_circle = []
101 | for i in range(len(self.obstacle[0])):
102 | center = (self.obstacle[0][i], self.obstacle[1][i])
103 | obs = plt.Circle(center, self.obstacle[2], color='g', fill=True)
104 | self.obstacle_circle.append(obs)
105 | self.ax.add_artist(obs)
106 |
107 | return self.target_circle, self.target_arr, self.robot_body, self.robot_arr, self.obstacle_circle
108 |
109 | def animation_loop(self, indx):
110 | position = self.robot_states[indx][:2]
111 | orientation = self.robot_states[indx][2]
112 | self.robot_body.center = position
113 | self.robot_arr.remove()
114 | self.robot_arr = mpatches.Arrow(position[0], position[1], self.rob_radius * np.cos(orientation),
115 | self.rob_radius * np.sin(orientation), width=0.2, color='r')
116 | self.ax.plot(self.robot_states[:indx, 0], self.robot_states[:indx, 1], color='r', linewidth=1.5)
117 |
118 | self.ax.add_patch(self.robot_arr)
119 | return self.robot_arr, self.robot_body
120 |
121 |
122 | class Draw_MPC_tracking(object):
123 | def __init__(self, robot_states: np.array, init_state: np.array, rob_diam=0.3, export_fig=False):
124 | self.init_state = init_state
125 | self.robot_states = robot_states
126 | self.rob_radius = rob_diam
127 | self.fig = plt.figure()
128 | self.ax = plt.axes(xlim=(-6.0, 6.0), ylim=(-6.0, 6.0))
129 | # self.fig.set_size_inches(7, 6.5)
130 | # init for plot
131 | self.animation_init()
132 |
133 | self.ani = animation.FuncAnimation(self.fig, self.animation_loop, range(len(self.robot_states)),
134 | init_func=self.animation_init, interval=100, repeat=False)
135 |
136 | plt.grid('--')
137 | if export_fig:
138 | self.ani.save('tracking.gif', writer='imagemagick', fps=100)
139 | plt.show()
140 |
141 | def animation_init(self, ):
142 | # draw target line
143 | step = np.arange(0,12.1,np.pi/50)
144 | x_ref = 4*np.cos(step)
145 | y_ref = 4*np.sin(step)
146 | self.target_line = plt.plot(x_ref, y_ref, '-b')
147 | # draw the initial position of the robot
148 | self.init_robot_position = plt.Circle(self.init_state[:2], self.rob_radius, color='r', fill=False)
149 | self.ax.add_artist(self.init_robot_position)
150 | self.robot_body = plt.Circle(self.init_state[:2], self.rob_radius, color='r', fill=False)
151 | self.ax.add_artist(self.robot_body)
152 | self.robot_arr = mpatches.Arrow(self.init_state[0], self.init_state[1],
153 | self.rob_radius * np.cos(self.init_state[2]),
154 | self.rob_radius * np.sin(self.init_state[2]), width=0.2, color='r')
155 | self.ax.add_patch(self.robot_arr)
156 | return self.target_line, self.init_robot_position, self.robot_body, self.robot_arr
157 |
158 | def animation_loop(self, indx):
159 | position = self.robot_states[indx,:2]
160 | orientation = self.robot_states[indx,2]
161 | self.robot_body.center = position
162 | self.robot_arr.remove()
163 | self.robot_arr = mpatches.Arrow(position[0], position[1], self.rob_radius * np.cos(orientation),
164 | self.rob_radius * np.sin(orientation), width=0.2, color='r')
165 | self.ax.add_patch(self.robot_arr)
166 | self.ax.plot(self.robot_states[:indx, 0], self.robot_states[:indx, 1], color='r', linewidth=1.5)
167 | return self.robot_arr, self.robot_body
168 |
169 | class Draw_MPC_tracking_Obstacle(object):
170 | def __init__(self, robot_states: np.array, init_state: np.array, obstacle: list, rob_diam=0.3, export_fig=False):
171 | self.init_state = init_state
172 | self.robot_states = robot_states
173 | self.rob_radius = rob_diam
174 | self.fig = plt.figure()
175 | self.ax = plt.axes(xlim=(-6.0, 6.0), ylim=(-6.0, 6.0))
176 | if obstacle is not None:
177 | self.obstacle = obstacle
178 | else:
179 | print('no obstacle given, break')
180 |
181 | self.animation_init()
182 |
183 | self.ani = animation.FuncAnimation(self.fig, self.animation_loop, range(len(self.robot_states)),
184 | init_func=self.animation_init, interval=100, repeat=False)
185 |
186 | plt.grid('--')
187 | if export_fig:
188 | self.ani.save('tracking_obs_avoid.gif', writer='imagemagick', fps=100)
189 | plt.show()
190 |
191 | def animation_init(self, ):
192 | # draw target line
193 | step = np.arange(0,12.1,np.pi/50)
194 | x_ref = 4*np.cos(step)
195 | y_ref = 4*np.sin(step)
196 | self.target_line = plt.plot(x_ref, y_ref, '-b')
197 | # draw the initial position of the robot
198 | self.init_robot_position = plt.Circle(self.init_state[:2], self.rob_radius, color='r', fill=False)
199 | self.ax.add_artist(self.init_robot_position)
200 | self.robot_body = plt.Circle(self.init_state[:2], self.rob_radius, color='r', fill=False)
201 | self.ax.add_artist(self.robot_body)
202 | self.robot_arr = mpatches.Arrow(self.init_state[0], self.init_state[1],
203 | self.rob_radius * np.cos(self.init_state[2]),
204 | self.rob_radius * np.sin(self.init_state[2]), width=0.2, color='r')
205 | self.ax.add_patch(self.robot_arr)
206 | self.obstacle_circle = []
207 | for i in range(len(self.obstacle[0])):
208 | center = (self.obstacle[0][i], self.obstacle[1][i])
209 | obs = plt.Circle(center, self.obstacle[2], color='g', fill=True)
210 | self.obstacle_circle.append(obs)
211 | self.ax.add_artist(obs)
212 | return self.target_line, self.init_robot_position, self.robot_body, self.robot_arr, self.obstacle_circle
213 |
214 | def animation_loop(self, indx):
215 | position = self.robot_states[indx,:2]
216 | orientation = self.robot_states[indx,2]
217 | self.robot_body.center = position
218 | self.robot_arr.remove()
219 | self.robot_arr = mpatches.Arrow(position[0], position[1], self.rob_radius * np.cos(orientation),
220 | self.rob_radius * np.sin(orientation), width=0.2, color='r')
221 | self.ax.add_patch(self.robot_arr)
222 | self.ax.plot(self.robot_states[:indx, 0], self.robot_states[:indx, 1], color='r', linewidth=1.5)
223 | return self.robot_arr, self.robot_body
--------------------------------------------------------------------------------
/kinematic/sim2_mpc_mul_shooting_opt.py:
--------------------------------------------------------------------------------
1 | import casadi as ca
2 | import numpy as np
3 | import time
4 |
5 | from draw import Draw_MPC_point_stabilization_v1
6 |
7 | def shift(T, t0, x0, u, x_n, f):
8 | f_value = f(x0, u[0])
9 | st = x0 + T*f_value
10 | t = t0 + T
11 | u_end = np.concatenate((u[1:], u[-1:]))
12 | x_n = np.concatenate((x_n[1:], x_n[-1:]))
13 | return t, st, u_end, x_n
14 |
15 | def predict_state(x0, u, T, N):
16 | # define prediction horizon function
17 | states = np.zeros((N+1, 3))
18 | states[0,:] = x0
19 | # euler method
20 | for i in range(N):
21 | states[i+1, 0] = states[i, 0] + (u[i, 0]*np.cos(states[i, 2]) - u[i, 1]*np.sin(states[i, 2]))*T
22 | states[i+1, 1] = states[i, 1] + (u[i, 0]*np.sin(states[i, 2]) + u[i, 1]*np.cos(states[i, 2]))*T
23 | states[i+1, 2] = states[i, 2] + u[i, 2]*T
24 | return states
25 |
26 | if __name__ == "__main__":
27 | T = 0.2 # time step
28 | N = 30 # horizon length
29 | v_max = 1.0 # linear velocity max
30 | omega_max = np.pi/3.0 # angular velocity max
31 |
32 | opti = ca.Opti()
33 | # control variables, liear velocity and angular velocity
34 | opt_controls = opti.variable(N, 3)
35 | vx = opt_controls[:, 0]
36 | vy = opt_controls[:, 1]
37 | omega = opt_controls[:, 2]
38 |
39 | opt_states = opti.variable(N+1, 3)
40 | x = opt_states[:, 0]
41 | y = opt_states[:, 1]
42 | theta = opt_states[:, 2]
43 |
44 | # parameters
45 | opt_x0 = opti.parameter(3)
46 | opt_xs = opti.parameter(3)
47 |
48 | # create model
49 | f = lambda x_, u_: ca.vertcat(*[u_[0]*ca.cos(x_[2]) - u_[1]*ca.sin(x_[2]),
50 | u_[0]*ca.sin(x_[2]) + u_[1]*ca.cos(x_[2]),
51 | u_[2]])
52 | f_np = lambda x_, u_: np.array([u_[0]*ca.cos(x_[2]) - u_[1]*ca.sin(x_[2]),
53 | u_[0]*ca.sin(x_[2]) + u_[1]*ca.cos(x_[2]),
54 | u_[2]])
55 |
56 | # initial condition
57 | opti.subject_to(opt_states[0, :] == opt_x0.T)
58 | for i in range(N):
59 | x_next = opt_states[i, :] + f(opt_states[i, :], opt_controls[i, :]).T*T
60 | opti.subject_to(opt_states[i+1, :] == x_next)
61 |
62 | # weight matrix
63 | Q = np.diag([10.0, 10.0, 1.0])
64 | R = np.diag([1.0, 1.0, 0.5])
65 |
66 | # cost function
67 | obj = 0
68 | for i in range(N):
69 | obj = obj + ca.mtimes([(opt_states[i, :] - opt_xs.T), Q, (opt_states[i, :] - opt_xs.T).T]) \
70 | + ca.mtimes([opt_controls[i, :], R, opt_controls[i, :].T])
71 | opti.minimize(obj)
72 |
73 | # boundary and control conditions
74 | opti.subject_to(opti.bounded(-10.0, x, 10.0))
75 | opti.subject_to(opti.bounded(-10.0, y, 10.0))
76 | opti.subject_to(opti.bounded(-v_max, vx, v_max))
77 | opti.subject_to(opti.bounded(-v_max, vy, v_max))
78 | opti.subject_to(opti.bounded(-omega_max, omega, omega_max))
79 |
80 | opts_setting = {'ipopt.max_iter': 100,
81 | 'ipopt.print_level': 0,
82 | 'print_time': 0,
83 | 'ipopt.acceptable_tol': 1e-8,
84 | 'ipopt.acceptable_obj_change_tol': 1e-6}
85 | opti.solver('ipopt', opts_setting)
86 |
87 | # The final state
88 | final_state = np.array([4.5, 4.5, np.pi/2])
89 | opti.set_value(opt_xs, final_state)
90 |
91 | # The initial state
92 | t0 = 0
93 | init_state = np.array([0.0, 0.0, 0.0])
94 | u0 = np.zeros((N, 3))
95 | current_state = init_state.copy()
96 | next_states = np.zeros((N+1, 3))
97 | x_c = [] # contains for the history of the state
98 | u_c = []
99 | t_c = [t0] # for the time
100 | xx = []
101 | sim_time = 30.0
102 |
103 | ## start MPC
104 | mpciter = 0
105 | start_time = time.time()
106 | index_t = []
107 | while(np.linalg.norm(current_state - final_state) > 1e-2 and mpciter - sim_time/T < 0.0 ):
108 | # set parameter, here only update initial state of x (x0)
109 | opti.set_value(opt_x0, current_state)
110 |
111 | # set optimizing target withe init guess
112 | opti.set_initial(opt_controls, u0)# (N, 3)
113 | opti.set_initial(opt_states, next_states) # (N+1, 3)
114 |
115 | # solve the problem once again
116 | t_ = time.time()
117 | sol = opti.solve()
118 | index_t.append(time.time()- t_)
119 | # opti.set_initial(opti.lam_g, sol.value(opti.lam_g))
120 |
121 | # obtain the control input
122 | u_res = sol.value(opt_controls)
123 | u_c.append(u_res[0, :])
124 | t_c.append(t0)
125 | next_states_pred = sol.value(opt_states)# prediction_state(x0=current_state, u=u_res, N=N, T=T)
126 |
127 | # next_states_pred = prediction_state(x0=current_state, u=u_res, N=N, T=T)
128 | x_c.append(next_states_pred)
129 |
130 | # for next loop
131 | t0, current_state, u0, next_states = shift(T, t0, current_state, u_res, next_states_pred, f_np)
132 | mpciter = mpciter + 1
133 | xx.append(current_state)
134 |
135 | t_v = np.array(index_t)
136 | print(t_v.mean())
137 | print((time.time() - start_time)/(mpciter))
138 |
139 | # after loop
140 | print(mpciter)
141 | print('final error {}'.format(np.linalg.norm(final_state-current_state)))
142 |
143 | # draw function
144 | draw_result = Draw_MPC_point_stabilization_v1(rob_diam=0.3,
145 | init_state=init_state,
146 | target_state=final_state,
147 | robot_states=np.array(xx))
148 |
--------------------------------------------------------------------------------
/kinematic/sim3_mpc_obs_avoid_mul_shooting_opt.py:
--------------------------------------------------------------------------------
1 | import casadi as ca
2 | import numpy as np
3 | import time
4 |
5 | from draw import Draw_MPC_Obstacle
6 |
7 | def shift(T, t0, x0, u, x_n, f):
8 | f_value = f(x0, u[0])
9 | st = x0 + T*f_value
10 | t = t0 + T
11 | u_end = np.concatenate((u[1:], u[-1:]))
12 | x_n = np.concatenate((x_n[1:], x_n[-1:]))
13 | return t, st, u_end, x_n
14 |
15 | def predict_state(x0, u, T, N):
16 | # define prediction horizon function
17 | states = np.zeros((N+1, 3))
18 | states[0,:] = x0
19 | # euler method
20 | for i in range(N):
21 | states[i+1, 0] = states[i, 0] + (u[i, 0]*np.cos(states[i, 2]) - u[i, 1]*np.sin(states[i, 2]))*T
22 | states[i+1, 1] = states[i, 1] + (u[i, 0]*np.sin(states[i, 2]) + u[i, 1]*np.cos(states[i, 2]))*T
23 | states[i+1, 2] = states[i, 2] + u[i, 2]*T
24 | return states
25 |
26 | if __name__ == "__main__":
27 | T = 0.2 # time step
28 | N = 30 # horizon length
29 | rob_diam = 0.3 # [m]
30 | v_max = 1.0 # linear velocity max
31 | omega_max = np.pi/3.0 # angular velocity max
32 |
33 | opti = ca.Opti()
34 | # control variables, liear velocity and angular velocity
35 | opt_controls = opti.variable(N, 3)
36 | vx = opt_controls[:, 0]
37 | vy = opt_controls[:, 1]
38 | omega = opt_controls[:, 2]
39 |
40 | opt_states = opti.variable(N+1, 3)
41 | x = opt_states[:, 0]
42 | y = opt_states[:, 1]
43 | theta = opt_states[:, 2]
44 |
45 | # parameters
46 | opt_x0 = opti.parameter(3)
47 | opt_xs = opti.parameter(3)
48 |
49 | # create model
50 | f = lambda x_, u_: ca.vertcat(*[u_[0]*ca.cos(x_[2]) - u_[1]*ca.sin(x_[2]), u_[0]*ca.sin(x_[2]) + u_[1]*ca.cos(x_[2]), u_[2]])
51 | f_np = lambda x_, u_: np.array([u_[0]*ca.cos(x_[2]) - u_[1]*ca.sin(x_[2]), u_[0]*ca.sin(x_[2]) + u_[1]*ca.cos(x_[2]), u_[2]])
52 |
53 | # initial condition
54 | opti.subject_to(opt_states[0, :] == opt_x0.T)
55 | for i in range(N):
56 | x_next = opt_states[i, :] + f(opt_states[i, :], opt_controls[i, :]).T*T
57 | opti.subject_to(opt_states[i+1, :] == x_next)
58 |
59 | # obstacle
60 | obs_x = [0.5, 1.5, 3.0, 3.0, 3.5, 4.5]
61 | obs_y = [1.0, 1.0, 1.5, 2.7, 4.0, 4.0]
62 | obs_diam = 0.5
63 | bias = 0.02
64 |
65 | # add constraints to obstacle
66 | for i in range(N+1):
67 | for j in range(len(obs_x)):
68 | temp_constraints_ = ca.sqrt((opt_states[i, 0]-obs_x[j]-bias)**2+(opt_states[i, 1]-obs_y[j])**2-bias)-rob_diam/2.0-obs_diam/2.0
69 | opti.subject_to(opti.bounded(0.0, temp_constraints_, 10.0))
70 |
71 |
72 | # weight matrix
73 | Q = np.array([[10.0, 0.0, 0.0], [0.0, 10.0, 0.0], [0.0, 0.0, 1.0]])
74 | R = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 0.1]])
75 |
76 | # cost function
77 | obj = 0
78 | for i in range(N):
79 | obj = obj + ca.mtimes([(opt_states[i, :] - opt_xs.T), Q, (opt_states[i, :] - opt_xs.T).T]) \
80 | + ca.mtimes([opt_controls[i, :], R, opt_controls[i, :].T])
81 | opti.minimize(obj)
82 |
83 | # boundary and control conditions
84 | opti.subject_to(opti.bounded(-10.0, x, 10.0))
85 | opti.subject_to(opti.bounded(-10.0, y, 10.0))
86 | opti.subject_to(opti.bounded(-v_max, vx, v_max))
87 | opti.subject_to(opti.bounded(-v_max, vy, v_max))
88 | opti.subject_to(opti.bounded(-omega_max, omega, omega_max))
89 |
90 | opts_setting = {'ipopt.max_iter': 100,
91 | 'ipopt.print_level': 0,
92 | 'print_time': 0,
93 | 'ipopt.acceptable_tol': 1e-8,
94 | 'ipopt.acceptable_obj_change_tol': 1e-6}
95 | opti.solver('ipopt', opts_setting)
96 |
97 | # The final state
98 | final_state = np.array([4.5, 4.5, 0.0])
99 | opti.set_value(opt_xs, final_state)
100 |
101 | # The initial state
102 | t0 = 0
103 | init_state = np.array([0.0, 0.0, 0.0])
104 | u0 = np.zeros((N, 3))
105 | current_state = init_state.copy()
106 | next_states = np.zeros((N+1, 3))
107 | x_c = [] # contains for the history of the state
108 | u_c = []
109 | t_c = [t0] # for the time
110 | xx = []
111 | sim_time = 30.0
112 |
113 | ## start MPC
114 | mpciter = 0
115 | start_time = time.time()
116 | index_t = []
117 | while(np.linalg.norm(current_state - final_state) > 1e-2 and mpciter - sim_time/T < 0.0 ):
118 | # set parameter, here only update initial state of x (x0)
119 | opti.set_value(opt_x0, current_state)
120 |
121 | # set optimizing target withe init guess
122 | opti.set_initial(opt_controls, u0)# (N, 3)
123 | opti.set_initial(opt_states, next_states) # (N+1, 3)
124 |
125 | # solve the problem once again
126 | t_ = time.time()
127 | sol = opti.solve()
128 | index_t.append(time.time()- t_)
129 | # opti.set_initial(opti.lam_g, sol.value(opti.lam_g))
130 |
131 | # obtain the control input
132 | u_res = sol.value(opt_controls)
133 | u_c.append(u_res[0, :])
134 | t_c.append(t0)
135 | next_states_pred = sol.value(opt_states)# prediction_state(x0=current_state, u=u_res, N=N, T=T)
136 |
137 | # next_states_pred = prediction_state(x0=current_state, u=u_res, N=N, T=T)
138 | x_c.append(next_states_pred)
139 |
140 | # for next loop
141 | t0, current_state, u0, next_states = shift(T, t0, current_state, u_res, next_states_pred, f_np)
142 | mpciter = mpciter + 1
143 | xx.append(current_state)
144 |
145 | t_v = np.array(index_t)
146 | print(t_v.mean())
147 | print((time.time() - start_time)/(mpciter))
148 |
149 | # after loop
150 | print(mpciter)
151 | print('final error {}'.format(np.linalg.norm(final_state-current_state)))
152 |
153 | # draw function
154 | draw_result = Draw_MPC_Obstacle(rob_diam=rob_diam,
155 | init_state=init_state,
156 | target_state=final_state,
157 | robot_states=np.array(xx),
158 | obstacle=[obs_x, obs_y, obs_diam/2.])
159 |
160 |
--------------------------------------------------------------------------------
/kinematic/sim4_mpc_tracking_mul_shooting_opt.py:
--------------------------------------------------------------------------------
1 | import casadi as ca
2 | import numpy as np
3 | import math
4 | import time
5 |
6 | from draw import Draw_MPC_tracking
7 |
8 | def shift(T, t0, x0, u, x_n, f):
9 | f_value = f(x0, u[0])
10 | st = x0 + T*f_value
11 | t = t0 + T
12 | u_end = np.concatenate((u[1:], u[-1:]))
13 | x_n = np.concatenate((x_n[1:], x_n[-1:]))
14 | return t, st
15 |
16 | def predict_state(x0, u, T, N):
17 | # define prediction horizon function
18 | states = np.zeros((N+1, 3))
19 | states[0,:] = x0
20 | # euler method
21 | for i in range(N):
22 | states[i+1, 0] = states[i, 0] + (u[i, 0]*np.cos(states[i, 2]) - u[i, 1]*np.sin(states[i, 2]))*T
23 | states[i+1, 1] = states[i, 1] + (u[i, 0]*np.sin(states[i, 2]) + u[i, 1]*np.cos(states[i, 2]))*T
24 | states[i+1, 2] = states[i, 2] + u[i, 2]*T
25 | if states[i+1, 2] > math.pi:
26 | states[i+1, 2] -= 2*math.pi
27 | elif states[i+1, 2] < math.pi:
28 | states[i+1, 2] += 2*math.pi
29 | return states
30 |
31 | def desired_command_and_trajectory(t, T, x0_:np.array, N_):
32 | # initial state / last state
33 | x_ = np.zeros((N_+1, 3))
34 | x_[0] = x0_
35 | u_ = np.zeros((N_, 3))
36 | # states for the next N_ trajectories
37 | for i in range(N_):
38 | t_predict = t + T*i
39 | x_ref_ = 0.5 * t_predict
40 | y_ref_ = 1.0
41 | theta_ref_ = 0.0
42 | v_ref_ = 0.5
43 | omega_ref_ = 0.0
44 | if x_ref_ >= 12.0:
45 | x_ref_ = 12.0
46 | v_ref_ = 0.0
47 | x_[i+1] = np.array([x_ref_, y_ref_, theta_ref_])
48 | u_[i] = np.array([v_ref_, 0, omega_ref_])
49 | # return pose and command
50 | return x_, u_
51 |
52 | def desired_command_and_trajectory2(t, T, x0_:np.array, N_):
53 | # initial state / last state
54 | x_ = np.zeros((N_+1, 3))
55 | x_[0] = x0_
56 | u_ = np.zeros((N_, 3))
57 | # states for the next N_ trajectories
58 | for i in range(N_):
59 | t_predict = t + T*i
60 | x_ref_ = 4*math.cos(2*math.pi/12*t_predict)
61 | y_ref_ = 4*math.sin(2*math.pi/12*t_predict)
62 | theta_ref_ = 2*math.pi/12*t_predict + math.pi/2
63 |
64 | dotx_ref_ = -2*math.pi/12*y_ref_
65 | doty_ref_ = 2*math.pi/12*x_ref_
66 | dotq_ref_ = 2*math.pi/12
67 |
68 | vx_ref_ = dotx_ref_*math.cos(dotq_ref_) + doty_ref_*math.sin(dotq_ref_)
69 | vy_ref_ = -dotx_ref_*math.sin(dotq_ref_) + doty_ref_*math.cos(dotq_ref_)
70 | omega_ref_ = dotq_ref_
71 |
72 | x_[i+1] = np.array([x_ref_, y_ref_, theta_ref_])
73 | u_[i] = np.array([vx_ref_, vy_ref_, omega_ref_])
74 | # return pose and command
75 | return x_, u_
76 |
77 | if __name__ == "__main__":
78 | T = 0.1 # time step
79 | N = 30 # horizon length
80 | rob_diam = 0.3 # [m]
81 | v_max = 3.0 # linear velocity max
82 | omega_max = np.pi/3.0 # angular velocity max
83 |
84 | opti = ca.Opti()
85 | # control variables, liear velocity and angular velocity
86 | opt_controls = opti.variable(N, 3)
87 | vx = opt_controls[:, 0]
88 | vy = opt_controls[:, 1]
89 | omega = opt_controls[:, 2]
90 |
91 | opt_states = opti.variable(N+1, 3)
92 | x = opt_states[:, 0]
93 | y = opt_states[:, 1]
94 | theta = opt_states[:, 2]
95 |
96 | # parameters, these parameters are the reference trajectories of the pose and inputs
97 | opt_u_ref = opti.parameter(N, 3)
98 | opt_x_ref = opti.parameter(N+1, 3)
99 |
100 | # create model
101 | f = lambda x_, u_: ca.vertcat(*[u_[0]*ca.cos(x_[2]) - u_[1]*ca.sin(x_[2]), u_[0]*ca.sin(x_[2]) + u_[1]*ca.cos(x_[2]), u_[2]])
102 | f_np = lambda x_, u_: np.array([u_[0]*ca.cos(x_[2]) - u_[1]*ca.sin(x_[2]), u_[0]*ca.sin(x_[2]) + u_[1]*ca.cos(x_[2]), u_[2]])
103 |
104 | # initial condition
105 | opti.subject_to(opt_states[0, :] == opt_x_ref[0, :])
106 | for i in range(N):
107 | x_next = opt_states[i, :] + f(opt_states[i, :], opt_controls[i, :]).T*T
108 | opti.subject_to(opt_states[i+1, :] == x_next)
109 |
110 | # weight matrix
111 | Q = np.array([[50.0, 0.0, 0.0], [0.0, 50.0, 0.0], [0.0, 0.0, 10.0]])
112 | R = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 0.1]])
113 |
114 | # cost function
115 | obj = 0
116 | for i in range(N):
117 | state_error_ = opt_states[i, :] - opt_x_ref[i+1, :]
118 | control_error_ = opt_controls[i, :] - opt_u_ref[i, :]
119 | obj = obj + ca.mtimes([state_error_, Q, state_error_.T]) + ca.mtimes([opt_controls[i, :], R, opt_controls[i, :].T])
120 | opti.minimize(obj)
121 |
122 | #### boundrary and control conditions
123 | opti.subject_to(opti.bounded(-math.inf, x, math.inf))
124 | opti.subject_to(opti.bounded(-math.inf, y, math.inf))
125 | opti.subject_to(opti.bounded(-math.inf, theta, math.inf))
126 | opti.subject_to(opti.bounded(-v_max, vx, v_max))
127 | opti.subject_to(opti.bounded(-v_max, vy, v_max))
128 | opti.subject_to(opti.bounded(-omega_max, omega, omega_max))
129 |
130 | opts_setting = {'ipopt.max_iter':2000,
131 | 'ipopt.print_level':0,
132 | 'print_time':0,
133 | 'ipopt.acceptable_tol':1e-8,
134 | 'ipopt.acceptable_obj_change_tol':1e-6}
135 |
136 | opti.solver('ipopt', opts_setting)
137 |
138 | t0 = 0
139 | init_state = np.array([2.0, 3.0, 0.0])
140 | current_state = init_state.copy()
141 | u0 = np.zeros((N, 3))
142 | next_trajectories = np.tile(init_state, N+1).reshape(N+1, -1) # set the initial state as the first trajectories for the robot
143 | next_controls = np.zeros((N, 3))
144 | next_states = np.tile(current_state, N+1).reshape(N+1, -1)
145 | x_c = [] # contains for the history of the state
146 | u_c = []
147 | t_c = [t0] # for the time
148 | xx = []
149 | sim_time = 12.0
150 |
151 | ## start MPC
152 | mpciter = 0
153 | start_time = time.time()
154 | index_t = []
155 | while(mpciter-sim_time/T<0.0):
156 | ## set parameter, here only update initial state of x (x0)
157 | opti.set_value(opt_x_ref, next_trajectories)
158 | opti.set_value(opt_u_ref, next_controls)
159 | ## provide the initial guess of the optimization targets
160 | opti.set_initial(opt_controls, u0.reshape(N, 3))# (N, 3)
161 | opti.set_initial(opt_states, next_states) # (N+1, 3)
162 | ## solve the problem once again
163 | t_ = time.time()
164 | sol = opti.solve()
165 | index_t.append(time.time()- t_)
166 | ## obtain the control input
167 | u_res = sol.value(opt_controls)
168 | x_m = sol.value(opt_states)
169 | # print(x_m[:3])
170 | u_c.append(u_res[0, :])
171 | t_c.append(t0)
172 | x_c.append(x_m)
173 | t0, current_state = shift(T, t0, current_state, u_res, x_m, f_np)
174 | xx.append(current_state)
175 | ## estimate the new desired trajectories and controls
176 | next_trajectories, next_controls = desired_command_and_trajectory2(t0, T, current_state, N)
177 | mpciter = mpciter + 1
178 |
179 |
180 | ## after loop
181 | print(mpciter)
182 | t_v = np.array(index_t)
183 | print(t_v.mean())
184 | print((time.time() - start_time)/(mpciter))
185 | ## draw function
186 | draw_result = Draw_MPC_tracking(rob_diam=0.3,
187 | init_state=init_state,
188 | robot_states=np.array(xx))
--------------------------------------------------------------------------------
/kinematic/sim5_mpc_tracking_obs_avoid_mul_shooting_opt.py:
--------------------------------------------------------------------------------
1 | import casadi as ca
2 | import numpy as np
3 | import math
4 | import time
5 |
6 | from draw import Draw_MPC_tracking_Obstacle
7 |
8 | def shift(T, t0, x0, u, x_n, f):
9 | f_value = f(x0, u[0])
10 | st = x0 + T*f_value
11 | t = t0 + T
12 | u_end = np.concatenate((u[1:], u[-1:]))
13 | x_n = np.concatenate((x_n[1:], x_n[-1:]))
14 | return t, st, u_end, x_n
15 |
16 | def predict_state(x0, u, T, N):
17 | # define prediction horizon function
18 | states = np.zeros((N+1, 3))
19 | states[0,:] = x0
20 | # euler method
21 | for i in range(N):
22 | states[i+1, 0] = states[i, 0] + (u[i, 0]*np.cos(states[i, 2]) - u[i, 1]*np.sin(states[i, 2]))*T
23 | states[i+1, 1] = states[i, 1] + (u[i, 0]*np.sin(states[i, 2]) + u[i, 1]*np.cos(states[i, 2]))*T
24 | states[i+1, 2] = states[i, 2] + u[i, 2]*T
25 | if states[i+1, 2] > math.pi:
26 | states[i+1, 2] -= 2*math.pi
27 | elif states[i+1, 2] < math.pi:
28 | states[i+1, 2] += 2*math.pi
29 | return states
30 |
31 | def desired_command_and_trajectory(t, T, x0_:np.array, N_):
32 | # initial state / last state
33 | x_ = np.zeros((N_+1, 3))
34 | x_[0] = x0_
35 | u_ = np.zeros((N_, 3))
36 | # states for the next N_ trajectories
37 | for i in range(N_):
38 | t_predict = t + T*i
39 | x_ref_ = 0.5 * t_predict
40 | y_ref_ = 1.0
41 | theta_ref_ = 0.0
42 | v_ref_ = 0.5
43 | omega_ref_ = 0.0
44 | if x_ref_ >= 12.0:
45 | x_ref_ = 12.0
46 | v_ref_ = 0.0
47 | x_[i+1] = np.array([x_ref_, y_ref_, theta_ref_])
48 | u_[i] = np.array([v_ref_, 0, omega_ref_])
49 | # return pose and command
50 | return x_, u_
51 |
52 | def desired_command_and_trajectory2(t, T, x0_:np.array, N_):
53 | # initial state / last state
54 | x_ = np.zeros((N_+1, 3))
55 | x_[0] = x0_
56 | u_ = np.zeros((N_, 3))
57 | # states for the next N_ trajectories
58 | for i in range(N_):
59 | t_predict = t + T*i
60 | x_ref_ = 4*math.cos(2*math.pi/12*t_predict)
61 | y_ref_ = 4*math.sin(2*math.pi/12*t_predict)
62 | theta_ref_ = 2*math.pi/12*t_predict + math.pi/2
63 |
64 | dotx_ref_ = -2*math.pi/12*y_ref_
65 | doty_ref_ = 2*math.pi/12*x_ref_
66 | dotq_ref_ = 2*math.pi/12
67 |
68 | vx_ref_ = dotx_ref_*math.cos(dotq_ref_) + doty_ref_*math.sin(dotq_ref_)
69 | vy_ref_ = -dotx_ref_*math.sin(dotq_ref_) + doty_ref_*math.cos(dotq_ref_)
70 | omega_ref_ = dotq_ref_
71 |
72 | x_[i+1] = np.array([x_ref_, y_ref_, theta_ref_])
73 | u_[i] = np.array([vx_ref_, vy_ref_, omega_ref_])
74 | # return pose and command
75 | return x_, u_
76 |
77 | if __name__ == "__main__":
78 | T = 0.1 # time step
79 | N = 30 # horizon length
80 | rob_diam = 0.3 # [m]
81 | v_max = 3.0 # linear velocity max
82 | omega_max = np.pi/3.0 # angular velocity max
83 |
84 | opti = ca.Opti()
85 | # control variables, liear velocity and angular velocity
86 | opt_controls = opti.variable(N, 3)
87 | vx = opt_controls[:, 0]
88 | vy = opt_controls[:, 1]
89 | omega = opt_controls[:, 2]
90 |
91 | opt_states = opti.variable(N+1, 3)
92 | x = opt_states[:, 0]
93 | y = opt_states[:, 1]
94 | theta = opt_states[:, 2]
95 |
96 | # parameters, these parameters are the reference trajectories of the pose and inputs
97 | opt_u_ref = opti.parameter(N, 3)
98 | opt_x_ref = opti.parameter(N+1, 3)
99 |
100 | # create model
101 | f = lambda x_, u_: ca.vertcat(*[u_[0]*ca.cos(x_[2]) - u_[1]*ca.sin(x_[2]), u_[0]*ca.sin(x_[2]) + u_[1]*ca.cos(x_[2]), u_[2]])
102 | f_np = lambda x_, u_: np.array([u_[0]*ca.cos(x_[2]) - u_[1]*ca.sin(x_[2]), u_[0]*ca.sin(x_[2]) + u_[1]*ca.cos(x_[2]), u_[2]])
103 |
104 | # initial condition
105 | opti.subject_to(opt_states[0, :] == opt_x_ref[0, :])
106 | for i in range(N):
107 | x_next = opt_states[i, :] + f(opt_states[i, :], opt_controls[i, :]).T*T
108 | opti.subject_to(opt_states[i+1, :] == x_next)
109 |
110 | # obstacle
111 | obs_x = [2.0, 0.0, -4.0, 1.0]
112 | obs_y = [1.0, 4.0, 0.0, -4.0]
113 | obs_diam = 0.5
114 | bias = 0.05
115 |
116 | # add constraints to obstacle
117 | for i in range(N+1):
118 | for j in range(len(obs_x)):
119 | temp_constraints_ = ca.sqrt((opt_states[i, 0]-obs_x[j]-bias)**2+(opt_states[i, 1]-obs_y[j])**2-bias) - rob_diam - obs_diam/2
120 | opti.subject_to(opti.bounded(0.0, temp_constraints_, 10.0))
121 |
122 | # weight matrix
123 | Q = np.array([[50.0, 0.0, 0.0], [0.0, 50.0, 0.0], [0.0, 0.0, 10.0]])
124 | R = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 0.1]])
125 |
126 | # cost function
127 | obj = 0
128 | for i in range(N):
129 | state_error_ = opt_states[i, :] - opt_x_ref[i+1, :]
130 | control_error_ = opt_controls[i, :] - opt_u_ref[i, :]
131 | obj = obj + ca.mtimes([state_error_, Q, state_error_.T]) + ca.mtimes([control_error_, R, control_error_.T])
132 | opti.minimize(obj)
133 |
134 | #### boundrary and control conditions
135 | opti.subject_to(opti.bounded(-math.inf, x, math.inf))
136 | opti.subject_to(opti.bounded(-math.inf, y, math.inf))
137 | opti.subject_to(opti.bounded(-math.inf, theta, math.inf))
138 | opti.subject_to(opti.bounded(-v_max, vx, v_max))
139 | opti.subject_to(opti.bounded(-v_max, vy, v_max))
140 | opti.subject_to(opti.bounded(-omega_max, omega, omega_max))
141 |
142 | opts_setting = {'ipopt.max_iter':2000,
143 | 'ipopt.print_level':0,
144 | 'print_time':0,
145 | 'ipopt.acceptable_tol':1e-8,
146 | 'ipopt.acceptable_obj_change_tol':1e-6}
147 |
148 | opti.solver('ipopt', opts_setting)
149 |
150 | t0 = 0
151 | init_state = np.array([0.0, 0.0, 0.0])
152 | current_state = init_state.copy()
153 | u0 = np.zeros((N, 3))
154 | next_trajectories = np.tile(init_state, N+1).reshape(N+1, -1) # set the initial state as the first trajectories for the robot
155 | next_controls = np.zeros((N, 3))
156 | next_states = np.zeros((N+1, 3))
157 | x_c = [] # contains for the history of the state
158 | u_c = []
159 | t_c = [t0] # for the time
160 | xx = []
161 | sim_time = 12.0
162 |
163 | ## start MPC
164 | mpciter = 0
165 | start_time = time.time()
166 | index_t = []
167 | while(mpciter-sim_time/T<0.0):
168 | ## set parameter, here only update initial state of x (x0)
169 | opti.set_value(opt_x_ref, next_trajectories)
170 | opti.set_value(opt_u_ref, next_controls)
171 | ## provide the initial guess of the optimization targets
172 | opti.set_initial(opt_controls, u0.reshape(N, 3))# (N, 3)
173 | opti.set_initial(opt_states, next_states) # (N+1, 3)
174 | ## solve the problem once again
175 | t_ = time.time()
176 | sol = opti.solve()
177 | index_t.append(time.time()- t_)
178 | ## obtain the control input
179 | u_res = sol.value(opt_controls)
180 | x_m = sol.value(opt_states)
181 | # print(x_m[:3])
182 | u_c.append(u_res[0, :])
183 | t_c.append(t0)
184 | x_c.append(x_m)
185 | t0, current_state, u0, next_states = shift(T, t0, current_state, u_res, x_m, f_np)
186 | xx.append(current_state)
187 | ## estimate the new desired trajectories and controls
188 | next_trajectories, next_controls = desired_command_and_trajectory2(t0, T, current_state, N)
189 | mpciter = mpciter + 1
190 |
191 |
192 | ## after loop
193 | print(mpciter)
194 | t_v = np.array(index_t)
195 | print(t_v.mean())
196 | print((time.time() - start_time)/(mpciter))
197 | ## draw function
198 | draw_result = Draw_MPC_tracking_Obstacle(rob_diam=0.3,
199 | init_state=init_state,
200 | robot_states=np.array(xx),
201 | obstacle=[obs_x, obs_y, obs_diam/2.],
202 | export_fig=False)
--------------------------------------------------------------------------------
/lnmpc_smc.py:
--------------------------------------------------------------------------------
1 | import casadi as ca
2 | import numpy as np
3 | import math
4 | import time
5 |
6 | from dynamic.draw import Draw_MPC_tracking
7 |
8 | def shift(T, t0, x0, u, x_n, f):
9 | f_value = f(x0, u[0])
10 | st = x0 + T*f_value
11 | t = t0 + T
12 | u_end = np.concatenate((u[1:], u[-1:]))
13 | x_n = np.concatenate((x_n[1:], x_n[-1:]))
14 | return t, st, u_end, x_n
15 |
16 | def predict_state(x0, u, T, N):
17 | bias = float(np.random.normal(0, 5, 1))
18 | # bias = 0
19 |
20 | # Parameter
21 | d = 0.2
22 | M = 10; J = 2
23 | Bx = 0.5; By = 0.5; Bw = 0.6
24 | # Cx = 0.5; Cy = 0.5; Cw = 0.6
25 | # define prediction horizon function
26 | states = np.zeros((N+1, 6))
27 | states[0,:] = x0
28 | # euler method
29 | for i in range(N):
30 | states[i+1, 0] = states[i, 0] + (states[i, 3]*np.cos(states[i, 2]) - states[i, 4]*np.sin(states[i, 2]))*T
31 | states[i+1, 1] = states[i, 1] + (states[i, 3]*np.sin(states[i, 2]) + states[i, 4]*np.cos(states[i, 2]))*T
32 | states[i+1, 2] = states[i, 2] + states[i, 5]*T
33 | states[i+1, 3] = (0*(u[i, 0]+bias) + np.sqrt(3)/2*(u[i, 1]+bias) - np.sqrt(3)/2*(u[i, 2]+bias) - Bx*states[3])/M
34 | states[i+1, 4] = (-1*(u[i, 0]+bias) + 1/2*(u[i, 1]+bias) + 1/2*(u[i, 2]+bias) - By*states[4])/M
35 | states[i+1, 5] = (d*(u[i, 0]+bias) + d*(u[i, 1]+bias) + d*(u[i, 2]+bias) - Bw*states[5])/J
36 | return states
37 |
38 |
39 | def desired_command_and_trajectory(t, T, x0_:np.array, N_):
40 | Bx = 0.5; By = 0.5; Bw = 0.6
41 | # initial state / last state
42 | x_ = np.zeros((N_+1, 6))
43 | x_[0] = x0_
44 | u_ = np.zeros((N_, 3))
45 | # states for the next N_ trajectories
46 | for i in range(N_):
47 | t_predict = t + T*i
48 | x_ref_ = 4*math.cos(2*math.pi/12*t_predict)
49 | y_ref_ = 4*math.sin(2*math.pi/12*t_predict)
50 | theta_ref_ = 2*math.pi/12*t_predict + math.pi/2
51 |
52 | dotx_ref_ = -2*math.pi/12*y_ref_
53 | doty_ref_ = 2*math.pi/12*x_ref_
54 | dotq_ref_ = 2*math.pi/12
55 |
56 | # vx_ref_ = dotx_ref_*math.cos(theta_ref_) + doty_ref_*math.sin(theta_ref_)
57 | # vy_ref_ = -dotx_ref_*math.sin(theta_ref_) + doty_ref_*math.cos(theta_ref_)
58 | vx_ref_ = 4*2*math.pi/12
59 | vy_ref_ = 0
60 | omega_ref_ = dotq_ref_
61 |
62 | x_[i+1] = np.array([x_ref_, y_ref_, theta_ref_, vx_ref_, vy_ref_, omega_ref_])
63 |
64 | u1_ref_ = 0*Bx*vx_ref_ - 2/3*By*vy_ref_ + 2/3*Bw*omega_ref_
65 | u2_ref_ = math.sqrt(3)/3*Bx*vx_ref_ + 1/3*By*vy_ref_ + 2/3*Bw*omega_ref_
66 | u3_ref_ = -math.sqrt(3)/3*Bx*vx_ref_ + 1/3*By*vy_ref_ + 2/3*Bw*omega_ref_
67 | u_[i] = np.array([u1_ref_, u2_ref_, u3_ref_])
68 | # return pose and command
69 | return x_, u_
70 |
71 | if __name__ == "__main__":
72 | # Parameter
73 | d = 0.2
74 | M = 10; J = 2
75 | Bx = 0.5; By = 0.5; Bw = 0.6
76 | Cx = 0.5; Cy = 0.5; Cw = 0.6
77 |
78 | T = 0.1 # time step
79 | N = 30 # horizon length
80 | rob_diam = 0.3 # [m]
81 | v_max = 3.0 # linear velocity max
82 | omega_max = np.pi/3.0 # angular velocity max
83 | u_max = 8 # force max of each direction
84 |
85 | opti = ca.Opti()
86 | # control variables, toruqe of each wheel
87 | opt_controls = opti.variable(N, 3)
88 | u1 = opt_controls[:, 0]
89 | u2 = opt_controls[:, 1]
90 | u3 = opt_controls[:, 2]
91 |
92 | # state variable: position and velocity
93 | opt_states = opti.variable(N+1, 6)
94 | x = opt_states[:, 0]
95 | y = opt_states[:, 1]
96 | theta = opt_states[:, 2]
97 | vx = opt_states[:, 3]
98 | vy = opt_states[:, 4]
99 | omega = opt_states[:, 5]
100 |
101 | # parameters
102 | opt_x0 = opti.parameter(6)
103 | opt_xs = opti.parameter(6)
104 |
105 | # create model
106 | f = lambda x_, u_: ca.vertcat(*[x_[3]*ca.cos(x_[2]) - x_[4]*ca.sin(x_[2]),
107 | x_[3]*ca.sin(x_[2]) + x_[4]*ca.cos(x_[2]),
108 | x_[5],
109 | (0*u_[0] + ca.sqrt(3)/2*u_[1] - ca.sqrt(3)/2*u_[2] - Bx*x_[3])/M,
110 | (-1*u_[0] + 1/2*u_[1] + 1/2*u_[2] - By*x_[4])/M,
111 | (d*u_[0] + d*u_[1] + d*u_[2] - Bw*x_[5])/J])
112 | f_np = lambda x_, u_: np.array([x_[3]*ca.cos(x_[2]) - x_[4]*ca.sin(x_[2]),
113 | x_[3]*ca.sin(x_[2]) + x_[4]*ca.cos(x_[2]),
114 | x_[5],
115 | (0*u_[0] + ca.sqrt(3)/2*u_[1] - ca.sqrt(3)/2*u_[2] - Bx*x_[3])/M,
116 | (-1*u_[0] + 1/2*u_[1] + 1/2*u_[2] - By*x_[4])/M,
117 | (d*u_[0] + d*u_[1] + d*u_[2] - Bw*x_[5])/J])
118 | # parameters, these parameters are the reference trajectories of the pose and inputs
119 | opt_u_ref = opti.parameter(N, 3)
120 | opt_x_ref = opti.parameter(N+1, 6)
121 |
122 | # initial condition
123 | opti.subject_to(opt_states[0, :] == opt_x_ref[0, :])
124 | for i in range(N):
125 | x_next = opt_states[i, :] + f(opt_states[i, :], opt_controls[i, :]).T*T
126 | opti.subject_to(opt_states[i+1, :] == x_next)
127 |
128 | # weight matrix
129 | Q = np.diag([30.0, 30.0, 10.0, 10.0, 10.0, 1.0])
130 | R = np.diag([1.0, 1.0, 1.0])
131 |
132 | # cost function
133 | obj = 0
134 | for i in range(N):
135 | state_error_ = opt_states[i, :] - opt_x_ref[i+1, :]
136 | control_error_ = opt_controls[i, :] - opt_u_ref[i, :]
137 | obj = obj + ca.mtimes([state_error_, Q, state_error_.T]) \
138 | + ca.mtimes([control_error_, R, control_error_.T])
139 | opti.minimize(obj)
140 |
141 | # Lyapunov NMPC - SMC constraints
142 | lamda = np.diag([2,2,2])
143 | H = ca.vertcat(ca.horzcat(ca.cos(opt_states[0,2]), -ca.sin(opt_states[0,2]), 0),
144 | ca.horzcat(ca.sin(opt_states[0,2]), ca.cos(opt_states[0,2]), 0),
145 | ca.horzcat(0, 0, 1),)
146 | H_inv = ca.transpose(H)
147 | H_dot = ca.vertcat(ca.horzcat(-ca.sin(opt_states[0,2]), -ca.cos(opt_states[0,2]), 0),
148 | ca.horzcat(ca.cos(opt_states[0,2]), -ca.sin(opt_states[0,2]), 0),
149 | ca.horzcat(0, 0, 1),)
150 |
151 | M_mat = np.diag([1/M,1/M,1/J])
152 | B_mat = np.diag([Bx, By, Bw])
153 | C_mat = np.diag([Cx, Cy, Cw])
154 | G_mat = ca.DM([ [0, np.sqrt(3)/2, -np.sqrt(3)/2],
155 | [-1, 1/2, 1/2],
156 | [d, d, d]])
157 |
158 | z1 = ca.transpose(state_error_[0,:3]); z2 = ca.transpose(state_error_[0,3:])
159 | S = H@z2 + lamda@z1
160 | c2 = 2; c3 = 2
161 | Ve = ca.transpose(S)@(c3@S) + ca.transpose(S)@H@(M_mat@(G_mat@ca.transpose(opt_controls[0,:])-B_mat@ca.transpose(opt_states[0,3:])) + (lamda + H_inv@H_dot)@z2)
162 |
163 | #### boundrary and control conditions
164 | # boundary and control conditions
165 | opti.subject_to(opti.bounded(-math.inf, x, math.inf))
166 | opti.subject_to(opti.bounded(-math.inf, y, math.inf))
167 | opti.subject_to(opti.bounded(-math.inf, theta, math.inf))
168 | opti.subject_to(opti.bounded(-v_max, vx, v_max))
169 | opti.subject_to(opti.bounded(-v_max, vy, v_max))
170 | opti.subject_to(opti.bounded(-omega_max, omega, omega_max))
171 | opti.subject_to(opti.bounded(-u_max, u1, u_max))
172 | opti.subject_to(opti.bounded(-u_max, u2, u_max))
173 | opti.subject_to(opti.bounded(-u_max, u3, u_max))
174 | opti.subject_to(opti.bounded(-math.inf, Ve, 0))
175 |
176 | opts_setting = {'ipopt.max_iter':2000,
177 | 'ipopt.print_level':0,
178 | 'print_time':0,
179 | 'ipopt.acceptable_tol':1e-8,
180 | 'ipopt.acceptable_obj_change_tol':1e-6}
181 |
182 | opti.solver('ipopt', opts_setting)
183 |
184 | t0 = 0
185 | init_state = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
186 | current_state = init_state.copy()
187 | u0 = np.zeros((N, 3))
188 | next_trajectories = np.tile(init_state, N+1).reshape(N+1, -1) # set the initial state as the first trajectories for the robot
189 | next_controls = np.zeros((N, 3))
190 | next_states = np.zeros((N+1, 6))
191 | x_c = [] # contains for the history of the state
192 | u_c = []
193 | t_c = [t0] # for the time
194 | xx = [current_state]
195 | sim_time = 12.0
196 |
197 | ## start MPC
198 | mpciter = 0
199 | start_time = time.time()
200 | index_t = []
201 | while(mpciter-sim_time/T<0.0):
202 | ## set parameter, here only update initial state of x (x0)
203 | opti.set_value(opt_x_ref, next_trajectories)
204 | opti.set_value(opt_u_ref, next_controls)
205 | ## provide the initial guess of the optimization targets
206 | opti.set_initial(opt_controls, u0.reshape(N, 3))# (N, 3)
207 | opti.set_initial(opt_states, next_states) # (N+1, 6)
208 | ## solve the problem once again
209 | t_ = time.time()
210 | sol = opti.solve()
211 | index_t.append(time.time()- t_)
212 | ## obtain the control input
213 | u_res = sol.value(opt_controls)
214 | x_m = sol.value(opt_states)
215 | # print(x_m[:3])
216 | u_c.append(u_res[0, :])
217 | t_c.append(t0)
218 | x_c.append(x_m)
219 | t0, current_state, u0, next_states = shift(T, t0, current_state, u_res, x_m, f_np)
220 | xx.append(np.array(current_state))
221 | ## estimate the new desired trajectories and controls
222 | next_trajectories, next_controls = desired_command_and_trajectory(t0, T, current_state, N)
223 | mpciter = mpciter + 1
224 |
225 |
226 | ## after loop
227 | print(mpciter)
228 | t_v = np.array(index_t)
229 | print(t_v.mean())
230 | print((time.time() - start_time)/(mpciter))
231 | ## draw function
232 | draw_result = Draw_MPC_tracking(rob_diam=0.3,
233 | init_state=init_state,
234 | robot_states=np.array(xx))
235 | # with open('lnmpc.npy', 'wb') as f:
236 | # np.save(f, xx)
237 |
238 | import scipy.io
239 | scipy.io.savemat('lnmpc_noise.mat', dict(xx=xx))
--------------------------------------------------------------------------------
/results/obstacle.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/duynamrcv/nmpc_casadi_python/8255b724008e3fd1f223dce398c5c6ab2cbaef60/results/obstacle.gif
--------------------------------------------------------------------------------
/results/sim2.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/duynamrcv/nmpc_casadi_python/8255b724008e3fd1f223dce398c5c6ab2cbaef60/results/sim2.png
--------------------------------------------------------------------------------
/results/sim3.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/duynamrcv/nmpc_casadi_python/8255b724008e3fd1f223dce398c5c6ab2cbaef60/results/sim3.png
--------------------------------------------------------------------------------
/results/sim4.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/duynamrcv/nmpc_casadi_python/8255b724008e3fd1f223dce398c5c6ab2cbaef60/results/sim4.png
--------------------------------------------------------------------------------
/results/sim5.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/duynamrcv/nmpc_casadi_python/8255b724008e3fd1f223dce398c5c6ab2cbaef60/results/sim5.png
--------------------------------------------------------------------------------
/results/tracking_obs_avoid.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/duynamrcv/nmpc_casadi_python/8255b724008e3fd1f223dce398c5c6ab2cbaef60/results/tracking_obs_avoid.gif
--------------------------------------------------------------------------------