9 template <
size_t STATE_DIM,
size_t CONTROL_DIM,
typename SCALAR>
11 : tpl::
OptVector<
SCALAR>(n), settings_(settings), numPairs_(settings.N_ + 1)
15 for (
size_t i = 0;
i < numPairs_;
i++)
17 pairNumToStateIdx_.insert(std::make_pair(
i, currIndex));
18 currIndex += STATE_DIM;
20 pairNumToControlIdx_.insert(std::make_pair(
i, currIndex));
21 currIndex += CONTROL_DIM;
23 stateSolution_.resize(numPairs_);
24 inputSolution_.resize(numPairs_);
27 template <
size_t STATE_DIM,
size_t CONTROL_DIM,
typename SCALAR>
32 return (this->
x_.segment(index, STATE_DIM));
35 template <
size_t STATE_DIM,
size_t CONTROL_DIM,
typename SCALAR>
40 return (this->
x_.segment(index, CONTROL_DIM));
43 template <
size_t STATE_DIM,
size_t CONTROL_DIM,
typename SCALAR>
47 for (
size_t i = 0;
i < numPairs_;
i++)
50 return stateSolution_;
54 template <
size_t STATE_DIM,
size_t CONTROL_DIM,
typename SCALAR>
58 for (
size_t i = 0;
i < numPairs_;
i++)
61 return inputSolution_;
65 template <
size_t STATE_DIM,
size_t CONTROL_DIM,
typename SCALAR>
68 return pairNumToStateIdx_.find(pairNum)->second;
72 template <
size_t STATE_DIM,
size_t CONTROL_DIM,
typename SCALAR>
75 return pairNumToControlIdx_.find(pairNum)->second;
78 template <
size_t STATE_DIM,
size_t CONTROL_DIM,
typename SCALAR>
82 this->
x_.segment(s_index, STATE_DIM) =
x0;
85 template <
size_t STATE_DIM,
size_t CONTROL_DIM,
typename SCALAR>
88 size_t s_index = pairNumToStateIdx_.find(settings_.
N_)->second;
89 this->
x_.segment(s_index, STATE_DIM) = xF;
92 template <
size_t STATE_DIM,
size_t CONTROL_DIM,
typename SCALAR>
100 for (
size_t i = 0;
i < numPairs_;
i++)
109 this->
xInit_.segment(s_index, STATE_DIM) =
x0;
113 this->
xInit_.segment(s_index, STATE_DIM) = x0 + (x_f -
x0) * (
i / (numPairs_ - 1));
116 this->
xInit_.segment(q_index, CONTROL_DIM) = u0;
121 template <
size_t STATE_DIM,
size_t CONTROL_DIM,
typename SCALAR>
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");
130 for (
size_t i = 0;
i < numPairs_;
i++)
135 this->
xInit_.segment(s_index, STATE_DIM) = x_init[
i];
136 this->
xInit_.segment(q_index, CONTROL_DIM) = u_init[
i];
141 template <
size_t STATE_DIM,
size_t CONTROL_DIM,
typename SCALAR>
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)
149 std::cout << x_sol[
i].transpose() << std::endl;
152 std::cout <<
"u_solution" << std::endl;
154 for (
size_t i = 0;
i < u_sol.size(); ++
i)
156 std::cout << u_sol[
i].transpose() << std::endl;
159 std::cout << std::endl;
160 std::cout <<
" ... done." << std::endl;
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