Created
March 29, 2020 12:48
-
-
Save tsaoyu/fd084f91c0aff21ce63dcf47d40649c2 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/*! | |
* Simple example how to linearize a system and design an LQR controller. | |
* | |
* \example LQR.cpp | |
*/ | |
#include <ct/optcon/optcon.h> // also includes ct_core | |
int main(int argc, char** argv) | |
{ | |
// get the state and control input dimension of the oscillator | |
const size_t state_dim = ct::core::SecondOrderSystem::STATE_DIM; | |
const size_t control_dim = ct::core::SecondOrderSystem::CONTROL_DIM; | |
// create an auto-differentiable instance of the oscillator dynamics | |
ct::core::ADCGScalar w_n(50.0); | |
std::shared_ptr<ct::core::ControlledSystem<state_dim, control_dim, ct::core::ADCGScalar>> oscillatorDynamics( | |
new ct::core::tpl::SecondOrderSystem<ct::core::ADCGScalar>(w_n)); | |
// create an Auto-Differentiation Linearizer with code generation on the quadrotor model (shared_ptr) | |
std::shared_ptr<ct::core::ADCodegenLinearizer<state_dim, control_dim>> adLinearizer(new ct::core::ADCodegenLinearizer<state_dim, control_dim>(oscillatorDynamics)); | |
// compile the linearized model just-in-time | |
adLinearizer->compileJIT(); | |
// define the linearization point around steady state | |
ct::core::StateVector<state_dim> x; | |
x.setZero(); | |
ct::core::ControlVector<control_dim> u; | |
u.setZero(); | |
double t = 0.0; | |
// compute the linearization around the nominal state using the Auto-Diff Linearizer | |
auto A = adLinearizer->getDerivativeState(x, u, t); | |
auto B = adLinearizer->getDerivativeControl(x, u, t); | |
// load the weighting matrices | |
ct::optcon::TermQuadratic<state_dim, control_dim> quadraticCost; | |
ct::core::StateMatrix<state_dim> Q = ct::core::StateMatrix<state_dim>::Identity(); | |
ct::core::ControlMatrix<control_dim> R = ct::core::ControlMatrix<control_dim>::Identity(); | |
// design the LQR controller | |
ct::optcon::LQR<state_dim, control_dim> lqrSolver; | |
ct::core::FeedbackMatrix<state_dim, control_dim> K; | |
std::cout << "A: " << std::endl << A << std::endl << std::endl; | |
std::cout << "B: " << std::endl << B << std::endl << std::endl; | |
std::cout << "Q: " << std::endl << Q << std::endl << std::endl; | |
std::cout << "R: " << std::endl << R << std::endl << std::endl; | |
lqrSolver.compute(Q, R, A, B, K); | |
std::cout << "LQR gain matrix:" << std::endl << K << std::endl; | |
ct::core::StateVector<state_dim> x0; | |
ct::core::Time timeHorizon = 1.0; | |
std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>> intermediateCost( | |
new ct::optcon::TermQuadratic<state_dim, control_dim>()); | |
std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>> finalCost( | |
new ct::optcon::TermQuadratic<state_dim, control_dim>()); | |
intermediateCost->setWeights(Q, R); | |
std::shared_ptr<ct::optcon::CostFunctionQuadratic<state_dim, control_dim>> costFunction( | |
new ct::optcon::CostFunctionAnalytical<state_dim, control_dim>()); | |
costFunction->addIntermediateTerm(intermediateCost); | |
ct::optcon::ContinuousOptConProblem<state_dim, control_dim, ct::core::ADCGScalar> optConProblem( | |
timeHorizon, x0, oscillatorDynamics, costFunction, adLinearizer); | |
return 1; | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
A working example: