├── LICENSE ├── Misc ├── Ex3.gif └── stree.png ├── README.md ├── ThirdParty └── Robotics Toolbox for MATLAB │ └── plot_vehicle.m ├── data ├── car_figures │ ├── car_human.png │ ├── car_robot_b.png │ ├── car_robot_g.png │ ├── car_robot_o.png │ ├── car_robot_r.png │ ├── car_robot_y.png │ ├── human.png │ ├── human_2.png │ ├── human_red.png │ └── human_red_2.png └── params.mat ├── main.m └── util ├── getCEMPCcontrol.m ├── getCEMPChumancontrol.m ├── getSMPCcontrol.m ├── getiLQresults.m ├── iLQGames ├── costs │ ├── Cost.m │ ├── PlayerCost.m │ ├── ProductStateProximityCost.m │ ├── QuadraticCost.m │ ├── ReferenceDeviationCost.m │ └── SemiquadraticCost.m ├── dynamics │ ├── Bicycle4D.m │ ├── DubinsCar4D.m │ └── ProductMultiPlayerDynamicalSystem.m ├── geometry │ ├── LineSegment.m │ ├── Point.m │ └── Polyline.m ├── getiLQcontrol.m └── iLQSolver.m ├── node.m ├── plotSim.m ├── solveiLQgames.m ├── tree.m ├── updateEmpiricalCost.m ├── updateM.m └── updateTheta.m /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2022, Safe Robotics Lab 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /Misc/Ex3.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SafeRoboticsLab/Dual_Control_HRI/b58243ee028ec428dc351c250638e61b9dfbea4c/Misc/Ex3.gif -------------------------------------------------------------------------------- /Misc/stree.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SafeRoboticsLab/Dual_Control_HRI/b58243ee028ec428dc351c250638e61b9dfbea4c/Misc/stree.png -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Active Uncertainty Reduction for Human-Robot Interaction: An Implicit Dual Control Approach 2 | 3 | 4 | [![License][license-shield]][license-url] 5 | [![Homepage][homepage-shield]][homepage-url] 6 | 7 | 8 | 9 |
10 |

11 | 12 | Logo 13 | 14 | 15 |

Active Uncertainty Reduction for HRI

16 | 17 |

18 | Active Uncertainty Reduction for Human-Robot Interaction: An Implicit Dual Control Approach 19 | 30 |

31 |

32 | 33 | 34 | 35 | 36 |
37 |

Table of Contents

 38 |
    39 |
  1. About The Project
    2. 40 |
  2. Dependencies
    3. 41 |
  3. Example
    4. 42 |
  4. License
    5. 43 |
  5. Contact
    6. 44 |
  6. Paper
    7. 45 |
