This class computes the gains for preview control for a given discrete system. The discrete system is defined by three matrix A, b, c such as : More...
#include <PreviewControl/OptimalControllerSolver.hh>
Public Member Functions | |
OptimalControllerSolver (MAL_MATRIX(&A, double), MAL_MATRIX(&b, double), MAL_MATRIX(&c, double), double Q, double R, unsigned int Nl) | |
~OptimalControllerSolver () | |
void | ComputeWeights (unsigned int Mode) |
void | DisplayWeights () |
bool | GeneralizedSchur (MAL_MATRIX(&A, double), MAL_MATRIX(&B, double), MAL_VECTOR(&alphar, double), MAL_VECTOR(&alphai, double), MAL_VECTOR(&beta, double), MAL_MATRIX(&L, double), MAL_MATRIX(&R, double)) |
void | GetF (MAL_MATRIX(&LF, double)) |
void | GetK (MAL_MATRIX(&LK, double)) |
Static Public Attributes | |
static const unsigned int | MODE_WITHOUT_INITIALPOS =1 |
static const unsigned int | MODE_WITH_INITIALPOS =0 |
Protected Member Functions | |
MAL_MATRIX (m_A, double) | |
MAL_MATRIX (m_b, double) | |
MAL_MATRIX (m_c, double) | |
MAL_MATRIX (m_K, double) | |
MAL_MATRIX (m_F, double) | |
Protected Attributes | |
double | m_Q |
double | m_R |
int | m_Nl |
This class computes the gains for preview control for a given discrete system. The discrete system is defined by three matrix A, b, c such as :
The optimal critera considered here is :
where
the solution is then:
\f{eqnarray*} {\bf K} & \equiv & (R + {\bf b}^T{\bf Pb})^{-1}{\bf b}^T{\bf PA} \\ K_p(i) & \equiv & (R + {\bf b}^T{\bf Pb})^{-1}{\bf b}^T({\bf A}-{\bf bK})^{T*(i-1)}{\bf c}^TQ \\ \f} where \f$ {\bf P} \f$ is solution of the following Riccati equation: \f[ {\bf P} = {\bf A}^T {\bf PA} + {\bf c}^TQ{\bf c} - {\bf A}^T{\bf Pb}(R + {\bf b}^T{\bf Pb})^{-1}{\bf b}^T{\bf PA} \f] The resolution of the Riccati equation is taken from \ref Laub1979, and is based on a Schur form .
To suppress the problem of the initial CoM position, we can reformulate the discrete problem by posing the following:
with
The augmented system is then
with the following cost function:
the solution is then:
where
Alan J. Laub A Schur method for solving Algebraic Riccati Equations, IEEE Transaction on Automatic Control, Vol AC-24, No.6 December 1979
OptimalControllerSolver::OptimalControllerSolver | ( | MAL_MATRIX &, double | , |
MAL_MATRIX &, double | , | ||
MAL_MATRIX &, double | , | ||
double | Q, | ||
double | R, | ||
unsigned int | Nl | ||
) |
A constructor
References b, c, i, j, m_Nl, m_Q, m_R, MAL_MATRIX, MAL_MATRIX_NB_COLS, MAL_MATRIX_NB_ROWS, and MAL_MATRIX_RESIZE.
OptimalControllerSolver::~OptimalControllerSolver | ( | ) |
Destructor.
void OptimalControllerSolver::ComputeWeights | ( | unsigned int | Mode | ) |
Compute the weights Following the mode, there is a the inclusion of the P matrix inside the weights.
References GeneralizedSchur(), i, j, k, m_Nl, m_Q, m_R, MAL_INVERSE, MAL_MATRIX(), MAL_MATRIX_DIM, MAL_MATRIX_NB_COLS, MAL_MATRIX_NB_ROWS, MAL_MATRIX_RESIZE, MAL_MATRIX_SET_IDENTITY, MAL_RET_A_by_B, MAL_RET_TRANSPOSE, MAL_VECTOR_DIM, MODE_WITHOUT_INITIALPOS, and n.
Referenced by PatternGeneratorJRL::PreviewControl::ComputeOptimalWeights().
void OptimalControllerSolver::DisplayWeights | ( | ) |
Display the weights
bool OptimalControllerSolver::GeneralizedSchur | ( | MAL_MATRIX &, double | , |
MAL_MATRIX &, double | , | ||
MAL_VECTOR &, double | , | ||
MAL_VECTOR &, double | , | ||
MAL_VECTOR &, double | , | ||
MAL_MATRIX &, double | , | ||
MAL_MATRIX &, double | |||
) |
References dgges_(), i, info, MAL_MATRIX, MAL_MATRIX_FILL, MAL_MATRIX_NB_ROWS, MAL_MATRIX_RESIZE, MAL_RET_MATRIX_DATABLOCK, MAL_RET_TRANSPOSE, MAL_RET_VECTOR_DATABLOCK, MAL_VECTOR, MAL_VECTOR_FILL, MAL_VECTOR_RESIZE, n, and sb02ox().
Referenced by ComputeWeights().
void OptimalControllerSolver::GetF | ( | MAL_MATRIX &, double | ) |
To take matrix F aka the weights of the preview window .
References MAL_MATRIX.
Referenced by PatternGeneratorJRL::PreviewControl::ComputeOptimalWeights().
void OptimalControllerSolver::GetK | ( | MAL_MATRIX &, double | ) |
To take matrix K, aka the weight of the other part of the command
References MAL_MATRIX.
Referenced by PatternGeneratorJRL::PreviewControl::ComputeOptimalWeights().
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
The weights themselves
|
protected |
The size of the window for the preview
Referenced by ComputeWeights(), and OptimalControllerSolver().
|
protected |
The coefficent of the index criteria:
Referenced by ComputeWeights(), and OptimalControllerSolver().
|
protected |
Referenced by ComputeWeights(), and OptimalControllerSolver().
|
static |
|
static |
Referenced by PatternGeneratorJRL::PreviewControl::CallMethod(), PatternGeneratorJRL::PreviewControl::ComputeOptimalWeights(), ComputeWeights(), PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::Setup(), and PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::ZMPPreviewControlWithMultiBodyZMP().