odqp作为二次规划库具有非常重要的作用，可以运用于模型预测控制算法（MPC）
ROS C++库调用方式可以参考这篇文章ROS c++调用osqp-eigen库的具体操作步骤
按如下过程就能正常地在python中使用osqp库。
准备工作
1.获取测试程序
在这个例子中我所使用的是osqp官网的python程序，用来设计MPC控制器：
2.放入功能包
将这个文件放入我们自己的功能包的scripts文件夹下：
右键属性，允许作为可执行文件
osqp库的调用
python调用osqp库中会遇到一些问题，我的版本是Ubuntu18.04+ROSmelodic，默认的python版本是python2.7。安装osqp会有如下提示：
这说明python2.7版本已经不行了，需要更换解释器，所以我打算 换成python3.6解释器。
接下来就是安装步骤了
步骤1：安装pip3
~$ sudo apt install python3-pip步骤2：安装osqp库和其他需要的库
~$ pip3 install osqp
~$ pip3 install catkin-tools
~$ pip3 install rospkg我们可以查看osqp的安装位置：
~$ 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: bartolomeo.stellato@gmail.com
License: Apache 2.0
Location: /home/zhz/.local/lib/python3.6/site-packages
Requires: numpy, scipy, qdldlosqp库保存在.local/lib/python3.6/site-packages中
步骤3：更改程序
编辑测试程序，将解释器更换为python3或python3.6（具体看保存在python哪个版本中）
#!/usr/bin/env python3 ######更改解释器为python3或者python3.6
# -*- coding=utf-8 -*-
import osqp
import numpy as np
import scipy as sp
from scipy import sparse测试结果
1.常规测试
启动roscore，命令行输入
~$ rosrun <your_package_name> mpc_test_file.py这里我将时间步更改为3步，同时打印控制输出的第一个元素，测试结果如下：
zhz@zhzrobot:~$ 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
zhz@zhzrobot:~$
2.ROS测试
在这个测试程序中加入ROS命令，发布话题，持续进行模型预测控制，测试程序如下：
#!/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
测试结果如下：
rosrun命令结果：
zhz@zhzrobot:~$ 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
zhz@zhzrobot:~$
topic订阅得话题内容如下：
zhz@zhzrobot:~$ rostopic echo mpc_pub
data: -0.991534988484
---
data: -0.991686079847
---
data: -0.425914685439
---
data: 0.75012029663
---
data: 0.829224461171
---
data: 0.560167648007
---
可以看到所需要的控制变量会按照预测结果逐个输出。
总结
python调用osqp库编写模型预测控制的程序比C++要精简很多，而且库的调用操作也没有那么繁琐。如果愿意学习python的话，真心建议用python写模型预测控制。
293
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
