ilqgames
A new real-time solver for large-scale differential games.
single_player_flat_car_6d.cpp
1 /*
2  * Copyright (c) 2019, The Regents of the University of California (Regents).
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are
7  * met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  *
12  * 2. Redistributions in binary form must reproduce the above
13  * copyright notice, this list of conditions and the following
14  * disclaimer in the documentation and/or other materials provided
15  * with the distribution.
16  *
17  * 3. Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS AS IS
22  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
25  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
26  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
27  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
28  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
29  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
30  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  *
33  * Please contact the author(s) of this library if you have any questions.
34  * Authors: David Fridovich-Keil ( dfk@eecs.berkeley.edu )
35  */
36 
37 ///////////////////////////////////////////////////////////////////////////////
38 //
39 // Single player dynamics modeling a car. 5 states and 2 control inputs.
40 // State is [x, y, theta, phi, v], control is [omega, a], and dynamics are:
41 // \dot px = v cos theta
42 // \dot py = v sin theta
43 // \dot theta = (v / L) * tan phi
44 // \dot phi = omega
45 // \dot v = a
46 // Please refer to
47 // https://www.sciencedirect.com/science/article/pii/S2405896316301215
48 // for further details.
49 //
50 ///////////////////////////////////////////////////////////////////////////////
51 
52 #include <ilqgames/dynamics/single_player_flat_car_6d.h>
53 #include <ilqgames/dynamics/single_player_flat_system.h>
54 #include <ilqgames/utils/types.h>
55 
56 #include <glog/logging.h>
57 
58 namespace ilqgames {
59 
60 // Constexprs for state indices.
61 const Dimension SinglePlayerFlatCar6D::kNumXDims = 6;
62 const Dimension SinglePlayerFlatCar6D::kPxIdx = 0;
63 const Dimension SinglePlayerFlatCar6D::kPyIdx = 1;
64 const Dimension SinglePlayerFlatCar6D::kThetaIdx = 2;
65 const Dimension SinglePlayerFlatCar6D::kPhiIdx = 3;
66 const Dimension SinglePlayerFlatCar6D::kVIdx = 4;
67 const Dimension SinglePlayerFlatCar6D::kAIdx = 5;
68 const Dimension SinglePlayerFlatCar6D::kVxIdx = 2;
69 const Dimension SinglePlayerFlatCar6D::kVyIdx = 3;
70 const Dimension SinglePlayerFlatCar6D::kAxIdx = 4;
71 const Dimension SinglePlayerFlatCar6D::kAyIdx = 5;
72 
73 // Constexprs for control indices.
74 const Dimension SinglePlayerFlatCar6D::kNumUDims = 2;
75 const Dimension SinglePlayerFlatCar6D::kOmegaIdx = 0;
76 const Dimension SinglePlayerFlatCar6D::kJerkIdx = 1;
77 
78 void SinglePlayerFlatCar6D::Partial(const VectorXf& xi,
79  std::vector<VectorXf>* grads,
80  std::vector<MatrixXf>* hesses) const {
81  CHECK_NOTNULL(grads);
82  CHECK_NOTNULL(hesses);
83 
84  // grads->clear();
85  // grads->resize(xi.size(), VectorXf::Zero(kNumXDims));
86 
87  // hesses->clear();
88  // hesses->resize(xi.size(), MatrixXf::Zero(kNumXDims, kNumXDims));
89 
90  if (grads->size() != xi.size())
91  grads->resize(xi.size(), VectorXf::Zero(kNumXDims));
92  else {
93  for (auto& grad : *grads) {
94  DCHECK_EQ(grad.size(), xi.size());
95  grad.setZero();
96  }
97  }
98 
99  if (hesses->size() != xi.size())
100  hesses->resize(xi.size(), MatrixXf::Zero(kNumXDims, kNumXDims));
101  else {
102  for (auto& hess : *hesses) {
103  DCHECK_EQ(hess.rows(), xi.size());
104  DCHECK_EQ(hess.cols(), xi.size());
105  hess.setZero();
106  }
107  }
108 
109  const float vx = xi(kVxIdx);
110  const float vy = xi(kVyIdx);
111  const float ax = xi(kAxIdx);
112  const float ay = xi(kAyIdx);
113  const float L = inter_axle_distance_;
114 
115  CHECK_GT(std::hypot(vx, vy), 1e-2);
116 
117  const float vx2 = vx * vx;
118  const float vx3 = vx2 * vx;
119  const float vx4 = vx3 * vx;
120  const float vx5 = vx4 * vx;
121  const float vx6 = vx5 * vx;
122  const float vx7 = vx6 * vx;
123  const float vx8 = vx7 * vx;
124  const float vx9 = vx8 * vx;
125 
126  const float vy2 = vy * vy;
127  const float vy3 = vy2 * vy;
128  const float vy4 = vy3 * vy;
129  const float vy5 = vy4 * vy;
130  const float vy6 = vy5 * vy;
131  const float vy7 = vy6 * vy;
132  const float vy8 = vy7 * vy;
133  const float vy9 = vy8 * vy;
134 
135  const float ax2 = ax * ax;
136  const float ax3 = ax2 * ax;
137  const float ay2 = ay * ay;
138  const float ay3 = ay2 * ay;
139 
140  const float L2 = L * L;
141 
142  const float norm_squared = vx2 + vy2;
143  const float norm = std::sqrt(norm_squared);
144  const float norm_ss = norm_squared * norm_squared;
145  const float norm_sss = norm_ss * norm_squared;
146  const float sqrt_norm_sss = std::sqrt(norm_sss);
147 
148  (*grads)[kPxIdx](kPxIdx) = 1.0;
149  (*grads)[kPyIdx](kPyIdx) = 1.0;
150  (*grads)[kThetaIdx](kVxIdx) = -vy / norm_squared;
151  (*grads)[kThetaIdx](kVyIdx) = vx / norm_squared;
152  (*grads)[kVIdx](kVxIdx) = vx / norm;
153  (*grads)[kVIdx](kVyIdx) = vy / norm;
154 
155  (*grads)[kAIdx](kVxIdx) = vy * (ax * vy - ay * vx) / sqrt_norm_sss;
156  (*grads)[kAIdx](kVyIdx) = vx * (ay * vx - ax * vy) / sqrt_norm_sss;
157  (*grads)[kAIdx](kAxIdx) = (*grads)[kVIdx](kVxIdx);
158  (*grads)[kAIdx](kAyIdx) = (*grads)[kVIdx](kVyIdx);
159  (*grads)[kPhiIdx](kVxIdx) =
160  (L * norm * (-2.0 * ay * vx2 + 3.0 * ax * vx * vy + ay * vy2)) /
161  (L * L * ax2 * vy2 - 2.0 * L2 * ax * ay * vx * vy + L2 * ay2 * vx2 + vx6 +
162  3.0 * vx4 * vy2 + 3.0 * vx2 * vy4 + vy4);
163  (*grads)[kPhiIdx](kVyIdx) =
164  -(L * norm * (ax * vx2 + 3.0 * ay * vx * vy - 2.0 * ax * vy2)) /
165  (L2 * ax * ax * vy2 - 2.0 * L2 * ax * ay * vx * vy + L2 * ay2 * vx2 +
166  vx6 + 3.0 * vx4 * vy2 + 3.0 * vx2 * vy4 + vy6);
167  (*grads)[kPhiIdx](kAxIdx) = -(L * vy * sqrt_norm_sss) /
168  (norm_ss * norm_squared + L2 * ax2 * vy2 +
169  L2 * ay2 * vx2 - 2.0 * L2 * ax * ay * vx * vy);
170  (*grads)[kPhiIdx](kAyIdx) = (L * vx * sqrt_norm_sss) /
171  (norm_ss * norm_squared + L2 * ax2 * vy2 +
172  L2 * ay2 * vx2 - 2.0 * L2 * ax * ay * vx * vy);
173 
174  (*hesses)[kThetaIdx](kVxIdx, kVxIdx) = 2.0 * vx * vy / norm_ss;
175  (*hesses)[kThetaIdx](kVxIdx, kVyIdx) = (vy * vy - vx * vx) / norm_ss;
176  (*hesses)[kThetaIdx](kVyIdx, kVxIdx) = (*hesses)[kThetaIdx](kVxIdx, kVyIdx);
177  (*hesses)[kThetaIdx](kVyIdx, kVyIdx) = -(*hesses)[kThetaIdx](kVxIdx, kVxIdx);
178  (*hesses)[kVIdx](kVxIdx, kVxIdx) = (vy * vy) / sqrt_norm_sss;
179  (*hesses)[kVIdx](kVxIdx, kVyIdx) = (-vx * vy) / sqrt_norm_sss;
180  (*hesses)[kVIdx](kVyIdx, kVxIdx) = (*hesses)[kVIdx](kVxIdx, kVyIdx);
181  (*hesses)[kVIdx](kVyIdx, kVyIdx) = (vx * vx) / sqrt_norm_sss;
182 
183  (*hesses)[kAIdx](kVxIdx, kVxIdx) =
184  -(vy * (-2.0 * ay * vx2 + 3.0 * ax * vx * vy + ay * vy2)) /
185  (sqrt_norm_sss * norm);
186  (*hesses)[kAIdx](kVxIdx, kVyIdx) =
187  -(ay * vx3 - 2.0 * ax * vx2 * vy - 2.0 * ay * vx * vy2 + ax * vy3) /
188  (sqrt_norm_sss * norm);
189  (*hesses)[kAIdx](kVxIdx, kAxIdx) = vy2 / sqrt_norm_sss;
190  (*hesses)[kAIdx](kVxIdx, kAyIdx) = -(vx * vy) / sqrt_norm_sss;
191  (*hesses)[kAIdx](kVyIdx, kVxIdx) = (*hesses)[kAIdx](kVxIdx, kVyIdx);
192  (*hesses)[kAIdx](kVyIdx, kVyIdx) =
193  -(vx * (ax * vx2 + 3.0 * ay * vx * vy - 2.0 * ax * vy2)) /
194  (sqrt_norm_sss * norm);
195  (*hesses)[kAIdx](kVyIdx, kAxIdx) = -(vx * vy) / sqrt_norm_sss;
196  (*hesses)[kAIdx](kVyIdx, kAyIdx) = vx2 / sqrt_norm_sss;
197  (*hesses)[kAIdx](kAxIdx, kVxIdx) = (*hesses)[kAIdx](kVxIdx, kAxIdx);
198  (*hesses)[kAIdx](kAxIdx, kVyIdx) = (*hesses)[kAIdx](kVyIdx, kAxIdx);
199  (*hesses)[kAIdx](kAyIdx, kVxIdx) = (*hesses)[kAIdx](kVxIdx, kAyIdx);
200  (*hesses)[kAIdx](kAyIdx, kVyIdx) = (*hesses)[kAIdx](kVyIdx, kAyIdx);
201 
202  const float denom =
203  (L2 * ax2 * vy2 - 2 * L2 * ax * ay * vx * vy + L2 * ay2 * vx2 + vx6 +
204  3 * vx4 * vy2 + 3 * vx2 * vy4 + vy6);
205  const float denom2 =
206  (norm_sss + L2 * ax2 * vy2 + L2 * ay2 * vx2 - 2 * L2 * ax * ay * vx * vy);
207  (*hesses)[kPhiIdx](kVxIdx, kVxIdx) =
208  -(L *
209  (-6 * L2 * ax3 * vx2 * vy3 - 3 * L2 * ax3 * vy5 +
210  12 * L2 * ax2 * ay * vx3 * vy2 + 3 * L2 * ax2 * ay * vx * vy4 -
211  8 * L2 * ax * ay2 * vx4 * vy - L2 * ax * ay2 * vx2 * vy3 -
212  2 * L2 * ax * ay2 * vy5 + 2 * L2 * ay3 * vx5 + L2 * ay3 * vx3 * vy2 +
213  2 * L2 * ay3 * vx * vy4 + 12 * ax * vx8 * vy + 33 * ax * vx6 * vy3 +
214  27 * ax * vx4 * vy5 + 3 * ax * vx2 * vy7 - 3 * ax * vy9 -
215  6 * ay * vx9 - 9 * ay * vx7 * vy2 + 9 * ay * vx5 * vy4 +
216  21 * ay * vx3 * vy6 + 9 * ay * vx * vy8)) /
217  (norm * denom * denom);
218  (*hesses)[kPhiIdx](kVxIdx, kVyIdx) =
219  (L *
220  (-3 * L2 * ax3 * vx3 * vy2 + 4 * L2 * ax2 * ay * vx4 * vy -
221  4 * L2 * ax2 * ay * vx2 * vy3 + L2 * ax2 * ay * vy5 -
222  L2 * ax * ay2 * vx5 + 4 * L2 * ax * ay2 * vx3 * vy2 -
223  4 * L2 * ax * ay2 * vx * vy4 + 3 * L2 * ay3 * vx2 * vy3 + 3 * ax * vx9 -
224  3 * ax * vx7 * vy2 - 27 * ax * vx5 * vy4 - 33 * ax * vx3 * vy6 -
225  12 * ax * vx * vy8 + 12 * ay * vx8 * vy + 33 * ay * vx6 * vy3 +
226  27 * ay * vx4 * vy5 + 3 * ay * vx2 * vy7 - 3 * ay * vy9)) /
227  (norm * denom * denom);
228  (*hesses)[kPhiIdx](kVxIdx, kAxIdx) =
229  (L * vy * norm *
230  (-3 * L2 * ax2 * vx * vy2 + 4 * L2 * ax * ay * vx2 * vy -
231  2 * L2 * ax * ay * vy3 - L2 * ay2 * vx3 + 2 * L2 * ay2 * vx * vy2 +
232  3 * vx7 + 9 * vx5 * vy2 + 9 * vx3 * vy4 + 3 * vx * vy6)) /
233  (denom * denom);
234  (*hesses)[kPhiIdx](kVxIdx, kAyIdx) =
235  (L * norm *
236  (4 * L2 * ax2 * vx2 * vy2 + L2 * ax2 * vy4 -
237  6 * L2 * ax * ay * vx3 * vy + 2 * L2 * ay2 * vx4 -
238  L2 * ay2 * vx2 * vy2 - 2 * vx8 - 5 * vx6 * vy2 - 3 * vx4 * vy4 +
239  vx2 * vy6 + vy8)) /
240  (denom * denom);
241  (*hesses)[kPhiIdx](kVyIdx, kVxIdx) = (*hesses)[kAIdx](kVxIdx, kVyIdx);
242  (*hesses)[kPhiIdx](kVyIdx, kVyIdx) =
243  (L * (2 * L2 * ax3 * vx4 * vy + L2 * ax3 * vx2 * vy3 +
244  2 * L2 * ax3 * vy5 - 2 * L2 * ax2 * ay * vx5 -
245  L2 * ax2 * ay * vx3 * vy2 - 8 * L2 * ax2 * ay * vx * vy4 +
246  3 * L2 * ax * ay2 * vx4 * vy + 12 * L2 * ax * ay2 * vx2 * vy3 -
247  3 * L2 * ay3 * vx5 - 6 * L2 * ay3 * vx3 * vy2 + 9 * ax * vx8 * vy +
248  21 * ax * vx6 * vy3 + 9 * ax * vx4 * vy5 - 9 * ax * vx2 * vy7 -
249  6 * ax * vy9 - 3 * ay * vx9 + 3 * ay * vx7 * vy2 +
250  27 * ay * vx5 * vy4 + 33 * ay * vx3 * vy6 + 12 * ay * vx * vy8)) /
251  (norm * denom * denom);
252  (*hesses)[kPhiIdx](kVyIdx, kAxIdx) =
253  (L * norm *
254  (L2 * ax2 * vx2 * vy2 - 2 * L2 * ax2 * vy4 +
255  6 * L2 * ax * ay * vx * vy3 - L2 * ay2 * vx4 -
256  4 * L2 * ay2 * vx2 * vy2 - vx8 - vx6 * vy2 + 3 * vx4 * vy4 +
257  5 * vx2 * vy6 + 2 * vy8)) /
258  (denom * denom);
259  (*hesses)[kPhiIdx](kVyIdx, kAyIdx) =
260  -(L * vx * norm *
261  (2 * L2 * ax2 * vx2 * vy - L2 * ax2 * vy3 - 2 * L2 * ax * ay * vx3 +
262  4 * L2 * ax * ay * vx * vy2 - 3 * L2 * ay2 * vx2 * vy + 3 * vx6 * vy +
263  9 * vx4 * vy3 + 9 * vx2 * vy5 + 3 * vy7)) /
264  (denom * denom);
265  (*hesses)[kPhiIdx](kAxIdx, kVxIdx) = (*hesses)[kPhiIdx](kVxIdx, kAxIdx);
266  (*hesses)[kPhiIdx](kAxIdx, kVyIdx) = (*hesses)[kPhiIdx](kVyIdx, kAxIdx);
267  (*hesses)[kPhiIdx](kAxIdx, kAxIdx) =
268  (L * vy * sqrt_norm_sss * (2 * ax * L2 * vy2 - 2 * ay * vx * L2 * vy)) /
269  (denom2 * denom2);
270  (*hesses)[kPhiIdx](kAxIdx, kAyIdx) =
271  (L * vy * sqrt_norm_sss * (2 * ay * L2 * vx2 - 2 * ax * vy * L2 * vx)) /
272  (denom2 * denom2);
273  (*hesses)[kPhiIdx](kAyIdx, kVxIdx) = (*hesses)[kPhiIdx](kVxIdx, kAyIdx);
274  (*hesses)[kPhiIdx](kAyIdx, kVyIdx) = (*hesses)[kPhiIdx](kVyIdx, kAyIdx);
275  (*hesses)[kPhiIdx](kAyIdx, kAxIdx) = (*hesses)[kPhiIdx](kAxIdx, kAyIdx);
276  (*hesses)[kPhiIdx](kAyIdx, kAyIdx) =
277  -(L * vx * sqrt_norm_sss * (2 * ay * L2 * vx2 - 2 * ax * vy * L2 * vx)) /
278  (denom2 * denom2);
279 }
280 
281 } // namespace ilqgames
Generated by   doxygen 1.8.11