crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
contact-force.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_FORCE_HPP_
10 #define CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_FORCE_HPP_
11 
12 #include "crocoddyl/multibody/fwd.hpp"
13 #include "crocoddyl/core/residual-base.hpp"
14 #include "crocoddyl/multibody/states/multibody.hpp"
15 #include "crocoddyl/multibody/contact-base.hpp"
16 #include "crocoddyl/multibody/impulse-base.hpp"
17 #include "crocoddyl/multibody/contacts/multiple-contacts.hpp"
18 #include "crocoddyl/multibody/contacts/contact-1d.hpp"
19 #include "crocoddyl/multibody/contacts/contact-3d.hpp"
20 #include "crocoddyl/multibody/contacts/contact-6d.hpp"
21 #include "crocoddyl/multibody/impulses/multiple-impulses.hpp"
22 #include "crocoddyl/multibody/impulses/impulse-3d.hpp"
23 #include "crocoddyl/multibody/impulses/impulse-6d.hpp"
24 #include "crocoddyl/multibody/data/contacts.hpp"
25 #include "crocoddyl/multibody/data/impulses.hpp"
26 #include "crocoddyl/core/utils/exception.hpp"
27 
28 namespace crocoddyl {
29 
50 template <typename _Scalar>
51 class ResidualModelContactForceTpl : public ResidualModelAbstractTpl<_Scalar> {
52  public:
53  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
54 
55  typedef _Scalar Scalar;
56  typedef MathBaseTpl<Scalar> MathBase;
57  typedef ResidualModelAbstractTpl<Scalar> Base;
58  typedef ResidualDataContactForceTpl<Scalar> Data;
59  typedef StateMultibodyTpl<Scalar> StateMultibody;
60  typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
61  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
62  typedef pinocchio::ForceTpl<Scalar> Force;
63  typedef typename MathBase::VectorXs VectorXs;
64  typedef typename MathBase::MatrixXs MatrixXs;
65 
75  ResidualModelContactForceTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
76  const Force& fref, const std::size_t nc, const std::size_t nu);
77 
88  ResidualModelContactForceTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
89  const Force& fref, const std::size_t nc);
91 
103  virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
104  const Eigen::Ref<const VectorXs>& u);
105 
115  virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
116 
128  virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
129  const Eigen::Ref<const VectorXs>& u);
130 
140  virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
141 
148  virtual boost::shared_ptr<ResidualDataAbstract> createData(DataCollectorAbstract* const data);
149 
153  pinocchio::FrameIndex get_id() const;
154 
158  const Force& get_reference() const;
159 
163  void set_id(const pinocchio::FrameIndex id);
164 
168  void set_reference(const Force& reference);
169 
175  virtual void print(std::ostream& os) const;
176 
177  protected:
178  using Base::nr_;
179  using Base::nu_;
180  using Base::state_;
181  using Base::unone_;
182 
183  private:
184  pinocchio::FrameIndex id_;
185  Force fref_;
186 };
187 
188 template <typename _Scalar>
189 struct ResidualDataContactForceTpl : public ResidualDataAbstractTpl<_Scalar> {
190  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
191 
192  typedef _Scalar Scalar;
193  typedef MathBaseTpl<Scalar> MathBase;
194  typedef ResidualDataAbstractTpl<Scalar> Base;
195  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
196  typedef ContactModelMultipleTpl<Scalar> ContactModelMultiple;
197  typedef ImpulseModelMultipleTpl<Scalar> ImpulseModelMultiple;
198  typedef pinocchio::ForceTpl<Scalar> Force;
199  typedef StateMultibodyTpl<Scalar> StateMultibody;
200  typedef typename MathBase::MatrixXs MatrixXs;
201 
202  template <template <typename Scalar> class Model>
203  ResidualDataContactForceTpl(Model<Scalar>* const model, DataCollectorAbstract* const data) : Base(model, data) {
204  contact_type = ContactUndefined;
205 
206  // Check that proper shared data has been passed
207  bool is_contact = true;
208  DataCollectorContactTpl<Scalar>* d1 = dynamic_cast<DataCollectorContactTpl<Scalar>*>(shared);
209  DataCollectorImpulseTpl<Scalar>* d2 = dynamic_cast<DataCollectorImpulseTpl<Scalar>*>(shared);
210  if (d1 == NULL && d2 == NULL) {
211  throw_pretty(
212  "Invalid argument: the shared data should be derived from DataCollectorContact or DataCollectorImpulse");
213  }
214  if (d2 != NULL) {
215  is_contact = false;
216  }
217 
218  // Avoids data casting at runtime
219  const pinocchio::FrameIndex id = model->get_id();
220  const boost::shared_ptr<StateMultibody>& state = boost::static_pointer_cast<StateMultibody>(model->get_state());
221  std::string frame_name = state->get_pinocchio()->frames[id].name;
222  bool found_contact = false;
223  if (is_contact) {
224  for (typename ContactModelMultiple::ContactDataContainer::iterator it = d1->contacts->contacts.begin();
225  it != d1->contacts->contacts.end(); ++it) {
226  if (it->second->frame == id) {
227  ContactData1DTpl<Scalar>* d1d = dynamic_cast<ContactData1DTpl<Scalar>*>(it->second.get());
228  if (d1d != NULL) {
229  contact_type = Contact1D;
230  found_contact = true;
231  contact = it->second;
232  break;
233  }
234  ContactData3DTpl<Scalar>* d3d = dynamic_cast<ContactData3DTpl<Scalar>*>(it->second.get());
235  if (d3d != NULL) {
236  contact_type = Contact3D;
237  found_contact = true;
238  contact = it->second;
239  break;
240  }
241  ContactData6DTpl<Scalar>* d6d = dynamic_cast<ContactData6DTpl<Scalar>*>(it->second.get());
242  if (d6d != NULL) {
243  contact_type = Contact6D;
244  found_contact = true;
245  contact = it->second;
246  break;
247  }
248  throw_pretty("Domain error: there isn't defined at least a 3d contact for " + frame_name);
249  break;
250  }
251  }
252  } else {
253  for (typename ImpulseModelMultiple::ImpulseDataContainer::iterator it = d2->impulses->impulses.begin();
254  it != d2->impulses->impulses.end(); ++it) {
255  if (it->second->frame == id) {
256  ImpulseData3DTpl<Scalar>* d3d = dynamic_cast<ImpulseData3DTpl<Scalar>*>(it->second.get());
257  if (d3d != NULL) {
258  contact_type = Contact3D;
259  found_contact = true;
260  contact = it->second;
261  break;
262  }
263  ImpulseData6DTpl<Scalar>* d6d = dynamic_cast<ImpulseData6DTpl<Scalar>*>(it->second.get());
264  if (d6d != NULL) {
265  contact_type = Contact6D;
266  found_contact = true;
267  contact = it->second;
268  break;
269  }
270  throw_pretty("Domain error: there isn't defined at least a 3d impulse for " + frame_name);
271  break;
272  }
273  }
274  }
275  if (!found_contact) {
276  throw_pretty("Domain error: there isn't defined contact/impulse data for " + frame_name);
277  }
278  }
279 
280  boost::shared_ptr<ForceDataAbstractTpl<Scalar> > contact;
281  ContactType contact_type;
282  using Base::r;
283  using Base::Ru;
284  using Base::Rx;
285  using Base::shared;
286 };
287 
288 } // namespace crocoddyl
289 
290 /* --- Details -------------------------------------------------------------- */
291 /* --- Details -------------------------------------------------------------- */
292 /* --- Details -------------------------------------------------------------- */
293 #include "crocoddyl/multibody/residuals/contact-force.hxx"
294 
295 #endif // CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_FORCE_HPP_
void set_reference(const Force &reference)
Modify the reference spatial contact force in the contact coordinates.
std::size_t nr_
Residual vector dimension.
const Force & get_reference() const
Return the reference spatial contact force in the contact coordinates.
virtual void calc(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the contact force residual.
ResidualModelContactForceTpl(boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const Force &fref, const std::size_t nc, const std::size_t nu)
Initialize the contact force residual model.
std::size_t nu_
Control dimension.
virtual void print(std::ostream &os) const
Print relevant information of the contact-force residual.
boost::shared_ptr< ForceDataAbstractTpl< Scalar > > contact
Contact force data.
pinocchio::FrameIndex get_id() const
Return the reference frame id.
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Create the contact force residual data.
MatrixXs Rx
Jacobian of the residual vector with respect the state.
MatrixXs Ru
Jacobian of the residual vector with respect the control.
boost::shared_ptr< StateAbstract > state_
State description.
VectorXs unone_
No control vector.
virtual void calcDiff(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the derivatives of the contact force residual.
VectorXs r
Residual vector.
ContactType contact_type
Type of contact (3D / 6D)
DataCollectorAbstract * shared
Shared data allocated by the action model.
void set_id(const pinocchio::FrameIndex id)
Modify the reference frame id.