crocoddyl  1.5.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
contact-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_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/multibody/states/multibody.hpp"
17 #include "crocoddyl/multibody/actuations/floating-base.hpp"
18 #include "crocoddyl/multibody/contacts/multiple-contacts.hpp"
19 #include "crocoddyl/multibody/data/contacts.hpp"
20 #include "crocoddyl/multibody/costs/cost-sum.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<ActuationModelFloatingBase> 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<ActuationModelFloatingBase>& 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 
69  protected:
71  using Base::nr_;
72  using Base::nu_;
73  using Base::state_;
74  using Base::u_lb_;
75  using Base::u_ub_;
76  using Base::unone_;
77 
78  private:
79  boost::shared_ptr<ActuationModelFloatingBase> actuation_;
80  boost::shared_ptr<ContactModelMultiple> contacts_;
81  boost::shared_ptr<CostModelSum> costs_;
82  pinocchio::ModelTpl<Scalar>& pinocchio_;
83  bool with_armature_;
84  VectorXs armature_;
85  Scalar JMinvJt_damping_;
86  bool enable_force_;
87 };
88 
89 template <typename _Scalar>
91  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
92  typedef _Scalar Scalar;
95  typedef typename MathBase::VectorXs VectorXs;
96  typedef typename MathBase::MatrixXs MatrixXs;
97 
98  template <template <typename Scalar> class Model>
99  explicit DifferentialActionDataContactFwdDynamicsTpl(Model<Scalar>* const model)
100  : Base(model),
101  pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
102  multibody(&pinocchio, model->get_actuation()->createData(), model->get_contacts()->createData(&pinocchio)),
103  costs(model->get_costs()->createData(&multibody)),
104  Kinv(model->get_state()->get_nv() + model->get_contacts()->get_nc_total(),
105  model->get_state()->get_nv() + model->get_contacts()->get_nc_total()),
106  df_dx(model->get_contacts()->get_nc_total(), model->get_state()->get_ndx()),
107  df_du(model->get_contacts()->get_nc_total(), model->get_nu()),
108  tmp_xstatic(model->get_state()->get_nx()),
109  tmp_Jstatic(model->get_state()->get_nv(), model->get_nu() + model->get_contacts()->get_nc_total()) {
110  costs->shareMemory(this);
111  Kinv.setZero();
112  df_dx.setZero();
113  df_du.setZero();
114  tmp_xstatic.setZero();
115  tmp_Jstatic.setZero();
116  pinocchio.lambda_c.resize(model->get_contacts()->get_nc_total());
117  pinocchio.lambda_c.setZero();
118  }
119 
120  pinocchio::DataTpl<Scalar> pinocchio;
122  boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
123  MatrixXs Kinv;
124  MatrixXs df_dx;
125  MatrixXs df_du;
126  VectorXs tmp_xstatic;
127  MatrixXs tmp_Jstatic;
128 
129  using Base::cost;
130  using Base::Fu;
131  using Base::Fx;
132  using Base::Lu;
133  using Base::Luu;
134  using Base::Lx;
135  using Base::Lxu;
136  using Base::Lxx;
137  using Base::r;
138  using Base::xout;
139 };
140 
141 } // namespace crocoddyl
142 
143 /* --- Details -------------------------------------------------------------- */
144 /* --- Details -------------------------------------------------------------- */
145 /* --- Details -------------------------------------------------------------- */
146 #include <crocoddyl/multibody/actions/contact-fwddyn.hxx>
147 
148 #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.
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.