crocoddyl  1.6.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
contact-fwddyn.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2020, LAAS-CNRS, University of Edinburgh, 2020 CTU, INRIA
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 
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<ActuationModelAbstract> 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_
crocoddyl::DifferentialActionDataAbstractTpl::Lxu
MatrixXs Lxu
Hessian of the cost function.
Definition: diff-action-base.hpp:251
crocoddyl::DifferentialActionModelAbstractTpl::nr_
std::size_t nr_
Dimension of the cost residual.
Definition: diff-action-base.hpp:200
crocoddyl::ActuationModelAbstractTpl
Definition: actuation-base.hpp:25
crocoddyl::MathBaseTpl< Scalar >
crocoddyl::DifferentialActionDataContactFwdDynamicsTpl
Definition: contact-fwddyn.hpp:90
crocoddyl::DifferentialActionDataAbstractTpl::Lx
VectorXs Lx
Jacobian of the cost function.
Definition: diff-action-base.hpp:248
crocoddyl::CostModelSumTpl< Scalar >
crocoddyl::DifferentialActionModelAbstractTpl::has_control_limits_
bool has_control_limits_
Indicates whether any of the control limits is finite.
Definition: diff-action-base.hpp:205
crocoddyl::DifferentialActionDataAbstractTpl::Lu
VectorXs Lu
Jacobian of the cost function.
Definition: diff-action-base.hpp:249
crocoddyl::DifferentialActionModelAbstractTpl::unone_
VectorXs unone_
Neutral state.
Definition: diff-action-base.hpp:202
crocoddyl::DifferentialActionDataAbstractTpl::Fx
MatrixXs Fx
Jacobian of the dynamics.
Definition: diff-action-base.hpp:245
crocoddyl::DifferentialActionDataAbstractTpl::xout
VectorXs xout
evolution state
Definition: diff-action-base.hpp:244
crocoddyl::DifferentialActionModelAbstractTpl::nu_
std::size_t nu_
Control dimension.
Definition: diff-action-base.hpp:199
crocoddyl::DifferentialActionModelContactFwdDynamicsTpl
Definition: contact-fwddyn.hpp:25
crocoddyl::StateMultibodyTpl
Definition: fwd.hpp:201
crocoddyl::DifferentialActionDataAbstractTpl::r
VectorXs r
Cost residual.
Definition: diff-action-base.hpp:247
crocoddyl::DifferentialActionDataAbstractTpl
Definition: diff-action-base.hpp:211
crocoddyl::DifferentialActionModelAbstractTpl::state_
boost::shared_ptr< StateAbstract > state_
Model of the state.
Definition: diff-action-base.hpp:201
crocoddyl::DifferentialActionDataAbstractTpl::Luu
MatrixXs Luu
Hessian of the cost function.
Definition: diff-action-base.hpp:252
crocoddyl::DifferentialActionModelContactFwdDynamicsTpl::checkData
virtual bool checkData(const boost::shared_ptr< DifferentialActionDataAbstract > &data)
Checks that a specific data belongs to this model.
crocoddyl::DifferentialActionModelContactFwdDynamicsTpl::createData
virtual boost::shared_ptr< DifferentialActionDataAbstract > createData()
Create the differential action data.
crocoddyl::DifferentialActionModelContactFwdDynamicsTpl::calcDiff
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.
crocoddyl::DifferentialActionDataAbstractTpl::Lxx
MatrixXs Lxx
Hessian of the cost function.
Definition: diff-action-base.hpp:250
crocoddyl::DifferentialActionModelContactFwdDynamicsTpl::quasiStatic
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.
crocoddyl::DifferentialActionModelAbstractTpl
Abstract class for differential action model.
Definition: diff-action-base.hpp:54
crocoddyl::DifferentialActionDataAbstractTpl::cost
Scalar cost
cost value
Definition: diff-action-base.hpp:243
crocoddyl::DifferentialActionModelContactFwdDynamicsTpl::calc
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.
crocoddyl::ContactModelMultipleTpl
Define a stack of contact models.
Definition: multiple-contacts.hpp:46
crocoddyl::DifferentialActionDataAbstractTpl::Fu
MatrixXs Fu
Jacobian of the dynamics.
Definition: diff-action-base.hpp:246
crocoddyl::DataCollectorActMultibodyInContactTpl
Definition: contacts.hpp:42
crocoddyl::DifferentialActionModelAbstractTpl::u_lb_
VectorXs u_lb_
Lower control limits.
Definition: diff-action-base.hpp:203
crocoddyl::DifferentialActionModelAbstractTpl::u_ub_
VectorXs u_ub_
Upper control limits.
Definition: diff-action-base.hpp:204