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 :

$\begin{eqnarray*} {\bf x}_{k+1} & =& {\bf A} x_k + {\bf b} u_k \\ p_k &=& {\bf cx}_k\\ \end{eqnarray*}$

. More...

#include <PreviewControl/OptimalControllerSolver.hh>

List of all members.

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

Detailed Description

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 :

$\begin{eqnarray*} {\bf x}_{k+1} & =& {\bf A} x_k + {\bf b} u_k \\ p_k &=& {\bf cx}_k\\ \end{eqnarray*}$

.

The optimal critera considered here is :

$J = \sum^{\infty}_{j=1} \{ Q(p^{ref}_j -p_j)^2 + Ru_j^2 \}$

where $ Q $ and $ R $ are also given as inputs.

the solution is then:

$\begin{eqnarray*} u_j = - {\bf K}x_k + [ f_1, f_2, \cdots, f_N] \left[ \begin{matrix} p^{ref}_{k+1} \\ \vdots \\ p^{ref}_{k+N} \end{matrix} \right] \end{eqnarray*}$

$\begin{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 \\ \end{eqnarray*}$

where ${\bf P}$ is solution of the following Riccati equation:

${\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}$

The resolution of the Riccati equation is taken from 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:

$\begin{eqnarray*} \begin{matrix} {\bf x}^*_{k+1} &= \widetilde{\bf A} {\bf x}^*_{k} + \widetilde{\bf b}\Delta u_k \\ p_k &= \widetilde{\bf c}{\bf x}^*_{k} \end{matrix} \end{eqnarray*}$

with

$\begin{eqnarray*} \Delta u_k \equiv u_k - u_{k-1} & \Delta {\bf x}_k \equiv {\bf x}_k - {\bf x}_{k-1}\\ {\bf x}_k \equiv \left[ \begin{matrix} p_k\\ \Delta {\bf x}_k \end{matrix} \right] \end{eqnarray*}$

The augmented system is then

$\begin{eqnarray*} \widetilde{\bf A} &\equiv & \left[ \begin{matrix} 1 & {\bf cA} \\ {\bf 0} & {\bf A} \\ \end{matrix} \right] \\ \tilde{\bf b} & \equiv & \left[ \begin{matrix} {\bf cb} \\ {\bf c} \end{matrix} \right] \\ \tilde{\bf c} & \equiv & [ 1 \; 0 \; 0 \; 0] \\ \end{eqnarray*}$

with the following cost function:

$J = \sum^{\infty}_{j=1} \{ Q(p^{ref}_j -p_j)^2 + R \Delta u_j^2 \}$

the solution is then:

$\begin{eqnarray*} u_j = - K_1 \sum_{i=0}^k e(i) - {\bf K}_2 x(k) - \sum_{j=1}^{N_L} K_p(j)p^{ref}_j(k+j) \end{eqnarray*}$

where

$\begin{eqnarray*} \left[ \begin{matrix} K_1 \\ {\bf K}_2 \\ \end{matrix} \right]= \widetilde{\bf K} \end{eqnarray*}$

Alan J. Laub A Schur method for solving Algebraic Riccati Equations, IEEE Transaction on Automatic Control, Vol AC-24, No.6 December 1979

Constructor & Destructor Documentation

OptimalControllerSolver::OptimalControllerSolver	(	MAL_MATRIX &,	double,
		MAL_MATRIX &,	double,
		MAL_MATRIX &,	double,
		double	Q,
		double	R,
		unsigned int	Nl
	)

A constructor

References b, c, MAL_MATRIX, MAL_MATRIX_NB_COLS, MAL_MATRIX_NB_ROWS, and MAL_MATRIX_RESIZE.

OptimalControllerSolver::~OptimalControllerSolver ( )

Destructor.

Member Function Documentation

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 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, 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_(), 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().

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().

PatternGeneratorJRL::OptimalControllerSolver::MAL_MATRIX	(	m_A	,
		double
	)		`[protected]`

The matrices needed for the dynamical system such as

$\begin{eqnarray*} {\bf x}_{k+1} & =& {\bf A} x_k + {\bf b} u_k \\ p_k &=& {\bf cx}_k\\ \end{eqnarray*}$

PatternGeneratorJRL::OptimalControllerSolver::MAL_MATRIX	(	m_b	,
		double
	)		`[protected]`

PatternGeneratorJRL::OptimalControllerSolver::MAL_MATRIX	(	m_c	,
		double
	)		`[protected]`

PatternGeneratorJRL::OptimalControllerSolver::MAL_MATRIX	(	m_K	,
		double
	)		`[protected]`

The weights themselves

PatternGeneratorJRL::OptimalControllerSolver::MAL_MATRIX	(	m_F	,
		double
	)		`[protected]`

Member Data Documentation

int PatternGeneratorJRL::OptimalControllerSolver::m_Nl [protected]

The size of the window for the preview

double PatternGeneratorJRL::OptimalControllerSolver::m_Q [protected]

The coefficent of the index criteria:

$J = \sum^{\infty}_{j=1} \{ Q(p^{ref}_j -p_j)^2 + Ru_j^2 \}$

double PatternGeneratorJRL::OptimalControllerSolver::m_R [protected]

const unsigned int PatternGeneratorJRL::OptimalControllerSolver::MODE_WITH_INITIALPOS = 0 [static]

Referenced by PatternGeneratorJRL::PreviewControl::CallMethod(), and PatternGeneratorJRL::PreviewControl::ComputeOptimalWeights().

const unsigned int PatternGeneratorJRL::OptimalControllerSolver::MODE_WITHOUT_INITIALPOS = 1 [static]

Referenced by PatternGeneratorJRL::PreviewControl::CallMethod(), PatternGeneratorJRL::PreviewControl::ComputeOptimalWeights(), PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::Setup(), and PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::ZMPPreviewControlWithMultiBodyZMP().

Public Member Functions

Static Public Attributes

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation