crocoddyl  1.4.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
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 
25 namespace crocoddyl {
26 
27 template <typename _Scalar>
29  public:
30  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
31 
32  typedef _Scalar Scalar;
40  typedef typename MathBase::VectorXs VectorXs;
41  typedef typename MathBase::MatrixXs MatrixXs;
42 
43  DifferentialActionModelFreeFwdDynamicsTpl(boost::shared_ptr<StateMultibody> state,
44  boost::shared_ptr<ActuationModelAbstract> actuation,
45  boost::shared_ptr<CostModelSum> costs);
47 
48  virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
49  const Eigen::Ref<const VectorXs>& u);
50  virtual void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
51  const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u);
52  virtual boost::shared_ptr<DifferentialActionDataAbstract> createData();
53  virtual bool checkData(const boost::shared_ptr<DifferentialActionDataAbstract>& data);
54 
55  const boost::shared_ptr<ActuationModelAbstract>& get_actuation() const;
56  const boost::shared_ptr<CostModelSum>& get_costs() const;
57  pinocchio::ModelTpl<Scalar>& get_pinocchio() const;
58  const VectorXs& get_armature() const;
59  void set_armature(const VectorXs& armature);
60 
61  protected:
63  using Base::nr_;
64  using Base::nu_;
65  using Base::state_;
66  using Base::u_lb_;
67  using Base::u_ub_;
68  using Base::unone_;
69 
70  private:
71  boost::shared_ptr<ActuationModelAbstract> actuation_;
72  boost::shared_ptr<CostModelSum> costs_;
73  pinocchio::ModelTpl<Scalar>& pinocchio_;
74  bool with_armature_;
75  VectorXs armature_;
76 };
77 
78 template <typename _Scalar>
80  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
81  typedef _Scalar Scalar;
84  typedef typename MathBase::VectorXs VectorXs;
85  typedef typename MathBase::MatrixXs MatrixXs;
86 
87  template <template <typename Scalar> class Model>
88  explicit DifferentialActionDataFreeFwdDynamicsTpl(Model<Scalar>* const model)
89  : Base(model),
90  pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
91  multibody(&pinocchio, model->get_actuation()->createData()),
92  costs(model->get_costs()->createData(&multibody)),
93  Minv(model->get_state()->get_nv(), model->get_state()->get_nv()),
94  u_drift(model->get_nu()),
95  dtau_dx(model->get_nu(), model->get_state()->get_ndx()) {
96  costs->shareMemory(this);
97  Minv.setZero();
98  u_drift.setZero();
99  dtau_dx.setZero();
100  }
101 
102  pinocchio::DataTpl<Scalar> pinocchio;
104  boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
105  MatrixXs Minv;
106  VectorXs u_drift;
107  MatrixXs dtau_dx;
108 
109  using Base::cost;
110  using Base::Fu;
111  using Base::Fx;
112  using Base::Lu;
113  using Base::Luu;
114  using Base::Lx;
115  using Base::Lxu;
116  using Base::Lxx;
117  using Base::r;
118  using Base::xout;
119 };
120 
121 } // namespace crocoddyl
122 
123 /* --- Details -------------------------------------------------------------- */
124 /* --- Details -------------------------------------------------------------- */
125 /* --- Details -------------------------------------------------------------- */
126 #include <crocoddyl/multibody/actions/free-fwddyn.hxx>
127 
128 #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.
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.