crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
diff-action.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh,
5// New York University, Max Planck Gesellschaft
6// Copyright note valid unless otherwise stated in individual files.
7// All rights reserved.
9
10#ifndef CROCODDYL_CORE_NUMDIFF_DIFF_ACTION_HPP_
11#define CROCODDYL_CORE_NUMDIFF_DIFF_ACTION_HPP_
12
13#include <vector>
14#include <iostream>
15
16#include "crocoddyl/core/diff-action-base.hpp"
17
18namespace crocoddyl {
19
41template <typename _Scalar>
43 public:
44 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
45
46 typedef _Scalar Scalar;
51 typedef typename MathBase::VectorXs VectorXs;
52 typedef typename MathBase::MatrixXs MatrixXs;
53
60 explicit DifferentialActionModelNumDiffTpl(boost::shared_ptr<Base> model, const bool with_gauss_approx = false);
62
66 virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
67 const Eigen::Ref<const VectorXs>& u);
68
73 virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
74 const Eigen::Ref<const VectorXs>& x);
75
79 virtual void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
80 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u);
81
86 virtual void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
87 const Eigen::Ref<const VectorXs>& x);
88
92 virtual boost::shared_ptr<DifferentialActionDataAbstract> createData();
93
97 const boost::shared_ptr<Base>& get_model() const;
98
102 const Scalar get_disturbance() const;
103
107 void set_disturbance(const Scalar disturbance);
108
113
114 protected:
116 using Base::nr_;
117 using Base::nu_;
118 using Base::state_;
119 using Base::u_lb_;
120 using Base::u_ub_;
121 using Base::unone_;
122
123 private:
124 void assertStableStateFD(const Eigen::Ref<const VectorXs>& x);
125 boost::shared_ptr<Base> model_;
126 bool with_gauss_approx_;
127 Scalar disturbance_;
128};
129
130template <typename _Scalar>
132 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
133
134 typedef _Scalar Scalar;
137 typedef typename MathBase::VectorXs VectorXs;
138 typedef typename MathBase::MatrixXs MatrixXs;
139
146 template <template <typename Scalar> class Model>
147 explicit DifferentialActionDataNumDiffTpl(Model<Scalar>* const model)
148 : Base(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()) {
154 Rx.setZero();
155 Ru.setZero();
156 dx.setZero();
157 du.setZero();
158 xp.setZero();
159
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());
165 }
166 for (std::size_t i = 0; i < nu; ++i) {
167 data_u.push_back(model->get_model()->createData());
168 }
169 }
170
171 MatrixXs Rx;
172 MatrixXs Ru;
173 VectorXs dx;
174 VectorXs du;
175 VectorXs xp;
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;
179
180 using Base::cost;
181 using Base::Fu;
182 using Base::Fx;
183 using Base::Lu;
184 using Base::Luu;
185 using Base::Lx;
186 using Base::Lxu;
187 using Base::Lxx;
188 using Base::r;
189 using Base::xout;
190};
191
192} // namespace crocoddyl
193
194/* --- Details -------------------------------------------------------------- */
195/* --- Details -------------------------------------------------------------- */
196/* --- Details -------------------------------------------------------------- */
197#include "crocoddyl/core/numdiff/diff-action.hxx"
198
199#endif // CROCODDYL_CORE_NUMDIFF_DIFF_ACTION_HPP_
Abstract class for differential action model.
bool has_control_limits_
Indicates whether any of the control limits is finite.
boost::shared_ptr< StateAbstract > state_
Model of the state.
std::size_t nr_
Dimension of the cost residual.
This class computes the numerical differentiation of a differential action model.
Definition: diff-action.hpp:42
virtual void calc(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
virtual boost::shared_ptr< DifferentialActionDataAbstract > createData()
Create the differential action data.
const boost::shared_ptr< Base > & get_model() const
Return the differential acton model that we use to numerical differentiate.
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.
const Scalar get_disturbance() const
Return the disturbance used in the numerical differentiation routine.
bool get_with_gauss_approx()
Identify if the Gauss approximation is going to be used or not.
void set_disturbance(const Scalar disturbance)
Modify the disturbance used in the numerical differentiation routine.
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.
DifferentialActionModelNumDiffTpl(boost::shared_ptr< Base > model, const bool with_gauss_approx=false)
Initialize the numdiff differential action model.
virtual void calcDiff(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
MatrixXs Fx
Jacobian of the dynamics.
MatrixXs Fu
Jacobian of the dynamics.
MatrixXs Luu
Hessian of the cost function.
VectorXs Lx
Jacobian of the cost function.
MatrixXs Lxx
Hessian of the cost function.
VectorXs Lu
Jacobian of the cost function.
MatrixXs Lxu
Hessian of the cost function.
DifferentialActionDataNumDiffTpl(Model< Scalar > *const model)
Construct a new ActionDataNumDiff object.