crocoddyl  1.5.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
free-fwddyn.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_
10 #define CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_
11 
12 #include <stdexcept>
13 
14 #ifdef PINOCCHIO_WITH_CPPAD_SUPPORT // TODO(cmastalli): Removed after merging Pinocchio v.2.4.8
15 #include <pinocchio/codegen/cppadcg.hpp>
16 #endif
17 
18 #include "crocoddyl/multibody/fwd.hpp"
19 #include "crocoddyl/core/diff-action-base.hpp"
20 #include "crocoddyl/core/actuation-base.hpp"
21 #include "crocoddyl/multibody/data/multibody.hpp"
22 #include "crocoddyl/multibody/states/multibody.hpp"
23 #include "crocoddyl/multibody/costs/cost-sum.hpp"
24 #include "crocoddyl/core/utils/exception.hpp"
25 
26 namespace crocoddyl {
27 
28 template <typename _Scalar>
30  public:
31  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
32 
33  typedef _Scalar Scalar;
41  typedef typename MathBase::VectorXs VectorXs;
42  typedef typename MathBase::MatrixXs MatrixXs;
43 
44  DifferentialActionModelFreeFwdDynamicsTpl(boost::shared_ptr<StateMultibody> state,
45  boost::shared_ptr<ActuationModelAbstract> actuation,
46  boost::shared_ptr<CostModelSum> costs);
48 
49  virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
50  const Eigen::Ref<const VectorXs>& u);
51  virtual void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
52  const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u);
53  virtual boost::shared_ptr<DifferentialActionDataAbstract> createData();
54  virtual bool checkData(const boost::shared_ptr<DifferentialActionDataAbstract>& data);
55 
56  virtual void quasiStatic(const boost::shared_ptr<DifferentialActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
57  const Eigen::Ref<const VectorXs>& x, const std::size_t& maxiter = 100,
58  const Scalar& tol = Scalar(1e-9));
59 
60  const boost::shared_ptr<ActuationModelAbstract>& get_actuation() const;
61  const boost::shared_ptr<CostModelSum>& get_costs() const;
62  pinocchio::ModelTpl<Scalar>& get_pinocchio() const;
63  const VectorXs& get_armature() const;
64  void set_armature(const VectorXs& armature);
65 
66  protected:
68  using Base::nr_;
69  using Base::nu_;
70  using Base::state_;
71  using Base::u_lb_;
72  using Base::u_ub_;
73  using Base::unone_;
74 
75  private:
76  boost::shared_ptr<ActuationModelAbstract> actuation_;
77  boost::shared_ptr<CostModelSum> costs_;
78  pinocchio::ModelTpl<Scalar>& pinocchio_;
79  bool with_armature_;
80  VectorXs armature_;
81 };
82 
83 template <typename _Scalar>
85  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
86  typedef _Scalar Scalar;
89  typedef typename MathBase::VectorXs VectorXs;
90  typedef typename MathBase::MatrixXs MatrixXs;
91 
92  template <template <typename Scalar> class Model>
93  explicit DifferentialActionDataFreeFwdDynamicsTpl(Model<Scalar>* const model)
94  : Base(model),
95  pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
96  multibody(&pinocchio, model->get_actuation()->createData()),
97  costs(model->get_costs()->createData(&multibody)),
98  Minv(model->get_state()->get_nv(), model->get_state()->get_nv()),
99  u_drift(model->get_nu()),
100  dtau_dx(model->get_nu(), model->get_state()->get_ndx()),
101  tmp_xstatic(model->get_state()->get_nx()) {
102  costs->shareMemory(this);
103  Minv.setZero();
104  u_drift.setZero();
105  dtau_dx.setZero();
106  tmp_xstatic.setZero();
107  }
108 
109  pinocchio::DataTpl<Scalar> pinocchio;
111  boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
112  MatrixXs Minv;
113  VectorXs u_drift;
114  MatrixXs dtau_dx;
115  VectorXs tmp_xstatic;
116 
117  using Base::cost;
118  using Base::Fu;
119  using Base::Fx;
120  using Base::Lu;
121  using Base::Luu;
122  using Base::Lx;
123  using Base::Lxu;
124  using Base::Lxx;
125  using Base::r;
126  using Base::xout;
127 };
128 
129 } // namespace crocoddyl
130 
131 /* --- Details -------------------------------------------------------------- */
132 /* --- Details -------------------------------------------------------------- */
133 /* --- Details -------------------------------------------------------------- */
134 #include <crocoddyl/multibody/actions/free-fwddyn.hxx>
135 
136 #endif // CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_
virtual bool checkData(const boost::shared_ptr< DifferentialActionDataAbstract > &data)
Checks that a specific data belongs to this model.
Abstract class for differential action model.
virtual boost::shared_ptr< DifferentialActionDataAbstract > createData()
Create the differential action data.
std::size_t nr_
Dimension of the cost residual.
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 has_control_limits_
Indicates whether any of the control limits is finite.
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.
boost::shared_ptr< StateAbstract > state_
Model of the state.
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.