crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
contact-friction-cone.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_FRICTION_CONE_HPP_
10 #define CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_FRICTION_CONE_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-2d.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/multibody/friction-cone.hpp"
27 #include "crocoddyl/core/utils/exception.hpp"
28 
29 namespace crocoddyl {
30 
53 template <typename _Scalar>
54 class ResidualModelContactFrictionConeTpl : public ResidualModelAbstractTpl<_Scalar> {
55  public:
56  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
57 
58  typedef _Scalar Scalar;
59  typedef MathBaseTpl<Scalar> MathBase;
60  typedef ResidualModelAbstractTpl<Scalar> Base;
61  typedef ResidualDataContactFrictionConeTpl<Scalar> Data;
62  typedef StateMultibodyTpl<Scalar> StateMultibody;
63  typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
64  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
65  typedef FrictionConeTpl<Scalar> FrictionCone;
66  typedef typename MathBase::VectorXs VectorXs;
67  typedef typename MathBase::MatrixXs MatrixXs;
68  typedef typename MathBase::MatrixX3s MatrixX3s;
69 
78  ResidualModelContactFrictionConeTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
79  const FrictionCone& fref, const std::size_t nu);
80 
90  ResidualModelContactFrictionConeTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
91  const FrictionCone& fref);
93 
101  virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
102  const Eigen::Ref<const VectorXs>& u);
103 
113  virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
114 
122  virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
123  const Eigen::Ref<const VectorXs>& u);
124 
134  virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
135 
139  virtual boost::shared_ptr<ResidualDataAbstract> createData(DataCollectorAbstract* const data);
140 
144  pinocchio::FrameIndex get_id() const;
145 
149  const FrictionCone& get_reference() const;
150 
154  void set_id(const pinocchio::FrameIndex id);
155 
159  void set_reference(const FrictionCone& reference);
160 
166  virtual void print(std::ostream& os) const;
167 
168  protected:
169  using Base::nu_;
170  using Base::state_;
171  using Base::unone_;
172 
173  private:
174  pinocchio::FrameIndex id_;
175  FrictionCone fref_;
176 };
177 
178 template <typename _Scalar>
179 struct ResidualDataContactFrictionConeTpl : public ResidualDataAbstractTpl<_Scalar> {
180  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
181 
182  typedef _Scalar Scalar;
183  typedef MathBaseTpl<Scalar> MathBase;
184  typedef ResidualDataAbstractTpl<Scalar> Base;
185  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
186  typedef ContactModelMultipleTpl<Scalar> ContactModelMultiple;
187  typedef ImpulseModelMultipleTpl<Scalar> ImpulseModelMultiple;
188  typedef StateMultibodyTpl<Scalar> StateMultibody;
189  typedef typename MathBase::MatrixXs MatrixXs;
190 
191  template <template <typename Scalar> class Model>
192  ResidualDataContactFrictionConeTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
193  : Base(model, data) {
194  contact_type = ContactUndefined;
195  // Check that proper shared data has been passed
196  bool is_contact = true;
197  DataCollectorContactTpl<Scalar>* d1 = dynamic_cast<DataCollectorContactTpl<Scalar>*>(shared);
198  DataCollectorImpulseTpl<Scalar>* d2 = dynamic_cast<DataCollectorImpulseTpl<Scalar>*>(shared);
199  if (d1 == NULL && d2 == NULL) {
200  throw_pretty(
201  "Invalid argument: the shared data should be derived from DataCollectorContact or DataCollectorImpulse");
202  }
203  if (d2 != NULL) {
204  is_contact = false;
205  }
206 
207  // Avoids data casting at runtime
208  const pinocchio::FrameIndex id = model->get_id();
209  const boost::shared_ptr<StateMultibody>& state = boost::static_pointer_cast<StateMultibody>(model->get_state());
210  std::string frame_name = state->get_pinocchio()->frames[id].name;
211  bool found_contact = false;
212  if (is_contact) {
213  for (typename ContactModelMultiple::ContactDataContainer::iterator it = d1->contacts->contacts.begin();
214  it != d1->contacts->contacts.end(); ++it) {
215  if (it->second->frame == id) {
216  ContactData2DTpl<Scalar>* d2d = dynamic_cast<ContactData2DTpl<Scalar>*>(it->second.get());
217  if (d2d != NULL) {
218  contact_type = Contact2D;
219  found_contact = true;
220  contact = it->second;
221  break;
222  }
223  ContactData3DTpl<Scalar>* d3d = dynamic_cast<ContactData3DTpl<Scalar>*>(it->second.get());
224  if (d3d != NULL) {
225  contact_type = Contact3D;
226  found_contact = true;
227  contact = it->second;
228  break;
229  }
230  ContactData6DTpl<Scalar>* d6d = dynamic_cast<ContactData6DTpl<Scalar>*>(it->second.get());
231  if (d6d != NULL) {
232  contact_type = Contact6D;
233  found_contact = true;
234  contact = it->second;
235  break;
236  }
237  throw_pretty("Domain error: there isn't defined at least a 2d contact for " + frame_name);
238  break;
239  }
240  }
241  } else {
242  for (typename ImpulseModelMultiple::ImpulseDataContainer::iterator it = d2->impulses->impulses.begin();
243  it != d2->impulses->impulses.end(); ++it) {
244  if (it->second->frame == id) {
245  ImpulseData3DTpl<Scalar>* d3d = dynamic_cast<ImpulseData3DTpl<Scalar>*>(it->second.get());
246  if (d3d != NULL) {
247  contact_type = Contact3D;
248  found_contact = true;
249  contact = it->second;
250  break;
251  }
252  ImpulseData6DTpl<Scalar>* d6d = dynamic_cast<ImpulseData6DTpl<Scalar>*>(it->second.get());
253  if (d6d != NULL) {
254  contact_type = Contact6D;
255  found_contact = true;
256  contact = it->second;
257  break;
258  }
259  throw_pretty("Domain error: there isn't defined at least a 3d contact for " + frame_name);
260  break;
261  }
262  }
263  }
264  if (!found_contact) {
265  throw_pretty("Domain error: there isn't defined contact data for " + frame_name);
266  }
267  }
268 
269  boost::shared_ptr<ForceDataAbstractTpl<Scalar> > contact;
270  ContactType contact_type;
271  using Base::r;
272  using Base::Ru;
273  using Base::Rx;
274  using Base::shared;
275 };
276 
277 } // namespace crocoddyl
278 
279 /* --- Details -------------------------------------------------------------- */
280 /* --- Details -------------------------------------------------------------- */
281 /* --- Details -------------------------------------------------------------- */
282 #include "crocoddyl/multibody/residuals/contact-friction-cone.hxx"
283 
284 #endif // CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_FRICTION_CONE_HPP_
const FrictionCone & get_reference() const
Return the reference contact friction cone.
virtual void calcDiff(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the Jacobians of the contact friction cone residual.
ContactType contact_type
Type of contact (2D / 3D / 6D)
boost::shared_ptr< ForceDataAbstractTpl< Scalar > > contact
Contact force data.
pinocchio::FrameIndex get_id() const
Return the reference frame id.
std::size_t nu_
Control dimension.
void set_reference(const FrictionCone &reference)
Modify the reference contact friction cone.
ResidualModelContactFrictionConeTpl(boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const FrictionCone &fref, const std::size_t nu)
Initialize the contact friction cone residual model.
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.
void set_id(const pinocchio::FrameIndex id)
Modify the reference frame id.
VectorXs unone_
No control vector.
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Create the contact friction cone residual data.
virtual void print(std::ostream &os) const
Print relevant information of the contact-friction-cone residual.
VectorXs r
Residual vector.
virtual void calc(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the contact friction cone residual.
DataCollectorAbstract * shared
Shared data allocated by the action model.