crocoddyl  1.3.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
contact-force.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_COSTS_CONTACT_FORCE_HPP_
10 #define CROCODDYL_MULTIBODY_COSTS_CONTACT_FORCE_HPP_
11 
12 #include "crocoddyl/multibody/fwd.hpp"
13 #include "crocoddyl/multibody/cost-base.hpp"
14 #include "crocoddyl/multibody/contact-base.hpp"
15 #include "crocoddyl/multibody/data/contacts.hpp"
16 #include "crocoddyl/multibody/frames.hpp"
17 #include "crocoddyl/core/utils/exception.hpp"
18 
19 namespace crocoddyl {
20 
21 template <typename _Scalar>
23  public:
24  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
25 
26  typedef _Scalar Scalar;
36  typedef typename MathBase::Vector6s Vector6s;
37  typedef typename MathBase::VectorXs VectorXs;
38  typedef typename MathBase::MatrixXs MatrixXs;
39 
40  CostModelContactForceTpl(boost::shared_ptr<StateMultibody> state,
41  boost::shared_ptr<ActivationModelAbstract> activation, const FrameForce& fref,
42  const std::size_t& nu);
43  CostModelContactForceTpl(boost::shared_ptr<StateMultibody> state,
44  boost::shared_ptr<ActivationModelAbstract> activation, const FrameForce& fref);
45  CostModelContactForceTpl(boost::shared_ptr<StateMultibody> state, const FrameForce& fref, const std::size_t& nu);
46  CostModelContactForceTpl(boost::shared_ptr<StateMultibody> state, const FrameForce& fref);
47  virtual ~CostModelContactForceTpl();
48 
49  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
50  const Eigen::Ref<const VectorXs>& u);
51  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
52  const Eigen::Ref<const VectorXs>& u);
53  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
54 
55  const FrameForce& get_fref() const;
56  void set_fref(const FrameForce& fref);
57 
58  protected:
59  virtual void set_referenceImpl(const std::type_info& ti, const void* pv);
60  virtual void get_referenceImpl(const std::type_info& ti, void* pv);
61 
62  using Base::activation_;
63  using Base::nu_;
64  using Base::state_;
65  using Base::unone_;
66 
67  protected:
68  FrameForce fref_;
69 };
70 
71 template <typename _Scalar>
72 struct CostDataContactForceTpl : public CostDataAbstractTpl<_Scalar> {
73  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
74 
75  typedef _Scalar Scalar;
80  typedef typename MathBase::VectorXs VectorXs;
81  typedef typename MathBase::MatrixXs MatrixXs;
82  typedef typename MathBase::Matrix6xs Matrix6xs;
83 
84  template <template <typename Scalar> class Model>
85  CostDataContactForceTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
86  : Base(model, data), Arr_Ru(model->get_activation()->get_nr(), model->get_state()->get_nv()) {
87  Arr_Ru.setZero();
88 
89  // Check that proper shared data has been passed
91  if (d == NULL) {
92  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorContact");
93  }
94 
95  // Avoids data casting at runtime
96  std::string frame_name = model->get_state()->get_pinocchio()->frames[model->get_fref().frame].name;
97  bool found_contact = false;
98  for (typename ContactModelMultiple::ContactDataContainer::iterator it = d->contacts->contacts.begin();
99  it != d->contacts->contacts.end(); ++it) {
100  if (it->second->frame == model->get_fref().frame) {
101  found_contact = true;
102  contact = it->second;
103  break;
104  }
105  }
106  if (!found_contact) {
107  throw_pretty("Domain error: there isn't defined contact data for " + frame_name);
108  }
109  }
110 
111  boost::shared_ptr<ContactDataAbstractTpl<Scalar> > contact;
112  MatrixXs Arr_Ru;
113  using Base::activation;
114  using Base::cost;
115  using Base::Lu;
116  using Base::Luu;
117  using Base::Lx;
118  using Base::Lxu;
119  using Base::Lxx;
120  using Base::r;
121  using Base::Ru;
122  using Base::Rx;
123  using Base::shared;
124 };
125 
126 } // namespace crocoddyl
127 /* --- Details -------------------------------------------------------------- */
128 /* --- Details -------------------------------------------------------------- */
129 /* --- Details -------------------------------------------------------------- */
130 #include "crocoddyl/multibody/costs/contact-force.hxx"
131 
132 #endif // CROCODDYL_MULTIBODY_COSTS_CONTACT_FORCE_HPP_