Abstract class for differential action model. More...
#include <diff-action-base.hpp>
Public Types | |
typedef DifferentialActionDataAbstractTpl< Scalar > | DifferentialActionDataAbstract |
typedef MathBaseTpl< Scalar > | MathBase |
typedef MathBase::MatrixXs | MatrixXs |
typedef ScalarSelector< Scalar >::type | ScalarType |
typedef StateAbstractTpl< Scalar > | StateAbstract |
typedef MathBase::VectorXs | VectorXs |
Public Member Functions | |
DifferentialActionModelAbstractTpl (std::shared_ptr< StateAbstract > state, const std::size_t nu, const std::size_t nr=0, const std::size_t ng=0, const std::size_t nh=0, const std::size_t ng_T=0, const std::size_t nh_T=0) | |
Initialize the differential action model. | |
virtual void | calc (const std::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) |
Compute the total cost value for nodes that depends only on the state. | |
virtual void | calc (const std::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)=0 |
Compute the system acceleration and cost value. | |
virtual void | calcDiff (const std::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) |
Compute the derivatives of the cost functions with respect to the state only. | |
virtual void | calcDiff (const std::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)=0 |
Compute the derivatives of the dynamics and cost functions. | |
virtual bool | checkData (const std::shared_ptr< DifferentialActionDataAbstract > &data) |
Checks that a specific data belongs to this model. | |
virtual std::shared_ptr< DifferentialActionDataAbstract > | createData () |
Create the differential action data. | |
virtual const VectorXs & | get_g_lb () const |
Return the lower bound of the inequality constraints. | |
virtual const VectorXs & | get_g_ub () const |
Return the upper bound of the inequality constraints. | |
bool | get_has_control_limits () const |
Indicates if there are defined control limits. | |
virtual std::size_t | get_ng () const |
Return the number of inequality constraints. | |
virtual std::size_t | get_ng_T () const |
Return the number of inequality terminal constraints. | |
virtual std::size_t | get_nh () const |
Return the number of equality constraints. | |
virtual std::size_t | get_nh_T () const |
Return the number of equality terminal constraints. | |
std::size_t | get_nr () const |
Return the dimension of the cost-residual vector. | |
std::size_t | get_nu () const |
Return the dimension of the control input. | |
const std::shared_ptr< StateAbstract > & | get_state () const |
Return the state. | |
const VectorXs & | get_u_lb () const |
Return the control lower bound. | |
const VectorXs & | get_u_ub () const |
Return the control upper bound. | |
virtual void | print (std::ostream &os) const |
Print relevant information of the differential action model. | |
virtual void | quasiStatic (const std::shared_ptr< DifferentialActionDataAbstract > &data, Eigen::Ref< VectorXs > u, const Eigen::Ref< const VectorXs > &x, const std::size_t maxiter=100, const Scalar tol=Scalar(1e-9)) |
Computes the quasic static commands. | |
VectorXs | quasiStatic_x (const std::shared_ptr< DifferentialActionDataAbstract > &data, const VectorXs &x, const std::size_t maxiter=100, const Scalar tol=Scalar(1e-9)) |
void | set_g_lb (const VectorXs &g_lb) |
Modify the lower bound of the inequality constraints. | |
void | set_g_ub (const VectorXs &g_ub) |
Modify the upper bound of the inequality constraints. | |
void | set_u_lb (const VectorXs &u_lb) |
Modify the control lower bounds. | |
void | set_u_ub (const VectorXs &u_ub) |
Modify the control upper bounds. | |
Public Attributes | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar | Scalar |
Protected Member Functions | |
void | update_has_control_limits () |
Update the status of the control limits (i.e. if there are defined limits) | |
Protected Attributes | |
VectorXs | g_lb_ |
Lower bound of the inequality constraints. | |
VectorXs | g_ub_ |
Lower bound of the inequality constraints. | |
bool | has_control_limits_ |
std::size_t | ng_ |
Number of inequality constraints. | |
std::size_t | ng_T_ |
Number of inequality terminal constraints. | |
std::size_t | nh_ |
Number of equality constraints. | |
std::size_t | nh_T_ |
Number of equality terminal constraints. | |
std::size_t | nr_ |
Dimension of the cost residual. | |
std::size_t | nu_ |
Control dimension. | |
std::shared_ptr< StateAbstract > | state_ |
Model of the state. | |
VectorXs | u_lb_ |
Lower control limits. | |
VectorXs | u_ub_ |
Upper control limits. | |
VectorXs | unone_ |
Neutral state. | |
Friends | |
template<class Scalar > | |
class | ConstraintModelManagerTpl |
template<class Scalar > | |
class | IntegratedActionModelAbstractTpl |
template<class Scalar > | |
std::ostream & | operator<< (std::ostream &os, const DifferentialActionModelAbstractTpl< Scalar > &model) |
Print information on the differential action model. | |
Abstract class for differential action model.
A differential action model combines dynamics, cost and constraints models. We can use it in each node of our optimal control problem thanks to dedicated integration rules (e.g., IntegratedActionModelEulerTpl
or IntegratedActionModelRKTpl
). These integrated action models produce action models (ActionModelAbstractTpl
). Thus, every time that we want to describe a problem, we need to provide ways of computing the dynamics, cost, constraints functions and their derivatives. All these are described inside the differential action model.
Concretely speaking, the differential action model is the time-continuous version of an action model, i.e.,
\[ \begin{aligned} &\dot{\mathbf{v}} = \mathbf{f}(\mathbf{q}, \mathbf{v}, \mathbf{u}), &\textrm{(dynamics)}\\ &\ell(\mathbf{q}, \mathbf{v},\mathbf{u}) = \int_0^{\delta t} a(\mathbf{r}(\mathbf{q}, \mathbf{v},\mathbf{u}))\,dt, &\textrm{(cost)}\\ &\mathbf{g}(\mathbf{q}, \mathbf{v},\mathbf{u})<\mathbf{0}, &\textrm{(inequality constraint)}\\ &\mathbf{h}(\mathbf{q}, \mathbf{v},\mathbf{u})=\mathbf{0}, &\textrm{(equality constraint)} \end{aligned} \]
where
nq
-tuple,nv
dimension,ResidualModelAbstractTpl
and ActivationModelAbstractTpl
, respectively),Both configuration and velocity describe the system space \(\mathbf{x}=(\mathbf{q}, \mathbf{v})\in\mathcal{X}\) which lies in the state manifold. Note that the acceleration \(\dot{\mathbf{v}}\in
T_{\mathbf{q}}\mathcal{Q}\) lies also in the tangent space of the configuration manifold. The computation of these equations are carried out inside calc()
function. In short, this function computes the system acceleration, cost and constraints values (also called constraints violations). This procedure is equivalent to running a forward pass of the action model.
However, during numerical optimization, we also need to run backward passes of the differential action model. These calculations are performed by calcDiff()
. In short, this function builds a linear-quadratic approximation of the differential action model, i.e.,
\[ \begin{aligned} &\delta\dot{\mathbf{v}} = \mathbf{f_{q}}\delta\mathbf{q}+\mathbf{f_{v}}\delta\mathbf{v}+\mathbf{f_{u}}\delta\mathbf{u}, &\textrm{(dynamics)}\\ &\ell(\delta\mathbf{q},\delta\mathbf{v},\delta\mathbf{u}) = \begin{bmatrix}1 \\ \delta\mathbf{q} \\ \delta\mathbf{v} \\ \delta\mathbf{u}\end{bmatrix}^T \begin{bmatrix}0 & \mathbf{\ell_q}^T & \mathbf{\ell_v}^T & \mathbf{\ell_u}^T \\ \mathbf{\ell_q} & \mathbf{\ell_{qq}} & \mathbf{\ell_{qv}} & \mathbf{\ell_{uq}}^T \\ \mathbf{\ell_v} & \mathbf{\ell_{vq}} & \mathbf{\ell_{vv}} & \mathbf{\ell_{uv}}^T \\ \mathbf{\ell_u} & \mathbf{\ell_{uq}} & \mathbf{\ell_{uv}} & \mathbf{\ell_{uu}}\end{bmatrix} \begin{bmatrix}1 \\ \delta\mathbf{q} \\ \delta\mathbf{v} \\ \delta\mathbf{u}\end{bmatrix}, &\textrm{(cost)}\\ &\mathbf{g_q}\delta\mathbf{q}+\mathbf{g_v}\delta\mathbf{v}+\mathbf{g_u}\delta\mathbf{u}\leq\mathbf{0}, &\textrm{(inequality constraints)}\\ &\mathbf{h_q}\delta\mathbf{q}+\mathbf{h_v}\delta\mathbf{v}+\mathbf{h_u}\delta\mathbf{u}=\mathbf{0}, &\textrm{(equality constraints)} \end{aligned} \]
where
Additionally, it is important to note that calcDiff()
computes the derivatives using the latest stored values by calc()
. Thus, we need to first run calc()
.
ActionModelAbstractTpl
, calc()
, calcDiff()
, createData()
Definition at line 122 of file diff-action-base.hpp.
typedef ScalarSelector<Scalar>::type ScalarType |
Definition at line 127 of file diff-action-base.hpp.
typedef MathBaseTpl<Scalar> MathBase |
Definition at line 128 of file diff-action-base.hpp.
typedef DifferentialActionDataAbstractTpl<Scalar> DifferentialActionDataAbstract |
Definition at line 130 of file diff-action-base.hpp.
typedef StateAbstractTpl<Scalar> StateAbstract |
Definition at line 131 of file diff-action-base.hpp.
typedef MathBase::VectorXs VectorXs |
Definition at line 132 of file diff-action-base.hpp.
typedef MathBase::MatrixXs MatrixXs |
Definition at line 133 of file diff-action-base.hpp.
DifferentialActionModelAbstractTpl | ( | std::shared_ptr< StateAbstract > | state, |
const std::size_t | nu, | ||
const std::size_t | nr = 0 , |
||
const std::size_t | ng = 0 , |
||
const std::size_t | nh = 0 , |
||
const std::size_t | ng_T = 0 , |
||
const std::size_t | nh_T = 0 |
||
) |
Initialize the differential action model.
[in] | state | State description |
[in] | nu | Dimension of control vector |
[in] | nr | Dimension of cost-residual vector |
[in] | ng | Number of inequality constraints (default 0) |
[in] | nh | Number of equality constraints (default 0) |
[in] | ng_T | Number of inequality terminal constraints (default 0) |
[in] | nh_T | Number of equality terminal constraints (default 0) |
|
inlineprotected |
Definition at line 375 of file diff-action-base.hpp.
|
pure virtual |
Compute the system acceleration and cost value.
[in] | data | Differential action data |
[in] | x | State point \(\mathbf{x}\in\mathbb{R}^{ndx}\) |
[in] | u | Control input \(\mathbf{u}\in\mathbb{R}^{nu}\) |
Implemented in DifferentialActionModelLQRTpl< _Scalar >, DifferentialActionModelNumDiffTpl< _Scalar >, DifferentialActionModelContactFwdDynamicsTpl< _Scalar >, DifferentialActionModelContactInvDynamicsTpl< _Scalar >, DifferentialActionModelFreeFwdDynamicsTpl< _Scalar >, and DifferentialActionModelFreeInvDynamicsTpl< _Scalar >.
|
virtual |
Compute the total cost value for nodes that depends only on the state.
It updates the total cost and the system acceleration is not updated as the control input is undefined. This function is used in the terminal nodes of an optimal control problem.
[in] | data | Differential action data |
[in] | x | State point \(\mathbf{x}\in\mathbb{R}^{ndx}\) |
Reimplemented in DifferentialActionModelLQRTpl< _Scalar >, DifferentialActionModelNumDiffTpl< _Scalar >, DifferentialActionModelContactFwdDynamicsTpl< _Scalar >, DifferentialActionModelContactInvDynamicsTpl< _Scalar >, DifferentialActionModelFreeFwdDynamicsTpl< _Scalar >, and DifferentialActionModelFreeInvDynamicsTpl< _Scalar >.
|
pure virtual |
Compute the derivatives of the dynamics and cost functions.
It computes the partial derivatives of the dynamical system and the cost function. It assumes that calc()
has been run first. This function builds a quadratic approximation of the time-continuous action model (i.e. dynamical system and cost function).
[in] | data | Differential action data |
[in] | x | State point \(\mathbf{x}\in\mathbb{R}^{ndx}\) |
[in] | u | Control input \(\mathbf{u}\in\mathbb{R}^{nu}\) |
Implemented in DifferentialActionModelLQRTpl< _Scalar >, DifferentialActionModelNumDiffTpl< _Scalar >, DifferentialActionModelContactFwdDynamicsTpl< _Scalar >, DifferentialActionModelContactInvDynamicsTpl< _Scalar >, DifferentialActionModelFreeFwdDynamicsTpl< _Scalar >, and DifferentialActionModelFreeInvDynamicsTpl< _Scalar >.
|
virtual |
Compute the derivatives of the cost functions with respect to the state only.
It updates the derivatives of the cost function with respect to the state only. This function is used in the terminal nodes of an optimal control problem.
[in] | data | Differential action data |
[in] | x | State point \(\mathbf{x}\in\mathbb{R}^{ndx}\) |
Reimplemented in DifferentialActionModelLQRTpl< _Scalar >, DifferentialActionModelNumDiffTpl< _Scalar >, DifferentialActionModelContactFwdDynamicsTpl< _Scalar >, DifferentialActionModelContactInvDynamicsTpl< _Scalar >, DifferentialActionModelFreeFwdDynamicsTpl< _Scalar >, and DifferentialActionModelFreeInvDynamicsTpl< _Scalar >.
|
virtual |
Create the differential action data.
Reimplemented in DifferentialActionModelLQRTpl< _Scalar >, DifferentialActionModelNumDiffTpl< _Scalar >, DifferentialActionModelContactFwdDynamicsTpl< _Scalar >, DifferentialActionModelContactInvDynamicsTpl< _Scalar >, DifferentialActionModelFreeFwdDynamicsTpl< _Scalar >, and DifferentialActionModelFreeInvDynamicsTpl< _Scalar >.
|
virtual |
Checks that a specific data belongs to this model.
Reimplemented in DifferentialActionModelLQRTpl< _Scalar >, DifferentialActionModelContactFwdDynamicsTpl< _Scalar >, DifferentialActionModelContactInvDynamicsTpl< _Scalar >, DifferentialActionModelFreeFwdDynamicsTpl< _Scalar >, and DifferentialActionModelFreeInvDynamicsTpl< _Scalar >.
|
virtual |
Computes the quasic static commands.
The quasic static commands are the ones produced for a the reference posture as an equilibrium point, i.e. for \(\mathbf{f}(\mathbf{q},\mathbf{v}=\mathbf{0},\mathbf{u})=\mathbf{0}\)
[in] | data | Differential action data |
[out] | u | Quasic static commands |
[in] | x | State point (velocity has to be zero) |
[in] | maxiter | Maximum allowed number of iterations |
[in] | tol | Tolerance |
Reimplemented in DifferentialActionModelNumDiffTpl< _Scalar >, DifferentialActionModelContactFwdDynamicsTpl< _Scalar >, DifferentialActionModelContactInvDynamicsTpl< _Scalar >, DifferentialActionModelFreeFwdDynamicsTpl< _Scalar >, and DifferentialActionModelFreeInvDynamicsTpl< _Scalar >.
VectorXs quasiStatic_x | ( | const std::shared_ptr< DifferentialActionDataAbstract > & | data, |
const VectorXs & | x, | ||
const std::size_t | maxiter = 100 , |
||
const Scalar | tol = Scalar(1e-9) |
||
) |
[in] | data | Differential action data |
[in] | x | State point (velocity has to be zero) |
[in] | maxiter | Maximum allowed number of iterations |
[in] | tol | Tolerance |
|
virtual |
Return the number of inequality constraints.
Reimplemented in DifferentialActionModelContactFwdDynamicsTpl< _Scalar >, DifferentialActionModelContactInvDynamicsTpl< _Scalar >, DifferentialActionModelFreeFwdDynamicsTpl< _Scalar >, and DifferentialActionModelFreeInvDynamicsTpl< _Scalar >.
|
virtual |
Return the number of equality constraints.
Reimplemented in DifferentialActionModelContactFwdDynamicsTpl< _Scalar >, DifferentialActionModelContactInvDynamicsTpl< _Scalar >, DifferentialActionModelFreeFwdDynamicsTpl< _Scalar >, and DifferentialActionModelFreeInvDynamicsTpl< _Scalar >.
|
virtual |
Return the number of inequality terminal constraints.
Reimplemented in DifferentialActionModelContactFwdDynamicsTpl< _Scalar >, DifferentialActionModelContactInvDynamicsTpl< _Scalar >, DifferentialActionModelFreeFwdDynamicsTpl< _Scalar >, and DifferentialActionModelFreeInvDynamicsTpl< _Scalar >.
|
virtual |
Return the number of equality terminal constraints.
Reimplemented in DifferentialActionModelContactFwdDynamicsTpl< _Scalar >, DifferentialActionModelContactInvDynamicsTpl< _Scalar >, DifferentialActionModelFreeFwdDynamicsTpl< _Scalar >, and DifferentialActionModelFreeInvDynamicsTpl< _Scalar >.
|
virtual |
Return the lower bound of the inequality constraints.
Reimplemented in DifferentialActionModelContactFwdDynamicsTpl< _Scalar >, DifferentialActionModelContactInvDynamicsTpl< _Scalar >, DifferentialActionModelFreeFwdDynamicsTpl< _Scalar >, and DifferentialActionModelFreeInvDynamicsTpl< _Scalar >.
|
virtual |
Return the upper bound of the inequality constraints.
Reimplemented in DifferentialActionModelContactFwdDynamicsTpl< _Scalar >, DifferentialActionModelContactInvDynamicsTpl< _Scalar >, DifferentialActionModelFreeFwdDynamicsTpl< _Scalar >, and DifferentialActionModelFreeInvDynamicsTpl< _Scalar >.
|
virtual |
Print relevant information of the differential action model.
[out] | os | Output stream object |
Reimplemented in DifferentialActionModelLQRTpl< _Scalar >, DifferentialActionModelNumDiffTpl< _Scalar >, DifferentialActionModelContactFwdDynamicsTpl< _Scalar >, DifferentialActionModelContactInvDynamicsTpl< _Scalar >, DifferentialActionModelFreeFwdDynamicsTpl< _Scalar >, and DifferentialActionModelFreeInvDynamicsTpl< _Scalar >.
|
friend |
Definition at line 385 of file diff-action-base.hpp.
Definition at line 387 of file diff-action-base.hpp.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar |
Definition at line 126 of file diff-action-base.hpp.
|
protected |
Control dimension.
Definition at line 361 of file diff-action-base.hpp.
|
protected |
Dimension of the cost residual.
Definition at line 362 of file diff-action-base.hpp.
|
protected |
Number of inequality constraints.
Definition at line 363 of file diff-action-base.hpp.
|
protected |
Number of equality constraints.
Definition at line 364 of file diff-action-base.hpp.
|
protected |
Number of inequality terminal constraints.
Definition at line 365 of file diff-action-base.hpp.
|
protected |
Number of equality terminal constraints.
Definition at line 366 of file diff-action-base.hpp.
|
protected |
Model of the state.
Definition at line 367 of file diff-action-base.hpp.
|
protected |
Neutral state.
Definition at line 368 of file diff-action-base.hpp.
|
protected |
Lower bound of the inequality constraints.
Definition at line 369 of file diff-action-base.hpp.
|
protected |
Lower bound of the inequality constraints.
Definition at line 370 of file diff-action-base.hpp.
|
protected |
Lower control limits.
Definition at line 371 of file diff-action-base.hpp.
|
protected |
Upper control limits.
Definition at line 372 of file diff-action-base.hpp.
|
protected |
Indicates whether any of the control limits is finite
Definition at line 373 of file diff-action-base.hpp.