crocoddyl  1.8.1
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-3d.hpp"
19 #include "crocoddyl/multibody/contacts/contact-6d.hpp"
20 #include "crocoddyl/multibody/impulses/multiple-impulses.hpp"
21 #include "crocoddyl/multibody/impulses/impulse-3d.hpp"
22 #include "crocoddyl/multibody/impulses/impulse-6d.hpp"
23 #include "crocoddyl/multibody/data/contacts.hpp"
24 #include "crocoddyl/multibody/data/impulses.hpp"
25 #include "crocoddyl/core/utils/exception.hpp"
26 
27 namespace crocoddyl {
28 
49 template <typename _Scalar>
50 class ResidualModelContactForceTpl : public ResidualModelAbstractTpl<_Scalar> {
51  public:
52  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
53 
54  typedef _Scalar Scalar;
55  typedef MathBaseTpl<Scalar> MathBase;
56  typedef ResidualModelAbstractTpl<Scalar> Base;
57  typedef ResidualDataContactForceTpl<Scalar> Data;
58  typedef StateMultibodyTpl<Scalar> StateMultibody;
59  typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
60  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
61  typedef pinocchio::ForceTpl<Scalar> Force;
62  typedef typename MathBase::VectorXs VectorXs;
63  typedef typename MathBase::MatrixXs MatrixXs;
64 
74  ResidualModelContactForceTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
75  const Force& fref, const std::size_t nc, const std::size_t nu);
76 
87  ResidualModelContactForceTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
88  const Force& fref, const std::size_t nc);
90 
102  virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
103  const Eigen::Ref<const VectorXs>& u);
104 
116  virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
117  const Eigen::Ref<const VectorXs>& u);
118 
125  virtual boost::shared_ptr<ResidualDataAbstract> createData(DataCollectorAbstract* const data);
126 
130  pinocchio::FrameIndex get_id() const;
131 
135  const Force& get_reference() const;
136 
140  void set_id(const pinocchio::FrameIndex id);
141 
145  void set_reference(const Force& reference);
146 
152  virtual void print(std::ostream& os) const;
153 
154  protected:
155  using Base::nr_;
156  using Base::nu_;
157  using Base::state_;
158  using Base::unone_;
159 
160  private:
161  pinocchio::FrameIndex id_;
162  Force fref_;
163 };
164 
165 template <typename _Scalar>
166 struct ResidualDataContactForceTpl : public ResidualDataAbstractTpl<_Scalar> {
167  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
168 
169  typedef _Scalar Scalar;
170  typedef MathBaseTpl<Scalar> MathBase;
171  typedef ResidualDataAbstractTpl<Scalar> Base;
172  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
173  typedef ContactModelMultipleTpl<Scalar> ContactModelMultiple;
174  typedef ImpulseModelMultipleTpl<Scalar> ImpulseModelMultiple;
175  typedef pinocchio::ForceTpl<Scalar> Force;
176  typedef StateMultibodyTpl<Scalar> StateMultibody;
177  typedef typename MathBase::MatrixXs MatrixXs;
178 
179  template <template <typename Scalar> class Model>
180  ResidualDataContactForceTpl(Model<Scalar>* const model, DataCollectorAbstract* const data) : Base(model, data) {
181  contact_type = ContactUndefined;
182 
183  // Check that proper shared data has been passed
184  bool is_contact = true;
185  DataCollectorContactTpl<Scalar>* d1 = dynamic_cast<DataCollectorContactTpl<Scalar>*>(shared);
186  DataCollectorImpulseTpl<Scalar>* d2 = dynamic_cast<DataCollectorImpulseTpl<Scalar>*>(shared);
187  if (d1 == NULL && d2 == NULL) {
188  throw_pretty(
189  "Invalid argument: the shared data should be derived from DataCollectorContact or DataCollectorImpulse");
190  }
191  if (d2 != NULL) {
192  is_contact = false;
193  }
194 
195  // Avoids data casting at runtime
196  const pinocchio::FrameIndex id = model->get_id();
197  const boost::shared_ptr<StateMultibody>& state = boost::static_pointer_cast<StateMultibody>(model->get_state());
198  std::string frame_name = state->get_pinocchio()->frames[id].name;
199  bool found_contact = false;
200  if (is_contact) {
201  for (typename ContactModelMultiple::ContactDataContainer::iterator it = d1->contacts->contacts.begin();
202  it != d1->contacts->contacts.end(); ++it) {
203  if (it->second->frame == id) {
204  ContactData3DTpl<Scalar>* d3d = dynamic_cast<ContactData3DTpl<Scalar>*>(it->second.get());
205  if (d3d != NULL) {
206  contact_type = Contact3D;
207  found_contact = true;
208  contact = it->second;
209  break;
210  }
211  ContactData6DTpl<Scalar>* d6d = dynamic_cast<ContactData6DTpl<Scalar>*>(it->second.get());
212  if (d6d != NULL) {
213  contact_type = Contact6D;
214  found_contact = true;
215  contact = it->second;
216  break;
217  }
218  throw_pretty("Domain error: there isn't defined at least a 3d contact for " + frame_name);
219  break;
220  }
221  }
222  } else {
223  for (typename ImpulseModelMultiple::ImpulseDataContainer::iterator it = d2->impulses->impulses.begin();
224  it != d2->impulses->impulses.end(); ++it) {
225  if (it->second->frame == id) {
226  ImpulseData3DTpl<Scalar>* d3d = dynamic_cast<ImpulseData3DTpl<Scalar>*>(it->second.get());
227  if (d3d != NULL) {
228  contact_type = Contact3D;
229  found_contact = true;
230  contact = it->second;
231  break;
232  }
233  ImpulseData6DTpl<Scalar>* d6d = dynamic_cast<ImpulseData6DTpl<Scalar>*>(it->second.get());
234  if (d6d != NULL) {
235  contact_type = Contact6D;
236  found_contact = true;
237  contact = it->second;
238  break;
239  }
240  throw_pretty("Domain error: there isn't defined at least a 3d impulse for " + frame_name);
241  break;
242  }
243  }
244  }
245  if (!found_contact) {
246  throw_pretty("Domain error: there isn't defined contact/impulse data for " + frame_name);
247  }
248  }
249 
250  boost::shared_ptr<ForceDataAbstractTpl<Scalar> > contact;
251  ContactType contact_type;
252  using Base::r;
253  using Base::Ru;
254  using Base::Rx;
255  using Base::shared;
256 };
257 
258 } // namespace crocoddyl
259 
260 /* --- Details -------------------------------------------------------------- */
261 /* --- Details -------------------------------------------------------------- */
262 /* --- Details -------------------------------------------------------------- */
263 #include "crocoddyl/multibody/residuals/contact-force.hxx"
264 
265 #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.
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.
ContactType contact_type
Type of contact (3D / 6D)
void set_id(const pinocchio::FrameIndex id)
Modify the reference frame id.