Abstract class for differential action model. More...
#include <crocoddyl/core/diff-action-base.hpp>
Public Types | |
typedef DifferentialActionDataAbstractTpl< Scalar > | DifferentialActionDataAbstract |
typedef MathBaseTpl< Scalar > | MathBase |
typedef MathBase::MatrixXs | MatrixXs |
typedef StateAbstractTpl< Scalar > | StateAbstract |
typedef MathBase::VectorXs | VectorXs |
Public Member Functions | |
DifferentialActionModelAbstractTpl (boost::shared_ptr< StateAbstract > state, const std::size_t nu, const std::size_t nr=0) | |
Initialize the differential action model. More... | |
virtual void | calc (const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) |
Compute the total cost value for nodes that depends only on the state. More... | |
virtual void | calc (const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)=0 |
Compute the system acceleration and cost value. More... | |
virtual void | calcDiff (const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) |
Compute the derivatives of the cost functions with respect to the state only. More... | |
virtual void | calcDiff (const boost::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. More... | |
virtual bool | checkData (const boost::shared_ptr< DifferentialActionDataAbstract > &data) |
Checks that a specific data belongs to this model. | |
virtual boost::shared_ptr< DifferentialActionDataAbstract > | createData () |
Create the differential action data. More... | |
bool | get_has_control_limits () const |
Indicates if there are defined control limits. | |
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 boost::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. More... | |
virtual void | quasiStatic (const boost::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. More... | |
VectorXs | quasiStatic_x (const boost::shared_ptr< DifferentialActionDataAbstract > &data, const VectorXs &x, const std::size_t maxiter=100, const Scalar tol=Scalar(1e-9)) |
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 () |
Protected Attributes | |
bool | has_control_limits_ |
Indicates whether any of the control limits is finite. | |
std::size_t | nr_ |
Dimension of the cost residual. | |
std::size_t | nu_ |
Control dimension. | |
boost::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 > | |
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 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)}\\ &l(\mathbf{x},\mathbf{u}) = \int_0^{\delta t} a(\mathbf{r}(\mathbf{x},\mathbf{u}))\,dt, &\textrm{(cost)} \end{aligned} \]
where the configuration \(\mathbf{q}\in\mathcal{Q}\) lies in the configuration manifold described with a nq
-tuple, the velocity \(\mathbf{v}\in T_{\mathbf{q}}\mathcal{Q}\) its a tangent vector to this manifold with nv
dimension, \(\mathbf{u}\in\mathbb{R}^{nu}\) is the input commands, \(\mathbf{r}\) and \(a\) is a residual and activation functions (see ActivationModelAbstractTpl
). Both configuration and velocity describe the system space \(\mathbf{x}\in\mathbf{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 main computations are carrying out in calc
and calcDiff
. calc
computes the acceleration and cost and calcDiff
computes the derivatives of the dynamics and cost function. Concretely speaking, calcDiff
builds a linear-quadratic approximation of the differential action, where the dynamics and cost functions have linear and quadratic forms, respectively. \(\mathbf{f_x}\in\mathbb{R}^{nv\times ndx}\), \(\mathbf{f_u}\in\mathbb{R}^{nv\times nu}\) are the Jacobians of the dynamics; \(\mathbf{l_x}\in\mathbb{R}^{ndx}\), \(\mathbf{l_u}\in\mathbb{R}^{nu}\), \(\mathbf{l_{xx}}\in\mathbb{R}^{ndx\times ndx}\), \(\mathbf{l_{xu}}\in\mathbb{R}^{ndx\times nu}\), \(\mathbf{l_{uu}}\in\mathbb{R}^{nu\times nu}\) are the Jacobians and Hessians of the cost function, respectively. Additionally, it is important remark that calcDiff()
computes the derivates using the latest stored values by calc()
. Thus, we need to run first calc()
.
calc()
, calcDiff()
, createData()
Definition at line 53 of file diff-action-base.hpp.
DifferentialActionModelAbstractTpl | ( | boost::shared_ptr< StateAbstract > | state, |
const std::size_t | nu, | ||
const std::size_t | nr = 0 |
||
) |
Initialize the differential action model.
[in] | state | State description |
[in] | nu | Dimension of control vector |
[in] | nr | Dimension of cost-residual vector |
|
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 DifferentialActionModelFreeFwdDynamicsTpl< _Scalar >, DifferentialActionModelContactFwdDynamicsTpl< _Scalar >, DifferentialActionModelNumDiffTpl< _Scalar >, and DifferentialActionModelLQRTpl< _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 DifferentialActionModelFreeFwdDynamicsTpl< _Scalar >, DifferentialActionModelContactFwdDynamicsTpl< _Scalar >, DifferentialActionModelNumDiffTpl< _Scalar >, and DifferentialActionModelLQRTpl< _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 DifferentialActionModelFreeFwdDynamicsTpl< _Scalar >, DifferentialActionModelContactFwdDynamicsTpl< _Scalar >, DifferentialActionModelNumDiffTpl< _Scalar >, and DifferentialActionModelLQRTpl< _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 DifferentialActionModelFreeFwdDynamicsTpl< _Scalar >, DifferentialActionModelContactFwdDynamicsTpl< _Scalar >, DifferentialActionModelNumDiffTpl< _Scalar >, and DifferentialActionModelLQRTpl< _Scalar >.
|
virtual |
Create the differential action data.
Reimplemented in DifferentialActionModelFreeFwdDynamicsTpl< _Scalar >, DifferentialActionModelContactFwdDynamicsTpl< _Scalar >, DifferentialActionModelNumDiffTpl< _Scalar >, and DifferentialActionModelLQRTpl< _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 DifferentialActionModelFreeFwdDynamicsTpl< _Scalar >, and DifferentialActionModelContactFwdDynamicsTpl< _Scalar >.
VectorXs quasiStatic_x | ( | const boost::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 |
Print relevant information of the differential action model.
[out] | os | Output stream object |
Reimplemented in DifferentialActionModelFreeFwdDynamicsTpl< _Scalar >, DifferentialActionModelContactFwdDynamicsTpl< _Scalar >, and DifferentialActionModelLQRTpl< _Scalar >.