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.
boost::shared_ptr< StateAbstract > state_
Model of the state.
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 dynamics and cost functions.
virtual boost::shared_ptr< DifferentialActionDataAbstract > createData()
Create the differential action data.
virtual void print(std::ostream &os) const
Print relevant information of the contact forward-dynamics model.
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.
virtual bool checkData(const boost::shared_ptr< DifferentialActionDataAbstract > &data)
Checks that a specific data belongs to this model.
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.