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_
crocoddyl::DifferentialActionDataAbstractTpl::Lxu
MatrixXs Lxu
Hessian of the cost function.
Definition: diff-action-base.hpp:271
crocoddyl::DifferentialActionModelContactFwdDynamicsTpl::DifferentialActionModelContactFwdDynamicsTpl
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.
crocoddyl::ActuationModelAbstractTpl
Abstract class for the actuation-mapping model.
Definition: actuation-base.hpp:39
crocoddyl::DifferentialActionModelContactFwdDynamicsTpl::get_armature
const VectorXs & get_armature() const
Return the armature vector.
crocoddyl::MathBaseTpl< Scalar >
crocoddyl::DifferentialActionModelContactFwdDynamicsTpl::set_armature
void set_armature(const VectorXs &armature)
Modify the armature vector.
crocoddyl::DifferentialActionModelContactFwdDynamicsTpl::get_contacts
const boost::shared_ptr< ContactModelMultiple > & get_contacts() const
Return the contact model.
crocoddyl::DifferentialActionModelContactFwdDynamicsTpl::get_costs
const boost::shared_ptr< CostModelSum > & get_costs() const
Return the cost model.
crocoddyl::DifferentialActionDataContactFwdDynamicsTpl
Definition: contact-fwddyn.hpp:226
crocoddyl::DifferentialActionModelContactFwdDynamicsTpl::print
virtual void print(std::ostream &os) const
Print relevant information of the contact forward-dynamics model.
crocoddyl::DifferentialActionDataAbstractTpl::Lx
VectorXs Lx
Jacobian of the cost function.
Definition: diff-action-base.hpp:268
crocoddyl::DifferentialActionModelContactFwdDynamicsTpl::get_pinocchio
pinocchio::ModelTpl< Scalar > & get_pinocchio() const
Return the Pinocchio model.
crocoddyl::CostModelSumTpl< Scalar >
crocoddyl::DifferentialActionDataAbstractTpl::Lu
VectorXs Lu
Jacobian of the cost function.
Definition: diff-action-base.hpp:269
crocoddyl::DifferentialActionDataAbstractTpl::Fx
MatrixXs Fx
Jacobian of the dynamics.
Definition: diff-action-base.hpp:265
crocoddyl::DifferentialActionDataAbstractTpl::xout
VectorXs xout
evolution state
Definition: diff-action-base.hpp:264
crocoddyl::DifferentialActionModelAbstractTpl::nu_
std::size_t nu_
Control dimension.
Definition: diff-action-base.hpp:219
crocoddyl::DifferentialActionModelContactFwdDynamicsTpl::set_damping_factor
void set_damping_factor(const Scalar damping)
Modify the damping factor used in operational space inertia matrix.
crocoddyl::DifferentialActionModelContactFwdDynamicsTpl
Differential action model for contact forward dynamics in multibody systems.
Definition: contact-fwddyn.hpp:59
crocoddyl::StateMultibodyTpl
State multibody representation.
Definition: fwd.hpp:305
crocoddyl::DifferentialActionModelContactFwdDynamicsTpl::get_actuation
const boost::shared_ptr< ActuationModelAbstract > & get_actuation() const
Return the actuation model.
crocoddyl::DifferentialActionDataAbstractTpl::r
VectorXs r
Cost residual.
Definition: diff-action-base.hpp:267
crocoddyl::DifferentialActionDataAbstractTpl
Definition: diff-action-base.hpp:231
crocoddyl::DifferentialActionModelAbstractTpl::state_
boost::shared_ptr< StateAbstract > state_
Model of the state.
Definition: diff-action-base.hpp:221
crocoddyl::DifferentialActionModelContactFwdDynamicsTpl::get_damping_factor
const Scalar get_damping_factor() const
Return the damping factor used in operational space inertia matrix.
crocoddyl::DifferentialActionDataAbstractTpl::Luu
MatrixXs Luu
Hessian of the cost function.
Definition: diff-action-base.hpp:272
crocoddyl::DifferentialActionModelContactFwdDynamicsTpl::checkData
virtual bool checkData(const boost::shared_ptr< DifferentialActionDataAbstract > &data)
Check that the given data belongs to the contact forward-dynamics data.
crocoddyl::DifferentialActionModelContactFwdDynamicsTpl::createData
virtual boost::shared_ptr< DifferentialActionDataAbstract > createData()
Create the contact forward-dynamics 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 contact dynamics, and cost function.
crocoddyl::DifferentialActionDataAbstractTpl::Lxx
MatrixXs Lxx
Hessian of the cost function.
Definition: diff-action-base.hpp:270
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:53
crocoddyl::DifferentialActionDataAbstractTpl::cost
Scalar cost
cost value
Definition: diff-action-base.hpp:263
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:55
crocoddyl::DifferentialActionDataAbstractTpl::Fu
MatrixXs Fu
Jacobian of the dynamics.
Definition: diff-action-base.hpp:266
crocoddyl::DataCollectorActMultibodyInContactTpl
Definition: contacts.hpp:42