crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
contact-cop-position.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-2021, University of Duisburg-Essen, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_COP_POSITION_HPP_
10 #define CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_COP_POSITION_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/cop-support.hpp"
26 #include "crocoddyl/core/utils/exception.hpp"
27 
28 namespace crocoddyl {
29 
62 template <typename _Scalar>
63 class ResidualModelContactCoPPositionTpl : public ResidualModelAbstractTpl<_Scalar> {
64  public:
65  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
66 
67  typedef _Scalar Scalar;
68  typedef MathBaseTpl<Scalar> MathBase;
69  typedef ResidualModelAbstractTpl<Scalar> Base;
70  typedef ResidualDataContactCoPPositionTpl<Scalar> Data;
71  typedef StateMultibodyTpl<Scalar> StateMultibody;
72  typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
73  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
74  typedef CoPSupportTpl<Scalar> CoPSupport;
75  typedef typename MathBase::VectorXs VectorXs;
76  typedef typename MathBase::MatrixXs MatrixXs;
77  typedef typename MathBase::Matrix46s Matrix46;
78 
87  ResidualModelContactCoPPositionTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
88  const CoPSupport& cref, const std::size_t nu);
89 
99  ResidualModelContactCoPPositionTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
100  const CoPSupport& cref);
102 
114  virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
115  const Eigen::Ref<const VectorXs>& u);
116 
126  virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
127 
139  virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
140  const Eigen::Ref<const VectorXs>& u);
141 
151  virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
152 
162  virtual boost::shared_ptr<ResidualDataAbstract> createData(DataCollectorAbstract* const data);
163 
167  pinocchio::FrameIndex get_id() const;
168 
172  const CoPSupport& get_reference() const;
173 
177  void set_id(pinocchio::FrameIndex id);
178 
182  void set_reference(const CoPSupport& reference);
183 
189  virtual void print(std::ostream& os) const;
190 
191  protected:
192  using Base::nu_;
193  using Base::state_;
194  using Base::unone_;
195 
196  private:
197  pinocchio::FrameIndex id_;
198  CoPSupport cref_;
199 };
200 
201 template <typename _Scalar>
202 struct ResidualDataContactCoPPositionTpl : public ResidualDataAbstractTpl<_Scalar> {
203  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
204 
205  typedef _Scalar Scalar;
206  typedef MathBaseTpl<Scalar> MathBase;
207  typedef ResidualDataAbstractTpl<Scalar> Base;
208  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
209  typedef FrameCoPSupportTpl<Scalar> FrameCoPSupport;
210  typedef StateMultibodyTpl<Scalar> StateMultibody;
211  typedef typename MathBase::MatrixXs MatrixXs;
212 
213  template <template <typename Scalar> class Model>
214  ResidualDataContactCoPPositionTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
215  : Base(model, data) {
216  // Check that proper shared data has been passed
217  bool is_contact = true;
218  DataCollectorContactTpl<Scalar>* d1 = dynamic_cast<DataCollectorContactTpl<Scalar>*>(shared);
219  DataCollectorImpulseTpl<Scalar>* d2 = dynamic_cast<DataCollectorImpulseTpl<Scalar>*>(shared);
220  if (d1 == NULL && d2 == NULL) {
221  throw_pretty(
222  "Invalid argument: the shared data should be derived from DataCollectorContact or DataCollectorImpulse");
223  }
224  if (d2 != NULL) {
225  is_contact = false;
226  }
227 
228  // Avoids data casting at runtime
229  const pinocchio::FrameIndex id = model->get_id();
230  const boost::shared_ptr<StateMultibody>& state = boost::static_pointer_cast<StateMultibody>(model->get_state());
231  std::string frame_name = state->get_pinocchio()->frames[id].name;
232  bool found_contact = false;
233  if (is_contact) {
234  for (typename ContactModelMultiple::ContactDataContainer::iterator it = d1->contacts->contacts.begin();
235  it != d1->contacts->contacts.end(); ++it) {
236  if (it->second->frame == id) {
237  ContactData3DTpl<Scalar>* d3d = dynamic_cast<ContactData3DTpl<Scalar>*>(it->second.get());
238  if (d3d != NULL) {
239  found_contact = true;
240  contact = it->second;
241  throw_pretty("Domain error: there isn't defined at least a 6d contact for " + frame_name);
242  break;
243  }
244  ContactData6DTpl<Scalar>* d6d = dynamic_cast<ContactData6DTpl<Scalar>*>(it->second.get());
245  if (d6d != NULL) {
246  found_contact = true;
247  contact = it->second;
248  break;
249  }
250  throw_pretty("Domain error: there isn't defined at least a 6d contact for " + frame_name);
251  break;
252  }
253  }
254  } else {
255  for (typename ImpulseModelMultiple::ImpulseDataContainer::iterator it = d2->impulses->impulses.begin();
256  it != d2->impulses->impulses.end(); ++it) {
257  if (it->second->frame == id) {
258  ImpulseData3DTpl<Scalar>* d3d = dynamic_cast<ImpulseData3DTpl<Scalar>*>(it->second.get());
259  if (d3d != NULL) {
260  found_contact = true;
261  contact = it->second;
262  throw_pretty("Domain error: there isn't defined at least a 6d contact for " + frame_name);
263  break;
264  }
265  ImpulseData6DTpl<Scalar>* d6d = dynamic_cast<ImpulseData6DTpl<Scalar>*>(it->second.get());
266  if (d6d != NULL) {
267  found_contact = true;
268  contact = it->second;
269  break;
270  }
271  throw_pretty("Domain error: there isn't defined at least a 6d contact for " + frame_name);
272  break;
273  }
274  }
275  }
276  if (!found_contact) {
277  throw_pretty("Domain error: there isn't defined contact data for " + frame_name);
278  }
279  }
280 
281  pinocchio::DataTpl<Scalar>* pinocchio;
282  boost::shared_ptr<ForceDataAbstractTpl<Scalar> > contact;
283  using Base::r;
284  using Base::Ru;
285  using Base::Rx;
286  using Base::shared;
287 };
288 
289 } // namespace crocoddyl
290 
291 /* --- Details -------------------------------------------------------------- */
292 /* --- Details -------------------------------------------------------------- */
293 /* --- Details -------------------------------------------------------------- */
294 #include "crocoddyl/multibody/residuals/contact-cop-position.hxx"
295 
296 #endif // CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_COP_POSITION_HPP_
const CoPSupport & get_reference() const
Return the reference support region of CoP.
boost::shared_ptr< ForceDataAbstractTpl< Scalar > > contact
Contact force.
pinocchio::DataTpl< Scalar > * pinocchio
Pinocchio data.
std::size_t nu_
Control dimension.
ResidualModelContactCoPPositionTpl(boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const CoPSupport &cref, const std::size_t nu)
Initialize the contact CoP residual model.
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Create the contact CoP residual data.
void set_reference(const CoPSupport &reference)
Modify the reference support region of CoP.
MatrixXs Rx
Jacobian of the residual vector with respect the state.
MatrixXs Ru
Jacobian of the residual vector with respect the control.
virtual void calc(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the contact CoP residual.
pinocchio::FrameIndex get_id() const
Return the reference frame id.
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 CoP residual.
boost::shared_ptr< StateAbstract > state_
State description.
VectorXs unone_
No control vector.
void set_id(pinocchio::FrameIndex id)
Modify the reference frame id.
VectorXs r
Residual vector.
DataCollectorAbstract * shared
Shared data allocated by the action model.
virtual void print(std::ostream &os) const
Print relevant information of the cop-position residual.