crocoddyl  1.8.1
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
free-fwddyn.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, 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/costs/cost-sum.hpp"
21 #include "crocoddyl/core/actuation-base.hpp"
22 #include "crocoddyl/multibody/data/multibody.hpp"
23 #include "crocoddyl/multibody/states/multibody.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 
71  virtual void print(std::ostream& os) const;
72 
73  protected:
74  using Base::nu_;
75  using Base::state_;
76 
77  private:
78  boost::shared_ptr<ActuationModelAbstract> actuation_;
79  boost::shared_ptr<CostModelSum> costs_;
80  pinocchio::ModelTpl<Scalar>& pinocchio_;
81  bool without_armature_;
82  VectorXs armature_;
83 };
84 
85 template <typename _Scalar>
87  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
88  typedef _Scalar Scalar;
91  typedef typename MathBase::VectorXs VectorXs;
92  typedef typename MathBase::MatrixXs MatrixXs;
93 
94  template <template <typename Scalar> class Model>
95  explicit DifferentialActionDataFreeFwdDynamicsTpl(Model<Scalar>* const model)
96  : Base(model),
97  pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
98  multibody(&pinocchio, model->get_actuation()->createData()),
99  costs(model->get_costs()->createData(&multibody)),
100  Minv(model->get_state()->get_nv(), model->get_state()->get_nv()),
101  u_drift(model->get_nu()),
102  dtau_dx(model->get_nu(), model->get_state()->get_ndx()),
103  tmp_xstatic(model->get_state()->get_nx()) {
104  costs->shareMemory(this);
105  Minv.setZero();
106  u_drift.setZero();
107  dtau_dx.setZero();
108  tmp_xstatic.setZero();
109  }
110 
111  pinocchio::DataTpl<Scalar> pinocchio;
113  boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
114  MatrixXs Minv;
115  VectorXs u_drift;
116  MatrixXs dtau_dx;
117  VectorXs tmp_xstatic;
118 
119  using Base::cost;
120  using Base::Fu;
121  using Base::Fx;
122  using Base::Lu;
123  using Base::Luu;
124  using Base::Lx;
125  using Base::Lxu;
126  using Base::Lxx;
127  using Base::r;
128  using Base::xout;
129 };
130 
131 } // namespace crocoddyl
132 
133 /* --- Details -------------------------------------------------------------- */
134 /* --- Details -------------------------------------------------------------- */
135 /* --- Details -------------------------------------------------------------- */
136 #include <crocoddyl/multibody/actions/free-fwddyn.hxx>
137 
138 #endif // CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_
Abstract class for differential action model.
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.
virtual boost::shared_ptr< DifferentialActionDataAbstract > createData()
Create the differential action data.
virtual void print(std::ostream &os) const
Print relevant information of the free forward-dynamics 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.
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.
virtual bool checkData(const boost::shared_ptr< DifferentialActionDataAbstract > &data)
Checks that a specific data belongs to this model.
State multibody representation.
Definition: multibody.hpp:31
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.