crocoddyl  1.6.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
contact-force.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-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/core/cost-base.hpp"
14 #include "crocoddyl/multibody/states/multibody.hpp"
15 #include "crocoddyl/multibody/contact-base.hpp"
16 #include "crocoddyl/multibody/contacts/contact-3d.hpp"
17 #include "crocoddyl/multibody/contacts/contact-6d.hpp"
18 #include "crocoddyl/multibody/data/contacts.hpp"
19 #include "crocoddyl/multibody/frames.hpp"
20 #include "crocoddyl/core/utils/exception.hpp"
21 #include "crocoddyl/core/utils/deprecate.hpp"
22 
23 namespace crocoddyl {
24 
43 template <typename _Scalar>
45  public:
46  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
47 
48  typedef _Scalar Scalar;
57  typedef typename MathBase::VectorXs VectorXs;
58  typedef typename MathBase::MatrixXs MatrixXs;
59 
70  CostModelContactForceTpl(boost::shared_ptr<StateMultibody> state,
71  boost::shared_ptr<ActivationModelAbstract> activation, const FrameForce& fref,
72  const std::size_t& nu);
73 
84  CostModelContactForceTpl(boost::shared_ptr<StateMultibody> state,
85  boost::shared_ptr<ActivationModelAbstract> activation, const FrameForce& fref);
86 
98  CostModelContactForceTpl(boost::shared_ptr<StateMultibody> state, const FrameForce& fref, const std::size_t& nr,
99  const std::size_t& nu);
100 
112  CostModelContactForceTpl(boost::shared_ptr<StateMultibody> state, const FrameForce& fref, const std::size_t& nr);
113 
123  CostModelContactForceTpl(boost::shared_ptr<StateMultibody> state, const FrameForce& fref);
124  virtual ~CostModelContactForceTpl();
125 
136  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
137  const Eigen::Ref<const VectorXs>& u);
138 
149  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
150  const Eigen::Ref<const VectorXs>& u);
151 
158  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
159 
160  DEPRECATED("Use set_reference<FrameForceTpl<Scalar> >()", void set_fref(const FrameForce& fref));
161  DEPRECATED("Use get_reference<FrameForceTpl<Scalar> >()", const FrameForce& get_fref() const);
162 
163  protected:
167  virtual void set_referenceImpl(const std::type_info& ti, const void* pv);
168 
172  virtual void get_referenceImpl(const std::type_info& ti, void* pv) const;
173 
174  using Base::activation_;
175  using Base::nu_;
176  using Base::state_;
177  using Base::unone_;
178 
179  protected:
180  FrameForce fref_;
181 };
182 
183 template <typename _Scalar>
185  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
186 
187  typedef _Scalar Scalar;
194  typedef typename MathBase::MatrixXs MatrixXs;
195 
196  template <template <typename Scalar> class Model>
197  CostDataContactForceTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
198  : Base(model, data), Arr_Ru(model->get_activation()->get_nr(), model->get_state()->get_nv()) {
199  Arr_Ru.setZero();
200  contact_type = ContactUndefined;
201 
202  // Check that proper shared data has been passed
204  if (d == NULL) {
205  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorContact");
206  }
207 
208  // Avoids data casting at runtime
209  FrameForce fref = model->template get_reference<FrameForce>();
210  const boost::shared_ptr<StateMultibody>& state = boost::static_pointer_cast<StateMultibody>(model->get_state());
211  std::string frame_name = state->get_pinocchio()->frames[fref.id].name;
212  bool found_contact = false;
213  for (typename ContactModelMultiple::ContactDataContainer::iterator it = d->contacts->contacts.begin();
214  it != d->contacts->contacts.end(); ++it) {
215  if (it->second->frame == fref.id) {
216  ContactData3DTpl<Scalar>* d3d = dynamic_cast<ContactData3DTpl<Scalar>*>(it->second.get());
217  if (d3d != NULL) {
218  contact_type = Contact3D;
219  if (model->get_activation()->get_nr() != 3) {
220  throw_pretty("Domain error: nr isn't defined as 3 in the activation model for the 3d contact in " +
221  frame_name);
222  }
223  found_contact = true;
224  contact = it->second;
225  break;
226  }
227  ContactData6DTpl<Scalar>* d6d = dynamic_cast<ContactData6DTpl<Scalar>*>(it->second.get());
228  if (d6d != NULL) {
229  contact_type = Contact6D;
230  if (model->get_activation()->get_nr() != 6) {
231  throw_pretty("Domain error: nr isn't defined as 6 in the activation model for the 3d contact in " +
232  frame_name);
233  }
234  found_contact = true;
235  contact = it->second;
236  break;
237  }
238  throw_pretty("Domain error: there isn't defined at least a 3d contact for " + frame_name);
239  break;
240  }
241  }
242  if (!found_contact) {
243  throw_pretty("Domain error: there isn't defined contact data for " + frame_name);
244  }
245  }
246 
247  boost::shared_ptr<ContactDataAbstractTpl<Scalar> > contact;
248  MatrixXs Arr_Ru;
249  ContactType contact_type;
250  using Base::activation;
251  using Base::cost;
252  using Base::Lu;
253  using Base::Luu;
254  using Base::Lx;
255  using Base::Lxu;
256  using Base::Lxx;
257  using Base::r;
258  using Base::Ru;
259  using Base::Rx;
260  using Base::shared;
261 };
262 
263 } // namespace crocoddyl
264 
265 /* --- Details -------------------------------------------------------------- */
266 /* --- Details -------------------------------------------------------------- */
267 /* --- Details -------------------------------------------------------------- */
268 #include "crocoddyl/multibody/costs/contact-force.hxx"
269 
270 #endif // CROCODDYL_MULTIBODY_COSTS_CONTACT_FORCE_HPP_
Abstract class for cost models.
Definition: cost-base.hpp:47
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 .
boost::shared_ptr< ActivationModelAbstract > activation_
Activation model.
Definition: cost-base.hpp:194
FrameForce fref_
Reference spatial contact force .
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 .
VectorXs unone_
No control vector.
Definition: cost-base.hpp:196
std::size_t nu_
Control dimension.
Definition: cost-base.hpp:195
boost::shared_ptr< StateAbstract > state_
State description.
Definition: cost-base.hpp:193