odqp As a quadratic programming library, it plays a very important role , It can be applied to model predictive control algorithm （MPC）
ROS C++ Refer to this article for library call methods ROS c++ call osqp-eigen Specific operation steps of the Library
According to the following process, you can work normally in python Use in osqp library .
What I use in this example is osqp Official website python Program , To design MPC controller ：
Application osqp solve MPC optimization problem
Put this file into our own feature pack scripts Under the folder ：
Right click Properties , Allow as executable
python call osqp There will be some problems in the Library , My version is Ubuntu18.04+ROSmelodic, default python The version is python2.7. install osqp Here are some tips ：
This explanation python2.7 The version is no longer available , Need to replace the interpreter , So I'm going to Switch to python3.6 Interpreter .
The next step is to install
step 1： install pip3
~$ sudo apt install python3-pipstep 2： install osqp Libraries and other required libraries
~$ pip3 install osqp
~$ pip3 install catkin-tools
~$ pip3 install rospkgWe can see osqp Installation position of ：
~$ pip show osqp~$ pip3 show osqp
Name: osqp
Version: 0.6.2.post5
Summary: OSQP: The Operator Splitting QP Solver
Home-page: https://osqp.org/
Author: Bartolomeo Stellato, Goran Banjac
Author-email: [email protected]
License: Apache 2.0
Location: /home/zhz/.local/lib/python3.6/site-packages
Requires: numpy, scipy, qdldlosqp Kubao exists .local/lib/python3.6/site-packages in
step 3： Change program
Edit test program , Replace the interpreter with python3 or python3.6（ Specifically, it is stored in python Which version ）
#!/usr/bin/env python3 ###### Change the interpreter to python3 perhaps python3.6
# -*- coding=utf-8 -*-
import osqp
import numpy as np
import scipy as sp
from scipy import sparsestart-up roscore, Command line input
~$ rosrun <your_package_name> mpc_test_file.pyHere I change the time step to 3 Step , At the same time, print the first element of the control output , The test results are as follows ：
[email protected]:~$ rosrun path_track mpc_test_file.py
-----------------------------------------------------------------
OSQP v0.6.2 - Operator Splitting QP Solver
(c) Bartolomeo Stellato, Goran Banjac
University of Oxford - Stanford University 2021
-----------------------------------------------------------------
problem: variables n = 172, constraints m = 304
nnz(P) + nnz(A) = 1161
settings: linear system solver = qdldl,
eps_abs = 1.0e-03, eps_rel = 1.0e-03,
eps_prim_inf = 1.0e-04, eps_dual_inf = 1.0e-04,
rho = 1.00e-01 (adaptive),
sigma = 1.00e-06, alpha = 1.60, max_iter = 4000
check_termination: on (interval 25),
scaling: on, scaled_termination: off
warm start: on, polish: off, time_limit: off
iter objective pri res dua res rho time
1 -3.2965e+01 8.15e-01 6.00e+00 1.00e-01 4.15e-04s
25 -4.0983e+01 6.50e-05 2.15e-04 1.00e-01 6.08e-04s
status: solved
number of iterations: 25
optimal objective: -40.9834
run time: 6.49e-04s
optimal rho estimate: 9.40e-02
the first element of u(k) is %0.6 1.7484185677919
iter objective pri res dua res rho time
1 -4.0983e+01 1.67e+00 9.40e+02 1.00e-01 2.18e-05s
25 -4.6383e+01 4.50e-04 4.45e-03 1.00e-01 2.18e-04s
status: solved
number of iterations: 25
optimal objective: -46.3833
run time: 2.42e-04s
optimal rho estimate: 4.51e-02
the first element of u(k) is %0.6 0.5814805859402932
iter objective pri res dua res rho time
1 -4.6383e+01 9.59e-01 5.39e+02 1.00e-01 2.14e-05s
25 -5.0969e+01 2.47e-04 2.56e-03 1.00e-01 1.89e-04s
status: solved
number of iterations: 25
optimal objective: -50.9693
run time: 2.06e-04s
optimal rho estimate: 4.41e-02
the first element of u(k) is %0.6 0.008330435236109587
[email protected]:~$
Add ROS command , Post topics , Continuous model predictive control , The test procedure is as follows ：
#!/usr/bin/env python3
# -*- coding=utf-8 -*-
import rospy
import osqp
import numpy as np
import scipy as sp
from scipy import sparse
from std_msgs.msg import Float64
def publisher():
rospy.init_node('result_publisher',anonymous=True)
pub=rospy.Publisher('mpc_pub',Float64,queue_size=10)
rate=rospy.Rate(10)
# Discrete time model of a quadcopter
Ad = sparse.csc_matrix([
[1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0., 0. ],
[0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0. ],
[0., 0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0. ],
[0.0488, 0., 0., 1., 0., 0., 0.0016, 0., 0., 0.0992, 0., 0. ],
[0., -0.0488, 0., 0., 1., 0., 0., -0.0016, 0., 0., 0.0992, 0. ],
[0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.0992],
[0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0. ],
[0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0. ],
[0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0. ],
[0.9734, 0., 0., 0., 0., 0., 0.0488, 0., 0., 0.9846, 0., 0. ],
[0., -0.9734, 0., 0., 0., 0., 0., -0.0488, 0., 0., 0.9846, 0. ],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.9846]
])
Bd = sparse.csc_matrix([
[0., -0.0726, 0., 0.0726],
[-0.0726, 0., 0.0726, 0. ],
[-0.0152, 0.0152, -0.0152, 0.0152],
[-0., -0.0006, -0., 0.0006],
[0.0006, 0., -0.0006, 0.0000],
[0.0106, 0.0106, 0.0106, 0.0106],
[0, -1.4512, 0., 1.4512],
[-1.4512, 0., 1.4512, 0. ],
[-0.3049, 0.3049, -0.3049, 0.3049],
[-0., -0.0236, 0., 0.0236],
[0.0236, 0., -0.0236, 0. ],
[0.2107, 0.2107, 0.2107, 0.2107]])
[nx, nu] = Bd.shape
# Constraints
u0 = 10.5916
umin = np.array([9.6, 9.6, 9.6, 9.6]) - u0
umax = np.array([13., 13., 13., 13.]) - u0
xmin = np.array([-np.pi/6,-np.pi/6,-np.inf,-np.inf,-np.inf,-1.,
-np.inf,-np.inf,-np.inf,-np.inf,-np.inf,-np.inf])
xmax = np.array([ np.pi/6, np.pi/6, np.inf, np.inf, np.inf, np.inf,
np.inf, np.inf, np.inf, np.inf, np.inf, np.inf])
# Objective function
Q = sparse.diags([0., 0., 10., 10., 10., 10., 0., 0., 0., 5., 5., 5.])
QN = Q
R = 0.1*sparse.eye(4)
# Initial and reference states
x0 = np.zeros(12)
xr = np.array([0.,0.,1.,0.,0.,0.,0.,0.,0.,0.,0.,0.])
# Prediction horizon
N = 10
# Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
# - quadratic objective
P = sparse.block_diag([sparse.kron(sparse.eye(N), Q), QN,
sparse.kron(sparse.eye(N), R)], format='csc')
# - linear objective
q = np.hstack([np.kron(np.ones(N), -Q.dot(xr)), -QN.dot(xr),
np.zeros(N*nu)])
# - linear dynamics
Ax = sparse.kron(sparse.eye(N+1),-sparse.eye(nx)) + sparse.kron(sparse.eye(N+1, k=-1), Ad)
Bu = sparse.kron(sparse.vstack([sparse.csc_matrix((1, N)), sparse.eye(N)]), Bd)
Aeq = sparse.hstack([Ax, Bu])
leq = np.hstack([-x0, np.zeros(N*nx)])
ueq = leq
# - input and state constraints
Aineq = sparse.eye((N+1)*nx + N*nu)
lineq = np.hstack([np.kron(np.ones(N+1), xmin), np.kron(np.ones(N), umin)])
uineq = np.hstack([np.kron(np.ones(N+1), xmax), np.kron(np.ones(N), umax)])
# - OSQP constraints
A = sparse.vstack([Aeq, Aineq], format='csc')
l = np.hstack([leq, lineq])
u = np.hstack([ueq, uineq])
# Create an OSQP object
prob = osqp.OSQP()
# Setup workspace
prob.setup(P, q, A, l, u, warm_start=True)
# Simulate in closed loop
ctrl=np.array([0,0,0,0])
while not rospy.is_shutdown():
num=Float64()
num=ctrl[0]
pub.publish(num)
rospy.loginfo("The first element of the u(k) is :%0.6f",num)
rate.sleep()
# Solve
res = prob.solve()
# Check solver status
if res.info.status != 'solved':raise ValueError('OSQP did not solve the problem!')
# Apply first control input to the plant
ctrl = res.x[-N*nu:-(N-1)*nu]
x0 = Ad.dot(x0) + Bd.dot(ctrl)
# Update initial state
l[:nx] = -x0
u[:nx] = -x0
prob.update(l=l, u=u)
if __name__ == '__main__':
try:
publisher()
except rospy.ROSInterruptException:
pass
The test results are as follows ：
rosrun The result of the order ：
[email protected]:~$ rosrun path_track mpc.py
-----------------------------------------------------------------
OSQP v0.6.2 - Operator Splitting QP Solver
(c) Bartolomeo Stellato, Goran Banjac
University of Oxford - Stanford University 2021
-----------------------------------------------------------------
problem: variables n = 172, constraints m = 304
nnz(P) + nnz(A) = 1161
settings: linear system solver = qdldl,
eps_abs = 1.0e-03, eps_rel = 1.0e-03,
eps_prim_inf = 1.0e-04, eps_dual_inf = 1.0e-04,
rho = 1.00e-01 (adaptive),
sigma = 1.00e-06, alpha = 1.60, max_iter = 4000
check_termination: on (interval 25),
scaling: on, scaled_termination: off
warm start: on, polish: off, time_limit: off
[INFO] [1643197163.344612]: The first element of the u(k) is :0.000000
iter objective pri res dua res rho time
1 -3.2965e+01 8.15e-01 6.00e+00 1.00e-01 7.91e-04s
25 -4.0983e+01 6.50e-05 2.15e-04 1.00e-01 1.20e-03s
status: solved
number of iterations: 25
optimal objective: -40.9834
run time: 1.24e-03s
optimal rho estimate: 9.40e-02
[INFO] [1643197163.427086]: The first element of the u(k) is :-0.991535
iter objective pri res dua res rho time
1 -4.0983e+01 1.67e+00 9.40e+02 1.00e-01 1.04e-04s
25 -4.6383e+01 4.50e-04 4.45e-03 1.00e-01 5.10e-04s
status: solved
number of iterations: 25
optimal objective: -46.3833
run time: 5.43e-04s
optimal rho estimate: 4.51e-02
[INFO] [1643197163.526112]: The first element of the u(k) is :-0.991686
iter objective pri res dua res rho time
1 -4.6383e+01 9.59e-01 5.39e+02 1.00e-01 9.74e-05s
25 -5.0969e+01 2.47e-04 2.56e-03 1.00e-01 5.11e-04s
status: solved
number of iterations: 25
optimal objective: -50.9693
run time: 5.45e-04s
optimal rho estimate: 4.41e-02
[INFO] [1643197163.625625]: The first element of the u(k) is :-0.425915
iter objective pri res dua res rho time
1 -5.0970e+01 2.76e-01 4.91e+02 1.00e-01 9.35e-05s
25 -5.3505e+01 5.87e-05 2.33e-03 1.00e-01 5.06e-04s
status: solved
number of iterations: 25
optimal objective: -53.5053
run time: 5.39e-04s
optimal rho estimate: 3.74e-02
[INFO] [1643197163.725921]: The first element of the u(k) is :0.750120
iter objective pri res dua res rho time
1 -5.3506e+01 9.31e-01 5.24e+02 1.00e-01 7.27e-05s
25 -5.4539e+01 2.58e-04 2.48e-03 1.00e-01 4.69e-04s
status: solved
number of iterations: 25
optimal objective: -54.5391
run time: 5.04e-04s
optimal rho estimate: 5.56e-02
[INFO] [1643197163.825315]: The first element of the u(k) is :0.829224
iter objective pri res dua res rho time
1 -5.4539e+01 1.01e+00 5.66e+02 1.00e-01 7.17e-05s
25 -5.4854e+01 2.74e-04 2.68e-03 1.00e-01 3.35e-04s
status: solved
number of iterations: 25
optimal objective: -54.8543
run time: 3.56e-04s
optimal rho estimate: 5.69e-02
[INFO] [1643197163.925019]: The first element of the u(k) is :0.560168
^Citer objective pri res dua res rho time
1 -5.4854e+01 6.77e-01 3.81e+02 1.00e-01 1.00e-04s
25 -5.4932e+01 1.83e-04 1.80e-03 1.00e-01 3.27e-04s
status: solved
number of iterations: 25
optimal objective: -54.9324
run time: 3.92e-04s
optimal rho estimate: 5.66e-02
[email protected]:~$
topic The topics subscribed are as follows ：
[email protected]:~$ rostopic echo mpc_pub
data: -0.991534988484
---
data: -0.991686079847
---
data: -0.425914685439
---
data: 0.75012029663
---
data: 0.829224461171
---
data: 0.560167648007
---
You can see that the required control variables will be output one by one according to the prediction results .
python call osqp Library programming model predictive control than C++ It needs to be much simpler , And the calling operation of the library is not so cumbersome . If you are willing to learn python Words , Sincerely suggest using python Write model predictive control .
