crocoddyl  1.4.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/contacts/contact-3d.hpp"
16 #include "crocoddyl/multibody/contacts/contact-6d.hpp"
17 #include "crocoddyl/multibody/data/contacts.hpp"
18 #include "crocoddyl/multibody/frames.hpp"
19 #include "crocoddyl/core/utils/exception.hpp"
20 #include "crocoddyl/core/utils/deprecate.hpp"
21 
22 namespace crocoddyl {
23 
24 enum ContactType { Contact3D, Contact6D, Undefined };
25 
46 template <typename _Scalar>
48  public:
49  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
50 
51  typedef _Scalar Scalar;
61  typedef typename MathBase::Vector6s Vector6s;
62  typedef typename MathBase::VectorXs VectorXs;
63  typedef typename MathBase::MatrixXs MatrixXs;
64 
76  CostModelContactForceTpl(boost::shared_ptr<StateMultibody> state,
77  boost::shared_ptr<ActivationModelAbstract> activation, const FrameForce& fref,
78  const std::size_t& nu);
79 
91  CostModelContactForceTpl(boost::shared_ptr<StateMultibody> state,
92  boost::shared_ptr<ActivationModelAbstract> activation, const FrameForce& fref);
93 
106  CostModelContactForceTpl(boost::shared_ptr<StateMultibody> state, const FrameForce& fref, const std::size_t& nr,
107  const std::size_t& nu);
108 
120  CostModelContactForceTpl(boost::shared_ptr<StateMultibody> state, const FrameForce& fref, const std::size_t& nr);
121 
132  CostModelContactForceTpl(boost::shared_ptr<StateMultibody> state, const FrameForce& fref);
133  virtual ~CostModelContactForceTpl();
134 
145  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
146  const Eigen::Ref<const VectorXs>& u);
147 
158  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
159  const Eigen::Ref<const VectorXs>& u);
160 
170  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
171 
172  DEPRECATED("Use set_reference<FrameForceTpl<Scalar> >()", void set_fref(const FrameForce& fref));
173  DEPRECATED("Use get_reference<FrameForceTpl<Scalar> >()", const FrameForce& get_fref() const);
174 
175  protected:
180  virtual void set_referenceImpl(const std::type_info& ti, const void* pv);
181 
186  virtual void get_referenceImpl(const std::type_info& ti, void* pv) const;
187 
188  using Base::activation_;
189  using Base::nu_;
190  using Base::state_;
191  using Base::unone_;
192 
193  protected:
194  FrameForce fref_;
195 };
197 
198 template <typename _Scalar>
200  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
201 
202  typedef _Scalar Scalar;
208  typedef typename MathBase::VectorXs VectorXs;
209  typedef typename MathBase::MatrixXs MatrixXs;
210  typedef typename MathBase::Matrix6xs Matrix6xs;
211 
212  template <template <typename Scalar> class Model>
213  CostDataContactForceTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
214  : Base(model, data), Arr_Ru(model->get_activation()->get_nr(), model->get_state()->get_nv()) {
215  Arr_Ru.setZero();
216  contact_type = Undefined;
217 
218  // Check that proper shared data has been passed
220  if (d == NULL) {
221  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorContact");
222  }
223 
224  // Avoids data casting at runtime
225  FrameForce fref = model->template get_reference<FrameForce>();
226  std::string frame_name = model->get_state()->get_pinocchio()->frames[fref.frame].name;
227  bool found_contact = false;
228  for (typename ContactModelMultiple::ContactDataContainer::iterator it = d->contacts->contacts.begin();
229  it != d->contacts->contacts.end(); ++it) {
230  if (it->second->frame == fref.frame) {
231  ContactData3DTpl<Scalar>* d3d = dynamic_cast<ContactData3DTpl<Scalar>*>(it->second.get());
232  if (d3d != NULL) {
233  contact_type = Contact3D;
234  if (model->get_activation()->get_nr() != 3) {
235  throw_pretty("Domain error: nr isn't defined as 3 in the activation model for the 3d contact in " +
236  frame_name);
237  }
238  found_contact = true;
239  contact = it->second;
240  break;
241  }
242  ContactData6DTpl<Scalar>* d6d = dynamic_cast<ContactData6DTpl<Scalar>*>(it->second.get());
243  if (d6d != NULL) {
244  contact_type = Contact6D;
245  if (model->get_activation()->get_nr() != 6) {
246  throw_pretty("Domain error: nr isn't defined as 6 in the activation model for the 3d contact in " +
247  frame_name);
248  }
249  found_contact = true;
250  contact = it->second;
251  break;
252  }
253  throw_pretty("Domain error: there isn't defined at least a 3d contact for " + frame_name);
254  break;
255  }
256  }
257  if (!found_contact) {
258  throw_pretty("Domain error: there isn't defined contact data for " + frame_name);
259  }
260  }
261 
262  boost::shared_ptr<ContactDataAbstractTpl<Scalar> > contact;
263  MatrixXs Arr_Ru;
264  ContactType contact_type;
265  using Base::activation;
266  using Base::cost;
267  using Base::Lu;
268  using Base::Luu;
269  using Base::Lx;
270  using Base::Lxu;
271  using Base::Lxx;
272  using Base::r;
273  using Base::Ru;
274  using Base::Rx;
275  using Base::shared;
276 };
277 
278 } // namespace crocoddyl
279 
280 /* --- Details -------------------------------------------------------------- */
281 /* --- Details -------------------------------------------------------------- */
282 /* --- Details -------------------------------------------------------------- */
283 #include "crocoddyl/multibody/costs/contact-force.hxx"
284 
285 #endif // CROCODDYL_MULTIBODY_COSTS_CONTACT_FORCE_HPP_
Define a stack of contact models.
virtual boost::shared_ptr< CostDataAbstract > createData(DataCollectorAbstract *const data)
Create the contact force cost data.
Define a contact force cost function.
virtual void set_referenceImpl(const std::type_info &ti, const void *pv)
Return the reference spatial contact force in the contact coordinates .
virtual void calc(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the contact force cost.
CostModelContactForceTpl(boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ActivationModelAbstract > activation, const FrameForce &fref, const std::size_t &nu)
Initialize the contact force cost model.
virtual void calcDiff(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the derivatives of the contact force cost.
virtual void get_referenceImpl(const std::type_info &ti, void *pv) const
Modify the reference spatial contact force in the contact coordinates .