crocoddyl  1.7.0
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 
72  template <class Scalar>
73  friend std::ostream& operator<<(std::ostream& os, const DifferentialActionModelContactFwdDynamicsTpl<Scalar>& model);
74 
75  protected:
77  using Base::nr_;
78  using Base::nu_;
79  using Base::state_;
80  using Base::u_lb_;
81  using Base::u_ub_;
82  using Base::unone_;
83 
84  private:
85  boost::shared_ptr<ActuationModelAbstract> actuation_;
86  boost::shared_ptr<ContactModelMultiple> contacts_;
87  boost::shared_ptr<CostModelSum> costs_;
88  pinocchio::ModelTpl<Scalar>& pinocchio_;
89  bool with_armature_;
90  VectorXs armature_;
91  Scalar JMinvJt_damping_;
92  bool enable_force_;
93 };
94 
95 template <typename _Scalar>
97  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
98  typedef _Scalar Scalar;
101  typedef typename MathBase::VectorXs VectorXs;
102  typedef typename MathBase::MatrixXs MatrixXs;
103 
104  template <template <typename Scalar> class Model>
105  explicit DifferentialActionDataContactFwdDynamicsTpl(Model<Scalar>* const model)
106  : Base(model),
107  pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
108  multibody(&pinocchio, model->get_actuation()->createData(), model->get_contacts()->createData(&pinocchio)),
109  costs(model->get_costs()->createData(&multibody)),
110  Kinv(model->get_state()->get_nv() + model->get_contacts()->get_nc_total(),
111  model->get_state()->get_nv() + model->get_contacts()->get_nc_total()),
112  df_dx(model->get_contacts()->get_nc_total(), model->get_state()->get_ndx()),
113  df_du(model->get_contacts()->get_nc_total(), model->get_nu()),
114  tmp_xstatic(model->get_state()->get_nx()),
115  tmp_Jstatic(model->get_state()->get_nv(), model->get_nu() + model->get_contacts()->get_nc_total()) {
116  costs->shareMemory(this);
117  Kinv.setZero();
118  df_dx.setZero();
119  df_du.setZero();
120  tmp_xstatic.setZero();
121  tmp_Jstatic.setZero();
122  pinocchio.lambda_c.resize(model->get_contacts()->get_nc_total());
123  pinocchio.lambda_c.setZero();
124  }
125 
126  pinocchio::DataTpl<Scalar> pinocchio;
128  boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
129  MatrixXs Kinv;
130  MatrixXs df_dx;
131  MatrixXs df_du;
132  VectorXs tmp_xstatic;
133  MatrixXs tmp_Jstatic;
134 
135  using Base::cost;
136  using Base::Fu;
137  using Base::Fx;
138  using Base::Lu;
139  using Base::Luu;
140  using Base::Lx;
141  using Base::Lxu;
142  using Base::Lxx;
143  using Base::r;
144  using Base::xout;
145 };
146 
147 } // namespace crocoddyl
148 
149 /* --- Details -------------------------------------------------------------- */
150 /* --- Details -------------------------------------------------------------- */
151 /* --- Details -------------------------------------------------------------- */
152 #include <crocoddyl/multibody/actions/contact-fwddyn.hxx>
153 
154 #endif // CROCODDYL_MULTIBODY_ACTIONS_CONTACT_FWDDYN_HPP_
Define a stack of contact models.
Abstract class for differential action model.
std::size_t nr_
Dimension of the cost residual.
virtual bool checkData(const boost::shared_ptr< DifferentialActionDataAbstract > &data)
Checks that a specific data belongs to this model.
State multibody representation.
Definition: fwd.hpp:215
bool has_control_limits_
Indicates whether any of the control limits is finite.
boost::shared_ptr< StateAbstract > state_
Model of the state.
virtual boost::shared_ptr< DifferentialActionDataAbstract > createData()
Create the differential action data.