10 #ifndef CROCODDYL_CORE_NUMDIFF_DIFF_ACTION_HPP_ 11 #define CROCODDYL_CORE_NUMDIFF_DIFF_ACTION_HPP_ 16 #include "crocoddyl/core/diff-action-base.hpp" 41 template <
typename _Scalar>
42 class DifferentialActionModelNumDiffTpl :
public DifferentialActionModelAbstractTpl<_Scalar> {
44 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
46 typedef _Scalar Scalar;
47 typedef MathBaseTpl<Scalar> MathBase;
48 typedef DifferentialActionModelAbstractTpl<Scalar> Base;
49 typedef DifferentialActionDataNumDiffTpl<Scalar> Data;
50 typedef DifferentialActionDataAbstractTpl<Scalar> DifferentialActionDataAbstract;
51 typedef typename MathBase::VectorXs VectorXs;
52 typedef typename MathBase::MatrixXs MatrixXs;
66 virtual void calc(
const boost::shared_ptr<DifferentialActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
67 const Eigen::Ref<const VectorXs>& u);
73 virtual void calc(
const boost::shared_ptr<DifferentialActionDataAbstract>& data,
74 const Eigen::Ref<const VectorXs>& x);
79 virtual void calcDiff(
const boost::shared_ptr<DifferentialActionDataAbstract>& data,
80 const Eigen::Ref<const VectorXs>& x,
const Eigen::Ref<const VectorXs>& u);
86 virtual void calcDiff(
const boost::shared_ptr<DifferentialActionDataAbstract>& data,
87 const Eigen::Ref<const VectorXs>& x);
92 virtual boost::shared_ptr<DifferentialActionDataAbstract>
createData();
97 const boost::shared_ptr<Base>&
get_model()
const;
124 void assertStableStateFD(
const Eigen::Ref<const VectorXs>& x);
125 boost::shared_ptr<Base> model_;
126 bool with_gauss_approx_;
130 template <
typename _Scalar>
131 struct DifferentialActionDataNumDiffTpl :
public DifferentialActionDataAbstractTpl<_Scalar> {
132 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
134 typedef _Scalar Scalar;
135 typedef MathBaseTpl<Scalar> MathBase;
136 typedef DifferentialActionDataAbstractTpl<Scalar> Base;
137 typedef typename MathBase::VectorXs VectorXs;
138 typedef typename MathBase::MatrixXs MatrixXs;
146 template <
template <
typename Scalar>
class Model>
149 Rx(model->get_model()->get_nr(), model->get_model()->get_state()->get_ndx()),
150 Ru(model->get_model()->get_nr(), model->get_model()->get_nu()),
151 dx(model->get_model()->get_state()->get_ndx()),
152 du(model->get_model()->get_nu()),
153 xp(model->get_model()->get_state()->get_nx()) {
160 const std::size_t ndx = model->get_model()->get_state()->get_ndx();
161 const std::size_t nu = model->get_model()->get_nu();
162 data_0 = model->get_model()->createData();
163 for (std::size_t i = 0; i < ndx; ++i) {
164 data_x.push_back(model->get_model()->createData());
166 for (std::size_t i = 0; i < nu; ++i) {
167 data_u.push_back(model->get_model()->createData());
176 boost::shared_ptr<Base> data_0;
177 std::vector<boost::shared_ptr<Base> > data_x;
178 std::vector<boost::shared_ptr<Base> > data_u;
197 #include "crocoddyl/core/numdiff/diff-action.hxx" 199 #endif // CROCODDYL_CORE_NUMDIFF_DIFF_ACTION_HPP_ VectorXs Lx
Jacobian of the cost function.
MatrixXs Fx
Jacobian of the dynamics.
MatrixXs Lxx
Hessian of the cost function.
virtual boost::shared_ptr< DifferentialActionDataAbstract > createData()
Create the differential action data.
const Scalar get_disturbance() const
Return the disturbance used in the numerical differentiation routine.
VectorXs u_lb_
Lower control limits.
MatrixXs Luu
Hessian of the cost function.
VectorXs unone_
Neutral state.
std::size_t nr_
Dimension of the cost residual.
std::size_t nu_
Control dimension.
bool has_control_limits_
Indicates whether any of the control limits is finite.
MatrixXs Lxu
Hessian of the cost function.
boost::shared_ptr< StateAbstract > state_
Model of the state.
const boost::shared_ptr< Base > & get_model() const
Return the differential acton model that we use to numerical differentiate.
void set_disturbance(const Scalar disturbance)
Modify the disturbance used in the numerical differentiation routine.
MatrixXs Fu
Jacobian of the dynamics.
VectorXs u_ub_
Upper control limits.
VectorXs xout
evolution state
DifferentialActionDataNumDiffTpl(Model< Scalar > *const model)
Construct a new ActionDataNumDiff object.
VectorXs Lu
Jacobian of the cost function.
DifferentialActionModelNumDiffTpl(boost::shared_ptr< Base > model, const bool with_gauss_approx=false)
Initialize the numdiff differential action model.
virtual void calc(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the system acceleration and cost value.
bool get_with_gauss_approx()
Identify if the Gauss approximation is going to be used or not.
virtual void calcDiff(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the derivatives of the dynamics and cost functions.