- 3.0.2 optimal control module.
OptConProblemBase-impl.h
Go to the documentation of this file.
1 /**********************************************************************************************************************
2 This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich.
3 Licensed under the BSD-2 license (see LICENSE file in main directory)
4  **********************************************************************************************************************/
5 
6 #pragma once
7 
8 namespace ct {
9 namespace optcon {
10 
11 template <size_t STATE_DIM,
12  size_t CONTROL_DIM,
13  typename SYSTEM_T,
14  typename LINEAR_SYSTEM_T,
15  typename LINEARIZER_T,
16  typename SCALAR>
17 OptConProblemBase<STATE_DIM, CONTROL_DIM, SYSTEM_T, LINEAR_SYSTEM_T, LINEARIZER_T, SCALAR>::OptConProblemBase(
18  DynamicsPtr_t nonlinDynamics,
19  CostFunctionPtr_t costFunction,
20  LinearPtr_t linearSystem)
21  : tf_(0.0),
22  x0_(state_vector_t::Zero()),
23  controlledSystem_(nonlinDynamics),
24  costFunction_(costFunction),
25  linearizedSystem_(linearSystem),
26  inputBoxConstraints_(nullptr),
27  stateBoxConstraints_(nullptr),
28  generalConstraints_(nullptr)
29 {
30  if (linearSystem == nullptr) // no linearization provided
31  {
32  linearizedSystem_ = std::shared_ptr<LINEARIZER_T>(new LINEARIZER_T(controlledSystem_));
33  }
34 }
35 
36 
37 template <size_t STATE_DIM,
38  size_t CONTROL_DIM,
39  typename SYSTEM_T,
40  typename LINEAR_SYSTEM_T,
41  typename LINEARIZER_T,
42  typename SCALAR>
43 OptConProblemBase<STATE_DIM, CONTROL_DIM, SYSTEM_T, LINEAR_SYSTEM_T, LINEARIZER_T, SCALAR>::OptConProblemBase(
44  const time_t tf,
45  const state_vector_t& x0,
46  DynamicsPtr_t nonlinDynamics,
47  CostFunctionPtr_t costFunction,
48  LinearPtr_t linearSystem)
49  : OptConProblemBase(nonlinDynamics, costFunction, linearSystem) // delegating constructor
50 {
51  tf_ = tf;
52  x0_ = x0;
53 }
54 
55 
56 template <size_t STATE_DIM,
57  size_t CONTROL_DIM,
58  typename SYSTEM_T,
59  typename LINEAR_SYSTEM_T,
60  typename LINEARIZER_T,
61  typename SCALAR>
62 OptConProblemBase<STATE_DIM, CONTROL_DIM, SYSTEM_T, LINEAR_SYSTEM_T, LINEARIZER_T, SCALAR>::OptConProblemBase(
63  DynamicsPtr_t nonlinDynamics,
64  CostFunctionPtr_t costFunction,
65  ConstraintPtr_t inputBoxConstraints,
66  ConstraintPtr_t stateBoxConstraints,
67  ConstraintPtr_t generalConstraints,
68  LinearPtr_t linearSystem)
69  : OptConProblemBase(nonlinDynamics, costFunction, linearSystem) // delegating constructor
70 {
71  inputBoxConstraints_ = inputBoxConstraints;
72  stateBoxConstraints_ = stateBoxConstraints;
73  generalConstraints_ = generalConstraints;
74 }
75 
76 
77 template <size_t STATE_DIM,
78  size_t CONTROL_DIM,
79  typename SYSTEM_T,
80  typename LINEAR_SYSTEM_T,
81  typename LINEARIZER_T,
82  typename SCALAR>
83 OptConProblemBase<STATE_DIM, CONTROL_DIM, SYSTEM_T, LINEAR_SYSTEM_T, LINEARIZER_T, SCALAR>::OptConProblemBase(
84  const time_t tf,
85  const state_vector_t& x0,
86  DynamicsPtr_t nonlinDynamics,
87  CostFunctionPtr_t costFunction,
88  ConstraintPtr_t inputBoxConstraints,
89  ConstraintPtr_t stateBoxConstraints,
90  ConstraintPtr_t generalConstraints,
91  LinearPtr_t linearSystem)
92  : OptConProblemBase(nonlinDynamics,
93  costFunction,
94  inputBoxConstraints,
95  stateBoxConstraints,
96  generalConstraints,
97  linearSystem) // delegating constructor
98 {
99  tf_ = tf;
100  x0_ = x0;
101 }
102 
103 
104 template <size_t STATE_DIM,
105  size_t CONTROL_DIM,
106  typename SYSTEM_T,
107  typename LINEAR_SYSTEM_T,
108  typename LINEARIZER_T,
109  typename SCALAR>
110 void OptConProblemBase<STATE_DIM, CONTROL_DIM, SYSTEM_T, LINEAR_SYSTEM_T, LINEARIZER_T, SCALAR>::verify() const
111 {
112  if (!controlledSystem_)
113  {
114  throw std::runtime_error("Dynamic system not set");
115  }
116  if (!linearizedSystem_)
117  {
118  throw std::runtime_error("Linearized system not set");
119  }
120  if (!costFunction_)
121  {
122  throw std::runtime_error("Cost function not set");
123  }
124  if (tf_ < 0.0)
125  {
126  throw std::runtime_error("Time horizon should not be negative");
127  }
128 }
129 
130 
131 template <size_t STATE_DIM,
132  size_t CONTROL_DIM,
133  typename SYSTEM_T,
134  typename LINEAR_SYSTEM_T,
135  typename LINEARIZER_T,
136  typename SCALAR>
137 const typename OptConProblemBase<STATE_DIM, CONTROL_DIM, SYSTEM_T, LINEAR_SYSTEM_T, LINEARIZER_T, SCALAR>::DynamicsPtr_t
138 OptConProblemBase<STATE_DIM, CONTROL_DIM, SYSTEM_T, LINEAR_SYSTEM_T, LINEARIZER_T, SCALAR>::getNonlinearSystem() const
139 {
140  return controlledSystem_;
141 }
142 
143 
144 template <size_t STATE_DIM,
145  size_t CONTROL_DIM,
146  typename SYSTEM_T,
147  typename LINEAR_SYSTEM_T,
148  typename LINEARIZER_T,
149  typename SCALAR>
150 const typename OptConProblemBase<STATE_DIM, CONTROL_DIM, SYSTEM_T, LINEAR_SYSTEM_T, LINEARIZER_T, SCALAR>::LinearPtr_t
151 OptConProblemBase<STATE_DIM, CONTROL_DIM, SYSTEM_T, LINEAR_SYSTEM_T, LINEARIZER_T, SCALAR>::getLinearSystem() const
152 {
153  return linearizedSystem_;
154 }
155 
156 template <size_t STATE_DIM,
157  size_t CONTROL_DIM,
158  typename SYSTEM_T,
159  typename LINEAR_SYSTEM_T,
160  typename LINEARIZER_T,
161  typename SCALAR>
162 const typename OptConProblemBase<STATE_DIM, CONTROL_DIM, SYSTEM_T, LINEAR_SYSTEM_T, LINEARIZER_T, SCALAR>::
163  CostFunctionPtr_t
164  OptConProblemBase<STATE_DIM, CONTROL_DIM, SYSTEM_T, LINEAR_SYSTEM_T, LINEARIZER_T, SCALAR>::getCostFunction() const
165 {
166  return costFunction_;
167 }
168 
169 template <size_t STATE_DIM,
170  size_t CONTROL_DIM,
171  typename SYSTEM_T,
172  typename LINEAR_SYSTEM_T,
173  typename LINEARIZER_T,
174  typename SCALAR>
175 void OptConProblemBase<STATE_DIM, CONTROL_DIM, SYSTEM_T, LINEAR_SYSTEM_T, LINEARIZER_T, SCALAR>::setNonlinearSystem(
176  const DynamicsPtr_t dyn)
177 {
178  controlledSystem_ = dyn;
179 }
180 
181 template <size_t STATE_DIM,
182  size_t CONTROL_DIM,
183  typename SYSTEM_T,
184  typename LINEAR_SYSTEM_T,
185  typename LINEARIZER_T,
186  typename SCALAR>
187 void OptConProblemBase<STATE_DIM, CONTROL_DIM, SYSTEM_T, LINEAR_SYSTEM_T, LINEARIZER_T, SCALAR>::setLinearSystem(
188  const LinearPtr_t lin)
189 {
190  linearizedSystem_ = lin;
191 }
192 
193 template <size_t STATE_DIM,
194  size_t CONTROL_DIM,
195  typename SYSTEM_T,
196  typename LINEAR_SYSTEM_T,
197  typename LINEARIZER_T,
198  typename SCALAR>
199 void OptConProblemBase<STATE_DIM, CONTROL_DIM, SYSTEM_T, LINEAR_SYSTEM_T, LINEARIZER_T, SCALAR>::setCostFunction(
200  const CostFunctionPtr_t cost)
201 {
202  costFunction_ = cost;
203 }
204 
205 template <size_t STATE_DIM,
206  size_t CONTROL_DIM,
207  typename SYSTEM_T,
208  typename LINEAR_SYSTEM_T,
209  typename LINEARIZER_T,
210  typename SCALAR>
211 void OptConProblemBase<STATE_DIM, CONTROL_DIM, SYSTEM_T, LINEAR_SYSTEM_T, LINEARIZER_T, SCALAR>::setInputBoxConstraints(
212  const ConstraintPtr_t constraint)
213 {
214  inputBoxConstraints_ = constraint;
215  if (!inputBoxConstraints_->isInitialized())
216  inputBoxConstraints_->initialize();
217 }
218 
219 template <size_t STATE_DIM,
220  size_t CONTROL_DIM,
221  typename SYSTEM_T,
222  typename LINEAR_SYSTEM_T,
223  typename LINEARIZER_T,
224  typename SCALAR>
225 void OptConProblemBase<STATE_DIM, CONTROL_DIM, SYSTEM_T, LINEAR_SYSTEM_T, LINEARIZER_T, SCALAR>::setStateBoxConstraints(
226  const ConstraintPtr_t constraint)
227 {
228  stateBoxConstraints_ = constraint;
229  if (!stateBoxConstraints_->isInitialized())
230  stateBoxConstraints_->initialize();
231 }
232 
233 template <size_t STATE_DIM,
234  size_t CONTROL_DIM,
235  typename SYSTEM_T,
236  typename LINEAR_SYSTEM_T,
237  typename LINEARIZER_T,
238  typename SCALAR>
239 void OptConProblemBase<STATE_DIM, CONTROL_DIM, SYSTEM_T, LINEAR_SYSTEM_T, LINEARIZER_T, SCALAR>::setGeneralConstraints(
240  const ConstraintPtr_t constraint)
241 {
242  generalConstraints_ = constraint;
243  if (!generalConstraints_->isInitialized())
244  generalConstraints_->initialize();
245 }
246 
247 template <size_t STATE_DIM,
248  size_t CONTROL_DIM,
249  typename SYSTEM_T,
250  typename LINEAR_SYSTEM_T,
251  typename LINEARIZER_T,
252  typename SCALAR>
253 const typename OptConProblemBase<STATE_DIM, CONTROL_DIM, SYSTEM_T, LINEAR_SYSTEM_T, LINEARIZER_T, SCALAR>::
254  ConstraintPtr_t
255  OptConProblemBase<STATE_DIM, CONTROL_DIM, SYSTEM_T, LINEAR_SYSTEM_T, LINEARIZER_T, SCALAR>::getInputBoxConstraints()
256  const
257 {
258  return inputBoxConstraints_;
259 }
260 
261 template <size_t STATE_DIM,
262  size_t CONTROL_DIM,
263  typename SYSTEM_T,
264  typename LINEAR_SYSTEM_T,
265  typename LINEARIZER_T,
266  typename SCALAR>
267 const typename OptConProblemBase<STATE_DIM, CONTROL_DIM, SYSTEM_T, LINEAR_SYSTEM_T, LINEARIZER_T, SCALAR>::
268  ConstraintPtr_t
269  OptConProblemBase<STATE_DIM, CONTROL_DIM, SYSTEM_T, LINEAR_SYSTEM_T, LINEARIZER_T, SCALAR>::getStateBoxConstraints()
270  const
271 {
272  return stateBoxConstraints_;
273 }
274 
275 template <size_t STATE_DIM,
276  size_t CONTROL_DIM,
277  typename SYSTEM_T,
278  typename LINEAR_SYSTEM_T,
279  typename LINEARIZER_T,
280  typename SCALAR>
281 const typename OptConProblemBase<STATE_DIM, CONTROL_DIM, SYSTEM_T, LINEAR_SYSTEM_T, LINEARIZER_T, SCALAR>::
282  ConstraintPtr_t
283  OptConProblemBase<STATE_DIM, CONTROL_DIM, SYSTEM_T, LINEAR_SYSTEM_T, LINEARIZER_T, SCALAR>::getGeneralConstraints()
284  const
285 {
286  return generalConstraints_;
287 }
288 
289 template <size_t STATE_DIM,
290  size_t CONTROL_DIM,
291  typename SYSTEM_T,
292  typename LINEAR_SYSTEM_T,
293  typename LINEARIZER_T,
294  typename SCALAR>
295 const typename OptConProblemBase<STATE_DIM, CONTROL_DIM, SYSTEM_T, LINEAR_SYSTEM_T, LINEARIZER_T, SCALAR>::
296  state_vector_t
297  OptConProblemBase<STATE_DIM, CONTROL_DIM, SYSTEM_T, LINEAR_SYSTEM_T, LINEARIZER_T, SCALAR>::getInitialState() const
298 {
299  return x0_;
300 }
301 
302 template <size_t STATE_DIM,
303  size_t CONTROL_DIM,
304  typename SYSTEM_T,
305  typename LINEAR_SYSTEM_T,
306  typename LINEARIZER_T,
307  typename SCALAR>
308 void OptConProblemBase<STATE_DIM, CONTROL_DIM, SYSTEM_T, LINEAR_SYSTEM_T, LINEARIZER_T, SCALAR>::setInitialState(
309  const state_vector_t& x0)
310 {
311  x0_ = x0;
312 }
313 
314 template <size_t STATE_DIM,
315  size_t CONTROL_DIM,
316  typename SYSTEM_T,
317  typename LINEAR_SYSTEM_T,
318  typename LINEARIZER_T,
319  typename SCALAR>
320 typename OptConProblemBase<STATE_DIM, CONTROL_DIM, SYSTEM_T, LINEAR_SYSTEM_T, LINEARIZER_T, SCALAR>::time_t
321 OptConProblemBase<STATE_DIM, CONTROL_DIM, SYSTEM_T, LINEAR_SYSTEM_T, LINEARIZER_T, SCALAR>::getTimeHorizon() const
322 {
323  return tf_;
324 }
325 
326 template <size_t STATE_DIM,
327  size_t CONTROL_DIM,
328  typename SYSTEM_T,
329  typename LINEAR_SYSTEM_T,
330  typename LINEARIZER_T,
331  typename SCALAR>
332 void OptConProblemBase<STATE_DIM, CONTROL_DIM, SYSTEM_T, LINEAR_SYSTEM_T, LINEARIZER_T, SCALAR>::setTimeHorizon(
333  const time_t tf)
334 {
335  tf_ = tf;
336 }
337 
338 
339 } // namespace optcon
340 } // namespace ct
ct::optcon::OptConProblemBase< STATE_DIM, CONTROL_DIM, SCALAR >::CostFunctionPtr_t
std::shared_ptr< optcon::CostFunctionQuadratic< STATE_DIM, CONTROL_DIM, double > > CostFunctionPtr_t
Definition: OptConProblemBase.h:52
ct::optcon::OptConProblemBase< STATE_DIM, CONTROL_DIM, SCALAR >::ConstraintPtr_t
std::shared_ptr< optcon::LinearConstraintContainer< STATE_DIM, CONTROL_DIM, double > > ConstraintPtr_t
Definition: OptConProblemBase.h:53
SCALAR
CppAD::AD< CppAD::cg::CG< double > > SCALAR
ct::optcon::OptConProblemBase< STATE_DIM, CONTROL_DIM, SCALAR >::time_t
SCALAR ::time_t time_t
Definition: OptConProblemBase.h:54
ct::optcon::OptConProblemBase< STATE_DIM, CONTROL_DIM, SCALAR >::LinearPtr_t
std::shared_ptr< LINEAR_SYSTEM_T > LinearPtr_t
Definition: OptConProblemBase.h:51
x0
StateVector< state_dim > x0
Definition: ConstrainedNLOCTest.cpp:14
ct::optcon::OptConProblemBase
Definition: OptConProblemBase.h:40
ct::optcon::OptConProblemBase< STATE_DIM, CONTROL_DIM, SCALAR >::DynamicsPtr_t
std::shared_ptr< SCALAR > DynamicsPtr_t
Definition: OptConProblemBase.h:50