- 3.0.2 optimal control module.
OptVectorDms-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 namespace ct {
7 namespace optcon {
8 
9 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
11  : tpl::OptVector<SCALAR>(n), settings_(settings), numPairs_(settings.N_ + 1)
12 {
13  size_t currIndex = 0;
14 
15  for (size_t i = 0; i < numPairs_; i++)
16  {
17  pairNumToStateIdx_.insert(std::make_pair(i, currIndex));
18  currIndex += STATE_DIM;
19 
20  pairNumToControlIdx_.insert(std::make_pair(i, currIndex));
21  currIndex += CONTROL_DIM;
22  }
23  stateSolution_.resize(numPairs_);
24  inputSolution_.resize(numPairs_);
25 }
26 
27 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
30 {
31  size_t index = getStateIndex(pairNum);
32  return (this->x_.segment(index, STATE_DIM));
33 }
34 
35 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
38 {
39  size_t index = getControlIndex(pairNum);
40  return (this->x_.segment(index, CONTROL_DIM));
41 }
42 
43 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
46 {
47  for (size_t i = 0; i < numPairs_; i++)
48  stateSolution_[i] = getOptimizedState(i);
49 
50  return stateSolution_;
51 }
52 
53 
54 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
57 {
58  for (size_t i = 0; i < numPairs_; i++)
59  inputSolution_[i] = getOptimizedControl(i);
60 
61  return inputSolution_;
62 }
63 
64 
65 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
67 {
68  return pairNumToStateIdx_.find(pairNum)->second;
69 }
70 
71 
72 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
74 {
75  return pairNumToControlIdx_.find(pairNum)->second;
76 }
77 
78 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
80 {
81  size_t s_index = getStateIndex(0);
82  this->x_.segment(s_index, STATE_DIM) = x0;
83 }
84 
85 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
87 {
88  size_t s_index = pairNumToStateIdx_.find(settings_.N_)->second;
89  this->x_.segment(s_index, STATE_DIM) = xF;
90 }
91 
92 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
94  const state_vector_t& x_f,
95  const control_vector_t& u0)
96 {
97  size_t type = 1; // 0 = constant init guess, 1 = linearly interpolated between x0 and x_f
98 
99  // init the states s_i and controls q_i
100  for (size_t i = 0; i < numPairs_; i++)
101  {
102  size_t s_index = getStateIndex(i);
103  size_t q_index = getControlIndex(i);
104 
105  switch (type)
106  {
107  case 0:
108  {
109  this->xInit_.segment(s_index, STATE_DIM) = x0;
110  break;
111  }
112  case 1:
113  this->xInit_.segment(s_index, STATE_DIM) = x0 + (x_f - x0) * (i / (numPairs_ - 1));
114  break;
115  }
116  this->xInit_.segment(q_index, CONTROL_DIM) = u0;
117  }
118 }
119 
120 
121 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
123  const control_vector_array_t& u_init)
124 {
125  if (x_init.size() != numPairs_)
126  throw std::runtime_error("initial guess state trajectory not matching number of shots");
127  if (u_init.size() != numPairs_)
128  throw std::runtime_error("initial guess input trajectory not matching number of shots");
129 
130  for (size_t i = 0; i < numPairs_; i++)
131  {
132  size_t s_index = getStateIndex(i);
133  size_t q_index = getControlIndex(i);
134 
135  this->xInit_.segment(s_index, STATE_DIM) = x_init[i];
136  this->xInit_.segment(q_index, CONTROL_DIM) = u_init[i];
137  }
138 }
139 
140 
141 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
143 {
144  std::cout << "... printing solutions: " << std::endl;
145  std::cout << "x_solution" << std::endl;
147  for (size_t i = 0; i < x_sol.size(); ++i)
148  {
149  std::cout << x_sol[i].transpose() << std::endl;
150  }
151 
152  std::cout << "u_solution" << std::endl;
154  for (size_t i = 0; i < u_sol.size(); ++i)
155  {
156  std::cout << u_sol[i].transpose() << std::endl;
157  }
158 
159  std::cout << std::endl;
160  std::cout << " ... done." << std::endl;
161 }
162 
163 } // namespace optcon
164 } // namespace ct
size_t getStateIndex(const size_t pairNum) const
Returns the starting index for the state at shot pairNum inside the optimization vector.
Definition: OptVectorDms-impl.h:66
const control_vector_array_t & getOptimizedInputs()
Returns the optimized control inputs for all shots.
Definition: OptVectorDms-impl.h:56
VectorXs xInit_
Definition: OptVector.h:311
state_vector_t getOptimizedState(const size_t pairNum) const
Returns the optimized state for a specific shot.
Definition: OptVectorDms-impl.h:29
size_t getControlIndex(const size_t pairNum) const
Returns the starting index for the control input at shot pairNum inside the optimization vector...
Definition: OptVectorDms-impl.h:73
control_vector_t getOptimizedControl(const size_t pairNum) const
Returns the optimized control input for a specific shot.
Definition: OptVectorDms-impl.h:37
void changeDesiredState(const state_vector_t &xF)
Updates the final state.
Definition: OptVectorDms-impl.h:86
void printoutSolution()
Prints out the solution trajectories.
Definition: OptVectorDms-impl.h:142
void changeInitialState(const state_vector_t &x0)
Updates the initial state.
Definition: OptVectorDms-impl.h:79
const state_vector_array_t & getOptimizedStates()
Returns the optimized state for all shots.
Definition: OptVectorDms-impl.h:45
size_t N_
Definition: DmsSettings.h:51
DIMENSIONS::control_vector_array_t control_vector_array_t
Definition: OptVectorDms.h:48
DIMENSIONS::state_vector_array_t state_vector_array_t
Definition: OptVectorDms.h:47
CppAD::AD< CppAD::cg::CG< double > > SCALAR
Defines basic types used in the DMS algorithm.
Definition: DmsDimensions.h:18
for i
Definition: mpc_unittest_plotting.m:14
VectorXs x_
Definition: OptVector.h:310
void setInitGuess(const state_vector_t &x0, const state_vector_t &x_f, const control_vector_t &u0)
Sets an initial guess for the optimal solution. The optimal solution is set as a linear interpolation...
Definition: OptVectorDms-impl.h:93
Class containing and managing all the optimization variables used for in the NLP solver IPOPT and SNO...
Definition: OptVector.h:21
Defines the DMS settings.
Definition: DmsSettings.h:23
StateVector< state_dim > x0
Definition: ConstrainedNLOCTest.cpp:14
DIMENSIONS::state_vector_t state_vector_t
Definition: OptVectorDms.h:45
DIMENSIONS::control_vector_t control_vector_t
Definition: OptVectorDms.h:46
OptVectorDms(size_t n, const DmsSettings &settings)
Custom constructor.
Definition: OptVectorDms-impl.h:10