crocoddyl  1.3.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 #include "crocoddyl/multibody/fwd.hpp"
15 #include "crocoddyl/core/diff-action-base.hpp"
16 #include "crocoddyl/core/actuation-base.hpp"
17 #include "crocoddyl/multibody/data/multibody.hpp"
18 #include "crocoddyl/multibody/states/multibody.hpp"
19 #include "crocoddyl/multibody/costs/cost-sum.hpp"
20 
21 namespace crocoddyl {
22 
23 template <typename _Scalar>
25  public:
26  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
27 
28  typedef _Scalar Scalar;
36  typedef typename MathBase::VectorXs VectorXs;
37  typedef typename MathBase::MatrixXs MatrixXs;
38 
39  DifferentialActionModelFreeFwdDynamicsTpl(boost::shared_ptr<StateMultibody> state,
40  boost::shared_ptr<ActuationModelAbstract> actuation,
41  boost::shared_ptr<CostModelSum> costs);
43 
44  virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
45  const Eigen::Ref<const VectorXs>& u);
46  virtual void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
47  const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u);
48  boost::shared_ptr<DifferentialActionDataAbstract> createData();
49 
50  const boost::shared_ptr<ActuationModelAbstract>& get_actuation() const;
51  const boost::shared_ptr<CostModelSum>& get_costs() const;
52  pinocchio::ModelTpl<Scalar>& get_pinocchio() const;
53  const VectorXs& get_armature() const;
54  void set_armature(const VectorXs& armature);
55 
56  protected:
58  using Base::nr_;
59  using Base::nu_;
60  using Base::state_;
61  using Base::u_lb_;
62  using Base::u_ub_;
63  using Base::unone_;
64 
65  private:
66  boost::shared_ptr<ActuationModelAbstract> actuation_;
67  boost::shared_ptr<CostModelSum> costs_;
68  pinocchio::ModelTpl<Scalar>& pinocchio_;
69  bool with_armature_;
70  VectorXs armature_;
71 };
72 
73 template <typename _Scalar>
75  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
76  typedef _Scalar Scalar;
79  typedef typename MathBase::VectorXs VectorXs;
80  typedef typename MathBase::MatrixXs MatrixXs;
81 
82  template <template <typename Scalar> class Model>
83  explicit DifferentialActionDataFreeFwdDynamicsTpl(Model<Scalar>* const model)
84  : Base(model),
85  pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
86  multibody(&pinocchio, model->get_actuation()->createData()),
87  costs(model->get_costs()->createData(&multibody)),
88  Minv(model->get_state()->get_nv(), model->get_state()->get_nv()),
89  u_drift(model->get_nu()),
90  dtau_dx(model->get_nu(), model->get_state()->get_ndx()) {
91  costs->shareMemory(this);
92  Minv.setZero();
93  u_drift.setZero();
94  dtau_dx.setZero();
95  }
96 
97  pinocchio::DataTpl<Scalar> pinocchio;
99  boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
100  MatrixXs Minv;
101  VectorXs u_drift;
102  MatrixXs dtau_dx;
103 
104  using Base::cost;
105  using Base::Fu;
106  using Base::Fx;
107  using Base::Lu;
108  using Base::Luu;
109  using Base::Lx;
110  using Base::Lxu;
111  using Base::Lxx;
112  using Base::r;
113  using Base::xout;
114 };
115 
116 } // namespace crocoddyl
117 
118 /* --- Details -------------------------------------------------------------- */
119 /* --- Details -------------------------------------------------------------- */
120 /* --- Details -------------------------------------------------------------- */
121 #include <crocoddyl/multibody/actions/free-fwddyn.hxx>
122 
123 #endif // CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_
This class DifferentialActionModelAbstract represents a first-order ODE, i.e. where and represents ...
std::size_t nr_
Dimension of the cost residual.
bool has_control_limits_
Indicates whether any of the control limits is finite.
boost::shared_ptr< StateAbstract > state_
Model of the state.