46 |
47 | 48 | 49 | 50 | 51 | ## About The Project 52 | 53 | We provide an MATLAB implementation of implicit dual control-based active uncertainty learning for autonomous driving applications, which can be found [here](https://github.com/SafeRoboticsLab/Dual_Control_HRI/tree/main). 54 | 55 | Click to watch our spotlight video: 56 | [![Watch the video](https://haiminhu.files.wordpress.com/2022/02/dual_control_hri_video_cover.png)](https://haiminhu.files.wordpress.com/2022/02/dual_control_hri.mp4) 57 | 58 | 59 | ## Dependencies 60 | 61 | #### Trajectory Optimization 62 | * [`MPT3`](https://www.mpt3.org/) (Toolbox for MPC and parametric optimization.) 63 | * [`SNOPT`](https://ccom.ucsd.edu/~optimizers/solvers/snopt/) (Nonlinear programming solver. Academic licenses are available.) 64 | 65 | #### Visualization 66 | * [`Robotics Toolbox for MATLAB`](https://petercorke.com/toolboxes/robotics-toolbox/) (Tools for plotting the vehicles. You need to first install it in MATLAB and then replace `plot_vehicle.m` in the root directory with [ours](https://github.com/SafeRoboticsLab/Dual_Control_HRI/blob/main/ThirdParty/Robotics%20Toolbox%20for%20MATLAB/plot_vehicle.m).) 67 | 68 | ## Example 69 | In this repository, we provide an example of our method applied for human-robot interactive driving scenarios. 70 | 71 | 1. Clone the repo 72 | ```sh 73 | git clone https://github.com/SafeRoboticsLab/Dual_Control_HRI.git 74 | ``` 75 | 2. In MATLAB, run the [`main.m`](https://github.com/SafeRoboticsLab/Dual_Control_HRI/blob/main/main.m) script to reproduce our results. 76 | 77 | 84 | 85 | 86 | 91 | 92 | 93 | 104 | 105 | 106 | 107 | ## License 108 | 109 | Distributed under the BSD 3-Clause License. See `LICENSE` for more information. 110 | 111 | 112 | 113 | 114 | ## Contact 115 | 116 | Haimin Hu - [@HaiminHu](https://twitter.com/HaiminHu) - haiminh@princeton.edu 117 | 118 | Project Link: [https://github.com/SafeRoboticsLab/Dual_Control_HRI](https://github.com/SafeRoboticsLab/Dual_Control_HRI) 119 | 120 | Homepage Link: [https://haiminhu.org/dual_control_hri](https://haiminhu.org/dual_control_hri) 121 | 122 | 123 | 124 | ## Papers 125 | ### Journal paper: 126 | ```tex 127 | @article{hu2023aur, 128 | title={Active uncertainty reduction for safe and efficient interaction planning: A shielding-aware dual control approach}, 129 | author={Hu, Haimin and Isele, David and Bae, Sangjae and Fisac, Jaime F}, 130 | journal={The International Journal of Robotics Research}, 131 | pages={02783649231215371}, 132 | year={2023}, 133 | publisher={SAGE Publications Sage UK: London, England} 134 | } 135 | ``` 136 | 137 | 138 | 139 | ### Conference paper/book chapter: 140 | ```tex 141 | @inproceedings{hu2023active, 142 | title={Active Uncertainty Reduction for Human-Robot Interaction: An Implicit Dual Control Approach}, 143 | author={Hu, Haimin and Fisac, Jaime F}, 144 | booktitle={Algorithmic Foundations of Robotics XV}, 145 | pages={385--401}, 146 | year={2023}, 147 | publisher={Springer International Publishing} 148 | } 149 | ``` 150 | 151 | ### Our prior work: 152 | 153 | ```tex 154 | @article{hu2022sharp, 155 | author={Hu, Haimin and Nakamura, Kensuke and Fisac, Jaime F.}, 156 | journal={IEEE Robotics and Automation Letters}, 157 | title={SHARP: Shielding-Aware Robust Planning for Safe and Efficient Human-Robot Interaction}, 158 | year={2022}, 159 | volume={7}, 160 | number={2}, 161 | pages={5591-5598}, 162 | doi={10.1109/LRA.2022.3155229} 163 | } 164 | ``` 165 | 166 | 167 | 168 | [contributors-shield]: https://img.shields.io/github/contributors/SafeRoboticsLab/repo.svg?style=for-the-badge 169 | [contributors-url]: https://github.com/SafeRoboticsLab/Dual_Control_HRI/contributors 170 | [forks-shield]: https://img.shields.io/github/forks/SafeRoboticsLab/repo.svg?style=for-the-badge 171 | [forks-url]: https://github.com/SafeRoboticsLab/Dual_Control_HRI/network/members 172 | [stars-shield]: https://img.shields.io/github/stars/SafeRoboticsLab/repo.svg?style=for-the-badge 173 | [stars-url]: https://github.com/SafeRoboticsLab/Dual_Control_HRI/stargazers 174 | [issues-shield]: https://img.shields.io/github/issues/SafeRoboticsLab/repo.svg?style=for-the-badge 175 | [issues-url]: https://github.com/SafeRoboticsLab/Dual_Control_HRI/issues 176 | [license-shield]: https://img.shields.io/badge/License-BSD%203--Clause-blue.svg 177 | [license-url]: https://opensource.org/licenses/BSD-3-Clause 178 | [linkedin-shield]: https://img.shields.io/badge/-LinkedIn-black.svg?style=for-the-badge&logo=linkedin&colorB=555 179 | [linkedin-url]: https://linkedin.com/in/SafeRoboticsLab 180 | [homepage-shield]: https://img.shields.io/badge/-Homepage-brightgreen 181 | [homepage-url]: https://haiminhu.org/dual_control_hri 182 | -------------------------------------------------------------------------------- /ThirdParty/Robotics Toolbox for MATLAB/plot_vehicle.m: -------------------------------------------------------------------------------- 1 | %plot_vehicle Draw mobile robot pose 2 | % 3 | % PLOT_VEHICLE(X,OPTIONS) draws an oriented triangle to represent the pose 4 | % of a mobile robot moving in a planar world. The pose X (1x3) = [x,y,theta]. 5 | % If X is a matrix (Nx3) then an animation of the robot motion is shown and 6 | % animated at the specified frame rate. 7 | % 8 | % Image mode:: 9 | % 10 | % Create a structure with the following elements and pass it with the 11 | % 'model' option. 12 | % 13 | % image an RGB image (HxWx3) 14 | % alpha an alpha plane (HxW) with pixels 0 if transparent 15 | % rotation image rotation in degrees required for front to pointing to the right 16 | % centre image coordinate (U,V) of the centre of the back axle 17 | % length length of the car in real-world units 18 | % 19 | % Animation mode:: 20 | % 21 | % H = PLOT_VEHICLE(X,OPTIONS) as above draws the robot and returns a handle. 22 | % 23 | % PLOT_VEHICLE(X, 'handle', H) updates the pose X (1x3) of the previously drawn 24 | % robot. 25 | % 26 | % Options:: 27 | % 'scale',S draw vehicle with length S x maximum axis dimension (default 28 | % 1/60) 29 | % 'size',S draw vehicle with length S 30 | % 'fillcolor',F the color of the circle's interior, MATLAB color spec 31 | % 'alpha',A transparency of the filled circle: 0=transparent, 1=solid 32 | % 'box' draw a box shape (default is triangle) 33 | % 'fps',F animate at F frames per second (default 10) 34 | % 'image',I use an image to represent the robot pose 35 | % 'retain' when X (Nx3) then retain the robots, leaving a trail 36 | % 'model',M animate an image of the vehicle. M is a structure with 37 | % elements: image, alpha, rotation (deg), centre (pix), length (m). 38 | % 'axis',h handle of axis or UIAxis to draw into (default is current axis) 39 | % 'movie',M create a movie file in file M 40 | % 41 | % Example:: 42 | % [car.image,~,car.alpha] = imread('car2.png'); % image and alpha layer 43 | % car.rotation = 180; % image rotation to align front with world x-axis 44 | % car.centre = [648; 173]; % image coordinates of centre of the back wheels 45 | % car.length = 4.2; % real world length for scaling (guess) 46 | % h = plot_vehicle(x, 'model', car) % draw car at configuration x 47 | % plot_vehicle(x, 'handle', h) % animate car to configuration x 48 | % 49 | % Notes:: 50 | % - The vehicle is drawn relative to the size of the axes, so set them 51 | % first using axis(). 52 | % - For backward compatibility, 'fill', is a synonym for 'fillcolor' 53 | % - For the 'model' option, you provide a monochrome or color image of the 54 | % vehicle. Optionally you can provide an alpha mask (0=transparent). 55 | % Specify the reference point on the vehicle as the (x,y) pixel 56 | % coordinate within the image. Specify the rotation, in degrees, so that 57 | % the car's front points to the right. Finally specify a length of the 58 | % car, the image is scaled to be that length in the plot. 59 | % - Set 'fps' to Inf to have zero pause 60 | % 61 | % See also Vehicle.plot, Animate, plot_poly, demos/car_animation 62 | 63 | 64 | % Copyright (C) 1993-2017, by Peter I. Corke 65 | % 66 | % This file is part of The Robotics Toolbox for MATLAB (RTB). 67 | % 68 | % RTB is free software: you can redistribute it and/or modify 69 | % it under the terms of the GNU Lesser General Public License as published by 70 | % the Free Software Foundation, either version 3 of the License, or 71 | % (at your option) any later version. 72 | % 73 | % RTB is distributed in the hope that it will be useful, 74 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 75 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 76 | % GNU Lesser General Public License for more details. 77 | % 78 | % You should have received a copy of the GNU Leser General Public License 79 | % along with RTB. If not, see . 80 | % 81 | % http://www.petercorke.com 82 | 83 | % TODO needs to work for 3D point 84 | 85 | function [h_, hDraw_] = plot_vehicle(x, varargin) 86 | 87 | opt.scale = 1/60; 88 | opt.size = []; 89 | opt.shape = {'triangle', 'box'}; 90 | opt.fps = 20; 91 | opt.handle = []; 92 | opt.image = []; 93 | opt.model = []; 94 | opt.retain = false; 95 | opt.axis = []; 96 | opt.trail = ''; 97 | opt.movie = []; 98 | 99 | if numel(x) == 3 100 | x = x(:)'; % enforce row vector 101 | end 102 | 103 | [opt,args] = tb_optparse(opt, varargin); 104 | 105 | hDraw = []; 106 | 107 | if isempty(opt.handle) 108 | % create a new robot 109 | % compute some default dimensions based on axis scaling 110 | if isempty(opt.axis) 111 | opt.axis = gca; 112 | end 113 | 114 | ax = opt.axis; 115 | a = [ax.XLim ax.YLim]; 116 | d = (a(2)+a(4) - a(1)-a(3)) * opt.scale; 117 | 118 | % create the robot 119 | [h.vehicle, hDraw] = draw_robot(d, opt, args); 120 | 121 | if ~isempty(opt.trail) 122 | hold on 123 | h.trail = plot(0,0, opt.trail); 124 | hold off 125 | end 126 | h.opt = opt; 127 | 128 | plot_vehicle(x, 'handle', h, varargin{:}); 129 | else 130 | % we are animating 131 | h = opt.handle; 132 | anim = Animate(opt.movie); 133 | 134 | for i=1:numrows(x) 135 | h.vehicle.Matrix = SE2(x(i,:)).SE3.T; % convert (x,y,th) to SE(3) 136 | 137 | % check if retain is on 138 | if h.opt.retain 139 | plot_vehicle(x(i,:)); 140 | end 141 | 142 | % extend the line if required 143 | if ~isempty(h.opt.trail) 144 | 145 | l = h.trail; 146 | l.XData = [l.XData x(i,1)]; 147 | l.YData = [l.YData x(i,2)]; 148 | end 149 | 150 | % pause a while 151 | anim.add(); 152 | if ~isinf(h.opt.fps) 153 | pause(1/h.opt.fps) 154 | end 155 | end 156 | anim.close(); 157 | end 158 | 159 | if nargout > 0 160 | h_ = h; 161 | hDraw_ = hDraw; 162 | end 163 | 164 | end 165 | 166 | function [h, hDraw] = draw_robot(d, opt, args) 167 | 168 | if ~isempty(opt.model) 169 | % display an image of a vehicle, pass in a struct 170 | if isstruct(opt.model) 171 | img = opt.model.image; 172 | rotation = opt.model.rotation; 173 | centre = opt.model.centre; 174 | scale = opt.model.length / max(numcols(img), numrows(img)) ; 175 | else 176 | img = opt.image; 177 | centre = [numcols(img)/2 numrows(img)/2]; 178 | end 179 | h = hgtransform('Tag', 'image'); 180 | h2 = hgtransform('Matrix', trotz(rotation, 'deg')*trscale(scale)*transl(-centre(1), -centre(2),0), 'Parent', h ); 181 | if isfield(opt.model, 'alpha') 182 | % use alpha data if provided 183 | alpha = opt.model.alpha; 184 | else 185 | % otherwise black pixels (0,0,0) are set to transparent 186 | alpha = any(img>0,3); 187 | end 188 | hDraw = image(img, 'AlphaData', alpha, 'Parent', h2); 189 | %axis equal 190 | else 191 | % display a simple polygon 192 | switch opt.shape 193 | case 'triangle' 194 | if ~isempty(opt.size) 195 | d = opt.size; 196 | end 197 | L = d; W = 0.6*d; 198 | corners = [ 199 | L 0 200 | -L -W 201 | -L W]'; 202 | case 'box' 203 | if ~isempty(opt.size) 204 | switch length(opt.size) 205 | case 1 206 | W = opt.size/2; L1 = opt.size/2; L2 = opt.size/2; 207 | case 2 208 | W = opt.size(1)/2; L1 = opt.size(2)/2; L2 = opt.size(2)/2; 209 | case 3 210 | W = opt.size(1)/2; L1 = opt.size(2); L2 = opt.size(3); 211 | end 212 | else 213 | L1 = d; L2 = d; W = 0.6*d; 214 | end 215 | corners = [ 216 | -L1 W 217 | 0.6*L2 W 218 | L2 0.5*W 219 | L2 -0.5*W 220 | 0.6*L2 -W 221 | -L1 -W ]'; 222 | end 223 | h = plot_poly(corners, 'animate', 'axis', opt.axis, args{:}); 224 | end 225 | end 226 | -------------------------------------------------------------------------------- /data/car_figures/car_human.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SafeRoboticsLab/Dual_Control_HRI/b58243ee028ec428dc351c250638e61b9dfbea4c/data/car_figures/car_human.png -------------------------------------------------------------------------------- /data/car_figures/car_robot_b.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SafeRoboticsLab/Dual_Control_HRI/b58243ee028ec428dc351c250638e61b9dfbea4c/data/car_figures/car_robot_b.png -------------------------------------------------------------------------------- /data/car_figures/car_robot_g.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SafeRoboticsLab/Dual_Control_HRI/b58243ee028ec428dc351c250638e61b9dfbea4c/data/car_figures/car_robot_g.png -------------------------------------------------------------------------------- /data/car_figures/car_robot_o.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SafeRoboticsLab/Dual_Control_HRI/b58243ee028ec428dc351c250638e61b9dfbea4c/data/car_figures/car_robot_o.png -------------------------------------------------------------------------------- /data/car_figures/car_robot_r.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SafeRoboticsLab/Dual_Control_HRI/b58243ee028ec428dc351c250638e61b9dfbea4c/data/car_figures/car_robot_r.png -------------------------------------------------------------------------------- /data/car_figures/car_robot_y.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SafeRoboticsLab/Dual_Control_HRI/b58243ee028ec428dc351c250638e61b9dfbea4c/data/car_figures/car_robot_y.png -------------------------------------------------------------------------------- /data/car_figures/human.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SafeRoboticsLab/Dual_Control_HRI/b58243ee028ec428dc351c250638e61b9dfbea4c/data/car_figures/human.png -------------------------------------------------------------------------------- /data/car_figures/human_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SafeRoboticsLab/Dual_Control_HRI/b58243ee028ec428dc351c250638e61b9dfbea4c/data/car_figures/human_2.png -------------------------------------------------------------------------------- /data/car_figures/human_red.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SafeRoboticsLab/Dual_Control_HRI/b58243ee028ec428dc351c250638e61b9dfbea4c/data/car_figures/human_red.png -------------------------------------------------------------------------------- /data/car_figures/human_red_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SafeRoboticsLab/Dual_Control_HRI/b58243ee028ec428dc351c250638e61b9dfbea4c/data/car_figures/human_red_2.png -------------------------------------------------------------------------------- /data/params.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SafeRoboticsLab/Dual_Control_HRI/b58243ee028ec428dc351c250638e61b9dfbea4c/data/params.mat -------------------------------------------------------------------------------- /main.m: -------------------------------------------------------------------------------- 1 | close all; clear all; clc 2 | addpath(genpath('./util')) 3 | addpath('./data') 4 | setenv('SNOPT_LICENSE','/Users/haiminhu/licenses/snopt7.lic'); %**REPLACE** 5 | % 6 | % Main simulation script of Implicit Dual Control for HRI 7 | % 8 | % Author: Haimin Hu (last modified 2022.2.13) 9 | 10 | 11 | %% Static paramters 12 | load ./data/params 13 | num_players = 2; % Number of players 14 | ts = 0.2; % Sampling time 15 | vDes_H = 12; % Human's desired cruising speed 16 | vDes_R = vDes_H + 3; % Robot's desired cruising speed 17 | lc = params.rd_bd_max/2; % Lane center position (lateral) 18 | 19 | % Horizons. 20 | N_ilq = 15; % iLQGame horizon 21 | N_sim = 80; % Maximum simulation horizon 22 | Nd = 2; % Number of dual control steps. 23 | Ne = 2; % Number of exploitation steps. 24 | 25 | % Inital states. 26 | xH_init = [15; lc; 0.0; 10]; 27 | xR_init = [0.0; -lc; 0.0; 10]; 28 | x_init = [xH_init; xR_init]; 29 | 30 | % Target states. 31 | % -> Human. 32 | xF_H_l = [0; 1.2; 0; vDes_H; 0; 0; 0; 0]; % Left lane 33 | xF_H_r = [0; -0.8; 0; vDes_H; 0; 0; 0; 0]; % Right lane 34 | uF_H = zeros(2,1); 35 | 36 | % -> Robot. 37 | xF_R = [0; 0; 0; 0; params.xr_tar_overtake; lc; 0; vDes_R]; 38 | uF_R = zeros(2,1); 39 | 40 | % Control limits. 41 | uH_ub = [2; 0.05]; % Human 42 | uH_lb = -uH_ub; 43 | uR_ub = [4; 0.05]; % Robot 44 | uR_lb = -uR_ub; 45 | 46 | uH_constraints.max = uH_ub; 47 | uH_constraints.min = uH_lb; 48 | uR_constraints.max = uR_ub; 49 | uR_constraints.min = uR_lb; 50 | 51 | u_constraints_cell = {uH_constraints; uR_constraints}; 52 | 53 | % Cost matrices. 54 | % -> Human. 55 | QH = blkdiag(0,5,1,1,0,0,0,0); 56 | RH = blkdiag(0.1,1); 57 | 58 | % -> Robot. 59 | QR_OT = 1*[ 1,0,0,0,-1,0,0,0; zeros(1,8); zeros(1,8); zeros(1,8); 60 | -1,0,0,0, 1,0,0,0; zeros(1,8); zeros(1,8); zeros(1,8)]; 61 | QR_x = blkdiag(0,0,0,0,0,2,1,1); 62 | QR = QR_OT + QR_x; 63 | RR = blkdiag(0.1,1); 64 | 65 | % Covariance of external disturbance d. 66 | Sigma_d = blkdiag(0.1,0.1,0.1,0.1, 0.1,0.1,0.1,0.1); 67 | 68 | % Verbose settings. 69 | verbose_iLQ = false; 70 | verbose_CEMPC = true; 71 | verbose_SMPC = true; 72 | 73 | 74 | %% iLQGame Setup - Focused Human 75 | % Set up iLQSolvers. 76 | alpha_scaling = 0.01; 77 | max_iteration = 100; 78 | tolerence_percentage = 1e-3; 79 | 80 | % Dynamical systems. 81 | Car_H = Bicycle4D(2.7, ts); % Human (Player 1) 82 | H_id = 1; 83 | Car_R = Bicycle4D(2.7, ts); % Robot (Player 2) 84 | R_id = 2; 85 | xH_dim = Car_H.x_dim; 86 | xR_dim = Car_R.x_dim; 87 | x_dim = xR_dim + xH_dim; 88 | xH_dims = 1:4; 89 | xR_dims = 5:8; 90 | x_dims = {xH_dims, xR_dims}; 91 | 92 | uH_dim = Car_H.u_dim; 93 | uR_dim = Car_R.u_dim; 94 | u_dim = uR_dim + uH_dim; 95 | uH_dims = 1:2; 96 | uR_dims = 3:4; 97 | u_dims = {uH_dims, uR_dims}; 98 | 99 | jointSys = ProductMultiPlayerDynamicalSystem(ts, x_dim, x_dims,... 100 | u_dim, u_dims, {Car_H, Car_R}); 101 | 102 | % Initialize Ps and alphas. 103 | Ps_cell = cell(num_players, N_ilq); 104 | alphas_cell = cell(num_players, N_ilq); 105 | for i = 1:num_players 106 | for k = 1:N_ilq 107 | Ps_cell{i,k} = zeros(jointSys.subsystems{i}.u_dim,x_dim); 108 | alphas_cell{i,k} = zeros(jointSys.subsystems{i}.u_dim,1); 109 | end 110 | end 111 | 112 | % -------------------- Set up Human's (Player 1) costs -------------------- 113 | % -> Tracking and control cost. 114 | RefDevCost_H_l = ReferenceDeviationCost(xF_H_l, uF_H, QH, RH,... 115 | "RefDevCost_H_l", x_dim, uH_dim); % left lane 116 | RefDevCost_H_r = ReferenceDeviationCost(xF_H_r, uF_H, QH, RH,... 117 | "RefDevCost_H_r", x_dim, uH_dim); % right lane 118 | 119 | % -> Proximity cost. 120 | px_indices = [1,5]; 121 | py_indices = [2,6]; 122 | max_distance_H = 5; 123 | ProxCost_H = ProductStateProximityCost(px_indices, py_indices,... 124 | max_distance_H, "ProxCost_H", x_dim, uH_dim); 125 | 126 | % -> Lane boundary cost. 127 | pyCost_upper_H = SemiquadraticCost(2, 0.7*params.rd_bd_max, true,... 128 | 'state', "pyCost_upper_H", x_dim, uH_dim); 129 | pyCost_lower_H = SemiquadraticCost(2, 0.7*params.rd_bd_min, false,... 130 | 'state', "pyCost_lower_H", x_dim, uH_dim); 131 | 132 | % -> Velocity constraint cost. 133 | vCost_upper_H = SemiquadraticCost(4, vDes_H*1.1, true, 'state',... 134 | "vCost_upper_H", x_dim, uH_dim); 135 | vCost_lower_H = SemiquadraticCost(4, vDes_R/1.5, false, 'state',... 136 | "vCost_lower_H", x_dim, uH_dim); 137 | 138 | % -------------------- Set up Robot's (Player 2) costs -------------------- 139 | % -> Tracking and control cost. 140 | RefDevCost_R = ReferenceDeviationCost(xF_R, uF_R, QR, RR,... 141 | "RefDevCostF_R", x_dim, uR_dim); 142 | 143 | % -> Proximity cost. 144 | max_distance_R = 7; 145 | ProxCost_R = ProductStateProximityCost(px_indices, py_indices,... 146 | max_distance_R, "ProxCost_R", x_dim, uR_dim); 147 | 148 | % -> Lane boundary cost. 149 | pyCost_upper_R = SemiquadraticCost(6, 0.7*params.rd_bd_max, true,... 150 | 'state', "pyCost_upper_R", x_dim, uR_dim); 151 | pyCost_lower_R = SemiquadraticCost(6, 0.7*params.rd_bd_min, false,... 152 | 'state', "pyCost_lower_R", x_dim, uR_dim); 153 | 154 | % -> Velocity constraint cost. 155 | vCost_upper_R = SemiquadraticCost(8, vDes_R*1.2, true, 'state',... 156 | "vCost_upper_R", x_dim, uR_dim); 157 | vCost_lower_R = SemiquadraticCost(8, vDes_R/1.5, false, 'state',... 158 | "vCost_lower_R", x_dim, uR_dim); 159 | 160 | % ----------- Set up (time-varying) total costs for each player ----------- 161 | % Build up total costs for H. 162 | Car_H_cost = PlayerCost(); 163 | Car_H_cost = Car_H_cost.add_cost(ProxCost_H, 'x', 1e4); 164 | Car_H_cost = Car_H_cost.add_cost(vCost_lower_H, 'x', 4e2); 165 | Car_H_cost = Car_H_cost.add_cost(pyCost_upper_H, 'x', 5e4); 166 | Car_H_cost = Car_H_cost.add_cost(pyCost_lower_H, 'x', 5e4); 167 | Car_H_cost_l = Car_H_cost.add_cost(RefDevCost_H_l, 'xu', 1); 168 | Car_H_cost_r = Car_H_cost.add_cost(RefDevCost_H_r, 'xu', 1); 169 | 170 | % Build up total costs for R. 171 | Car_R_cost = PlayerCost(); 172 | Car_R_cost = Car_R_cost.add_cost(ProxCost_R, 'x', 1e4); 173 | % Car_R_cost = Car_R_cost.add_cost(vCost_upper_R, 'x', 4e2); 174 | % Car_R_cost = Car_R_cost.add_cost(vCost_lower_R, 'x', 4e2); 175 | Car_R_cost = Car_R_cost.add_cost(pyCost_upper_R, 'x', 5e4); 176 | Car_R_cost = Car_R_cost.add_cost(pyCost_lower_R, 'x', 5e4); 177 | Car_R_cost = Car_R_cost.add_cost(RefDevCost_R, 'xu', 1); 178 | 179 | % Set up cost cells. 180 | player_costs_cell_l = cell(num_players, N_ilq); 181 | for k = 1:N_ilq 182 | player_costs_cell_l{H_id, k} = Car_H_cost_l; 183 | player_costs_cell_l{R_id, k} = Car_R_cost; 184 | end 185 | 186 | player_costs_cell_r = cell(num_players, N_ilq); 187 | for k = 1:N_ilq 188 | player_costs_cell_r{H_id, k} = Car_H_cost_r; 189 | player_costs_cell_r{R_id, k} = Car_R_cost; 190 | end 191 | 192 | 193 | %% iLQGame Setup - Distracted Human 194 | jointSys_DH = ProductMultiPlayerDynamicalSystem(ts, xH_dim, x_dims,... 195 | uH_dim, u_dims, {Car_H}); 196 | 197 | % Initialize Ps and alphas. 198 | Ps_cell_EH = cell(1, N_ilq); 199 | alphas_cell_EH = cell(1, N_ilq); 200 | for k = 1:N_ilq 201 | Ps_cell_EH{1,k} = zeros(jointSys.subsystems{1}.u_dim, xH_dim); 202 | alphas_cell_EH{1,k} = zeros(jointSys.subsystems{1}.u_dim, 1); 203 | end 204 | 205 | % Tracking and control cost. 206 | RefDevCost_EH_l = ReferenceDeviationCost(xF_H_l(xH_dims), uF_H,... 207 | QH(xH_dims,xH_dims), RH, "RefDevCost_EH_l", xH_dim, uH_dim); 208 | RefDevCost_EH_r = ReferenceDeviationCost(xF_H_r(xH_dims), uF_H,... 209 | QH(xH_dims,xH_dims), RH, "RefDevCost_EH_r", xH_dim, uH_dim); 210 | 211 | % Lane boundary cost. 212 | pyCost_upper_EH = SemiquadraticCost(2, 0.7*params.rd_bd_max, true,... 213 | 'state', "pyCost_upper_EH", xH_dim, uH_dim); 214 | pyCost_lower_EH = SemiquadraticCost(2, 0.7*params.rd_bd_min, false,... 215 | 'state', "pyCost_lower_EH", xH_dim, uH_dim); 216 | 217 | % Build up total costs. 218 | Car_H_cost_EH = PlayerCost(); 219 | Car_H_cost_EH = Car_H_cost_EH.add_cost(pyCost_upper_EH, 'x', 5e3); 220 | Car_H_cost_EH = Car_H_cost_EH.add_cost(pyCost_lower_EH, 'x', 5e3); 221 | Car_H_cost_EH_l = Car_H_cost_EH.add_cost(RefDevCost_EH_l, 'xu', 1); 222 | Car_H_cost_EH_r = Car_H_cost_EH.add_cost(RefDevCost_EH_r, 'xu', 1); 223 | 224 | % Set up cost cells. 225 | player_costs_cell_EH_l = cell(1, N_ilq); 226 | for k = 1:N_ilq 227 | player_costs_cell_EH_l{1,k} = Car_H_cost_EH_l; 228 | end 229 | 230 | player_costs_cell_EH_r = cell(1, N_ilq); 231 | for k = 1:N_ilq 232 | player_costs_cell_EH_r{1,k} = Car_H_cost_EH_r; 233 | end 234 | 235 | 236 | %% Main Planning loop 237 | 238 | % ===================== Choose your planner ===================== 239 | method = 'IDSMPC'; % 'IDSMPC', 'EDSMPC', 'NDSMPC', 'CEMPC', 'iLQ' 240 | % =============================================================== 241 | 242 | % Ground-truth human's hidden state. 243 | theta = 1.0; % [0,1] 244 | M = 'r'; % {'l', 'r'} 245 | 246 | % Prior distribution of theta. 247 | mu_theta_init = [0.5; 0.5]; 248 | Sigma_theta_init = 5*eye(2); 249 | theta_distr.l.mu = mu_theta_init; 250 | theta_distr.l.covar = Sigma_theta_init; 251 | theta_distr.r.mu = mu_theta_init; 252 | theta_distr.r.covar = Sigma_theta_init; 253 | 254 | % Prior distribution of M. 255 | M_distr = [0.5; 0.5]; 256 | 257 | % LQR terminal cost design. 258 | [AF_R, BF_R] = Car_R.linearizeDiscrete(xF_R(xR_dims), [0;0]); 259 | [~, PR] = dlqr(AF_R, BF_R, QR(xR_dims,xR_dims), RR); 260 | 261 | % Create the planner object. 262 | planner.ts = ts; 263 | planner.vDes_H = vDes_H; 264 | planner.vDes_R = vDes_R; 265 | planner.lc = lc; 266 | planner.params = params; 267 | planner.RH = RH; 268 | planner.alpha_scaling = alpha_scaling; 269 | planner.H_id = H_id; 270 | planner.R_id = R_id; 271 | planner.xH_dim = xH_dim; 272 | planner.xR_dim = xR_dim; 273 | planner.xH_dims = xH_dims; 274 | planner.xR_dims = xR_dims; 275 | planner.uH_dim = uH_dim; 276 | planner.uR_dim = uR_dim; 277 | planner.uH_dims = uH_dims; 278 | planner.uR_dims = uR_dims; 279 | planner.jointSys = jointSys; 280 | planner.M_distr_const = [0.5; 0.5]; 281 | planner.M_eps = 0.01; 282 | 283 | % MPC settings. 284 | extraArg_CEMPC.w_uH = 1e8; % human's control consistency weight 285 | extraArg_CEMPC.w_RB_l = 1e6; % road boundary (linear penalty) 286 | extraArg_CEMPC.w_RB_q = 5e6; % road boundary (quadratic penalty) 287 | extraArg_CEMPC.w_CA_l = 1e7; % collision avoidance (linear penalty) 288 | extraArg_CEMPC.w_CA_q = 5e7; % collision avoidance (quadratic penalty) 289 | 290 | extraArg_SMPC.M_branch = 1; % If M branch 291 | extraArg_SMPC.N_M = min(1, Nd); % M branching horizon 292 | extraArg_SMPC.M_dual = and(1, extraArg_SMPC.M_branch); % If M dual control 293 | extraArg_SMPC.theta_sample_sn = 0.3; % Sampling parameter of theta 294 | 295 | extraArg_SMPC.scale_ellips = 1.2; % Collision avoidance ellipsoid scaling 296 | extraArg_SMPC.w_uH = extraArg_CEMPC.w_uH; 297 | extraArg_SMPC.w_RB_l = extraArg_CEMPC.w_RB_l; 298 | extraArg_SMPC.w_RB_q = extraArg_CEMPC.w_RB_q; 299 | extraArg_SMPC.w_CA_l = extraArg_CEMPC.w_CA_l; 300 | extraArg_SMPC.w_CA_q = extraArg_CEMPC.w_CA_q; 301 | 302 | % Initialize simulation. 303 | x = x_init; 304 | X = x; 305 | U = []; 306 | J_em = 0; % Empirical cost of the robot 307 | THETA = theta_distr; % theta trajectory 308 | M_traj = M_distr; % M trajectory 309 | 310 | % Main loop. 311 | for t = 1:N_sim 312 | 313 | % Change human's hidden states 314 | if t >= 10 315 | theta = 0.0; % [0,1] 316 | M = 'l'; % 'l', 'r' 317 | end 318 | 319 | % Check goal reaching. 320 | if x(5) - x(1) >= params.xr_tar_overtake 321 | break 322 | end 323 | 324 | % Report simulation time. 325 | disp(['Simulation time: t = ', num2str(t)]) 326 | 327 | % ========================== Solve iLQGames =========================== 328 | ilq_solvers = solveiLQgames(x, xH_dims, jointSys, jointSys_DH,... 329 | Ps_cell, Ps_cell_EH, alphas_cell, alphas_cell_EH,... 330 | player_costs_cell_l, player_costs_cell_r,... 331 | player_costs_cell_EH_l, player_costs_cell_EH_r,... 332 | u_constraints_cell, alpha_scaling, max_iteration,... 333 | tolerence_percentage, verbose_iLQ); 334 | 335 | % ============================ Robot Block ============================ 336 | if strcmp(method, 'iLQ') 337 | % ISA-iLQ. 338 | if M_distr(1) >= M_distr(2) 339 | u_iLQ = ilq_solvers.AH_l.best_operating_point.us(:,1); 340 | else 341 | u_iLQ = ilq_solvers.AH_r.best_operating_point.us(:,1); 342 | end 343 | uR = u_iLQ(uR_dims); 344 | else 345 | % CEMPC. 346 | [uR_ce, xR_ce] = getCEMPCcontrol(x, xF_R, Nd + Ne, QR, RR, PR,... 347 | uR_constraints, uH_constraints, ilq_solvers, theta_distr,... 348 | M_distr, planner, extraArg_CEMPC, verbose_CEMPC); 349 | 350 | if strcmp(method, 'CEMPC') 351 | uR = uR_ce(:,1); 352 | 353 | elseif strcmp(method, 'NDSMPC') || strcmp(method, 'IDSMPC') ||... 354 | strcmp(method, 'EDSMPC') 355 | 356 | % Warm-start NDSMPC. 357 | extraArg_SMPC.uR_ws = uR_ce; 358 | extraArg_SMPC.xR_ws = xR_ce; 359 | extraArg_SMPC.opt_mode = 'ws'; 360 | [uR_ws, ~, ~, stree_ws, flag_ws] = getSMPCcontrol(x, xF_R,... 361 | theta_distr, M_distr, Nd, Ne, QR, RR, PR, Sigma_d,... 362 | uR_constraints, uH_constraints, ilq_solvers, planner,... 363 | verbose_SMPC, extraArg_SMPC); 364 | 365 | if strcmp(method, 'IDSMPC') 366 | % Implicit Dual SMPC. 367 | if flag_ws 368 | % Warm-start SMPC failed. 369 | uR = uR_ce(:,1); 370 | else 371 | % Implicit Dual SMPC. 372 | extraArg_SMPC.opt_mode = 'std'; 373 | extraArg_SMPC.fixed_values = 1; 374 | extraArg_SMPC.stree = stree_ws; 375 | [uR, ~, ~, stree, ~] = getSMPCcontrol(x, xF_R,... 376 | theta_distr, M_distr, Nd, Ne, QR, RR, PR,... 377 | Sigma_d, uR_constraints, uH_constraints,... 378 | ilq_solvers, planner, verbose_SMPC, extraArg_SMPC); 379 | end 380 | 381 | elseif strcmp(method, 'EDSMPC') 382 | % Explicit Dual SMPC. 383 | if flag_ws 384 | % Warm-start SMPC failed. 385 | uR = uR_ce(:,1); 386 | else 387 | % Explicit Dual SMPC. 388 | extraArg_SMPC.opt_mode = 'std'; 389 | extraArg_SMPC.fixed_values = 1; 390 | extraArg_SMPC.stree = stree_ws; 391 | extraArg_SMPC.weight_IG = 1e10; %(1e12) 392 | [uR, ~, ~, stree, ~] = getSMPCcontrol(x, xF_R,... 393 | theta_distr, M_distr, Nd, Ne, QR, RR, PR,... 394 | Sigma_d, uR_constraints, uH_constraints,... 395 | ilq_solvers, planner, verbose_SMPC,... 396 | extraArg_SMPC); 397 | end 398 | 399 | elseif strcmp(method, 'NDSMPC') 400 | % NDSMPC. 401 | if flag_ws 402 | % Warm-start SMPC failed. 403 | uR = uR_ce(:,1); 404 | else 405 | uR = uR_ws; 406 | end 407 | end 408 | else 409 | error('Invalid planner option!') 410 | end 411 | end 412 | 413 | % ============================ Human Block ============================ 414 | % Human control basis functions. 415 | if M == 'l' 416 | u_FH = ilq_solvers.AH_l.best_operating_point.us(:,1); 417 | u_DH = ilq_solvers.EH_l.best_operating_point.us(:,1); 418 | elseif M == 'r' 419 | u_FH = ilq_solvers.AH_r.best_operating_point.us(:,1); 420 | u_DH = ilq_solvers.EH_r.best_operating_point.us(:,1); 421 | end 422 | 423 | % Rational human action. 424 | uH_rat = theta*u_FH(uH_dims) + (1-theta)*u_DH(uH_dims); 425 | 426 | % Randomize human action. 427 | eps_noise = [0.2; 0.005]; 428 | uH = uH_rat + eps_noise.*(1-2*rand(2,1)); 429 | 430 | % Project uH back to its bound. 431 | uH = max(min(uH,uH_ub),uH_lb); 432 | 433 | % ========================= Update Simulation ========================= 434 | u = [uH; uR]; 435 | x_next = jointSys.integrate(x, u, ts); 436 | 437 | % Check collision. 438 | xR = x_next(xR_dims); 439 | xH = x_next(xH_dims); 440 | coll_flag = is_collision(xR, xH, params); 441 | if coll_flag 442 | disp('Collision!') 443 | end 444 | 445 | % ======================== Bayesian Inference ========================= 446 | [A_now, B_cell_now] = jointSys.linearizeDiscrete_B_cell(x, u); 447 | f_x_now = A_now * x; 448 | iLQ_result = getiLQresults(x, B_cell_now, ilq_solvers, planner, 1); 449 | 450 | % theta inference. 451 | [mu_theta_l, Sigma_theta_l] = updateTheta('l', theta_distr.l.mu,... 452 | theta_distr.l.covar, Sigma_d, x_next, u, f_x_now, B_cell_now,... 453 | iLQ_result, planner, []); 454 | mu_theta_l = mu_theta_l ./ sum(mu_theta_l); 455 | theta_distr.l.mu = mu_theta_l; 456 | theta_distr.l.covar = Sigma_theta_l; 457 | 458 | [mu_theta_r, Sigma_theta_r] = updateTheta('r', theta_distr.r.mu,... 459 | theta_distr.r.covar, Sigma_d, x_next, u, f_x_now, B_cell_now,... 460 | iLQ_result, planner, []); 461 | mu_theta_r = mu_theta_r ./ sum(mu_theta_r); 462 | theta_distr.r.mu = mu_theta_r; 463 | theta_distr.r.covar = Sigma_theta_r; 464 | 465 | % M inference. 466 | extraArg_updateM.scale = 5e2; 467 | M_distr = updateM(M_distr, theta_distr, x_next, uR, f_x_now,... 468 | B_cell_now, iLQ_result, planner, extraArg_updateM); 469 | 470 | % Store results. 471 | X = [X x_next]; 472 | U = [U u]; 473 | THETA = [THETA theta_distr]; 474 | M_traj = [M_traj M_distr]; 475 | 476 | % Shift current state. 477 | x = x_next; 478 | 479 | % Update the empirical cost. 480 | J_em = updateEmpiricalCost(J_em, x, xF_R, uR, QR, RR); 481 | end 482 | 483 | 484 | disp('*******************************************************') 485 | disp(['Simulation finished! Empirical cost = ', num2str(J_em)]) 486 | disp('*******************************************************') 487 | 488 | 489 | %% Visualization 490 | XH = X(xH_dims,:); 491 | UH = U(uH_dims,:); 492 | XR = X(xR_dims,:); 493 | UR = U(uR_dims,:); 494 | 495 | option.coordinate = 'rel'; % 'rel', 'abs' 496 | option.keep_traj = 1; 497 | option.is_fading = 1; 498 | option.t_skip = 1; 499 | option.t_start = []; 500 | option.t_end = []; 501 | option.pause = 0.0; 502 | option.N_interp = 2; 503 | option.UI = 0; 504 | plotSim(planner, XR, XH, THETA, M_traj, option) 505 | 506 | 507 | %% Auxiliary function 508 | function flag = is_collision(xR, xH, params) 509 | flag = false; 510 | % Check collision with the human. 511 | if (abs(xR(1)-xH(1)) <= 3) && (abs(xR(2)-xH(2)) <= 1) 512 | flag = true; 513 | end 514 | % Check driving out of lanes. 515 | if (xR(2) >= params.rd_bd_max) || (xR(2) <= params.rd_bd_min) 516 | flag = true; 517 | end 518 | end -------------------------------------------------------------------------------- /util/getCEMPCcontrol.m: -------------------------------------------------------------------------------- 1 | function [uR_Sol, xR_Sol, xH_Sol, uH_Sol, VSol, status] =... 2 | getCEMPCcontrol(x0, xF_R, N, QR, RR, PR, uR_constraints,... 3 | uH_constraints, ilq_solvers, theta_distr, M_distr, planner,... 4 | extraArg, verbose) 5 | % Nominal certainty-equivalent MPC planner that accounts for human 6 | % responses to robot's action. 7 | % 8 | % Author: Haimin Hu (last modified 2021.11.16) 9 | 10 | yalmip('clear') 11 | params = planner.params; 12 | 13 | % Get ILQ solutions. 14 | if M_distr(1) >= M_distr(2) 15 | mu_theta_MAP = theta_distr.l.mu; 16 | ilq_solver_AH = ilq_solvers.AH_l; 17 | ilq_solver_EH = ilq_solvers.EH_l; 18 | else 19 | mu_theta_MAP = theta_distr.r.mu; 20 | ilq_solver_AH = ilq_solvers.AH_r; 21 | ilq_solver_EH = ilq_solvers.EH_r; 22 | end 23 | 24 | Ps_AH = ilq_solver_AH.best_operating_point.Ps; 25 | Ps_EH = ilq_solver_EH.best_operating_point.Ps; 26 | 27 | alphas_AH = ilq_solver_AH.best_operating_point.alphas; 28 | alphas_EH = ilq_solver_EH.best_operating_point.alphas; 29 | 30 | % Systems. 31 | jointSys = ilq_solver_AH.dynamics; 32 | ts = jointSys.ts; 33 | 34 | H_id = planner.H_id; 35 | subsys_H = ilq_solver_AH.dynamics.subsystems{H_id}; 36 | xH_dim = subsys_H.x_dim; 37 | uH_dim = subsys_H.u_dim; 38 | xH_dims = jointSys.x_dims{H_id}; 39 | uH_dims = jointSys.u_dims{H_id}; 40 | 41 | R_id = planner.R_id; 42 | subsys_R = ilq_solver_AH.dynamics.subsystems{R_id}; 43 | xR_dim = subsys_R.x_dim; 44 | uR_dim = subsys_R.u_dim; 45 | xR_dims = jointSys.x_dims{R_id}; 46 | 47 | % Soft constraint weights. 48 | w_uH = extraArg.w_uH; % human's control 49 | w_RB_l = extraArg.w_RB_l; % road boundary (linear penalty) 50 | w_RB_q = extraArg.w_RB_q; % road boundary (quadratic penalty) 51 | w_CA_l = extraArg.w_CA_l; % collision avoidance (linear penalty) 52 | w_CA_q = extraArg.w_CA_q; % collision avoidance (quadratic penalty) 53 | 54 | % Define solver settings 55 | options = sdpsettings('verbose', 0, 'solver', 'snopt', 'usex0', 1,... 56 | 'cachesolvers', 1); 57 | 58 | %---------------------------------------------------------------------- 59 | % Define decision variables 60 | %---------------------------------------------------------------------- 61 | % States. 62 | xR = sdpvar(xR_dim, N+1); 63 | xH = sdpvar(xH_dim, N+1); 64 | 65 | % Nominal states. 66 | xR_ref_AH = ilq_solver_AH.best_operating_point.xs(xR_dims,:); 67 | xH_ref_AH = ilq_solver_AH.best_operating_point.xs(xH_dims,:); 68 | xH_ref_EH = ilq_solver_EH.best_operating_point.xs(xH_dims,:); 69 | 70 | % Inputs. 71 | uR = sdpvar(uR_dim, N); 72 | uH = sdpvar(uH_dim, N); 73 | uH_nom = sdpvar(uH_dim, N); 74 | 75 | % Nominal inputs. 76 | uH_ref_AH = ilq_solver_AH.best_operating_point.us(uH_dims,:); 77 | uH_ref_EH = ilq_solver_EH.best_operating_point.us(uH_dims,:); 78 | 79 | % Soft constraint slack variables. 80 | eps_RB_R = sdpvar(1, N+1); 81 | eps_RB_H = sdpvar(1, N+1); 82 | eps_CA_R = sdpvar(1, N+1); 83 | 84 | %---------------------------------------------------------------------- 85 | % Objective function 86 | %---------------------------------------------------------------------- 87 | % Terminal cost. 88 | objective = (xR(:,N+1)-(xF_R(xR_dims)+[xH(1,N+1);0;0;0]))'*... 89 | PR * (xR(:,N+1)-(xF_R(xR_dims)+[xH(1,N+1);0;0;0])); 90 | 91 | % Tracking cost for states and controls. 92 | for k = 1:N 93 | objective = objective + uR(:,k)'*RR*uR(:,k) +... 94 | ([xH(:,k); xR(:,k)] - xF_R)'*QR*([xH(:,k); xR(:,k)] - xF_R); 95 | end 96 | 97 | % Enforce human's controls. 98 | for k = 1:N 99 | objective = objective + w_uH*norm(uH(:,k) - uH_nom(:,k))^2; 100 | end 101 | 102 | % Soft state constraint penalties. 103 | for k = 1:N+1 104 | objective = objective + w_RB_l*eps_RB_R(k) + w_RB_q*eps_RB_R(k)^2; 105 | objective = objective + w_RB_l*eps_RB_H(k) + w_RB_q*eps_RB_H(k)^2; 106 | objective = objective + w_CA_l*eps_CA_R(k) + w_CA_q*eps_CA_R(k)^2; 107 | end 108 | 109 | %---------------------------------------------------------------------- 110 | % Input constraints 111 | %---------------------------------------------------------------------- 112 | constraint = []; 113 | for k = 1:N 114 | % Robot's control bounds. 115 | constraint = [constraint uR_constraints.min <= uR(:,k),... 116 | uR(:,k) <= uR_constraints.max]; 117 | 118 | % Human's rational control. 119 | uH_AH_k = getiLQcontrol(... 120 | [xH(:,k); xR(:,k)], [xH_ref_AH(:,k); xR_ref_AH(:,k)],... 121 | uH_ref_AH(:,k), Ps_AH{H_id,k}, alphas_AH{H_id,k},... 122 | ilq_solver_AH.alpha_scaling); 123 | uH_EH_k = getiLQcontrol(... 124 | xH(:,k), xH_ref_EH(:,k),... 125 | uH_ref_EH(:,k), Ps_EH{H_id,k}, alphas_EH{H_id,k},... 126 | ilq_solver_EH.alpha_scaling); 127 | 128 | constraint = [constraint... 129 | uH_nom(:,k) == mu_theta_MAP(1)*uH_AH_k + mu_theta_MAP(2)*uH_EH_k]; 130 | 131 | % Human's control bounds. 132 | constraint = [constraint... 133 | uH_constraints.min <= uH(:,k), uH(:,k) <= uH_constraints.max]; 134 | end 135 | 136 | %---------------------------------------------------------------------- 137 | % State constraints 138 | %---------------------------------------------------------------------- 139 | % Collision avoidance ellipsoids. 140 | scale_ellips = 1.2; 141 | ellips = ellipsoid(zeros(2,1),... 142 | blkdiag(scale_ellips*params.avoid.lgt_ub,... 143 | scale_ellips*params.avoid.lat_bd)^2); 144 | [~, Q_CA] = double(ellips); 145 | 146 | for k = 1:N+1 147 | % Road boundaries. 148 | constraint = [constraint... 149 | params.rd_bd_min*0.9 - eps_RB_R <= xR(2,k),... 150 | params.rd_bd_max*0.9 + eps_RB_R >= xR(2,k)]; 151 | 152 | constraint = [constraint... 153 | params.rd_bd_min*0.9 - eps_RB_H <= xH(2,k),... 154 | params.rd_bd_max*0.9 + eps_RB_H >= xH(2,k)]; 155 | 156 | % Collision avoidance. 157 | constraint = [constraint... 158 | -(xR(1:2,k)-xH(1:2,k))'*inv(Q_CA)*(xR(1:2,k)-xH(1:2,k))... 159 | + 1 <= eps_CA_R]; 160 | end 161 | 162 | %---------------------------------------------------------------------- 163 | % Initial state constraints 164 | %---------------------------------------------------------------------- 165 | constraint = [constraint xR(:,1) == x0(xR_dims)]; 166 | constraint = [constraint xH(:,1) == x0(xH_dims)]; 167 | 168 | %---------------------------------------------------------------------- 169 | % Dynamics constraints 170 | %---------------------------------------------------------------------- 171 | for k = 1:N 172 | % Dynamics. 173 | constraint = [constraint... 174 | xR(:,k+1) == subsys_R.integrate(xR(:,k), uR(:,k), ts)]; 175 | constraint = [constraint... 176 | xH(:,k+1) == subsys_H.integrate(xH(:,k), uH(:,k), ts)]; 177 | end 178 | 179 | %---------------------------------------------------------------------- 180 | % Warm-start 181 | %---------------------------------------------------------------------- 182 | % assign(xR, ilq_solver_AH.best_operating_point.xs(xR_dims, 1:N+1)) 183 | % assign(xH, ilq_solver_AH.best_operating_point.xs(xH_dims, 1:N+1)) 184 | % assign(uR, ilq_solver_AH.best_operating_point.us(uR_dims, 1:N)) 185 | % assign(uH, ilq_solver_AH.best_operating_point.us(uH_dims, 1:N)) 186 | 187 | %---------------------------------------------------------------------- 188 | % Solve the MPC problem 189 | %---------------------------------------------------------------------- 190 | tic 191 | status = optimize(constraint, objective, options); 192 | total_time = toc; 193 | stime = status.solvertime; 194 | isSolved = (status.problem==0); 195 | if ~isSolved 196 | message = ['[CE-MPC solver issue] ', status.info ]; 197 | warning(message); 198 | end 199 | 200 | if verbose 201 | disp(['CE-MPC modeling time: ', num2str(total_time-stime), ' s']) 202 | disp(['CE-SMPC solving time: ', num2str(stime), ' s']) 203 | end 204 | 205 | %---------------------------------------------------------------------- 206 | % Return values 207 | %---------------------------------------------------------------------- 208 | xR_Sol = value(xR); 209 | uR_Sol = value(uR); 210 | xH_Sol = value(xH); 211 | uH_Sol = value(uH); 212 | % uH_nom_Sol = value(uH_nom); 213 | VSol = value(objective); 214 | end -------------------------------------------------------------------------------- /util/getCEMPChumancontrol.m: -------------------------------------------------------------------------------- 1 | function [xH_Sol, uH_Sol, VSol, status] = getCEMPChumancontrol(x0, xR,... 2 | xF_H, N, QH, RH, PH, uH_constraints, scale_ellips, jointSys,... 3 | planner, extraArg, verbose) 4 | % Nominal Certainty-Equivalent MPC planner that produces the human's 5 | % controls as a response to the robot's plans. 6 | % 7 | % Author: Haimin Hu (last modified 2022.12.24) 8 | 9 | yalmip('clear') 10 | params = planner.params; 11 | 12 | % Systems 13 | ts = jointSys.ts; 14 | H_id = planner.H_id; 15 | subsys_H = jointSys.subsystems{H_id}; 16 | xH_dim = subsys_H.x_dim; 17 | uH_dim = subsys_H.u_dim; 18 | xH_dims = jointSys.x_dims{H_id}; 19 | 20 | % Soft constraint weights 21 | w_RB_l = extraArg.w_RB_l; % road boundary (linear penalty) 22 | w_RB_q = extraArg.w_RB_q; % road boundary (quadratic penalty) 23 | w_CA_l = extraArg.w_CA_l; % collision avoidance (linear penalty) 24 | w_CA_q = extraArg.w_CA_q; % collision avoidance (quadratic penalty) 25 | 26 | % Define solver settings 27 | options = sdpsettings('verbose', 0, 'solver', 'snopt', 'usex0', 1,... 28 | 'cachesolvers', 1); 29 | options.snopt.Major_feasibility_tolerance = 1e-5; 30 | options.snopt.Major_optimality_tolerance = 1e-5; 31 | options.snopt.Minor_feasibility_tolerance = 1e-5; 32 | options.snopt.Iterations_limit = 4000; 33 | options.snopt.Major_iterations_limit = 250; 34 | options.snopt.Minor_iterations_limit = 125; 35 | options.snopt.Time_limit = 2; 36 | 37 | %---------------------------------------------------------------------- 38 | % Define decision variables 39 | %---------------------------------------------------------------------- 40 | % States 41 | xH = sdpvar(xH_dim, N+1); 42 | 43 | % Inputs 44 | uH = sdpvar(uH_dim, N); 45 | 46 | % Soft constraint slack variables 47 | eps_RB_H_min = sdpvar(1, N+1); 48 | eps_RB_H_max = sdpvar(1, N+1); 49 | eps_CA_H = sdpvar(1, N+1); 50 | 51 | %---------------------------------------------------------------------- 52 | % Objective function 53 | %---------------------------------------------------------------------- 54 | % Terminal cost 55 | objective = (xH(:,N+1) - xF_H)'*PH*(xH(:,N+1) - xF_H); 56 | 57 | % Tracking cost for states and controls 58 | for k = 1:N 59 | objective = objective + uH(:,k)'*RH*uH(:,k) +... 60 | (xH(:,k) - xF_H)'*QH*(xH(:,k) - xF_H); 61 | end 62 | 63 | % Soft state constraint penalties 64 | for k = 1:N+1 65 | objective = objective + w_RB_l*eps_RB_H_min(k) +... 66 | w_RB_q*eps_RB_H_min(k)^2; 67 | objective = objective + w_RB_l*eps_RB_H_max(k) +... 68 | w_RB_q*eps_RB_H_max(k)^2; 69 | objective = objective + w_CA_l*eps_CA_H(k) + w_CA_q*eps_CA_H(k)^2; 70 | end 71 | 72 | %---------------------------------------------------------------------- 73 | % Input constraints 74 | %---------------------------------------------------------------------- 75 | constraint = []; 76 | for k = 1:N 77 | constraint = [constraint... 78 | uH_constraints.min <= uH(:,k), uH(:,k) <= uH_constraints.max]; 79 | end 80 | 81 | %---------------------------------------------------------------------- 82 | % State constraints 83 | %---------------------------------------------------------------------- 84 | % Collision avoidance ellipsoids 85 | ellips = ellipsoid(zeros(2,1),... 86 | blkdiag(scale_ellips*params.avoid.lgt_ub,... 87 | scale_ellips*params.avoid.lat_bd)^2); 88 | [~, Q_CA] = double(ellips); 89 | 90 | for k = 1:N+1 91 | % Road boundaries 92 | constraint = [constraint... 93 | params.rd_bd_min - eps_RB_H_min(k) <= xH(2,k),... 94 | params.rd_bd_max + eps_RB_H_max(k) >= xH(2,k)]; 95 | 96 | % Collision avoidance 97 | constraint = [constraint... 98 | 1 - (xR(1:2,k)-xH(1:2,k))'*inv(Q_CA)*... 99 | (xR(1:2,k)-xH(1:2,k)) <= eps_CA_H(k)]; %#ok 100 | end 101 | 102 | % Slack variables 103 | constraint = [constraint, eps_RB_H_min >= 0, eps_RB_H_max >= 0,... 104 | eps_CA_H >= 0]; 105 | 106 | %---------------------------------------------------------------------- 107 | % Initial state constraints 108 | %---------------------------------------------------------------------- 109 | constraint = [constraint xH(:,1) == x0(xH_dims)]; 110 | 111 | %---------------------------------------------------------------------- 112 | % Dynamics constraints 113 | %---------------------------------------------------------------------- 114 | for k = 1:N 115 | constraint = [constraint... 116 | xH(:,k+1) == subsys_H.integrate(xH(:,k), uH(:,k), ts)]; 117 | end 118 | 119 | %---------------------------------------------------------------------- 120 | % Warm-start 121 | %---------------------------------------------------------------------- 122 | % assign(xH, extraArg.xH_ws) 123 | % assign(uH, extraArg.uH_ws) 124 | 125 | %---------------------------------------------------------------------- 126 | % Solve the MPC problem 127 | %---------------------------------------------------------------------- 128 | tic 129 | status = optimize(constraint, objective, options); 130 | total_time = toc; 131 | stime = status.solvertime; 132 | isSolved = (status.problem==0); 133 | if ~isSolved 134 | message = ['[Human CEMPC solver issue] ', status.info ]; 135 | warning(message); 136 | end 137 | 138 | if verbose 139 | disp(['Human CEMPC modeling time: ', num2str(total_time-stime),... 140 | ' s']) 141 | disp(['Human CEMPC solving time: ', num2str(stime), ' s']) 142 | end 143 | 144 | %---------------------------------------------------------------------- 145 | % Return values 146 | %---------------------------------------------------------------------- 147 | xH_Sol = value(xH); 148 | uH_Sol = value(uH); 149 | VSol = value(objective); 150 | end 151 | -------------------------------------------------------------------------------- /util/getSMPCcontrol.m: -------------------------------------------------------------------------------- 1 | function [uR_Sol, VSol, stime, stree, flag] = getSMPCcontrol(x0, xF_R,... 2 | theta_distr_0, M_distr_0, Nd, Ne, QR, RR, PR, Sigma_d,... 3 | uR_constraints, uH_constraints, ilq_solvers, planner, verbose,... 4 | extraArg) 5 | % Computes the dual SMPC policy. 6 | % 7 | % NOTE: 8 | % 1. Human's controls and M samples at time t are stored in nodes whose 9 | % time is t+1. 10 | % 11 | % Author: Haimin Hu (last modified 2021.12.13) 12 | 13 | yalmip('clear') 14 | 15 | params = planner.params; 16 | 17 | if strcmp(extraArg.opt_mode, 'std') 18 | stree_ws = extraArg.stree; 19 | end 20 | 21 | flag = 0; 22 | 23 | %---------------------------------------------------------------------- 24 | % Solver setup 25 | %---------------------------------------------------------------------- 26 | % Soft constraint weights. 27 | w_uH = extraArg.w_uH; % human's control 28 | w_RB_l = extraArg.w_RB_l; % road boundary (linear penalty) 29 | w_RB_q = extraArg.w_RB_q; % road boundary (quadratic penalty) 30 | w_CA_l = extraArg.w_CA_l; % collision avoidance (linear penalty) 31 | w_CA_q = extraArg.w_CA_q; % collision avoidance (quadratic penalty) 32 | 33 | % Define solver settings. 34 | options = sdpsettings('verbose', 0, 'solver', 'snopt', 'usex0', 1,... 35 | 'cachesolvers', 1, 'showprogress', 0, 'debug', 0); 36 | 37 | %---------------------------------------------------------------------- 38 | % iLQ solutions 39 | %---------------------------------------------------------------------- 40 | % Systems. 41 | if M_distr_0(1) >= M_distr_0(2) 42 | ilq_solver_AH_tmp = ilq_solvers.AH_l; % Altruistic Human, M = 'l' 43 | else 44 | ilq_solver_AH_tmp = ilq_solvers.AH_r; % Altruistic Human, M = 'r' 45 | end 46 | 47 | jointSys = ilq_solver_AH_tmp.dynamics; 48 | ts = jointSys.ts; 49 | 50 | H_id = planner.H_id; 51 | subsys_H = ilq_solver_AH_tmp.dynamics.subsystems{H_id}; 52 | xH_dims = jointSys.x_dims{H_id}; 53 | uH_dims = jointSys.u_dims{H_id}; 54 | 55 | R_id = planner.R_id; 56 | subsys_R = ilq_solver_AH_tmp.dynamics.subsystems{R_id}; 57 | xR_dims = jointSys.x_dims{R_id}; 58 | uR_dims = jointSys.u_dims{R_id}; 59 | 60 | % Nominal states (for warm-start). 61 | xH_ref_AH = ilq_solver_AH_tmp.best_operating_point.xs(xH_dims,:); 62 | % xR_ref_AH = ilq_solver_AH_tmp.best_operating_point.xs(xR_dims,:); 63 | 64 | % Nominal inputs (for warm-start). 65 | uH_ref_AH = ilq_solver_AH_tmp.best_operating_point.us(uH_dims,:); 66 | % uR_ref_AH = ilq_solver_AH_tmp.best_operating_point.us(uR_dims,:); 67 | 68 | %---------------------------------------------------------------------- 69 | % Construct a scenario tree 70 | %---------------------------------------------------------------------- 71 | % Define the root node. 72 | path_prob_root = 1; 73 | theta_dim = 2; 74 | M_dim = 2; 75 | root_node = node(1, 0, 1, path_prob_root, [], [],... 76 | planner.xR_dim, planner.xH_dim, planner.uR_dim, planner.uH_dim,... 77 | theta_dim, M_dim, planner.xR_dims, planner.xH_dims,... 78 | planner.uR_dims, planner.uH_dims); 79 | 80 | if M_distr_0(1) >= M_distr_0(2) 81 | root_node.theta_distr = theta_distr_0.l; 82 | else 83 | root_node.theta_distr = theta_distr_0.r; 84 | end 85 | root_node.M_distr = M_distr_0; 86 | if M_distr_0(1) >= M_distr_0(2) 87 | root_node.M_sample = 'l'; 88 | else 89 | root_node.M_sample = 'r'; 90 | end 91 | 92 | % Initialize a scenario tree. 93 | stree = tree(root_node, planner); 94 | 95 | % Tree branching options (M branching/non-branching). 96 | if extraArg.M_branch 97 | % Branching M. 98 | M_set = ['l', 'r']; 99 | else 100 | % Non-branching M: Use the MAP value of M. 101 | if M_distr_0(1) >= M_distr_0(2) 102 | M_set = 'l'; 103 | else 104 | M_set = 'r'; 105 | end 106 | end 107 | 108 | % Construct the scenario tree. 109 | stree = stree.constructTree(Nd, Ne, M_set, extraArg, verbose); 110 | 111 | %---------------------------------------------------------------------- 112 | % Assign probabilities of M 113 | %---------------------------------------------------------------------- 114 | constraint = []; 115 | if extraArg.M_branch 116 | % Branching M. 117 | for i = 1:numel(stree.N) 118 | if stree.N(i).t == 1 119 | continue 120 | end 121 | 122 | if strcmp(extraArg.opt_mode,'ws') 123 | % Warmstart mode: Assign probabilities of M with the prior. 124 | if stree.N(i).M_sample == 'l' 125 | stree.N(i).M_sample_prob = M_distr_0(1); 126 | elseif stree.N(i).M_sample == 'r' 127 | stree.N(i).M_sample_prob = M_distr_0(2); 128 | end 129 | elseif ~extraArg.M_dual || strcmp(extraArg.opt_mode,'std') 130 | % Non-dual control of M: Assign probabilities of M using 131 | % the warmstart results. 132 | ip = stree.N(i).pre_node_idx; 133 | if stree.N(i).M_sample == 'l' 134 | stree.N(i).M_sample_prob = stree_ws.N(ip).M_distr(1); 135 | elseif stree.N(i).M_sample == 'r' 136 | stree.N(i).M_sample_prob = stree_ws.N(ip).M_distr(2); 137 | end 138 | elseif extraArg.M_dual || strcmp(extraArg.opt_mode,'std') 139 | % Dual control of M: 'Optimize' M probabilities as decision 140 | % variables. 141 | ip = stree.N(i).pre_node_idx; 142 | if stree.N(i).M_sample == 'l' 143 | stree.N(i).M_sample_prob = stree.N(ip).M_distr(1); 144 | elseif stree.N(i).M_sample == 'r' 145 | stree.N(i).M_sample_prob = stree.N(ip).M_distr(2); 146 | end 147 | else 148 | % Sanity check. 149 | error('Invalid M mode and/or optimization mode!') 150 | end 151 | end 152 | else 153 | % Non-branching M. 154 | for i = 1:numel(stree.N) 155 | stree.N(i).M_sample_prob = 1; 156 | end 157 | end 158 | 159 | %---------------------------------------------------------------------- 160 | % Objective function 161 | %---------------------------------------------------------------------- 162 | objective = 0; 163 | for i = 1:numel(stree.N) 164 | % Terminal costs. 165 | if stree.N(i).t == Nd+Ne+1 166 | objective = objective +... 167 | stree.N(i).M_sample_prob * stree.N(i).path_prob *... 168 | (stree.N(i).xR-(xF_R(xR_dims)+[stree.N(i).xH(1);0;0;0]))'*... 169 | PR*(stree.N(i).xR-(xF_R(xR_dims)+[stree.N(i).xH(1);0;0;0])); 170 | end 171 | 172 | % Enforce human's controls. 173 | if stree.N(i).t > 1 174 | objective = objective +... 175 | stree.N(i).M_sample_prob * stree.N(i).path_prob *... 176 | w_uH * norm(stree.N(i).uH - stree.N(i).uH_nom)^2; 177 | end 178 | 179 | % Stage costs. 180 | if stree.N(i).t < Nd+Ne+1 181 | objective = objective +... 182 | stree.N(i).M_sample_prob * stree.N(i).path_prob *... 183 | (stree.N(i).uR' * RR * stree.N(i).uR +... 184 | ([stree.N(i).xH; stree.N(i).xR] - xF_R)' * QR *... 185 | ([stree.N(i).xH; stree.N(i).xR] - xF_R)); 186 | end 187 | end 188 | 189 | %---------------------------------------------------------------------- 190 | % Information gathering / entropy minimization (Explicit Dual Control) 191 | %---------------------------------------------------------------------- 192 | if isfield(extraArg, 'weight_IG') 193 | for i = 1:numel(stree.N) 194 | if stree.N(i).t == Nd+1 195 | objective = objective +... 196 | stree.N(i).M_sample_prob * extraArg.weight_IG *... 197 | (stree.N(i).theta_distr.covar(1) +... 198 | stree.N(i).theta_distr.covar(2) -... 199 | (stree.N(i).M_distr(1)*log(stree.N(i).M_distr(1))+... 200 | stree.N(i).M_distr(2)*log(stree.N(i).M_distr(2)))/... 201 | sum(stree.N(i).M_distr)); 202 | end 203 | end 204 | end 205 | 206 | %---------------------------------------------------------------------- 207 | % Inputs constraints 208 | %---------------------------------------------------------------------- 209 | for i = 1:numel(stree.N) 210 | if stree.N(i).t < Nd+Ne+1 211 | % Robot's control bounds. 212 | constraint = [constraint uR_constraints.min<=stree.N(i).uR,... 213 | stree.N(i).uR<=uR_constraints.max]; 214 | end 215 | 216 | if stree.N(i).t > 1 217 | % Human's rational control (linear combination of basis). 218 | ip = stree.N(i).pre_node_idx; 219 | 220 | iLQ_result = getiLQresults([stree.N(ip).xH; stree.N(ip).xR],... 221 | [], ilq_solvers,planner, stree.N(ip).t); 222 | 223 | if stree.N(i).M_sample == 'l' 224 | uH_AH_k = iLQ_result.AH_l.uH; 225 | uH_EH_k = iLQ_result.EH_l.uH; 226 | elseif stree.N(i).M_sample == 'r' 227 | uH_AH_k = iLQ_result.AH_r.uH; 228 | uH_EH_k = iLQ_result.EH_r.uH; 229 | end 230 | 231 | if strcmp(extraArg.opt_mode, 'ws') 232 | theta_1_k = root_node.theta_distr.mu(1); 233 | theta_2_k = root_node.theta_distr.mu(2); 234 | elseif strcmp(extraArg.opt_mode, 'std') 235 | theta_1_k = stree.N(i).theta_sample(1)/... 236 | (stree.N(i).theta_sample(1)+stree.N(i).theta_sample(2)); 237 | theta_2_k = stree.N(i).theta_sample(2)/... 238 | (stree.N(i).theta_sample(1)+stree.N(i).theta_sample(2)); 239 | end 240 | 241 | constraint = [constraint... 242 | stree.N(i).uH_nom == theta_1_k*uH_AH_k+theta_2_k*uH_EH_k]; 243 | 244 | % Human's control bounds. 245 | constraint = [constraint uH_constraints.min<=stree.N(i).uH,... 246 | stree.N(i).uH<=uH_constraints.max]; 247 | end 248 | end 249 | 250 | %---------------------------------------------------------------------- 251 | % State constraints 252 | %---------------------------------------------------------------------- 253 | % Collision avoidance ellipsoids. 254 | scale_ellips = extraArg.scale_ellips; 255 | ellips = ellipsoid(zeros(2,1),... 256 | blkdiag(scale_ellips * params.avoid.lgt_ub,... 257 | scale_ellips * params.avoid.lat_bd)^2); 258 | [~, Q_CA] = double(ellips); 259 | 260 | for i = 1:numel(stree.N) 261 | if stree.N(i).t == 1 262 | continue 263 | end 264 | 265 | % Road boundaries. 266 | constraint = [constraint... 267 | params.rd_bd_min*0.9-stree.N(i).eps.RB_R<=stree.N(i).xR(2),... 268 | params.rd_bd_max*0.9+stree.N(i).eps.RB_R>=stree.N(i).xR(2)]; 269 | 270 | constraint = [constraint... 271 | params.rd_bd_min*0.9-stree.N(i).eps.RB_H<=stree.N(i).xH(2),... 272 | params.rd_bd_max*0.9+stree.N(i).eps.RB_H>=stree.N(i).xH(2)]; 273 | 274 | % Collision avoidance. 275 | constraint = [constraint... 276 | -(stree.N(i).xR(1:2)-stree.N(i).xH(1:2))' * inv(Q_CA) *... 277 | (stree.N(i).xR(1:2)-stree.N(i).xH(1:2))+1<=stree.N(i).eps.CA]; 278 | 279 | % Soft state constraint penalties. 280 | objective = objective +... 281 | stree.N(i).M_sample_prob * stree.N(i).path_prob *... 282 | (w_RB_l*stree.N(i).eps.RB_R + w_RB_q*stree.N(i).eps.RB_R^2); 283 | objective = objective +... 284 | stree.N(i).M_sample_prob * stree.N(i).path_prob *... 285 | (w_RB_l*stree.N(i).eps.RB_H + w_RB_q*stree.N(i).eps.RB_H^2); 286 | objective = objective +... 287 | stree.N(i).M_sample_prob * stree.N(i).path_prob *... 288 | (w_CA_l*stree.N(i).eps.CA + w_CA_q*stree.N(i).eps.CA^2); 289 | end 290 | 291 | %---------------------------------------------------------------------- 292 | % Initial state constraints 293 | %---------------------------------------------------------------------- 294 | for i = 1:numel(stree.N) 295 | if stree.N(i).t == 1 296 | constraint = [constraint stree.N(i).xR == x0(xR_dims)]; 297 | constraint = [constraint stree.N(i).xH == x0(xH_dims)]; 298 | end 299 | end 300 | 301 | %---------------------------------------------------------------------- 302 | % Dynamics constraints 303 | %---------------------------------------------------------------------- 304 | for i = 1:numel(stree.N) 305 | if stree.N(i).t > 1 306 | ip = stree.N(i).pre_node_idx; 307 | constraint = [constraint... 308 | stree.N(i).xR == subsys_R.integrate(stree.N(ip).xR,... 309 | stree.N(ip).uR, ts)]; 310 | constraint = [constraint... 311 | stree.N(i).xH == subsys_H.integrate(stree.N(ip).xH,... 312 | stree.N(i).uH, ts)]; % Be careful with human's control. 313 | end 314 | end 315 | 316 | %---------------------------------------------------------------------- 317 | % Belief state dynamics constraints 318 | %---------------------------------------------------------------------- 319 | if strcmp(extraArg.opt_mode, 'std') 320 | for i = 1:numel(stree.N) 321 | % Dual control steps: propagate the belief state via the 322 | % recursive Bayesian estimation. 323 | if 1 < stree.N(i).t && stree.N(i).t <= Nd+1 324 | % Linearize dynamics. 325 | ip = stree.N(i).pre_node_idx; 326 | [A_ip, B_cell_ip] = jointSys.linearizeDiscrete_B_cell(... 327 | [stree_ws.N(ip).xH; stree_ws.N(ip).xR],... 328 | [stree_ws.N(i).uH; stree_ws.N(ip).uR]); 329 | 330 | % Compute fixed basis of human's control. 331 | k = stree.N(ip).t; 332 | 333 | iLQ_result_ws = getiLQresults(... 334 | [stree_ws.N(ip).xH; stree_ws.N(ip).xR], B_cell_ip,... 335 | ilq_solvers, planner, k); 336 | 337 | uH_AH_fixed = iLQ_result_ws.AH_l.uH; 338 | uH_EH_fixed = iLQ_result_ws.EH_l.uH; 339 | 340 | extraArg.U_fixed = [uH_AH_fixed uH_EH_fixed]; 341 | extraArg.mu_theta_fixed = stree_ws.N(ip).theta_distr.mu; 342 | 343 | % Propagate theta. 344 | iLQ_result = getiLQresults(... 345 | [stree.N(ip).xH; stree.N(ip).xR], B_cell_ip,... 346 | ilq_solvers, planner, k); 347 | 348 | [mu_next, covar_next] = updateTheta(... 349 | stree.N(i).M_sample, stree.N(ip).theta_distr.mu,... 350 | stree.N(ip).theta_distr.covar, Sigma_d,... 351 | [stree.N(i).xH; stree.N(i).xR],... 352 | [stree.N(i).uH; stree.N(ip).uR],... 353 | A_ip * [stree.N(ip).xH; stree.N(ip).xR], B_cell_ip,... 354 | iLQ_result, planner, extraArg); %!!! uH <- uH_nom 355 | 356 | constraint = [constraint... 357 | stree.N(i).theta_distr.mu == mu_next 358 | stree.N(i).theta_distr.covar == covar_next]; 359 | 360 | % Propagate M. 361 | if extraArg.M_dual 362 | extraArg.theta_distr_M_fixed =... 363 | stree_ws.N(ip).theta_distr_M_ws; 364 | M_distr_next = updateM(stree.N(ip).M_distr, [],... 365 | [stree.N(i).xH; stree.N(i).xR], stree.N(ip).uR,... 366 | A_ip * [stree.N(ip).xH; stree.N(ip).xR],... 367 | B_cell_ip, iLQ_result, planner, extraArg); 368 | constraint = [constraint... 369 | stree.N(i).M_distr == M_distr_next]; 370 | end 371 | end 372 | 373 | % Exploitation steps. 374 | if stree.N(i).t > Nd+1 375 | ip = stree.N(i).pre_node_idx; 376 | constraint = [constraint... 377 | stree.N(i).theta_distr.mu==stree.N(ip).theta_distr.mu,... 378 | stree.N(i).theta_distr.covar==stree.N(ip).theta_distr.covar]; 379 | end 380 | end 381 | end 382 | 383 | %---------------------------------------------------------------------- 384 | % Reconstruct theta samples 385 | %---------------------------------------------------------------------- 386 | if strcmp(extraArg.opt_mode, 'std') 387 | for i = 1:numel(stree.N) 388 | % Dual control steps. 389 | if 1 < stree.N(i).t && stree.N(i).t <= Nd+1 390 | constraint = [constraint... 391 | stree.N(i).theta_sample == transform_theta(stree.N(i))]; 392 | end 393 | 394 | % Exploitation steps. 395 | if stree.N(i).t > Nd+1 396 | constraint = [constraint... 397 | stree.N(i).theta_sample == stree.N(i).theta_distr.mu]; 398 | end 399 | end 400 | end 401 | 402 | %---------------------------------------------------------------------- 403 | % Warm-start 404 | %---------------------------------------------------------------------- 405 | if strcmp(extraArg.opt_mode, 'ws') 406 | for i = 1:numel(stree.N) 407 | k = stree.N(i).t; 408 | assign(stree.N(i).xH, xH_ref_AH(:,k)) 409 | % assign(stree.N(i).xR, xR_ref_AH(:,k)) 410 | assign(stree.N(i).xR, extraArg.xR_ws(:,k)) 411 | if k > 1 412 | assign(stree.N(i).uH, uH_ref_AH(:,k-1)) 413 | assign(stree.N(i).uH_nom, uH_ref_AH(:,k-1)) 414 | % assign(stree.N(i).uR, uR_ref_AH(:,k-1)) 415 | assign(stree.N(i).uR, extraArg.uR_ws(:,k-1)) 416 | end 417 | end 418 | elseif strcmp(extraArg.opt_mode, 'std') 419 | for i = 1:numel(stree.N) 420 | assign(stree.N(i).xH, stree_ws.N(i).xH) 421 | assign(stree.N(i).xR, stree_ws.N(i).xR) 422 | if stree.N(i).t > 1 423 | assign(stree.N(i).uH, stree_ws.N(i).uH) 424 | assign(stree.N(i).uH_nom, stree_ws.N(i).uH_nom) 425 | assign(stree.N(i).uR, stree_ws.N(i).uR) 426 | % assign(stree.N(i).theta_distr.mu, stree_ws.N(i).theta_distr.mu) 427 | assign(stree.N(i).theta_distr.covar, stree_ws.N(i).theta_distr.covar) 428 | % assign(stree.N(i).theta_sample, stree_ws.N(i).theta_sample) 429 | % if extraArg.M_dual 430 | % assign(stree.N(i).M_distr, stree_ws.N(i).M_distr) 431 | % end 432 | end 433 | end 434 | end 435 | 436 | %---------------------------------------------------------------------- 437 | % Solve the ST-SMPC problem 438 | %---------------------------------------------------------------------- 439 | tic 440 | status = optimize(constraint, objective, options); 441 | total_time = toc; 442 | stime = status.solvertime; 443 | isSolved = (status.problem==0); 444 | if ~isSolved 445 | if strcmp(extraArg.opt_mode, 'ws') 446 | message = ['[WS-SMPC solver issue] ', status.info]; 447 | elseif strcmp(extraArg.opt_mode, 'std') 448 | message = ['[Dual-SMPC solver issue] ', status.info]; 449 | else 450 | message = ['[Unknown-SMPC solver issue] ', status.info]; 451 | end 452 | warning(message); 453 | if strcmp(status.info,... 454 | 'Maximum iterations or time limit exceeded (SNOPT)') 455 | flag = 1; 456 | end 457 | end 458 | 459 | if verbose 460 | if strcmp(extraArg.opt_mode, 'ws') 461 | disp(['WS-SMPC modeling time: ', num2str(total_time-stime),... 462 | ' s']) 463 | disp(['WS-SMPC solving time: ', num2str(stime), ' s']) 464 | elseif strcmp(extraArg.opt_mode, 'std') 465 | disp(['Dual-SMPC modeling time: ',... 466 | num2str(total_time-stime), ' s']) 467 | disp(['Dual-SMPC solving time: ', num2str(stime), ' s']) 468 | end 469 | end 470 | 471 | %---------------------------------------------------------------------- 472 | % Retrieve value and propagate the belief states (for warm-start only) 473 | %---------------------------------------------------------------------- 474 | if strcmp(extraArg.opt_mode, 'ws') 475 | for i = 1:numel(stree.N) 476 | stree.N(i).xH = value(stree.N(i).xH); 477 | stree.N(i).xR = value(stree.N(i).xR); 478 | stree.N(i).uH = value(stree.N(i).uH); 479 | stree.N(i).uH_nom = value(stree.N(i).uH_nom); 480 | stree.N(i).uR = value(stree.N(i).uR); 481 | end 482 | 483 | stree.N(1).theta_distr_M_ws = theta_distr_0; 484 | stree.N(1).theta_sample = root_node.theta_distr.mu; 485 | for i = 1:numel(stree.N) 486 | if stree.N(i).t > 1 487 | ip = stree.N(i).pre_node_idx; 488 | 489 | % Get iLQ results. 490 | [A_ip, B_cell_ip] = jointSys.linearizeDiscrete_B_cell(... 491 | [stree.N(ip).xH; stree.N(ip).xR],... 492 | [stree.N(i).uH; stree.N(ip).uR]); 493 | 494 | iLQ_result = getiLQresults(... 495 | [stree.N(ip).xH; stree.N(ip).xR], B_cell_ip,... 496 | ilq_solvers, planner, stree.N(i).t); 497 | 498 | % Propagate theta. 499 | if extraArg.M_branch 500 | for M = M_set 501 | if M == 'l' 502 | theta_distr_now=stree.N(ip).theta_distr_M_ws.l; 503 | elseif M == 'r' 504 | theta_distr_now=stree.N(ip).theta_distr_M_ws.r; 505 | end 506 | 507 | [mu_next, covar_next] = updateTheta(... 508 | M, theta_distr_now.mu,... 509 | theta_distr_now.covar, Sigma_d,... 510 | [stree.N(i).xH; stree.N(i).xR],... 511 | [stree.N(i).uH; stree.N(ip).uR],... 512 | A_ip*[stree.N(ip).xH; stree.N(ip).xR],... 513 | B_cell_ip, iLQ_result, planner, []); 514 | 515 | if M == 'l' 516 | stree.N(i).theta_distr_M_ws.l.mu = mu_next; 517 | stree.N(i).theta_distr_M_ws.l.covar =... 518 | covar_next; 519 | elseif M == 'r' 520 | stree.N(i).theta_distr_M_ws.r.mu = mu_next; 521 | stree.N(i).theta_distr_M_ws.r.covar =... 522 | covar_next; 523 | end 524 | 525 | if stree.N(i).M_sample == M 526 | theta_distr_i.mu = mu_next; 527 | theta_distr_i.covar = covar_next; 528 | stree.N(i).theta_distr = theta_distr_i; 529 | stree.N(i).theta_sample =... 530 | transform_theta(stree.N(i)); 531 | end 532 | end 533 | else 534 | [mu_next, covar_next] = updateTheta(... 535 | stree.N(i).M_sample, stree.N(ip).theta_distr.mu,... 536 | stree.N(ip).theta_distr.covar, Sigma_d,... 537 | [stree.N(i).xH; stree.N(i).xR],... 538 | [stree.N(i).uH; stree.N(ip).uR],... 539 | A_ip*[stree.N(ip).xH; stree.N(ip).xR],... 540 | B_cell_ip, iLQ_result, planner, []); 541 | 542 | theta_distr_i.mu = mu_next; 543 | theta_distr_i.covar = covar_next; 544 | stree.N(i).theta_distr = theta_distr_i; 545 | stree.N(i).theta_sample = transform_theta(stree.N(i)); 546 | end 547 | 548 | % Propagate M. 549 | if extraArg.M_branch 550 | M_distr_next = updateM(stree.N(ip).M_distr,... 551 | stree.N(ip).theta_distr_M_ws,... 552 | [stree.N(i).xH; stree.N(i).xR], stree.N(ip).uR,... 553 | A_ip*[stree.N(ip).xH; stree.N(ip).xR],... 554 | B_cell_ip, iLQ_result, planner, []); 555 | 556 | stree.N(i).M_distr = M_distr_next; 557 | end 558 | end 559 | end 560 | end 561 | 562 | %---------------------------------------------------------------------- 563 | % Return values 564 | %---------------------------------------------------------------------- 565 | uR_Sol = value(stree.N(1).uR); 566 | VSol = value(objective); 567 | 568 | function theta_out = transform_theta(node) 569 | theta_out = node.theta_distr.mu + sqrtm(node.theta_distr.covar)*... 570 | node.theta_sample_sn; 571 | end 572 | end 573 | 574 | -------------------------------------------------------------------------------- /util/getiLQresults.m: -------------------------------------------------------------------------------- 1 | function iLQ_result = getiLQresults(x, B_cell, ilq_solvers, planner, t) 2 | % Get iLQ computation results. 3 | 4 | ilq_solver_AH_l = ilq_solvers.AH_l; 5 | ilq_solver_EH_l = ilq_solvers.EH_l; 6 | ilq_solver_AH_r = ilq_solvers.AH_r; 7 | ilq_solver_EH_r = ilq_solvers.EH_r; 8 | 9 | xH_dims = planner.xH_dims; 10 | uH_dims = planner.uH_dims; 11 | 12 | H_id = planner.H_id; 13 | R_id = planner.R_id; 14 | 15 | if H_id ~= 1 || R_id ~= 2 16 | error('Incorrect subsystem index!') % sanity check 17 | end 18 | 19 | if ~isempty(B_cell) 20 | BH = B_cell{H_id}; 21 | end 22 | 23 | % --------- M = 'l' --------- 24 | % Altruistic human, M = 'l'. 25 | x_nom_AH_l = ilq_solver_AH_l.best_operating_point.xs(:, t); 26 | u_nom_AH_l = ilq_solver_AH_l.best_operating_point.us(:, t); 27 | P_H_AH_l = [ilq_solver_AH_l.best_operating_point.Ps{1, t}; 28 | ilq_solver_AH_l.best_operating_point.Ps{2, t}]; 29 | alpha_H_AH_l = [ilq_solver_AH_l.best_operating_point.alphas{1, t};... 30 | ilq_solver_AH_l.best_operating_point.alphas{2, t}]; 31 | Z_H_AH_l = ilq_solver_AH_l.best_operating_point.Zs{H_id, t}; 32 | 33 | uH_AH_l = getiLQcontrol(x, x_nom_AH_l, u_nom_AH_l, P_H_AH_l,... 34 | alpha_H_AH_l, planner.alpha_scaling); 35 | uH_AH_l = uH_AH_l(uH_dims); 36 | 37 | % Egoistic Human, M = 'l'. 38 | x_nom_EH_l = ilq_solver_EH_l.best_operating_point.xs(:, t); 39 | u_nom_EH_l = ilq_solver_EH_l.best_operating_point.us(:, t); 40 | P_H_EH_l = ilq_solver_EH_l.best_operating_point.Ps{1, t}; 41 | alpha_H_EH_l = ilq_solver_EH_l.best_operating_point.alphas{1, t}; 42 | Z_H_EH_l = ilq_solver_EH_l.best_operating_point.Zs{1, t}; 43 | 44 | uH_EH_l = getiLQcontrol(x(xH_dims), x_nom_EH_l, u_nom_EH_l,... 45 | P_H_EH_l, alpha_H_EH_l, planner.alpha_scaling); 46 | 47 | % --------- M = 'r' --------- 48 | % Altruistic human, M = 'r'. 49 | x_nom_AH_r = ilq_solver_AH_r.best_operating_point.xs(:, t); 50 | u_nom_AH_r = ilq_solver_AH_r.best_operating_point.us(:, t); 51 | P_H_AH_r = [ilq_solver_AH_r.best_operating_point.Ps{1, t}; 52 | ilq_solver_AH_r.best_operating_point.Ps{2, t}]; 53 | alpha_H_AH_r = [ilq_solver_AH_r.best_operating_point.alphas{1, t};... 54 | ilq_solver_AH_r.best_operating_point.alphas{2, t}]; 55 | Z_H_AH_r = ilq_solver_AH_r.best_operating_point.Zs{H_id, t}; 56 | 57 | uH_AH_r = getiLQcontrol(x, x_nom_AH_r, u_nom_AH_r, P_H_AH_r,... 58 | alpha_H_AH_r, planner.alpha_scaling); 59 | uH_AH_r = uH_AH_r(uH_dims); 60 | 61 | % Egoistic Human, M = 'l'. 62 | x_nom_EH_r = ilq_solver_EH_r.best_operating_point.xs(:, t); 63 | u_nom_EH_r = ilq_solver_EH_r.best_operating_point.us(:, t); 64 | P_H_EH_r = ilq_solver_EH_r.best_operating_point.Ps{1, t}; 65 | alpha_H_EH_r = ilq_solver_EH_r.best_operating_point.alphas{1, t}; 66 | Z_H_EH_r = ilq_solver_EH_r.best_operating_point.Zs{1, t}; 67 | 68 | uH_EH_r = getiLQcontrol(x(xH_dims), x_nom_EH_r, u_nom_EH_r,... 69 | P_H_EH_r, alpha_H_EH_r, planner.alpha_scaling); 70 | 71 | % Return results. 72 | iLQ_result.AH_l.uH = uH_AH_l; 73 | iLQ_result.EH_l.uH = uH_EH_l; 74 | iLQ_result.AH_r.uH = uH_AH_r; 75 | iLQ_result.EH_r.uH = uH_EH_r; 76 | 77 | if ~isempty(B_cell) 78 | Sigma_AH_l = planner.RH + BH'*Z_H_AH_l*BH; 79 | iLQ_result.AH_l.Sigma = Sigma_AH_l; 80 | 81 | Sigma_EH_l = planner.RH + BH(1:4,:)'*Z_H_EH_l*BH(1:4,:); 82 | iLQ_result.EH_l.Sigma = Sigma_EH_l; 83 | 84 | Sigma_AH_r = planner.RH + BH'*Z_H_AH_r*BH; 85 | iLQ_result.AH_r.Sigma = Sigma_AH_r; 86 | 87 | Sigma_EH_r = planner.RH + BH(1:4,:)'*Z_H_EH_r*BH(1:4,:); 88 | iLQ_result.EH_r.Sigma = Sigma_EH_r; 89 | end 90 | end 91 | 92 | -------------------------------------------------------------------------------- /util/iLQGames/costs/Cost.m: -------------------------------------------------------------------------------- 1 | classdef Cost 2 | % Base class for all cost functions. 3 | % Author: Haimin Hu 4 | % Reference: ilqgames by David Fridovich-Keil 5 | % Created: 2021-11-09, Last modified: 2021-11-09 6 | 7 | properties 8 | name % Name of the cost 9 | x_dim % State dimension 10 | u_dim % Control dimension 11 | dldx % Gradient of the cost l w.r.t. state x 12 | Hx % Hessian of the cost l w.r.t. state x 13 | Hu % Hessian of the cost l w.r.t. control u 14 | end 15 | 16 | methods 17 | function obj = Cost(name, x_dim, u_dim) 18 | % Constructor. 19 | obj.name = name; 20 | obj.x_dim = x_dim; 21 | obj.u_dim = u_dim; 22 | end 23 | 24 | function out = get_dldx(obj, x0) 25 | % Evaluate gradient dldx at x0. 26 | out = obj.dldx(x0); 27 | end 28 | 29 | function out = get_Hx(obj, x0) 30 | % Evaluate hessian Hx at x0. 31 | out = obj.Hx(x0); 32 | end 33 | 34 | function out = get_Hu(obj, u0) 35 | % Evaluate hessian Hu at u0. 36 | out = obj.Hu(u0); 37 | end 38 | end % end methods 39 | end % end class 40 | 41 | -------------------------------------------------------------------------------- /util/iLQGames/costs/PlayerCost.m: -------------------------------------------------------------------------------- 1 | classdef PlayerCost 2 | % Container to hold a bunch of different Costs and keep track of the 3 | % arguments to each one. 4 | % Author: Haimin Hu 5 | % Reference: ilqgames by David Fridovich-Keil 6 | % Created: 2021-11-09, Last modified: 2021-11-09 7 | 8 | properties 9 | costs % cell of costs 10 | args % cell of args `x` or `u` or `xu` 11 | weights % vector of weights 12 | end 13 | 14 | methods 15 | function obj = PlayerCost() 16 | % Constructor. 17 | obj.costs = {}; 18 | obj.args = {}; 19 | obj.weights = []; 20 | end 21 | 22 | function total_cost = cost(obj, x, u) 23 | % Evaluate the game cost function at the current state 24 | % and/or controls. 25 | total_cost = 0; 26 | for i = 1:length(obj.costs) 27 | current_cost = obj.costs{i}; 28 | if strcmp(obj.args{i},'x') 29 | current_term = obj.weights(i) * current_cost.cost(x); 30 | elseif strcmp(obj.args{i},'u') 31 | current_term = obj.weights(i) * current_cost.cost(u); 32 | elseif strcmp(obj.args{i},'xu') 33 | current_term = obj.weights(i) * current_cost.cost(x,u); 34 | else 35 | error('Invalid args') 36 | end 37 | 38 | if current_term > 1e12 39 | warning(["cost "+current_cost.name+" is "+... 40 | num2str(current_term)]) 41 | warning(['Input is ', obj.args{i}]) 42 | end 43 | 44 | total_cost = total_cost + current_term; 45 | end 46 | end 47 | 48 | function obj = add_cost(obj, cost, arg, weight) 49 | % Add a new cost to the game. 50 | obj.costs{end+1} = cost; 51 | obj.args{end+1} = arg; 52 | obj.weights = [obj.weights weight]; 53 | end 54 | 55 | function [total_cost, dldx, Hx, Hu] = quadraticize(obj, x, u) 56 | % Compute a quadratic approximation to the overall cost for a 57 | % particular choice of operating point `x`, `u` or `xu` 58 | % Returns the gradient and Hessian of the overall cost s.t.: 59 | % 60 | % cost(x + dx, u + du) \approx 61 | % cost(x, u) + 62 | % dldx^T dx + 63 | % 0.5 * (dx^T Hx dx + du^T Hu du) 64 | 65 | % Evaluate cost at the operating point. 66 | total_cost = obj.cost(x,u); 67 | 68 | % Compute gradient and Hessians. 69 | dldx = zeros(length(x), 1); 70 | Hx = zeros(length(x), length(x)); 71 | Hu = zeros(length(u), length(u)); 72 | 73 | for i = 1:length(obj.costs) 74 | current_cost = obj.costs{i}; 75 | dldx = dldx + current_cost.get_dldx(x); 76 | Hx = Hx + current_cost.get_Hx(x); 77 | Hu = Hu + current_cost.get_Hu(u); 78 | end 79 | end 80 | end % end methods 81 | end % end class 82 | 83 | -------------------------------------------------------------------------------- /util/iLQGames/costs/ProductStateProximityCost.m: -------------------------------------------------------------------------------- 1 | classdef ProductStateProximityCost < Cost 2 | % Proximity cost for state spaces that are Cartesian products of 3 | % individual systems' state spaces. Penalizes 4 | % ``` sum_{i \ne j} min(distance(i, j) - max_distance, 0)^2 ``` 5 | % for all players i, j. 6 | % Author: Haimin Hu 7 | % Reference: ilqgames by David Fridovich-Keil 8 | % Created: 2021-11-09, Last modified: 2021-11-09 9 | 10 | properties 11 | px_indices % px indices of all players 12 | py_indices % py indices of all players 13 | max_distance % Maximum value of distance to penalize 14 | num_players % Number of players 15 | end 16 | 17 | methods 18 | function obj = ProductStateProximityCost(px_indices,... 19 | py_indices, max_distance, name, x_dim, u_dim) 20 | % Constructor. 21 | % Initialize with dimension to add cost to and threshold 22 | % BELOW which to impose quadratic cost. 23 | super_args{1} = name; 24 | super_args{2} = x_dim; 25 | super_args{3} = u_dim; 26 | obj@Cost(super_args{:}); 27 | 28 | obj.px_indices = px_indices; 29 | obj.py_indices = py_indices; 30 | obj.max_distance = max_distance; 31 | obj.num_players = length(px_indices); 32 | 33 | % Define symbolic gradients and Hessians. 34 | x = sym('x', [obj.x_dim 1]); 35 | u = sym('u', [obj.u_dim 1]); 36 | l = 0; 37 | for i = 1:obj.num_players 38 | xi_idx = [obj.px_indices(i), obj.py_indices(i)]; 39 | for j = 1:obj.num_players 40 | if i ~= j 41 | xj_idx = [obj.px_indices(j), obj.py_indices(j)]; 42 | 43 | % Compute relative distance. 44 | dx = x(xi_idx) - x(xj_idx); 45 | relative_distance = norm(dx); 46 | 47 | % Note: min does not work with sym. 48 | l_tmp = symfun(piecewise(relative_distance-... 49 | obj.max_distance<=0,(relative_distance-... 50 | obj.max_distance)^2,0),x); 51 | 52 | l = l + l_tmp; 53 | end 54 | end 55 | end 56 | 57 | file_name = "./util/iLQGames/Costs/ProductStateProximityCost_" + name; 58 | 59 | obj.dldx = matlabFunction(gradient(l,x),'Vars',{x},... 60 | 'file', file_name + "_dldx.m"); 61 | obj.Hx = matlabFunction(hessian(l,x),'Vars',{x},... 62 | 'file', file_name + "_Hx.m"); 63 | obj.Hu = matlabFunction(hessian(l,u),'Vars',{u,x},... 64 | 'file', file_name + "_Hu.m"); 65 | end 66 | 67 | function total_cost = cost(obj, x) 68 | % Evaluate this cost function on the given state. 69 | % x: concatenated state vector of all subsystems (column vector) 70 | total_cost = 0; 71 | for ii = 1:obj.num_players 72 | xi_idx = [obj.px_indices(ii), obj.py_indices(ii)]; 73 | for jj = 1:obj.num_players 74 | if ii ~= jj 75 | xj_idx = [obj.px_indices(jj), obj.py_indices(jj)]; 76 | 77 | % Compute relative distance. 78 | dx = x(xi_idx) - x(xj_idx); 79 | relative_distance = norm(dx); 80 | 81 | total_cost = total_cost + min(relative_distance-... 82 | obj.max_distance, 0.0)^2; 83 | end 84 | end 85 | end 86 | end 87 | 88 | function out = get_Hu(obj, u0) 89 | % Evaluate hessian Hu at u0. 90 | out = obj.Hu(u0, zeros(obj.x_dim,1)); 91 | end 92 | 93 | end % end methods 94 | end % end class 95 | 96 | -------------------------------------------------------------------------------- /util/iLQGames/costs/QuadraticCost.m: -------------------------------------------------------------------------------- 1 | classdef QuadraticCost < Cost 2 | % Quadratic cost, derived from Cost base class. 3 | % Author: Haimin Hu 4 | % Reference: ilqgames by David Fridovich-Keil 5 | % Created: 2021-11-09, Last modified: 2021-11-09 6 | 7 | properties 8 | dimensions % dimensions to add cost 9 | origin % Value along the specified dim where the cost is 0 10 | type % 'state' or 'control' 11 | P % Cost matrix 12 | end 13 | 14 | methods 15 | function obj = QuadraticCost(dimensions, origin, type, P,... 16 | name, x_dim, u_dim) 17 | % Constructor. 18 | % Initialize with dimensions to add cost to and origin to 19 | % center the quadratic cost about. 20 | super_args{1} = name; 21 | super_args{2} = x_dim; 22 | super_args{3} = u_dim; 23 | obj@Cost(super_args{:}); 24 | 25 | obj.dimensions = dimensions; 26 | obj.origin = origin; 27 | obj.type = type; 28 | obj.P = P; 29 | 30 | % Define symbolic gradients and Hessians. 31 | x = sym('x', [obj.x_dim 1]); 32 | u = sym('u', [obj.u_dim 1]); 33 | if strcmp(obj.type,'state') 34 | l = (x(obj.dimensions) - obj.origin)'*obj.P*... 35 | (x(obj.dimensions) - obj.origin); 36 | elseif strcmp(obj.type,'control') 37 | l = (u(obj.dimensions) - obj.origin)'*obj.P*... 38 | (u(obj.dimensions) - obj.origin); 39 | else 40 | error('Invalid type') 41 | end 42 | obj.dldx = matlabFunction(gradient(l,x),'Vars',{x}); 43 | obj.Hx = matlabFunction(hessian(l,x),'Vars',{x}); 44 | obj.Hu = matlabFunction(hessian(l,u),'Vars',{u}); 45 | end 46 | 47 | function out = cost(obj, xu) 48 | % Evaluate this cost function on the given input and time, 49 | % which might either be a state `x` or a control `u`. Hence 50 | % the input is named `xu`. 51 | out = (xu(obj.dimensions) - obj.origin)'*obj.P*... 52 | (xu(obj.dimensions) - obj.origin); 53 | end 54 | end % end methods 55 | end % end class 56 | 57 | -------------------------------------------------------------------------------- /util/iLQGames/costs/ReferenceDeviationCost.m: -------------------------------------------------------------------------------- 1 | classdef ReferenceDeviationCost < Cost 2 | % Reference following cost. 3 | % Author: Haimin Hu 4 | % Reference: ilqgames by David Fridovich-Keil 5 | % Created: 2021-11-09, Last modified: 2021-11-09 6 | 7 | properties 8 | x_ref % Reference state (nx-by-1) 9 | u_ref % Reference control (nu-by-1) 10 | Q % Cost matrix of states 11 | R % Cost matrix of controls 12 | end 13 | 14 | methods 15 | function obj = ReferenceDeviationCost(x_ref, u_ref, Q, R,... 16 | name, x_dim, u_dim) 17 | % Constructor 18 | % NOTE: This list will be updated externally as the reference 19 | % trajectory evolves with each iteration. 20 | super_args{1} = name; 21 | super_args{2} = x_dim; 22 | super_args{3} = u_dim; 23 | obj@Cost(super_args{:}); 24 | 25 | obj.x_ref = x_ref; 26 | obj.u_ref = u_ref; 27 | obj.Q = Q; 28 | obj.R = R; 29 | 30 | % Define symbolic gradients and Hessians. 31 | x = sym('x', [obj.x_dim 1]); 32 | u = sym('u', [obj.u_dim 1]); 33 | l = obj.cost(x, u); 34 | obj.dldx = matlabFunction(gradient(l,x),'Vars',{x}); 35 | obj.Hx = matlabFunction(hessian(l,x),'Vars',{x}); 36 | obj.Hu = matlabFunction(hessian(l,u),'Vars',{u}); 37 | end 38 | 39 | function out = cost(obj, x, u) 40 | % Evaluate this cost function on the given vector and time. 41 | out = (x-obj.x_ref)'*obj.Q*(x-obj.x_ref) +... 42 | (u-obj.u_ref)'*obj.R*(u-obj.u_ref); 43 | end 44 | end % end methods 45 | end % end class 46 | 47 | -------------------------------------------------------------------------------- /util/iLQGames/costs/SemiquadraticCost.m: -------------------------------------------------------------------------------- 1 | classdef SemiquadraticCost < Cost 2 | % Semiquadratic cost, derived from Cost base class. Implements a 3 | % cost function that is flat below a threshold and quadratic above, in 4 | % the given dimension. 5 | % Author: Haimin Hu 6 | % Reference: ilqgames by David Fridovich-Keil 7 | % Created: 2021-11-09, Last modified: 2021-11-09 8 | 9 | properties 10 | dimension % Dimension to add cost 11 | threshold % Value above which to impose quadratic cost 12 | oriented_right % Boolean flag determining which side of threshold 13 | % to penalize 14 | type % 'state' or 'control' 15 | end 16 | 17 | methods 18 | function obj = SemiquadraticCost(dimension, threshold,... 19 | oriented_right, type, name, x_dim, u_dim) 20 | % Constructor. 21 | % Initialize with dimension to add cost to and threshold 22 | % above which to impose quadratic cost. 23 | super_args{1} = name; 24 | super_args{2} = x_dim; 25 | super_args{3} = u_dim; 26 | obj@Cost(super_args{:}); 27 | 28 | obj.dimension = dimension; 29 | obj.threshold = threshold; 30 | obj.oriented_right = oriented_right; 31 | obj.type = type; 32 | 33 | % Define symbolic gradients and Hessians. 34 | x = sym('x', [obj.x_dim 1]); 35 | u = sym('u', [obj.u_dim 1]); 36 | if strcmp(obj.type,'state') 37 | if obj.oriented_right 38 | l = symfun(piecewise(x(obj.dimension)>obj.threshold,... 39 | (x(obj.dimension)-obj.threshold)^2, 0), x); 40 | else 41 | l = symfun(piecewise(x(obj.dimension)obj.threshold,... 47 | (u(obj.dimension)-obj.threshold)^2, 0), u); 48 | else 49 | l = symfun(piecewise(u(obj.dimension) obj.threshold 72 | out = (xu(obj.dimension) - obj.threshold)^2; 73 | else 74 | out = 0; 75 | end 76 | else 77 | if xu(obj.dimension) < obj.threshold 78 | out = (xu(obj.dimension) - obj.threshold)^2; 79 | else 80 | out = 0; 81 | end 82 | end 83 | end 84 | 85 | function out = get_Hu(obj, u0) 86 | % Evaluate hessian Hu at u0. 87 | out = obj.Hu(u0, zeros(obj.x_dim,1)); 88 | end 89 | end % end methods 90 | end % end class 91 | 92 | -------------------------------------------------------------------------------- /util/iLQGames/dynamics/Bicycle4D.m: -------------------------------------------------------------------------------- 1 | classdef Bicycle4D 2 | % 4D kinematic bicycle model. 3 | % Author: Haimin Hu 4 | % Reference: ilqgames by David Fridovich-Keil 5 | % Created: 2021-11-08, Last modified: 2021-11-08 6 | 7 | properties 8 | L % Wheel base 9 | ts % Sampling time (s) 10 | x_dim % State dimension 11 | u_dim % Control dimension 12 | dfdx % Symbolic Jacobian matrix dfdx 13 | dfdu % Symbolic Jacobian matrix dfdu 14 | end 15 | 16 | methods 17 | function obj = Bicycle4D(L, ts) 18 | % Constructor. 19 | 20 | % Static parameters. 21 | obj.L = L; 22 | obj.ts = ts; 23 | obj.x_dim = 4; 24 | obj.u_dim = 2; 25 | 26 | % Define linearized dynamics symbolically. 27 | x_sym = sym('x', [obj.x_dim 1]); 28 | u_sym = sym('u', [obj.u_dim 1]); 29 | 30 | f = [x_sym(4) * cos(x_sym(3)); 31 | x_sym(4) * sin(x_sym(3)); 32 | (x_sym(4) / obj.L) * tan(u_sym(2)); 33 | u_sym(1)]; 34 | 35 | obj.dfdx = matlabFunction(jacobian(f,x_sym),'Vars',{x_sym,u_sym}); 36 | obj.dfdu = matlabFunction(jacobian(f,u_sym),'Vars',{x_sym,u_sym}); 37 | end 38 | 39 | function x_dot = dynamicsODE(obj, x, u) 40 | % Compute the time derivative of state for a particular x/u. 41 | % 4D (kinematic) bicycle model. Dynamics are as follows: 42 | % \dot x = v cos(phi) 43 | % \dot y = v sin(phi) 44 | % \dot phi = (v / L) tan(u2) 45 | % \dot v = u1 46 | % 47 | % Dynamics were taken from: 48 | % https://arxiv.org/abs/1711.03449 49 | 50 | x_dot(1,1) = x(4) * cos(x(3)); 51 | x_dot(2,1) = x(4) * sin(x(3)); 52 | x_dot(3,1) = (x(4) / obj.L) * tan(u(2)); 53 | x_dot(4,1) = u(1); 54 | end 55 | 56 | function x = integrate(obj, x0, u, dt) 57 | % Integrate initial state x0 (applying constant control u) over 58 | % a time interval of length dt. 59 | 60 | t = 0.0; 61 | x = x0; 62 | while t < obj.ts - 1e-8 63 | % Make sure we don't step past T. 64 | step = min(dt, obj.ts - t); 65 | 66 | % Use Runge-Kutta order 4 integration. For details please 67 | % refer to https://en.wikipedia.org/wiki/Runge-Kutta_methods. 68 | k1 = step * obj.dynamicsODE(x, u); 69 | k2 = step * obj.dynamicsODE(x + 0.5 * k1, u); 70 | k3 = step * obj.dynamicsODE(x + 0.5 * k2, u); 71 | k4 = step * obj.dynamicsODE(x + k3, u); 72 | 73 | x = x + (k1 + 2.0 * k2 + 2.0 * k3 + k4) / 6.0; 74 | t = t + step; 75 | end 76 | end 77 | 78 | function [Ac, Bc] = linearize(obj, x0, u0) 79 | % Compute the Jacobian linearization of the dynamics for a 80 | % particular state x0 and control u0. Outputs A and B matrices 81 | % of a linear system: 82 | % \dot x - f(x0, u0) = A (x - x0) + B (u - u0) 83 | Ac = obj.dfdx(x0, u0); 84 | Bc = obj.dfdu(x0, u0); 85 | end 86 | 87 | function [Ad, Bd] = linearizeDiscrete(obj, x0, u0) 88 | % Compute the Jacobian linearization of the dynamics for a 89 | % particular state x0 and control u0. Outputs A and B matrices 90 | % of a discrete-time linear system: 91 | % x(k + 1) - x0 = A (x(k) - x0) + B (u(k) - u0) 92 | [Ac, Bc] = obj.linearize(x0, u0); 93 | % sysc = ss(Ac,Bc,eye(obj.x_dim),[]); 94 | % sysd = c2d(sysc,obj.ts); 95 | % Ad = sysd.A; 96 | % Bd = sysd.B; 97 | 98 | % See https://en.wikipedia.org/wiki/Discretization#Discretization_of_linear_state_space_models 99 | % for derivation of discrete-time from continuous time linear 100 | % system. 101 | Ad = expm(Ac * obj.ts); 102 | Bd = pinv(Ac)*(Ad - eye(obj.x_dim))*Bc; 103 | end 104 | 105 | function P = LQRTerminalCost(obj, xF, Q, R) 106 | % Compute LQR terminal cost. 107 | [Ad, Bd] = obj.linearizeDiscrete(xF, [0;0]); 108 | [~,P,~] = dlqr(Ad,Bd,Q,R); 109 | end 110 | 111 | function P = LQRTerminalCost_no_px(obj, xF, Q, R) 112 | % Compute LQR terminal cost (no px). 113 | [Ad, Bd] = obj.linearizeDiscrete(xF, [0;0]); 114 | [~,P,~] = dlqr(Ad(2:4,2:4),Bd(2:4,:),Q(2:4,2:4),R); 115 | end 116 | 117 | function [uSol, xSol, VSol] = NMPC(obj, x0, xF, N, Q, R, ulb,... 118 | uub, plb, pub, vlb, vub, planner) 119 | % Compute an NMPC control. 120 | 121 | % yalmip('clear') 122 | params = planner.params; 123 | 124 | % Define solver settings. 125 | options = sdpsettings('verbose', 0, 'solver', 'snopt',... 126 | 'usex0', 0, 'cachesolvers', 1); 127 | 128 | %-------------------------------------------------------------- 129 | % Define decision variables 130 | %-------------------------------------------------------------- 131 | % States. 132 | x = sdpvar(obj.x_dim, N+1); 133 | 134 | % Inputs. 135 | u = sdpvar(obj.u_dim, N); 136 | 137 | %-------------------------------------------------------------- 138 | % Objective function 139 | %-------------------------------------------------------------- 140 | % DLQR-based terminal cost. 141 | [Ad, Bd] = obj.linearizeDiscrete(xF, [0;0]); 142 | [~,P,~] = dlqr(Ad(2:4,2:4),Bd(2:4,:),Q(2:4,2:4),R); 143 | 144 | objective = (x(2:4,N+1)-xF(2:4))'*P*(x(2:4,N+1)-xF(2:4)); 145 | for i = 1:N 146 | objective = objective + u(:,i)'*R*u(:,i)+... 147 | (x(2:4,i)-xF(2:4))'*Q(2:4,2:4)*(x(2:4,i)-xF(2:4)); 148 | end 149 | 150 | %-------------------------------------------------------------- 151 | % Bounds on the states and inputs 152 | %-------------------------------------------------------------- 153 | constraint = []; 154 | % Input constraints. 155 | for i = 1:N 156 | constraint = [constraint ulb<=u(:,i)<=uub]; 157 | end 158 | 159 | % State constraints. 160 | for i = 1:N+1 161 | constraint = [constraint plb<=x(3,i)<=pub,... % phi bounds 162 | constraint vlb<=x(4,i)<=vub,... % velocity bounds 163 | params.rd_bd_min<=x(2,i)<=params.rd_bd_max]; % road boundaries 164 | end 165 | 166 | %-------------------------------------------------------------- 167 | % Initial state constraints 168 | %-------------------------------------------------------------- 169 | constraint = [constraint x(:,1)==x0]; 170 | 171 | %-------------------------------------------------------------- 172 | % Dynamics constraints 173 | %-------------------------------------------------------------- 174 | for i = 1:N 175 | constraint = [constraint... 176 | x(:,i+1)==x(:,i)+obj.ts*obj.dynamicsODE(x(:,i),u(:,i))]; 177 | end 178 | 179 | %-------------------------------------------------------------- 180 | % Solve the MPC problem 181 | %-------------------------------------------------------------- 182 | status = optimize(constraint, objective, options); 183 | isSolved = (status.problem==0); 184 | if ~isSolved 185 | message = ['solver info: ', status.info ]; 186 | warning(message); 187 | end 188 | 189 | %-------------------------------------------------------------- 190 | % Return values 191 | %-------------------------------------------------------------- 192 | xSol = value(x); 193 | uSol = value(u); 194 | VSol = value(objective); 195 | end 196 | end % end methods 197 | end % end class 198 | 199 | -------------------------------------------------------------------------------- /util/iLQGames/dynamics/DubinsCar4D.m: -------------------------------------------------------------------------------- 1 | classdef DubinsCar4D 2 | % 4D Dubins car model. 3 | % Author: Haimin Hu 4 | % Reference: ilqgames by David Fridovich-Keil 5 | % Created: 2022-02-01, Last modified: 2022-02-01 6 | 7 | properties 8 | ts % Sampling time (s) 9 | x_dim % State dimension 10 | u_dim % Control dimension 11 | dfdx % Symbolic Jacobian matrix dfdx 12 | dfdu % Symbolic Jacobian matrix dfdu 13 | end 14 | 15 | methods 16 | function obj = DubinsCar4D(ts) 17 | % Constructor 18 | 19 | % Static parameters 20 | obj.ts = ts; 21 | obj.x_dim = 4; 22 | obj.u_dim = 2; 23 | 24 | % Define linearized dynamics symbolically 25 | x_sym = sym('x', [obj.x_dim 1]); 26 | u_sym = sym('u', [obj.u_dim 1]); 27 | 28 | f = [x_sym(4) * cos(x_sym(3)); 29 | x_sym(4) * sin(x_sym(3)); 30 | u_sym(2); 31 | u_sym(1)]; 32 | 33 | obj.dfdx = matlabFunction(jacobian(f,x_sym),'Vars',{x_sym,u_sym}); 34 | obj.dfdu = matlabFunction(jacobian(f,u_sym),'Vars',{x_sym,u_sym}); 35 | end 36 | 37 | function x_dot = dynamicsODE(obj, x, u) 38 | % Compute the time derivative of state for a particular x/u. 39 | % 4D Dubins car model. Dynamics are as follows: 40 | % \dot x = v cos(theta) 41 | % \dot y = v sin(theta) 42 | % \dot theta = u2 43 | % \dot v = u1 44 | % 45 | % `u1` is the acceleration. 46 | % `u2` is the steering angle. 47 | 48 | x_dot(1,1) = x(4) * cos(x(3)); 49 | x_dot(2,1) = x(4) * sin(x(3)); 50 | x_dot(3,1) = u(2); 51 | x_dot(4,1) = u(1); 52 | end 53 | 54 | function x = integrate(obj, x0, u, dt) 55 | % Integrate initial state x0 (applying constant control u) over 56 | % a time interval of length dt, using a time discretization of 57 | % dt 58 | 59 | t = 0.0; 60 | x = x0; 61 | while t < obj.ts - 1e-8 62 | % Make sure we don't step past T. 63 | step = min(dt, obj.ts - t); 64 | 65 | % Use Runge-Kutta order 4 integration. For details please 66 | % refer to: 67 | % https://en.wikipedia.org/wiki/Runge-Kutta_methods. 68 | k1 = step * obj.dynamicsODE(x, u); 69 | k2 = step * obj.dynamicsODE(x + 0.5 * k1, u); 70 | k3 = step * obj.dynamicsODE(x + 0.5 * k2, u); 71 | k4 = step * obj.dynamicsODE(x + k3, u); 72 | 73 | x = x + (k1 + 2.0 * k2 + 2.0 * k3 + k4) / 6.0; 74 | t = t + step; 75 | end 76 | end 77 | 78 | function [Ac, Bc] = linearize(obj, x0, u0) 79 | % Compute the Jacobian linearization of the dynamics for a 80 | % particular state x0 and control u0. Outputs A and B matrices 81 | % of a linear system: 82 | % \dot x - f(x0, u0) = A (x - x0) + B (u - u0) 83 | Ac = obj.dfdx(x0, u0); 84 | Bc = obj.dfdu(x0, u0); 85 | end 86 | 87 | function [Ad, Bd] = linearizeDiscrete(obj, x0, u0) 88 | % Compute the Jacobian linearization of the dynamics for a 89 | % particular state x0 and control u0. Outputs A and B matrices 90 | % of a discrete-time linear system: 91 | % x(k + 1) - x0 = A (x(k) - x0) + B (u(k) - u0) 92 | [Ac, Bc] = obj.linearize(x0, u0); 93 | % sysc = ss(Ac,Bc,eye(obj.x_dim),[]); 94 | % sysd = c2d(sysc,obj.ts); 95 | % Ad = sysd.A; 96 | % Bd = sysd.B; 97 | 98 | % See https://en.wikipedia.org/wiki/Discretization#Discretization_of_linear_state_space_models 99 | % for derivation of discrete-time from continuous time linear 100 | % system. 101 | Ad = expm(Ac * obj.ts); 102 | Bd = pinv(Ac)*(Ad - eye(obj.x_dim))*Bc; 103 | end 104 | 105 | function P = LQRTerminalCost(obj, xF, Q, R) 106 | % Compute LQR terminal cost. 107 | [Ad, Bd] = obj.linearizeDiscrete(xF, [0;0]); 108 | [~,P,~] = dlqr(Ad,Bd,Q,R); 109 | end 110 | 111 | function P = LQRTerminalCost_no_px(obj, xF, Q, R) 112 | % Compute LQR terminal cost (no px). 113 | [Ad, Bd] = obj.linearizeDiscrete(xF, [0;0]); 114 | [~,P,~] = dlqr(Ad(2:4,2:4),Bd(2:4,:),Q(2:4,2:4),R); 115 | end 116 | end % end methods 117 | end % end class 118 | 119 | -------------------------------------------------------------------------------- /util/iLQGames/dynamics/ProductMultiPlayerDynamicalSystem.m: -------------------------------------------------------------------------------- 1 | classdef ProductMultiPlayerDynamicalSystem 2 | % Implements a multiplayer dynamical system who's dynamics decompose 3 | % into a Cartesian product of single-player dynamical systems. 4 | % Author: Haimin Hu 5 | % Reference: ilqgames by David Fridovich-Keil 6 | % Created: 2021-11-10, Last modified: 2021-11-10 7 | 8 | properties 9 | ts % Sampling time (s) 10 | x_dim % State dimension 11 | x_dims % Cell of index lists of states of each player 12 | u_dim % Control dimension 13 | u_dims % Cell of index lists of controls of each player 14 | num_players % Number of players 15 | subsystems % Cell of DynamicalSystem objects 16 | is_linear % Flag indicating if all subsystems are linear 17 | Ac_lin % Continuous-time A matrix (only for linear case) 18 | Bc_lin % Continuous-time B matrix (only for linear case) 19 | Ad_lin % Discrete-time A matrix (only for linear case) 20 | Bd_lin % Discrete-time B matrix (only for linear case) 21 | end 22 | 23 | methods 24 | function obj = ProductMultiPlayerDynamicalSystem(ts, x_dim,... 25 | x_dims, u_dim, u_dims, subsystems, is_linear) 26 | % Constructor. 27 | obj.ts = ts; 28 | obj.x_dim = x_dim; 29 | obj.x_dims = x_dims; 30 | obj.u_dim = u_dim; 31 | obj.u_dims = u_dims; 32 | obj.num_players = length(subsystems); 33 | obj.subsystems = subsystems; 34 | 35 | if nargin < 7 36 | obj.is_linear = 0; 37 | else 38 | obj.is_linear = is_linear; 39 | end 40 | 41 | if obj.is_linear 42 | Ac = []; 43 | Bc = []; 44 | for i = 1:obj.num_players 45 | [Ac_i, Bc_i] = obj.subsystems{i}.linearize([], []); 46 | Ac = blkdiag(Ac, Ac_i); 47 | Bc = blkdiag(Bc, Bc_i); 48 | end 49 | obj.Ac_lin = Ac; 50 | obj.Bc_lin = Bc; 51 | sysc = ss(obj.Ac_lin, obj.Bc_lin, eye(obj.x_dim), []); 52 | sysd = c2d(sysc, obj.ts); 53 | obj.Ad_lin = sysd.A; 54 | obj.Bd_lin = sysd.B; 55 | end 56 | end 57 | 58 | function x = integrate(obj, x0, u, dt) 59 | % Integrate initial state x0 (applying constant control u) over 60 | % a time interval of length dt, using a time discretization of 61 | % dt. 62 | x = []; 63 | for i = 1:obj.num_players 64 | xi = x0(obj.x_dims{i}); 65 | ui = u(obj.u_dims{i}); 66 | xi = obj.subsystems{i}.integrate(xi, ui, dt); 67 | x = [x; xi]; 68 | end 69 | end 70 | 71 | function [Ac, Bc] = linearize(obj, x0, u0) 72 | % Compute the Jacobian linearization of the dynamics. 73 | Ac = []; 74 | Bc = []; 75 | for i = 1:obj.num_players 76 | x0_i = x0(obj.x_dims{i}); 77 | u0_i = u0(obj.u_dims{i}); 78 | [Ac_i, Bc_i] = obj.subsystems{i}.linearize(x0_i, u0_i); 79 | Ac = blkdiag(Ac, Ac_i); 80 | Bc = blkdiag(Bc, Bc_i); 81 | end 82 | end 83 | 84 | function [Ad, Bd] = linearizeDiscrete(obj, x0, u0) 85 | % Compute linearization & discretization of the system. 86 | [Ac, Bc] = obj.linearize(x0, u0); 87 | % sysc = ss(Ac,Bc,eye(obj.x_dim),[]); 88 | % sysd = c2d(sysc,obj.ts); 89 | % Ad = sysd.A; 90 | % Bd = sysd.B; 91 | 92 | Ad = expm(Ac * obj.ts); 93 | Bd = pinv(Ac)*(Ad - eye(obj.x_dim))*Bc; 94 | 95 | % Ad = eye(obj.x_dim) + Ac*obj.ts; 96 | % Bd = Bc*obj.ts; 97 | end 98 | 99 | function [Ad, Bd_cell] = linearizeDiscrete_B_cell(obj, x0, u0) 100 | % Compute linearization & discretization of the system, output 101 | % Bd as a cell. 102 | if obj.is_linear 103 | Ad = obj.Ad_lin; 104 | Bd_cell = {}; 105 | idx = 1; 106 | for i = 1:obj.num_players 107 | Bd_cell{end+1} =... 108 | obj.Bd_lin(:,idx:idx+obj.subsystems{i}.u_dim-1); 109 | idx = idx+obj.subsystems{i}.u_dim; 110 | end 111 | else 112 | [Ad, Bd] = obj.linearizeDiscrete(x0, u0); 113 | Bd_cell = {}; 114 | idx = 1; 115 | for i = 1:obj.num_players 116 | Bd_cell{end+1} =... 117 | Bd(:,idx:idx+obj.subsystems{i}.u_dim-1); 118 | idx = idx+obj.subsystems{i}.u_dim; 119 | end 120 | end 121 | end 122 | end % end methods 123 | end % end class 124 | 125 | -------------------------------------------------------------------------------- /util/iLQGames/geometry/LineSegment.m: -------------------------------------------------------------------------------- 1 | classdef LineSegment 2 | % Class for 2D line segments. 3 | % Author: Haimin Hu 4 | % Reference: ilqgames by David Fridovich-Keil 5 | % Created: 2021-11-08, Last modified: 2021-11-08 6 | 7 | properties 8 | p1 9 | p2 10 | end 11 | 12 | methods 13 | function obj = LineSegment(p1, p2) 14 | % Constructor 15 | obj.p1 = p1; 16 | obj.p2 = p2; 17 | end 18 | 19 | function out = len(obj) 20 | out = norm(obj.p1 - obj.p2); 21 | end 22 | 23 | function out = signed_distance_to(obj, point) 24 | % Compute signed distance to other point. 25 | % Sign convention is positive to the right and negative to the left, e.g.: 26 | % * 27 | % | 28 | % negative | positive 29 | % | 30 | % | 31 | % * 32 | 33 | % Vector from p1 to query. 34 | relative = point - obj.p1; 35 | 36 | % Compute the unit direction of this line segment. 37 | direction = obj.p2 - obj.p1; 38 | direction = direction / norm(direction); 39 | 40 | % Find signed length of projection and of cross product. 41 | projection = relative.x * direction.x + relative.y * direction.y; 42 | cross = relative.x * direction.y - direction.x * relative.y; 43 | if cross >= 0.0 44 | cross_sign = 1.0; 45 | else 46 | cross_sign = -1.0; 47 | end 48 | 49 | if projection < 0.0 50 | % Query lies behind this line segment, so closest distance 51 | % will be from p1. 52 | out = cross_sign * norm(relative); 53 | elseif projection > obj.len() 54 | % Closest distance will be to p2. 55 | out = cross_sign * norm(obj.p2 - point); 56 | else 57 | out = cross; 58 | end 59 | end 60 | end % end methods 61 | end % end class 62 | 63 | -------------------------------------------------------------------------------- /util/iLQGames/geometry/Point.m: -------------------------------------------------------------------------------- 1 | classdef Point 2 | % Point class for 2D points. 3 | % Author: Haimin Hu 4 | % Reference: ilqgames by David Fridovich-Keil 5 | % Created: 2021-11-08, Last modified: 2021-11-08 6 | 7 | properties 8 | x 9 | y 10 | end 11 | 12 | methods 13 | function obj = Point(x, y) 14 | % Constructor 15 | obj.x = x; 16 | obj.y = y; 17 | end 18 | 19 | function new_Point = plus(obj, rhs) 20 | new_Point = Point(obj.x + rhs.x, obj.y + rhs.y); 21 | end 22 | 23 | function new_Point = minus(obj, rhs) 24 | new_Point = Point(obj.x - rhs.x, obj.y - rhs.y); 25 | end 26 | 27 | function new_Point = mtimes(obj, rhs) 28 | new_Point = Point(obj.x * rhs, obj.y * rhs); 29 | end 30 | 31 | function new_Point = mrdivide(obj, rhs) 32 | new_Point = Point(obj.x / rhs, obj.y / rhs); 33 | end 34 | 35 | function out = norm_squared(obj) 36 | out = obj.x^2 + obj.y^2; 37 | end 38 | 39 | function out = norm(obj) 40 | out = sqrt(obj.norm_squared()); 41 | end 42 | end % end methods 43 | end % end class 44 | 45 | -------------------------------------------------------------------------------- /util/iLQGames/geometry/Polyline.m: -------------------------------------------------------------------------------- 1 | classdef Polyline 2 | % Polyline class to represent piecewise linear path in 2D. 3 | % Author: Haimin Hu 4 | % Reference: ilqgames by David Fridovich-Keil 5 | % Created: 2021-11-08, Last modified: 2021-11-08 6 | 7 | properties 8 | points 9 | end 10 | 11 | methods 12 | function obj = Polyline(points) 13 | % Initialize from a list of points. Keeps only a reference to 14 | % input list. 15 | obj.points = points; 16 | end 17 | 18 | function out = signed_distance_to(obj, point) 19 | % Compute signed distance from this polyline to the given point. 20 | % Sign convention is positive to the right and negative to the left, e.g.: 21 | % * 22 | % | 23 | % negative | positive 24 | % | 25 | % | 26 | % * 27 | % NOTE: for now, we'll just implement this with a naive linear search. 28 | % In future, if we want to optimize at all we can pass in a guess of 29 | % which index we expect the closest point to be close to. 30 | best_signed_distance = 1e8; 31 | for ii = 1:length(obj.points)-1 32 | segment = LineSegment(obj.points(ii), obj.points(ii + 1)); 33 | signed_distance = segment.signed_distance_to(point); 34 | 35 | if abs(signed_distance) < abs(best_signed_distance) 36 | best_signed_distance = signed_distance; 37 | end 38 | end 39 | out = best_signed_distance; 40 | end 41 | 42 | end % end methods 43 | end % end class 44 | 45 | -------------------------------------------------------------------------------- /util/iLQGames/getiLQcontrol.m: -------------------------------------------------------------------------------- 1 | function u_ik = getiLQcontrol(x_k, x_ref_k, u_ref_k, P_ik, alpha_ik,... 2 | alpha_scaling) 3 | % Computes iLQ control law. 4 | % 5 | % Author: Haimin Hu (last modified 2021.11.11) 6 | u_ik = u_ref_k - P_ik * (x_k - x_ref_k) - alpha_scaling * alpha_ik; 7 | end 8 | 9 | -------------------------------------------------------------------------------- /util/iLQGames/iLQSolver.m: -------------------------------------------------------------------------------- 1 | classdef iLQSolver 2 | % Iterative LQ solver. 3 | % Author: Haimin Hu 4 | % Reference: ilqgames by David Fridovich-Keil 5 | % Created: 2021-11-10, Last modified: 2021-11-10 6 | 7 | properties 8 | dynamics % Joint dynamics (an ProductMultiPlayerDynamicalSystem object) 9 | x0 % Initial states 10 | Ps_cell % Cell of P matrices (feedback gains, row = player, col = horizon) 11 | alphas_cell % Cell of alphas (feedforward terms, row = player, col = horizon) 12 | Zs_cell % Cell of Zs (value function matrices, row = player, col = horizon) 13 | player_costs_cell % Cell of PlayerCost (row = player, col = horizon) 14 | u_constraints_cell % Cell of constraints on controls (row = player) 15 | horizon % Game horizon 16 | num_players % Number of players 17 | alpha_scaling % Step size on the alpha 18 | max_iteration % Maximum number of iterations 19 | last_operating_point % Previous operating points 20 | current_operating_point % Current operating points 21 | best_operating_point % Best operating points 22 | tolerence_percentage % Convergence tolerence percentage ([0,1]) 23 | name % Name of the object 24 | end 25 | 26 | methods 27 | function obj = iLQSolver(dynamics, x0, Ps_cell, alphas_cell,... 28 | player_costs_cell, u_constraints_cell, alpha_scaling,... 29 | max_iteration, tolerence_percentage, name) 30 | % Construct an instance of this class. 31 | obj.dynamics = dynamics; 32 | obj.x0 = x0; 33 | obj.Ps_cell = Ps_cell; 34 | obj.alphas_cell = alphas_cell; 35 | obj.player_costs_cell = player_costs_cell; 36 | obj.u_constraints_cell = u_constraints_cell; 37 | obj.num_players = size(player_costs_cell, 1); 38 | obj.horizon = size(player_costs_cell, 2); 39 | obj.alpha_scaling = alpha_scaling; 40 | obj.max_iteration = max_iteration; 41 | obj.tolerence_percentage = tolerence_percentage; 42 | obj.name = name; 43 | 44 | % Operating points consist of x, u, costs, Z. 45 | obj.last_operating_point = []; 46 | obj.current_operating_point = []; 47 | obj.best_operating_point = []; 48 | end 49 | 50 | function obj = run(obj, verbose) 51 | % Run the algorithm for the specified parameters. 52 | iteration = 0; 53 | Ps = obj.Ps_cell; 54 | alphas = obj.alphas_cell; 55 | Zs = cell(obj.num_players, obj.horizon + 1); 56 | while ~(obj.is_converged_cost(verbose)) &&... 57 | (iteration<=obj.max_iteration) 58 | 59 | % (1) Compute current operating point and update last one. 60 | [xs, us, costs] = obj.compute_operating_point(); 61 | obj.last_operating_point = obj.current_operating_point; 62 | obj.current_operating_point.xs = xs; 63 | obj.current_operating_point.us = us; 64 | obj.current_operating_point.costs = costs; 65 | obj.current_operating_point.Ps = Ps; 66 | obj.current_operating_point.alphas = alphas; 67 | obj.current_operating_point.Zs = Zs; 68 | 69 | % (2) Linearize about this operating point. Make sure to 70 | % stack appropriately since we will concatenate state 71 | % vectors but not control vectors, so that 72 | % ``` x_{k+1} - xs_k = A_k (x_k - xs_k) + 73 | % sum_i Bi_k (ui_k - uis_k) ``` 74 | As = cell(1, obj.horizon); 75 | Bs = cell(obj.num_players, obj.horizon); 76 | for k = 1:obj.horizon 77 | [A, B] = obj.dynamics.linearizeDiscrete_B_cell(... 78 | xs(:,k), us(:,k)); 79 | for i = 1:obj.num_players 80 | As{k} = A; 81 | Bs{i,k} = B{i}; 82 | end 83 | end 84 | 85 | % (3) Quadraticize costs. 86 | ls = cell(obj.num_players, obj.horizon); 87 | Qs = cell(obj.num_players, obj.horizon); 88 | Rs = cell(obj.num_players, obj.horizon); 89 | for i = 1:obj.num_players 90 | for k = 1:obj.horizon 91 | us_ik = us(obj.dynamics.u_dims{i}, k); 92 | [~, l, Q, R] =... 93 | obj.player_costs_cell{i,k}.quadraticize(xs(:,k), us_ik); 94 | ls{i,k} = l; 95 | Qs{i,k} = Q; 96 | Rs{i,k} = R; 97 | end 98 | end 99 | 100 | % (4) Compute feedback Nash equilibrium of the resulting 101 | % LQ game. 102 | [Ps, alphas, Zs] = obj.solve_lq_game(As, Bs, ls, Qs, Rs); 103 | 104 | % Report time-accumulated cost for all players. 105 | if verbose 106 | disp(['Total cost for all players: ',... 107 | num2str(sum(costs,2)')]) 108 | end 109 | 110 | % Update the member variables. 111 | obj.Ps_cell = Ps; 112 | obj.alphas_cell = alphas; 113 | 114 | % (5) Check if the current operating point is the best. 115 | if isempty(obj.best_operating_point) 116 | obj.best_operating_point = obj.current_operating_point; 117 | best_cost = sum(costs(:)); 118 | elseif isempty(obj.best_operating_point.Zs{1,1}) 119 | obj.best_operating_point = obj.current_operating_point; 120 | best_cost = sum(costs(:)); 121 | elseif sum(costs(:)) < best_cost &&... 122 | ~isempty(obj.current_operating_point.Zs{1,1}) 123 | obj.best_operating_point = obj.current_operating_point; 124 | best_cost = sum(costs(:)); 125 | end 126 | 127 | iteration = iteration + 1; 128 | end 129 | end 130 | 131 | function [xs, us, costs] = compute_operating_point(obj) 132 | % Compute current operating point by propagating through 133 | % the dynamics. 134 | % OUTPUT: 135 | % xs: x_dim-by-horizon 136 | % us: u_dim-by-horizon 137 | % costs: num_player-by-horizon 138 | xs = obj.x0; 139 | us = []; 140 | costs = []; 141 | 142 | for k = 1:obj.horizon 143 | % Get current state and control. 144 | if isempty(obj.current_operating_point) 145 | current_x = zeros(obj.dynamics.x_dim,1); 146 | current_u = zeros(obj.dynamics.u_dim,1); 147 | else 148 | current_x = obj.current_operating_point.xs(:,k); 149 | current_u = obj.current_operating_point.us(:,k); 150 | end 151 | 152 | % Compute control for each player. 153 | uk = []; 154 | for i = 1:obj.num_players 155 | % Feedback control law for Player i. 156 | x_ref = current_x; 157 | u_ref = current_u(obj.dynamics.u_dims{i}); 158 | P_ik = obj.Ps_cell{i,k}; 159 | alpha_ik = obj.alphas_cell{i,k}; 160 | ui = getiLQcontrol(xs(:,k), x_ref, u_ref, P_ik,... 161 | alpha_ik, obj.alpha_scaling); 162 | 163 | % Clip the control. 164 | if ~isempty(obj.u_constraints_cell{i}) 165 | ui = max(min(ui, obj.u_constraints_cell{i}.max),... 166 | obj.u_constraints_cell{i}.min); 167 | end 168 | 169 | uk = [uk; ui]; 170 | end 171 | us = [us uk]; 172 | 173 | % Compute cost for each player. 174 | costk = []; 175 | for i = 1:obj.num_players 176 | costi = obj.player_costs_cell{i,k}.cost(xs(:,k),... 177 | uk(obj.dynamics.u_dims{i})); 178 | costk = [costk; costi]; 179 | end 180 | costs = [costs costk]; 181 | 182 | % Evolve the dynamics. 183 | xs = [xs obj.dynamics.integrate(xs(:,k), uk,... 184 | obj.dynamics.ts)]; 185 | end 186 | end 187 | 188 | function flag = is_converged_cost(obj, verbose) 189 | % Check if the last two operating points are close enough 190 | % using costs. 191 | if isempty(obj.last_operating_point) 192 | flag = false; 193 | else 194 | last_costs = obj.last_operating_point.costs; 195 | current_costs = obj.current_operating_point.costs; 196 | for i = 1:obj.num_players 197 | last_cost = sum(last_costs(i,:)); 198 | current_cost = sum(current_costs(i,:)); 199 | if norm((current_cost - last_cost)/last_cost) >... 200 | obj.tolerence_percentage 201 | if verbose 202 | disp(['Player ', num2str(i),... 203 | ': (current_cost-last_cost)/last_cost = ',... 204 | num2str(((current_cost-last_cost)/last_cost)),... 205 | '. Convergence criterion not met!']) 206 | end 207 | flag = false; 208 | return 209 | end 210 | end 211 | flag = true; 212 | end 213 | end 214 | 215 | function [Ps, alphas, Zs] = solve_lq_game(obj, As, Bs, ls, Qs, Rs) 216 | % Function to solve time-varying finite horizon LQ game. 217 | % Please refer to Corollary 6.1 on pp. 279 of Dynamic 218 | % Noncooperative Game Theory (second edition) by Tamer Basar 219 | % and Geert Jan Olsder. 220 | 221 | % Initialization. 222 | Ps = cell(obj.num_players, obj.horizon); 223 | alphas = cell(obj.num_players, obj.horizon); 224 | Zs = cell(obj.num_players, obj.horizon + 1); 225 | zetas = cell(obj.num_players, obj.horizon + 1); 226 | for i = 1:obj.num_players 227 | Zs{i,end} = Qs{i,end}; 228 | zetas{i,end} = ls{i,end}; 229 | end 230 | 231 | % Cache dimensions. 232 | u_dims = obj.dynamics.u_dims; 233 | 234 | % Note: notation and variable naming closely follows that 235 | % introduced in the "Preliminary Notation for Corollary 6.1" 236 | % section, which may be found on pp. 279 of Basar and Olsder. 237 | % NOTE: we will assume that `c` from Basar and Olsder is 238 | % always `0`. 239 | 240 | % Backward coupled-Riccati 241 | for k = obj.horizon:-1:1 242 | 243 | % Compute Ps given previously computed Zs. 244 | % Refer to equation 6.17a in Basar and Olsder. 245 | % This will involve solving a system of matrix linear 246 | % equations of the form: 247 | % [S1s; S2s; ...] * [P1; P2; ...] = [Y1; Y2; ...]. 248 | S = []; 249 | for i = 1:obj.num_players 250 | Sis = []; 251 | for j = 1:obj.num_players 252 | if j == 1 253 | Sis = [Sis Rs{i,k}+Bs{i,k}'*Zs{i,k+1}*Bs{i,k}]; 254 | else 255 | Sis = [Sis Bs{i,k}'*Zs{i,k+1}*Bs{j,k}]; 256 | end 257 | end 258 | S = [S; Sis]; 259 | end 260 | 261 | Y = []; 262 | for i = 1:obj.num_players 263 | Y = [Y; Bs{i,k}'*Zs{i,k+1}*As{k}]; 264 | end 265 | 266 | % P = S \ Y; 267 | P = pinv(S)*Y; 268 | 269 | for i = 1:obj.num_players 270 | Ps{i,k} = P(u_dims{i},:); 271 | end 272 | 273 | % Compute F_k = A_k - B1_k P1_k - B2_k P2_k. 274 | % This is eq. 6.17c from Basar and Olsder. 275 | F = As{k}; 276 | for i = 1:obj.num_players 277 | F = F - Bs{i,k}*Ps{i,k}; 278 | end 279 | 280 | % Compute Zs to be the next step earlier in time. 281 | for i = 1:obj.num_players 282 | Z = F'*Zs{i,k+1}*F+Qs{i,k}+Ps{i,k}'*Rs{i,k}*Ps{i,k}; 283 | Zs{i,k} = Z; 284 | end 285 | 286 | % Compute alphas using previously computed zetas. 287 | % Refer to equation 6.17d in Basar and Olsder. 288 | % This will involve solving a system of linear matrix 289 | % equations of the form: 290 | % [S1s; S2s; ...] * [alpha1; alpha2; ..] = [Y1; Y2; ...]. 291 | % In fact, this is the same S matrix as before (just a 292 | % different Y). 293 | Y = []; 294 | for i = 1:obj.num_players 295 | Y = [Y; Bs{i,k}'*zetas{i,k+1}]; 296 | end 297 | 298 | % alpha = S \ Y; 299 | alpha = pinv(S)*Y; 300 | 301 | for i = 1:obj.num_players 302 | alphas{i,k} = alpha(u_dims{i},:); 303 | end 304 | 305 | % Compute beta_k = -B1_k alpha1 - B2_k alpha2_k. 306 | % This is eq. 6.17f in Basar and Olsder (with `c = 0`). 307 | beta = 0; 308 | for i = 1:obj.num_players 309 | beta = beta - Bs{i,k}*alphas{i,k}; 310 | end 311 | 312 | % Compute zetas to be the next step earlier in time. 313 | % This is Remark 6.3 in Basar and Olsder. 314 | for i = 1:obj.num_players 315 | zeta = F'*(zetas{i,k+1}+Zs{i,k+1}*beta)+ls{i,k} +... 316 | Ps{i,k}'*Rs{i,k}*alphas{i,k}; 317 | zetas{i,k} = zeta; 318 | end 319 | end 320 | end 321 | end % end methods 322 | end % end class 323 | 324 | -------------------------------------------------------------------------------- /util/node.m: -------------------------------------------------------------------------------- 1 | classdef node 2 | % Class for a node in the scenario tree. 3 | % Author: Haimin Hu 4 | % Created: 2021-12-11, Last modified: 2021-12-11 5 | 6 | properties 7 | idx % Node index. 8 | pre_node_idx % Index of the pre node. 9 | t % Time step. 10 | xR % Robot's state. 11 | xH % Human's state. 12 | uR % Robot's control. 13 | uH % Human's control. 14 | uH_nom % Human's nominal control (rational). 15 | eps % Soft constraint slack variables. 16 | theta_sample % Sample of theta (transformed). 17 | theta_sample_sn % Sample of theta (sampled from a std. normal). 18 | theta_distr % Distribution of theta. 19 | theta_distr_M_ws % Distribution of theta given M (for warmstart only). 20 | M_sample % Sample of M. 21 | M_sample_prob % Probability of sample of M. 22 | M_distr % Distribution of M. 23 | path_prob % Path transition probability of the node. 24 | xR_dim % Dimension of robot's state. 25 | xH_dim % Dimension of human's state. 26 | uR_dim % Dimension of robot's control. 27 | uH_dim % Dimension of human's control. 28 | theta_dim % Dimension of theta. 29 | M_dim % Dimension of M. 30 | xR_dims % Indices of robot's state. 31 | xH_dims % Indices of human's state. 32 | uR_dims % Indices of robot's control. 33 | uH_dims % Indices of human's control. 34 | end 35 | 36 | methods 37 | function obj = node(idx, pre_node_idx, t, path_prob,... 38 | theta_sample_sn, M_sample, xR_dim, xH_dim, uR_dim, uH_dim,... 39 | theta_dim, M_dim, xR_dims, xH_dims, uR_dims, uH_dims) 40 | % Constructor for the node class. 41 | obj.idx = idx; 42 | obj.pre_node_idx = pre_node_idx; 43 | obj.t = t; 44 | obj.xR = sdpvar(xR_dim,1); 45 | obj.xH = sdpvar(xH_dim,1); 46 | obj.uR = sdpvar(uR_dim,1); 47 | obj.uH = sdpvar(uH_dim,1); 48 | obj.uH_nom = sdpvar(uH_dim,1); 49 | obj.eps.RB_R = sdpvar(1,1); 50 | obj.eps.RB_H = sdpvar(1,1); 51 | obj.eps.CA = sdpvar(1,1); 52 | obj.theta_distr.mu = sdpvar(theta_dim,1); 53 | obj.theta_distr.covar = sdpvar(theta_dim,theta_dim,'diagonal'); 54 | obj.path_prob = path_prob; 55 | obj.theta_sample = sdpvar(theta_dim,1); 56 | obj.theta_sample_sn = theta_sample_sn; 57 | obj.M_sample = M_sample; 58 | obj.M_distr = sdpvar(M_dim,1); 59 | obj.xR_dim = xR_dim; 60 | obj.xH_dim = xH_dim; 61 | obj.uR_dim = uR_dim; 62 | obj.uH_dim = uH_dim; 63 | obj.theta_dim = theta_dim; 64 | obj.M_dim = M_dim; 65 | obj.xR_dims = xR_dims; 66 | obj.xH_dims = xH_dims; 67 | obj.uR_dims = uR_dims; 68 | obj.uH_dims = uH_dims; 69 | end 70 | 71 | 72 | function node_vec = branch(obj, M_set, extraArg) 73 | % Branches from this node to obtain descendants (for dual 74 | % control steps). 75 | node_vec = []; 76 | 77 | % Generate samples of theta from a std. normal (binary). 78 | sample_sn = extraArg.theta_sample_sn; 79 | theta_sample_sn_1 = [sample_sn;-sample_sn]; theta_prob_1 = 1/2; 80 | theta_sample_sn_2 = [-sample_sn;sample_sn]; theta_prob_2 = 1/2; 81 | theta_sample_sn_vec = [theta_sample_sn_1 theta_sample_sn_2]; 82 | theta_prob_vec = [theta_prob_1 theta_prob_2]; 83 | if obj.t >= extraArg.N_M + 1 84 | M_set = obj.M_sample; 85 | end 86 | 87 | for i = 1:numel(theta_prob_vec) 88 | % Create new nodes. 89 | for M = M_set 90 | new_node_path_prob = obj.path_prob * theta_prob_vec(i); 91 | 92 | new_node = node([], obj.idx, obj.t + 1,... 93 | new_node_path_prob, theta_sample_sn_vec(:,i), M,... 94 | obj.xR_dim, obj.xH_dim, obj.uR_dim, obj.uH_dim,... 95 | obj.theta_dim, obj.M_dim, obj.xR_dims,... 96 | obj.xH_dims, obj.uR_dims, obj.uH_dims); 97 | 98 | % Update the descendant node vector. 99 | node_vec = [node_vec new_node]; 100 | end 101 | end 102 | end 103 | 104 | 105 | function new_node = extend(obj) 106 | % Extends this node to obtain one descendant (for exploitation 107 | % steps). 108 | theta_sample_sn_1 = [0; 0]; % mean of the std. normal. 109 | 110 | new_node = node([], obj.idx, obj.t + 1, obj.path_prob,... 111 | theta_sample_sn_1, obj.M_sample, obj.xR_dim, obj.xH_dim,... 112 | obj.uR_dim, obj.uH_dim, obj.theta_dim, obj.M_dim,... 113 | obj.xR_dims, obj.xH_dims, obj.uR_dims, obj.uH_dims); 114 | end 115 | end % end methods 116 | end % end class 117 | 118 | -------------------------------------------------------------------------------- /util/plotSim.m: -------------------------------------------------------------------------------- 1 | function plotSim(planner, XR_in, XH_in, THETA, M_traj, option) 2 | % Plot simulation trajectories. 3 | % 4 | % Author: Haimin Hu (last modified 2021.11.11) 5 | 6 | params = planner.params; 7 | 8 | if option.N_interp == 1 9 | XR = XR_in; 10 | XH = XH_in; 11 | else 12 | % Interpolate data. 13 | XR = []; 14 | for i = 1:size(XR_in,1) 15 | XR = [XR; interp(XR_in(i,:), option.N_interp)]; 16 | end 17 | 18 | XH = []; 19 | for i = 1:size(XH_in,1) 20 | XH = [XH; interp(XH_in(i,:), option.N_interp)]; 21 | end 22 | end 23 | 24 | % Extend the road (default factor: 1.6). 25 | params.rd_len_ub = params.rd_len_ub*1.6; 26 | option.scale = 1; 27 | 28 | % Image rotation to align the front with the world-frame x-axis. 29 | option.rotation = 0; 30 | 31 | % Image coordinates of the centre of the back wheels. 32 | option.centre = [348; 203]; 33 | 34 | % Real world length for scaling. 35 | option.length = 4.5; 36 | option.fps = Inf; 37 | 38 | % Start and end time. 39 | if ~isempty(option.t_start) 40 | t_start = option.t_start; 41 | if t_start ~= 1 42 | t_start = t_start * option.N_interp; 43 | end 44 | else 45 | t_start = 1; 46 | end 47 | 48 | if ~isempty(option.t_end) 49 | t_end = option.t_end * option.N_interp; 50 | else 51 | t_end = length(XR); 52 | end 53 | 54 | % Transparency settings. 55 | if option.keep_traj && option.is_fading 56 | % alpha_vec = linspace(0.1,1,t_end-t_start+option.t_skip); 57 | alpha_vec = logspace(-1,0,t_end-t_start+option.t_skip); 58 | else 59 | alpha_vec = linspace(1,1,length(XR)); 60 | end 61 | 62 | % Figure setup. 63 | fs = 25; 64 | f = figure('Color','white'); 65 | 66 | % Plot in the relative coordinate. 67 | if strcmp(option.coordinate,'rel') 68 | f.Position = [1050 660 1500 665]; 69 | % Plot in the absolute coordinate. 70 | elseif strcmp(option.coordinate,'abs') 71 | f.Position = [0 660 3000 665]; 72 | else 73 | error('Invalid coordinate option') 74 | end 75 | set(gca,'FontSize',fs) 76 | if ~option.UI 77 | set(gca,'Visible','off') 78 | end 79 | hold on 80 | daspect([1,1,1]) 81 | 82 | % Initial (px,py) limits. 83 | road_h_lb = -XH_in(1,1)-4; % horizontal lower bound of the road 84 | if strcmp(option.coordinate,'rel') 85 | xlimSpan = [road_h_lb, params.rd_len_ub]; 86 | elseif strcmp(option.coordinate,'abs') 87 | xlimSpan = [params.rd_len_lb + XH(1,t_start),... 88 | params.rd_len_ub + XR(1,t_start)]; 89 | end 90 | ylimSpan = [params.rd_bd_min-1.5, params.rd_bd_max+1.5]; 91 | xlim(xlimSpan) 92 | ylim(ylimSpan) 93 | 94 | % Plot the roads. 95 | if strcmp(option.coordinate,'rel') 96 | % Road color. 97 | fill([road_h_lb,road_h_lb,params.rd_len_ub,params.rd_len_ub],... 98 | [params.rd_bd_min,params.rd_bd_max,params.rd_bd_max,... 99 | params.rd_bd_min],[191,191,191]/255,'LineStyle','none'); 100 | % Road boundaries. 101 | plot(linspace(road_h_lb,params.rd_len_ub,2),... 102 | linspace(params.rd_bd_min,params.rd_bd_min,2),... 103 | 'k-','LineWidth',5) 104 | plot(linspace(road_h_lb,params.rd_len_ub,2),... 105 | linspace(params.rd_bd_max,params.rd_bd_max,2),... 106 | 'k-','LineWidth',5) 107 | % Center line of the road. 108 | plot(linspace(road_h_lb,params.rd_len_ub,2),... 109 | linspace(0,0,2),'--','LineWidth',8,'Color',[255,255,255]/255) 110 | elseif strcmp(option.coordinate,'abs') 111 | road_end = XR(1,end)+params.rd_len_ub; 112 | % Road color. 113 | fill([xlimSpan(1),xlimSpan(1),road_end,road_end],... 114 | [params.rd_bd_min,params.rd_bd_max,params.rd_bd_max,... 115 | params.rd_bd_min],[191,191,191]/255); 116 | % Road boundaries. 117 | plot(linspace(params.rd_len_lb,road_end,2),... 118 | linspace(params.rd_bd_min,params.rd_bd_min,2),... 119 | 'k-','LineWidth',5) 120 | plot(linspace(params.rd_len_lb,road_end,2),... 121 | linspace(params.rd_bd_max,params.rd_bd_max,2),... 122 | 'k-','LineWidth',5) 123 | % Center line of the road. 124 | plot(linspace(params.rd_len_lb,road_end,2),... 125 | linspace(0,0,2),'--','LineWidth',8,'Color',[255,255,255]/255) 126 | end 127 | 128 | % Plot vehicle movements. 129 | cnt = 1; 130 | for t = t_start:t_end 131 | 132 | if mod(t-t_start,option.t_skip)~=0 && t~=t_end 133 | continue 134 | end 135 | 136 | % Human uncertainty model parameters. 137 | theta_distr_t = THETA(:, ceil(t / option.N_interp)); 138 | M_distr_t = M_traj(:, ceil(t / option.N_interp)); 139 | 140 | % Top-down view of the cars in the relative coordinate. 141 | if strcmp(option.coordinate,'rel') 142 | xR_plt = [XR(1,t)-XH(1,t); XR(2:3,t)]; 143 | [option.image,~,option.alpha] = imread('car_robot_y.png'); 144 | % Transparent snapshots. 145 | try 146 | option.alpha = option.alpha*alpha_vec(cnt); 147 | catch 148 | option.alpha = option.alpha*alpha_vec(end); 149 | end 150 | [~, hR] = plot_vehicle(xR_plt', 'model', option); 151 | xH_plt = [0; XH(2:3,t)]; 152 | [option.image, ~, option.alpha] = imread('car_human.png'); 153 | if ~option.keep_traj && t~=t_end && t>t_start 154 | delete(hH) 155 | end 156 | try 157 | option.alpha = option.alpha*alpha_vec(cnt); 158 | catch 159 | option.alpha = option.alpha*alpha_vec(end); 160 | end 161 | [~, hH] = plot_vehicle(xH_plt', 'model', option); 162 | % Top-down view of the cars in the absolute coordinate. 163 | elseif strcmp(option.coordinate,'abs') 164 | if t>t_start 165 | xlimSpan(1) = xlimSpan(1) + 0.8*(XH(1,t) -... 166 | XH(1,t-option.t_skip)); 167 | xlimSpan(2) = xlimSpan(2) + XR(1,t) -... 168 | XR(1,t-option.t_skip); 169 | xlim(xlimSpan) 170 | end 171 | xR_plt = XR(1:3,t); 172 | [option.image,~,option.alpha] = imread('car_robot_y.png'); 173 | % Transparent snapshots. 174 | option.alpha = option.alpha*alpha_vec(cnt); 175 | [~, hR] = plot_vehicle(xR_plt', 'model', option); 176 | xH_plt = XH(1:3,t); 177 | [option.image, ~, option.alpha] = imread('car_human.png'); 178 | if ~option.keep_traj && t~=t_end && t>t_start 179 | delete(hH) 180 | end 181 | option.alpha = option.alpha*alpha_vec(cnt); 182 | [~, hH] = plot_vehicle(xH_plt', 'model', option); 183 | end 184 | 185 | % Axis and title. 186 | if option.UI 187 | if strcmp(option.coordinate,'rel') 188 | xlabel('$p_x^r$','Interpreter','latex','FontSize',1.2*fs) 189 | elseif strcmp(option.coordinate,'abs') 190 | xlabel('$p_x$','Interpreter','latex','FontSize',1.2*fs) 191 | end 192 | ylabel('$p_y$','Interpreter','latex','FontSize',1.2*fs) 193 | title(['$t = $',... 194 | num2str((t-1)*planner.ts/option.N_interp,'%.1f'),... 195 | ', $\mu^{\theta^l} =$ [',... 196 | num2str(theta_distr_t.l.mu(1),'%.2f'),... 197 | ',', num2str(theta_distr_t.l.mu(2),'%.2f'),... 198 | '], $\mu^{\theta^r} =$ [',... 199 | num2str(theta_distr_t.r.mu(1),'%.2f'),... 200 | ',', num2str(theta_distr_t.r.mu(2),'%.2f'),... 201 | '], $P(M) =$ [', num2str(M_distr_t(1),'%.2f'),... 202 | ',', num2str(M_distr_t(2),'%.2f'), ']'],... 203 | 'Interpreter', 'latex', 'FontSize',1.2*fs) 204 | end 205 | 206 | % Pausing settings. 207 | if t == t_start 208 | pause(1.0) 209 | else 210 | pause(option.pause) 211 | end 212 | 213 | % Delete the last frame. 214 | if t~=t_end 215 | if ~option.keep_traj 216 | delete(hR) 217 | end 218 | end 219 | cnt = cnt + option.t_skip; 220 | end 221 | if ~option.keep_traj 222 | delete(hH) 223 | end 224 | end 225 | 226 | -------------------------------------------------------------------------------- /util/solveiLQgames.m: -------------------------------------------------------------------------------- 1 | function ilq_solvers = solveiLQgames(x, xH_dims, jointSys, jointSys_EH,... 2 | Ps_cell, Ps_cell_EH, alphas_cell, alphas_cell_EH,... 3 | player_costs_cell_l, player_costs_cell_r, player_costs_cell_EH_l,... 4 | player_costs_cell_EH_r, u_constraints_cell, alpha_scaling,... 5 | max_iteration, tolerence_percentage, verbose) 6 | % Solve iLQGames for different theta's and M's. 7 | % 8 | % Author: Haimin Hu (last modified 2021.11.11) 9 | 10 | % -> Human targets the left lane (M = 'l'). 11 | % - Altruistic Human. 12 | ilq_solver_AH_l = iLQSolver(jointSys, x, Ps_cell, alphas_cell,... 13 | player_costs_cell_l, u_constraints_cell, alpha_scaling,... 14 | max_iteration, tolerence_percentage, 'ilq_solver_AH_l'); 15 | ilq_solver_AH_l = ilq_solver_AH_l.run(verbose); 16 | 17 | % - Egoistic Human. 18 | ilq_solver_EH_l = iLQSolver(jointSys_EH, x(xH_dims), Ps_cell_EH,... 19 | alphas_cell_EH, player_costs_cell_EH_l, u_constraints_cell,... 20 | alpha_scaling, max_iteration, tolerence_percentage,... 21 | 'ilq_solver_EH_l'); 22 | ilq_solver_EH_l = ilq_solver_EH_l.run(verbose); 23 | 24 | % -> Human targets the right lane (M = 'r'). 25 | % - Altruistic Human. 26 | ilq_solver_AH_r = iLQSolver(jointSys, x, Ps_cell, alphas_cell,... 27 | player_costs_cell_r, u_constraints_cell, alpha_scaling,... 28 | max_iteration, tolerence_percentage, 'ilq_solver_AH_r'); 29 | ilq_solver_AH_r = ilq_solver_AH_r.run(verbose); 30 | 31 | % - Egoistic Human. 32 | ilq_solver_EH_r = iLQSolver(jointSys_EH, x(xH_dims), Ps_cell_EH,... 33 | alphas_cell_EH, player_costs_cell_EH_r, u_constraints_cell,... 34 | alpha_scaling, max_iteration, tolerence_percentage,... 35 | 'ilq_solver_EH_r'); 36 | ilq_solver_EH_r = ilq_solver_EH_r.run(verbose); 37 | 38 | ilq_solvers.AH_l = ilq_solver_AH_l; 39 | ilq_solvers.AH_r = ilq_solver_AH_r; 40 | ilq_solvers.EH_l = ilq_solver_EH_l; 41 | ilq_solvers.EH_r = ilq_solver_EH_r; 42 | end 43 | 44 | -------------------------------------------------------------------------------- /util/tree.m: -------------------------------------------------------------------------------- 1 | classdef tree 2 | % Class for a scenario tree. 3 | % Author: Haimin Hu 4 | % Created: 2021-12-11, Last modified: 2021-12-11 5 | 6 | properties 7 | Layer % Index set of nodes in each layer. 8 | N % Node set. 9 | num_node % Number of nodes. 10 | planner % The planner object. 11 | end 12 | 13 | methods 14 | function obj = tree(root_node, planner) 15 | % Constructor for the scenario tree. 16 | obj.Layer = cell(1,100); 17 | obj.Layer{1} = 1; 18 | if isnan(root_node.idx) 19 | root_node.idx = 1; 20 | end 21 | if root_node.t ~= 1 22 | root_node.t = 1; 23 | end 24 | obj.N = root_node; 25 | obj.num_node = 1; 26 | obj.planner = planner; 27 | end 28 | 29 | 30 | function obj = addNewNode(obj, node) 31 | % Adds a new node to the tree. 32 | obj.num_node = obj.num_node + 1; 33 | node.idx = obj.num_node; 34 | obj.N = [obj.N node]; 35 | 36 | % Update Layer. 37 | obj.Layer{node.t} = [obj.Layer{node.t} node.idx]; 38 | 39 | % Sanity check. 40 | if length(obj.N) ~= node.idx 41 | error('Node index incorrect! Check for bugs!') 42 | end 43 | end 44 | 45 | 46 | function obj = constructTree(obj, Nd, Ne, M_set, extraArg, verbose) 47 | % Constructs a scenario tree with Nd dual steps and Ne 48 | % exploitation steps. 49 | 50 | % ------- Begin tree construction ------- 51 | % if verbose 52 | % disp('Constructing the scenario tree...') 53 | % end 54 | 55 | % ------- Dual control steps ------- 56 | num_node_dual = 0; 57 | for k = 1:Nd 58 | for idx = obj.Layer{k} 59 | new_node_vec = obj.N(idx).branch(M_set, extraArg); 60 | for new_node = new_node_vec 61 | obj = obj.addNewNode(new_node); 62 | num_node_dual = num_node_dual + 1; 63 | end 64 | end 65 | end 66 | 67 | % ------- Exploitation steps ------- 68 | num_node_expl = 0; 69 | for k = Nd+1:Nd+Ne 70 | for idx = obj.Layer{k} 71 | new_node = obj.N(idx).extend(); 72 | obj = obj.addNewNode(new_node); 73 | num_node_expl = num_node_expl + 1; 74 | end 75 | end 76 | 77 | % ------- End tree construction ------- 78 | 79 | % Report construction results. 80 | % if verbose 81 | % disp('Tree construction completed!') 82 | % for k = 1:Nd+Ne+1 83 | % disp(['Layer #', num2str(k), ': ',... 84 | % num2str(length(obj.Layer{k})), ' node(s)']) 85 | % end 86 | % disp(['Number of dual control node(s): ',... 87 | % num2str(num_node_dual)]) 88 | % disp(['Number of exploitation node(s): ',... 89 | % num2str(num_node_expl)]) 90 | % end 91 | end % end constructTree 92 | end % end methods 93 | end % end class 94 | 95 | -------------------------------------------------------------------------------- /util/updateEmpiricalCost.m: -------------------------------------------------------------------------------- 1 | function J_em = updateEmpiricalCost(J_em, x, xF_R, uR, QR, RR) 2 | % Update the robot's empirical (closed-loop) cost. 3 | % 4 | % Author: Haimin Hu (last modified 2021.11.11) 5 | 6 | J_em = J_em + (x - xF_R)' * QR * (x - xF_R) + uR' * RR * uR; 7 | end 8 | 9 | -------------------------------------------------------------------------------- /util/updateM.m: -------------------------------------------------------------------------------- 1 | function M_distr = updateM(M_distr_now, theta_distr_now, x_next, uR,... 2 | f_x, B_cell, iLQ_result, planner, extraArg) 3 | % Update the distribution of M. 4 | % 5 | % Author: Haimin Hu (last modified 2021.11.11) 6 | 7 | fixed_values = 0; 8 | if isfield(extraArg, 'fixed_values') 9 | if extraArg.fixed_values 10 | fixed_values = 1; 11 | theta_distr_M_fixed = extraArg.theta_distr_M_fixed; 12 | end 13 | end 14 | 15 | xH_dims = planner.xH_dims; 16 | 17 | H_id = planner.H_id; 18 | R_id = planner.R_id; 19 | 20 | BH = B_cell{H_id}; 21 | BR = B_cell{R_id}; 22 | 23 | if fixed_values 24 | mu_theta_l = theta_distr_M_fixed.l.mu; 25 | mu_theta_r = theta_distr_M_fixed.r.mu; 26 | else 27 | mu_theta_l = theta_distr_now.l.mu; 28 | mu_theta_r = theta_distr_now.r.mu; 29 | end 30 | 31 | % Get iLQ results. 32 | uH_AH_l = iLQ_result.AH_l.uH; 33 | uH_EH_l = iLQ_result.EH_l.uH; 34 | uH_l = mu_theta_l(1) * uH_AH_l + mu_theta_l(2) * uH_EH_l; 35 | 36 | uH_AH_r = iLQ_result.AH_r.uH; 37 | uH_EH_r = iLQ_result.EH_r.uH; 38 | uH_r = mu_theta_r(1) * uH_AH_r + mu_theta_r(2) * uH_EH_r; 39 | 40 | % Propagate states with different M's. 41 | x_next_l = f_x + BH * uH_l + BR * uR; 42 | x_next_r = f_x + BH * uH_r + BR * uR; 43 | 44 | % Distribution update. 45 | if isfield(extraArg, 'scale') 46 | scale = extraArg.scale; 47 | else 48 | scale = 1e1; 49 | end 50 | 51 | % Compute surrogate weights. 52 | % weight_l = exp(-scale*norm(x_next_l(xH_dims) - x_next(xH_dims))^2); 53 | % weight_r = exp(-scale*norm(x_next_r(xH_dims) - x_next(xH_dims))^2); 54 | weight_l = exp(-scale*norm(x_next_l(2:3) - x_next(2:3))^2); 55 | weight_r = exp(-scale*norm(x_next_r(2:3) - x_next(2:3))^2); 56 | denom = weight_l * M_distr_now(1) + weight_r * M_distr_now(2); 57 | 58 | M_distr = [weight_l * M_distr_now(1) / denom; 59 | weight_r * M_distr_now(2) / denom]; 60 | 61 | M_distr = (1-planner.M_eps)*M_distr + planner.M_eps*... 62 | planner.M_distr_const; 63 | end 64 | 65 | -------------------------------------------------------------------------------- /util/updateTheta.m: -------------------------------------------------------------------------------- 1 | function [mu, Sigma] = updateTheta(M, mu_now, Sigma_now, Sigma_d,... 2 | x_next, u, f_x, B_cell, iLQ_result, planner, extraArg) 3 | % Update the conditional distribution of theta given M. 4 | % Assumes that Sigma_now and Sigma_d are diagonal matrices. 5 | % 6 | % Author: Haimin Hu (last modified 2021.11.11) 7 | 8 | fixed_values = 0; 9 | if isfield(extraArg, 'fixed_values') 10 | if extraArg.fixed_values 11 | fixed_values = 1; 12 | U_fixed = extraArg.U_fixed; 13 | mu_fixed = extraArg.mu_theta_fixed; 14 | end 15 | end 16 | 17 | uR_dims = planner.uR_dims; 18 | 19 | H_id = planner.H_id; 20 | R_id = planner.R_id; 21 | 22 | BH = B_cell{H_id}; 23 | BR = B_cell{R_id}; 24 | 25 | % Get iLQ results. 26 | if M == 'l' 27 | uH_AH = iLQ_result.AH_l.uH; 28 | Sigma_AH = iLQ_result.AH_l.Sigma; 29 | 30 | uH_EH = iLQ_result.EH_l.uH; 31 | Sigma_EH = iLQ_result.EH_l.Sigma; 32 | elseif M == 'r' 33 | uH_AH = iLQ_result.AH_r.uH; 34 | Sigma_AH = iLQ_result.AH_r.Sigma; 35 | 36 | uH_EH = iLQ_result.EH_r.uH; 37 | Sigma_EH = iLQ_result.EH_r.Sigma; 38 | end 39 | 40 | % Augment covariance. 41 | U = [uH_AH uH_EH]; 42 | 43 | if fixed_values 44 | Sigma_d_aug = Sigma_d +... 45 | mu_fixed(1)^2*BH*U_fixed*Sigma_AH*(BH*U_fixed)'+... 46 | mu_fixed(2)^2*BH*U_fixed*Sigma_EH*(BH*U_fixed)'; 47 | else 48 | Sigma_d_aug = Sigma_d+mu_now(1)^2*BH*U*Sigma_AH*(BH*U)'+... 49 | mu_now(2)^2*BH*U*Sigma_EH*(BH*U)'; 50 | end 51 | 52 | Sigma_now_inv = diag(1 ./ diag(Sigma_now)'); 53 | 54 | % Covariance update. 55 | Sigma_inv = Sigma_now_inv + (BH*U)'*inv(Sigma_d_aug)*(BH*U); 56 | Sigma = diag(1 ./ diag(Sigma_inv)'); 57 | 58 | % Mean update. 59 | uR = u(uR_dims); 60 | mu = Sigma * ((BH*U)'*inv(Sigma_d_aug)*(x_next - f_x - BR*uR) +... 61 | Sigma_now_inv*mu_now); 62 | end 63 | 64 | --------------------------------------------------------------------------------