crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
diff-action-base.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh, University of Oxford
5// Copyright note valid unless otherwise stated in individual files.
6// All rights reserved.
8
9#ifndef CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_
10#define CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_
11
12#include <stdexcept>
13#include <boost/shared_ptr.hpp>
14#include <boost/make_shared.hpp>
15
16#include "crocoddyl/core/fwd.hpp"
17#include "crocoddyl/core/state-base.hpp"
18#include "crocoddyl/core/utils/math.hpp"
19
20namespace crocoddyl {
21
52template <typename _Scalar>
54 public:
55 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
56
57 typedef _Scalar Scalar;
61 typedef typename MathBase::VectorXs VectorXs;
62 typedef typename MathBase::MatrixXs MatrixXs;
63
71 DifferentialActionModelAbstractTpl(boost::shared_ptr<StateAbstract> state, const std::size_t nu,
72 const std::size_t nr = 0);
74
82 virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
83 const Eigen::Ref<const VectorXs>& u) = 0;
84
94 virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
95 const Eigen::Ref<const VectorXs>& x);
96
108 virtual void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
109 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) = 0;
110
120 virtual void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
121 const Eigen::Ref<const VectorXs>& x);
122
128 virtual boost::shared_ptr<DifferentialActionDataAbstract> createData();
129
133 virtual bool checkData(const boost::shared_ptr<DifferentialActionDataAbstract>& data);
134
147 virtual void quasiStatic(const boost::shared_ptr<DifferentialActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
148 const Eigen::Ref<const VectorXs>& x, const std::size_t maxiter = 100,
149 const Scalar tol = Scalar(1e-9));
150
162 VectorXs quasiStatic_x(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const VectorXs& x,
163 const std::size_t maxiter = 100, const Scalar tol = Scalar(1e-9));
164
168 std::size_t get_nu() const;
169
173 std::size_t get_nr() const;
174
178 const boost::shared_ptr<StateAbstract>& get_state() const;
179
183 const VectorXs& get_u_lb() const;
184
188 const VectorXs& get_u_ub() const;
189
194
198 void set_u_lb(const VectorXs& u_lb);
199
203 void set_u_ub(const VectorXs& u_ub);
204
208 template <class Scalar>
209 friend std::ostream& operator<<(std::ostream& os, const DifferentialActionModelAbstractTpl<Scalar>& model);
210
216 virtual void print(std::ostream& os) const;
217
218 protected:
219 std::size_t nu_;
220 std::size_t nr_;
221 boost::shared_ptr<StateAbstract> state_;
222 VectorXs unone_;
223 VectorXs u_lb_;
224 VectorXs u_ub_;
226
227 void update_has_control_limits();
228};
229
230template <typename _Scalar>
232 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
233
234 typedef _Scalar Scalar;
236 typedef typename MathBase::VectorXs VectorXs;
237 typedef typename MathBase::MatrixXs MatrixXs;
238
239 template <template <typename Scalar> class Model>
240 explicit DifferentialActionDataAbstractTpl(Model<Scalar>* const model)
241 : cost(0.),
242 xout(model->get_state()->get_nv()),
243 Fx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
244 Fu(model->get_state()->get_nv(), model->get_nu()),
245 r(model->get_nr()),
246 Lx(model->get_state()->get_ndx()),
247 Lu(model->get_nu()),
248 Lxx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
249 Lxu(model->get_state()->get_ndx(), model->get_nu()),
250 Luu(model->get_nu(), model->get_nu()) {
251 xout.setZero();
252 r.setZero();
253 Fx.setZero();
254 Fu.setZero();
255 Lx.setZero();
256 Lu.setZero();
257 Lxx.setZero();
258 Lxu.setZero();
259 Luu.setZero();
260 }
262
263 Scalar cost;
264 VectorXs xout;
265 MatrixXs Fx;
266 MatrixXs Fu;
267 VectorXs r;
268 VectorXs Lx;
269 VectorXs Lu;
270 MatrixXs Lxx;
271 MatrixXs Lxu;
272 MatrixXs Luu;
273};
274
275} // namespace crocoddyl
276
277/* --- Details -------------------------------------------------------------- */
278/* --- Details -------------------------------------------------------------- */
279/* --- Details -------------------------------------------------------------- */
280#include "crocoddyl/core/diff-action-base.hxx"
281
282#endif // CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_
Abstract class for differential action model.
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.
virtual boost::shared_ptr< DifferentialActionDataAbstract > createData()
Create the differential action data.
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.
VectorXs quasiStatic_x(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const VectorXs &x, const std::size_t maxiter=100, const Scalar tol=Scalar(1e-9))
DifferentialActionModelAbstractTpl(boost::shared_ptr< StateAbstract > state, const std::size_t nu, const std::size_t nr=0)
Initialize the differential action model.
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.
void set_u_ub(const VectorXs &u_ub)
Modify the control upper bounds.
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.
bool has_control_limits_
Indicates whether any of the control limits is finite.
bool get_has_control_limits() const
Indicates if there are defined control limits.
const boost::shared_ptr< StateAbstract > & get_state() const
Return the state.
boost::shared_ptr< StateAbstract > state_
Model of the state.
std::size_t nr_
Dimension of the cost residual.
std::size_t get_nr() const
Return the dimension of the cost-residual vector.
const VectorXs & get_u_lb() const
Return the control lower bound.
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.
void set_u_lb(const VectorXs &u_lb)
Modify the control lower bounds.
virtual bool checkData(const boost::shared_ptr< DifferentialActionDataAbstract > &data)
Checks that a specific data belongs to this model.
friend std::ostream & operator<<(std::ostream &os, const DifferentialActionModelAbstractTpl< Scalar > &model)
Print information on the differential action model.
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.
std::size_t get_nu() const
Return the dimension of the control input.
Abstract class for the state representation.
Definition: state-base.hpp:42
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.