crocoddyl  1.9.0
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 
49 template <typename _Scalar>
51  public:
52  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
53 
54  typedef _Scalar Scalar;
62  typedef typename MathBase::VectorXs VectorXs;
63  typedef typename MathBase::MatrixXs MatrixXs;
64 
65  DifferentialActionModelFreeFwdDynamicsTpl(boost::shared_ptr<StateMultibody> state,
66  boost::shared_ptr<ActuationModelAbstract> actuation,
67  boost::shared_ptr<CostModelSum> costs);
69 
79  virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
80  const Eigen::Ref<const VectorXs>& u);
81 
86  virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
87  const Eigen::Ref<const VectorXs>& x);
88 
96  virtual void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
97  const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u);
98 
103  virtual void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
104  const Eigen::Ref<const VectorXs>& x);
105 
111  virtual boost::shared_ptr<DifferentialActionDataAbstract> createData();
112 
116  virtual bool checkData(const boost::shared_ptr<DifferentialActionDataAbstract>& data);
117 
121  virtual void quasiStatic(const boost::shared_ptr<DifferentialActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
122  const Eigen::Ref<const VectorXs>& x, const std::size_t maxiter = 100,
123  const Scalar tol = Scalar(1e-9));
124 
128  const boost::shared_ptr<ActuationModelAbstract>& get_actuation() const;
129 
133  const boost::shared_ptr<CostModelSum>& get_costs() const;
134 
138  pinocchio::ModelTpl<Scalar>& get_pinocchio() const;
139 
143  const VectorXs& get_armature() const;
144 
148  void set_armature(const VectorXs& armature);
149 
155  virtual void print(std::ostream& os) const;
156 
157  protected:
158  using Base::nu_;
159  using Base::state_;
160 
161  private:
162  boost::shared_ptr<ActuationModelAbstract> actuation_;
163  boost::shared_ptr<CostModelSum> costs_;
164  pinocchio::ModelTpl<Scalar>& pinocchio_;
165  bool without_armature_;
166  VectorXs armature_;
167 };
168 
169 template <typename _Scalar>
171  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
172  typedef _Scalar Scalar;
175  typedef typename MathBase::VectorXs VectorXs;
176  typedef typename MathBase::MatrixXs MatrixXs;
177 
178  template <template <typename Scalar> class Model>
179  explicit DifferentialActionDataFreeFwdDynamicsTpl(Model<Scalar>* const model)
180  : Base(model),
181  pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
182  multibody(&pinocchio, model->get_actuation()->createData()),
183  costs(model->get_costs()->createData(&multibody)),
184  Minv(model->get_state()->get_nv(), model->get_state()->get_nv()),
185  u_drift(model->get_nu()),
186  dtau_dx(model->get_nu(), model->get_state()->get_ndx()),
187  tmp_xstatic(model->get_state()->get_nx()) {
188  costs->shareMemory(this);
189  Minv.setZero();
190  u_drift.setZero();
191  dtau_dx.setZero();
192  tmp_xstatic.setZero();
193  }
194 
195  pinocchio::DataTpl<Scalar> pinocchio;
197  boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
198  MatrixXs Minv;
199  VectorXs u_drift;
200  MatrixXs dtau_dx;
201  VectorXs tmp_xstatic;
202 
203  using Base::cost;
204  using Base::Fu;
205  using Base::Fx;
206  using Base::Lu;
207  using Base::Luu;
208  using Base::Lx;
209  using Base::Lxu;
210  using Base::Lxx;
211  using Base::r;
212  using Base::xout;
213 };
214 
215 } // namespace crocoddyl
216 
217 /* --- Details -------------------------------------------------------------- */
218 /* --- Details -------------------------------------------------------------- */
219 /* --- Details -------------------------------------------------------------- */
220 #include <crocoddyl/multibody/actions/free-fwddyn.hxx>
221 
222 #endif // CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_
Abstract class for the actuation-mapping model.
Abstract class for differential action model.
boost::shared_ptr< StateAbstract > state_
Model of the state.
Differential action model for free forward dynamics in multibody systems.
Definition: free-fwddyn.hpp:50
pinocchio::ModelTpl< Scalar > & get_pinocchio() const
Return the Pinocchio model.
virtual void calc(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
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 contact dynamics, and cost function.
const VectorXs & get_armature() const
Return the armature vector.
virtual boost::shared_ptr< DifferentialActionDataAbstract > createData()
Create the free forward-dynamics data.
virtual void print(std::ostream &os) const
Print relevant information of the free forward-dynamics model.
const boost::shared_ptr< CostModelSum > & get_costs() const
Return the cost model.
void set_armature(const VectorXs &armature)
Modify the armature vector.
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.
const boost::shared_ptr< ActuationModelAbstract > & get_actuation() const
Return the actuation model.
virtual bool checkData(const boost::shared_ptr< DifferentialActionDataAbstract > &data)
Check that the given data belongs to the free forward-dynamics data.
virtual void calcDiff(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
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.