crocoddyl  1.8.1
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
contact-fwddyn.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh, CTU, INRIA, University of Oxford
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_MULTIBODY_ACTIONS_CONTACT_FWDDYN_HPP_
10 #define CROCODDYL_MULTIBODY_ACTIONS_CONTACT_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/costs/cost-sum.hpp"
17 #include "crocoddyl/multibody/states/multibody.hpp"
18 #include "crocoddyl/core/actuation-base.hpp"
19 #include "crocoddyl/multibody/contacts/multiple-contacts.hpp"
20 #include "crocoddyl/multibody/data/contacts.hpp"
21 
22 namespace crocoddyl {
23 
24 template <typename _Scalar>
26  public:
27  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
28 
29  typedef _Scalar Scalar;
38  typedef typename MathBase::VectorXs VectorXs;
39  typedef typename MathBase::MatrixXs MatrixXs;
40 
41  DifferentialActionModelContactFwdDynamicsTpl(boost::shared_ptr<StateMultibody> state,
42  boost::shared_ptr<ActuationModelAbstract> actuation,
43  boost::shared_ptr<ContactModelMultiple> contacts,
44  boost::shared_ptr<CostModelSum> costs,
45  const Scalar JMinvJt_damping = Scalar(0.),
46  const bool enable_force = false);
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  virtual void quasiStatic(const boost::shared_ptr<DifferentialActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
56  const Eigen::Ref<const VectorXs>& x, const std::size_t maxiter = 100,
57  const Scalar tol = Scalar(1e-9));
58 
59  const boost::shared_ptr<ActuationModelAbstract>& get_actuation() const;
60  const boost::shared_ptr<ContactModelMultiple>& get_contacts() const;
61  const boost::shared_ptr<CostModelSum>& get_costs() const;
62  pinocchio::ModelTpl<Scalar>& get_pinocchio() const;
63  const VectorXs& get_armature() const;
64  const Scalar get_damping_factor() const;
65 
66  void set_armature(const VectorXs& armature);
67  void set_damping_factor(const Scalar damping);
68 
74  virtual void print(std::ostream& os) const;
75 
76  protected:
77  using Base::nu_;
78  using Base::state_;
79 
80  private:
81  boost::shared_ptr<ActuationModelAbstract> actuation_;
82  boost::shared_ptr<ContactModelMultiple> contacts_;
83  boost::shared_ptr<CostModelSum> costs_;
84  pinocchio::ModelTpl<Scalar>& pinocchio_;
85  bool with_armature_;
86  VectorXs armature_;
87  Scalar JMinvJt_damping_;
88  bool enable_force_;
89 };
90 
91 template <typename _Scalar>
93  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
94  typedef _Scalar Scalar;
97  typedef typename MathBase::VectorXs VectorXs;
98  typedef typename MathBase::MatrixXs MatrixXs;
99 
100  template <template <typename Scalar> class Model>
101  explicit DifferentialActionDataContactFwdDynamicsTpl(Model<Scalar>* const model)
102  : Base(model),
103  pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
104  multibody(&pinocchio, model->get_actuation()->createData(), model->get_contacts()->createData(&pinocchio)),
105  costs(model->get_costs()->createData(&multibody)),
106  Kinv(model->get_state()->get_nv() + model->get_contacts()->get_nc_total(),
107  model->get_state()->get_nv() + model->get_contacts()->get_nc_total()),
108  df_dx(model->get_contacts()->get_nc_total(), model->get_state()->get_ndx()),
109  df_du(model->get_contacts()->get_nc_total(), model->get_nu()),
110  tmp_xstatic(model->get_state()->get_nx()),
111  tmp_Jstatic(model->get_state()->get_nv(), model->get_nu() + model->get_contacts()->get_nc_total()) {
112  costs->shareMemory(this);
113  Kinv.setZero();
114  df_dx.setZero();
115  df_du.setZero();
116  tmp_xstatic.setZero();
117  tmp_Jstatic.setZero();
118  pinocchio.lambda_c.resize(model->get_contacts()->get_nc_total());
119  pinocchio.lambda_c.setZero();
120  }
121 
122  pinocchio::DataTpl<Scalar> pinocchio;
124  boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
125  MatrixXs Kinv;
126  MatrixXs df_dx;
127  MatrixXs df_du;
128  VectorXs tmp_xstatic;
129  MatrixXs tmp_Jstatic;
130 
131  using Base::cost;
132  using Base::Fu;
133  using Base::Fx;
134  using Base::Lu;
135  using Base::Luu;
136  using Base::Lx;
137  using Base::Lxu;
138  using Base::Lxx;
139  using Base::r;
140  using Base::xout;
141 };
142 
143 } // namespace crocoddyl
144 
145 /* --- Details -------------------------------------------------------------- */
146 /* --- Details -------------------------------------------------------------- */
147 /* --- Details -------------------------------------------------------------- */
148 #include <crocoddyl/multibody/actions/contact-fwddyn.hxx>
149 
150 #endif // CROCODDYL_MULTIBODY_ACTIONS_CONTACT_FWDDYN_HPP_
Define a stack of contact models.
Abstract class for differential action model.
virtual bool checkData(const boost::shared_ptr< DifferentialActionDataAbstract > &data)
Checks that a specific data belongs to this model.
State multibody representation.
Definition: fwd.hpp:300
virtual void print(std::ostream &os) const
Print relevant information of the contact forward-dynamics model.
boost::shared_ptr< StateAbstract > state_
Model of the state.
virtual boost::shared_ptr< DifferentialActionDataAbstract > createData()
Create the differential action data.