crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
contact-wrench-cone.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-2021, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_WRENCH_CONE_HPP_
10 #define CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_WRENCH_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-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/multibody/wrench-cone.hpp"
26 #include "crocoddyl/core/utils/exception.hpp"
27 
28 namespace crocoddyl {
29 
49 template <typename _Scalar>
50 class ResidualModelContactWrenchConeTpl : 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 ResidualDataContactWrenchConeTpl<Scalar> Data;
58  typedef StateMultibodyTpl<Scalar> StateMultibody;
59  typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
60  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
61  typedef WrenchConeTpl<Scalar> WrenchCone;
62  typedef typename MathBase::VectorXs VectorXs;
63  typedef typename MathBase::MatrixXs MatrixXs;
64  typedef typename MathBase::MatrixX6s MatrixX6s;
65 
74  ResidualModelContactWrenchConeTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
75  const WrenchCone& fref, const std::size_t nu);
76 
86  ResidualModelContactWrenchConeTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
87  const WrenchCone& fref);
89 
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 
126  virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
127  const Eigen::Ref<const VectorXs>& u);
128 
138  virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
139 
146  virtual boost::shared_ptr<ResidualDataAbstract> createData(DataCollectorAbstract* const data);
147 
151  pinocchio::FrameIndex get_id() const;
152 
156  const WrenchCone& get_reference() const;
157 
161  void set_id(const pinocchio::FrameIndex id);
162 
166  void set_reference(const WrenchCone& reference);
167 
173  virtual void print(std::ostream& os) const;
174 
175  protected:
176  using Base::nu_;
177  using Base::state_;
178  using Base::unone_;
179 
180  private:
181  pinocchio::FrameIndex id_;
182  WrenchCone fref_;
183 };
184 
185 template <typename _Scalar>
186 struct ResidualDataContactWrenchConeTpl : public ResidualDataAbstractTpl<_Scalar> {
187  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
188 
189  typedef _Scalar Scalar;
190  typedef MathBaseTpl<Scalar> MathBase;
191  typedef ResidualDataAbstractTpl<Scalar> Base;
192  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
193  typedef ContactModelMultipleTpl<Scalar> ContactModelMultiple;
194  typedef ImpulseModelMultipleTpl<Scalar> ImpulseModelMultiple;
195  typedef StateMultibodyTpl<Scalar> StateMultibody;
196  typedef typename MathBase::MatrixXs MatrixXs;
197 
198  template <template <typename Scalar> class Model>
199  ResidualDataContactWrenchConeTpl(Model<Scalar>* const model, DataCollectorAbstract* const data) : Base(model, data) {
200  // Check that proper shared data has been passed
201  bool is_contact = true;
202  DataCollectorContactTpl<Scalar>* d1 = dynamic_cast<DataCollectorContactTpl<Scalar>*>(shared);
203  DataCollectorImpulseTpl<Scalar>* d2 = dynamic_cast<DataCollectorImpulseTpl<Scalar>*>(shared);
204  if (d1 == NULL && d2 == NULL) {
205  throw_pretty(
206  "Invalid argument: the shared data should be derived from DataCollectorContact or DataCollectorImpulse");
207  }
208  if (d2 != NULL) {
209  is_contact = false;
210  }
211 
212  // Avoids data casting at runtime
213  const pinocchio::FrameIndex id = model->get_id();
214  const boost::shared_ptr<StateMultibody>& state = boost::static_pointer_cast<StateMultibody>(model->get_state());
215  std::string frame_name = state->get_pinocchio()->frames[id].name;
216  bool found_contact = false;
217  if (is_contact) {
218  for (typename ContactModelMultiple::ContactDataContainer::iterator it = d1->contacts->contacts.begin();
219  it != d1->contacts->contacts.end(); ++it) {
220  if (it->second->frame == id) {
221  ContactData3DTpl<Scalar>* d3d = dynamic_cast<ContactData3DTpl<Scalar>*>(it->second.get());
222  if (d3d != NULL) {
223  found_contact = true;
224  contact = it->second;
225  throw_pretty("Domain error: there isn't defined at least a 6d contact for " + frame_name);
226  break;
227  }
228  ContactData6DTpl<Scalar>* d6d = dynamic_cast<ContactData6DTpl<Scalar>*>(it->second.get());
229  if (d6d != NULL) {
230  found_contact = true;
231  contact = it->second;
232  break;
233  }
234  throw_pretty("Domain error: there isn't defined at least a 6d contact for " + frame_name);
235  break;
236  }
237  }
238  } else {
239  for (typename ImpulseModelMultiple::ImpulseDataContainer::iterator it = d2->impulses->impulses.begin();
240  it != d2->impulses->impulses.end(); ++it) {
241  if (it->second->frame == id) {
242  ImpulseData3DTpl<Scalar>* d3d = dynamic_cast<ImpulseData3DTpl<Scalar>*>(it->second.get());
243  if (d3d != NULL) {
244  found_contact = true;
245  contact = it->second;
246  throw_pretty("Domain error: there isn't defined at least a 6d contact for " + frame_name);
247  break;
248  }
249  ImpulseData6DTpl<Scalar>* d6d = dynamic_cast<ImpulseData6DTpl<Scalar>*>(it->second.get());
250  if (d6d != NULL) {
251  found_contact = true;
252  contact = it->second;
253  break;
254  }
255  throw_pretty("Domain error: there isn't defined at least a 6d contact for " + frame_name);
256  break;
257  }
258  }
259  }
260  if (!found_contact) {
261  throw_pretty("Domain error: there isn't defined contact data for " + frame_name);
262  }
263  }
264 
265  boost::shared_ptr<ForceDataAbstractTpl<Scalar> > contact;
266  using Base::r;
267  using Base::Ru;
268  using Base::Rx;
269  using Base::shared;
270 };
271 
272 } // namespace crocoddyl
273 
274 /* --- Details -------------------------------------------------------------- */
275 /* --- Details -------------------------------------------------------------- */
276 /* --- Details -------------------------------------------------------------- */
277 #include "crocoddyl/multibody/residuals/contact-wrench-cone.hxx"
278 
279 #endif // CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_WRENCH_CONE_HPP_
virtual void print(std::ostream &os) const
Print relevant information of the contact-wrench-cone residual.
pinocchio::FrameIndex get_id() const
Return the reference frame id.
void set_id(const pinocchio::FrameIndex id)
Modify the reference frame id.
std::size_t nu_
Control dimension.
virtual void calc(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the contact wrench cone residual.
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 wrench cone residual.
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.
ResidualModelContactWrenchConeTpl(boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const WrenchCone &fref, const std::size_t nu)
Initialize the contact wrench cone residual model.
const WrenchCone & get_reference() const
Return the reference contact wrench cone.
void set_reference(const WrenchCone &reference)
Modify the reference contact wrench cone.
boost::shared_ptr< ForceDataAbstractTpl< Scalar > > contact
Contact force data.
VectorXs r
Residual vector.
DataCollectorAbstract * shared
Shared data allocated by the action model.
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Create the contact wrench cone residual data.