crocoddyl  1.4.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 
56  const boost::shared_ptr<ActuationModelFloatingBase>& get_actuation() const;
57  const boost::shared_ptr<ContactModelMultiple>& get_contacts() const;
58  const boost::shared_ptr<CostModelSum>& get_costs() const;
59  pinocchio::ModelTpl<Scalar>& get_pinocchio() const;
60  const VectorXs& get_armature() const;
61  const Scalar& get_damping_factor() const;
62 
63  void set_armature(const VectorXs& armature);
64  void set_damping_factor(const Scalar& damping);
65 
66  protected:
68  using Base::nr_;
69  using Base::nu_;
70  using Base::state_;
71  using Base::u_lb_;
72  using Base::u_ub_;
73  using Base::unone_;
74 
75  private:
76  boost::shared_ptr<ActuationModelFloatingBase> actuation_;
77  boost::shared_ptr<ContactModelMultiple> contacts_;
78  boost::shared_ptr<CostModelSum> costs_;
79  pinocchio::ModelTpl<Scalar>& pinocchio_;
80  bool with_armature_;
81  VectorXs armature_;
82  Scalar JMinvJt_damping_;
83  bool enable_force_;
84 };
85 
86 template <typename _Scalar>
88  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
89  typedef _Scalar Scalar;
92  typedef typename MathBase::VectorXs VectorXs;
93  typedef typename MathBase::MatrixXs MatrixXs;
94 
95  template <template <typename Scalar> class Model>
96  explicit DifferentialActionDataContactFwdDynamicsTpl(Model<Scalar>* const model)
97  : Base(model),
98  pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
99  multibody(&pinocchio, model->get_actuation()->createData(), model->get_contacts()->createData(&pinocchio)),
100  costs(model->get_costs()->createData(&multibody)),
101  Kinv(model->get_state()->get_nv() + model->get_contacts()->get_nc_total(),
102  model->get_state()->get_nv() + model->get_contacts()->get_nc_total()),
103  df_dx(model->get_contacts()->get_nc_total(), model->get_state()->get_ndx()),
104  df_du(model->get_contacts()->get_nc_total(), model->get_nu()) {
105  costs->shareMemory(this);
106  Kinv.setZero();
107  df_dx.setZero();
108  df_du.setZero();
109  }
110 
111  pinocchio::DataTpl<Scalar> pinocchio;
113  boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
114  MatrixXs Kinv;
115  MatrixXs df_dx;
116  MatrixXs df_du;
117 
118  using Base::cost;
119  using Base::Fu;
120  using Base::Fx;
121  using Base::Lu;
122  using Base::Luu;
123  using Base::Lx;
124  using Base::Lxu;
125  using Base::Lxx;
126  using Base::r;
127  using Base::xout;
128 };
129 
130 } // namespace crocoddyl
131 
132 /* --- Details -------------------------------------------------------------- */
133 /* --- Details -------------------------------------------------------------- */
134 /* --- Details -------------------------------------------------------------- */
135 #include <crocoddyl/multibody/actions/contact-fwddyn.hxx>
136 
137 #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.