crocoddyl  1.9.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 
58 template <typename _Scalar>
60  public:
61  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
62 
63  typedef _Scalar Scalar;
72  typedef typename MathBase::VectorXs VectorXs;
73  typedef typename MathBase::MatrixXs MatrixXs;
74 
88  DifferentialActionModelContactFwdDynamicsTpl(boost::shared_ptr<StateMultibody> state,
89  boost::shared_ptr<ActuationModelAbstract> actuation,
90  boost::shared_ptr<ContactModelMultiple> contacts,
91  boost::shared_ptr<CostModelSum> costs,
92  const Scalar JMinvJt_damping = Scalar(0.),
93  const bool enable_force = false);
95 
105  virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
106  const Eigen::Ref<const VectorXs>& u);
107 
118  virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
119  const Eigen::Ref<const VectorXs>& x);
120 
128  virtual void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
129  const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u);
130 
141  virtual void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
142  const Eigen::Ref<const VectorXs>& x);
143 
149  virtual boost::shared_ptr<DifferentialActionDataAbstract> createData();
150 
154  virtual bool checkData(const boost::shared_ptr<DifferentialActionDataAbstract>& data);
155 
159  virtual void quasiStatic(const boost::shared_ptr<DifferentialActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
160  const Eigen::Ref<const VectorXs>& x, const std::size_t maxiter = 100,
161  const Scalar tol = Scalar(1e-9));
162 
166  const boost::shared_ptr<ActuationModelAbstract>& get_actuation() const;
167 
171  const boost::shared_ptr<ContactModelMultiple>& get_contacts() const;
172 
176  const boost::shared_ptr<CostModelSum>& get_costs() const;
177 
181  pinocchio::ModelTpl<Scalar>& get_pinocchio() const;
182 
186  const VectorXs& get_armature() const;
187 
191  const Scalar get_damping_factor() const;
192 
196  void set_armature(const VectorXs& armature);
197 
201  void set_damping_factor(const Scalar damping);
202 
208  virtual void print(std::ostream& os) const;
209 
210  protected:
211  using Base::nu_;
212  using Base::state_;
213 
214  private:
215  boost::shared_ptr<ActuationModelAbstract> actuation_;
216  boost::shared_ptr<ContactModelMultiple> contacts_;
217  boost::shared_ptr<CostModelSum> costs_;
218  pinocchio::ModelTpl<Scalar>& pinocchio_;
219  bool with_armature_;
220  VectorXs armature_;
221  Scalar JMinvJt_damping_;
222  bool enable_force_;
223 };
224 
225 template <typename _Scalar>
227  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
228  typedef _Scalar Scalar;
231  typedef typename MathBase::VectorXs VectorXs;
232  typedef typename MathBase::MatrixXs MatrixXs;
233 
234  template <template <typename Scalar> class Model>
235  explicit DifferentialActionDataContactFwdDynamicsTpl(Model<Scalar>* const model)
236  : Base(model),
237  pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
238  multibody(&pinocchio, model->get_actuation()->createData(), model->get_contacts()->createData(&pinocchio)),
239  costs(model->get_costs()->createData(&multibody)),
240  Kinv(model->get_state()->get_nv() + model->get_contacts()->get_nc_total(),
241  model->get_state()->get_nv() + model->get_contacts()->get_nc_total()),
242  df_dx(model->get_contacts()->get_nc_total(), model->get_state()->get_ndx()),
243  df_du(model->get_contacts()->get_nc_total(), model->get_nu()),
244  tmp_xstatic(model->get_state()->get_nx()),
245  tmp_Jstatic(model->get_state()->get_nv(), model->get_nu() + model->get_contacts()->get_nc_total()) {
246  costs->shareMemory(this);
247  Kinv.setZero();
248  df_dx.setZero();
249  df_du.setZero();
250  tmp_xstatic.setZero();
251  tmp_Jstatic.setZero();
252  pinocchio.lambda_c.resize(model->get_contacts()->get_nc_total());
253  pinocchio.lambda_c.setZero();
254  }
255 
256  pinocchio::DataTpl<Scalar> pinocchio;
258  boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
259  MatrixXs Kinv;
260  MatrixXs df_dx;
261  MatrixXs df_du;
262  VectorXs tmp_xstatic;
263  MatrixXs tmp_Jstatic;
264 
265  using Base::cost;
266  using Base::Fu;
267  using Base::Fx;
268  using Base::Lu;
269  using Base::Luu;
270  using Base::Lx;
271  using Base::Lxu;
272  using Base::Lxx;
273  using Base::r;
274  using Base::xout;
275 };
276 
277 } // namespace crocoddyl
278 
279 /* --- Details -------------------------------------------------------------- */
280 /* --- Details -------------------------------------------------------------- */
281 /* --- Details -------------------------------------------------------------- */
282 #include <crocoddyl/multibody/actions/contact-fwddyn.hxx>
283 
284 #endif // CROCODDYL_MULTIBODY_ACTIONS_CONTACT_FWDDYN_HPP_
Abstract class for the actuation-mapping model.
Define a stack of contact models.
Abstract class for differential action model.
boost::shared_ptr< StateAbstract > state_
Model of the state.
Differential action model for contact forward dynamics in multibody systems.
pinocchio::ModelTpl< Scalar > & get_pinocchio() const
Return the Pinocchio model.
const boost::shared_ptr< ContactModelMultiple > & get_contacts() const
Return the contact model.
virtual void calc(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the total cost value for nodes that depends only on 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 contact dynamics, and cost function.
const VectorXs & get_armature() const
Return the armature vector.
virtual boost::shared_ptr< DifferentialActionDataAbstract > createData()
Create the contact forward-dynamics data.
virtual void print(std::ostream &os) const
Print relevant information of the contact forward-dynamics model.
const boost::shared_ptr< CostModelSum > & get_costs() const
Return the cost model.
void set_damping_factor(const Scalar damping)
Modify the damping factor used in operational space inertia matrix.
void set_armature(const VectorXs &armature)
Modify the armature vector.
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.
const Scalar get_damping_factor() const
Return the damping factor used in operational space inertia matrix.
DifferentialActionModelContactFwdDynamicsTpl(boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ActuationModelAbstract > actuation, boost::shared_ptr< ContactModelMultiple > contacts, boost::shared_ptr< CostModelSum > costs, const Scalar JMinvJt_damping=Scalar(0.), const bool enable_force=false)
Initialize the contact forward-dynamics action model.
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.
const boost::shared_ptr< ActuationModelAbstract > & get_actuation() const
Return the actuation model.
virtual bool checkData(const boost::shared_ptr< DifferentialActionDataAbstract > &data)
Check that the given data belongs to the contact forward-dynamics data.
virtual void calcDiff(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the derivatives of the cost functions with respect to the state only.
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.