Crocoddyl
contact-cop-position.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-2024, University of Duisburg-Essen,
5 // University of Edinburgh, Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
9 
10 #ifndef CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_COP_POSITION_HPP_
11 #define CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_COP_POSITION_HPP_
12 
13 #include "crocoddyl/core/residual-base.hpp"
14 #include "crocoddyl/core/utils/exception.hpp"
15 #include "crocoddyl/multibody/contact-base.hpp"
16 #include "crocoddyl/multibody/contacts/contact-3d.hpp"
17 #include "crocoddyl/multibody/contacts/contact-6d.hpp"
18 #include "crocoddyl/multibody/contacts/multiple-contacts.hpp"
19 #include "crocoddyl/multibody/cop-support.hpp"
20 #include "crocoddyl/multibody/data/contacts.hpp"
21 #include "crocoddyl/multibody/data/impulses.hpp"
22 #include "crocoddyl/multibody/fwd.hpp"
23 #include "crocoddyl/multibody/impulse-base.hpp"
24 #include "crocoddyl/multibody/impulses/impulse-3d.hpp"
25 #include "crocoddyl/multibody/impulses/impulse-6d.hpp"
26 #include "crocoddyl/multibody/impulses/multiple-impulses.hpp"
27 #include "crocoddyl/multibody/states/multibody.hpp"
28 
29 namespace crocoddyl {
30 
66 template <typename _Scalar>
68  : public ResidualModelAbstractTpl<_Scalar> {
69  public:
70  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
71 
72  typedef _Scalar Scalar;
80  typedef typename MathBase::VectorXs VectorXs;
81  typedef typename MathBase::MatrixXs MatrixXs;
82  typedef typename MathBase::Matrix46s Matrix46;
83 
97  ResidualModelContactCoPPositionTpl(boost::shared_ptr<StateMultibody> state,
98  const pinocchio::FrameIndex id,
99  const CoPSupport& cref,
100  const std::size_t nu,
101  const bool fwddyn = true);
102 
113  ResidualModelContactCoPPositionTpl(boost::shared_ptr<StateMultibody> state,
114  const pinocchio::FrameIndex id,
115  const CoPSupport& cref);
117 
130  virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data,
131  const Eigen::Ref<const VectorXs>& x,
132  const Eigen::Ref<const VectorXs>& u);
133 
144  virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data,
145  const Eigen::Ref<const VectorXs>& x);
146 
159  virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data,
160  const Eigen::Ref<const VectorXs>& x,
161  const Eigen::Ref<const VectorXs>& u);
162 
174  virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data,
175  const Eigen::Ref<const VectorXs>& x);
176 
187  virtual boost::shared_ptr<ResidualDataAbstract> createData(
188  DataCollectorAbstract* const data);
189 
195  void updateJacobians(const boost::shared_ptr<ResidualDataAbstract>& data);
196 
201  bool is_fwddyn() const;
202 
206  pinocchio::FrameIndex get_id() const;
207 
211  const CoPSupport& get_reference() const;
212 
216  DEPRECATED("Do not use set_id, instead create a new model",
217  void set_id(pinocchio::FrameIndex id);)
218 
222  void set_reference(const CoPSupport& reference);
223 
229  virtual void print(std::ostream& os) const;
230 
231  protected:
232  using Base::nu_;
233  using Base::state_;
234 
235  private:
236  bool fwddyn_;
238  bool update_jacobians_;
240  pinocchio::FrameIndex id_;
241  CoPSupport cref_;
242 };
243 
244 template <typename _Scalar>
246  : public ResidualDataAbstractTpl<_Scalar> {
247  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
248 
249  typedef _Scalar Scalar;
254  typedef typename MathBase::MatrixXs MatrixXs;
255 
256  template <template <typename Scalar> class Model>
257  ResidualDataContactCoPPositionTpl(Model<Scalar>* const model,
258  DataCollectorAbstract* const data)
259  : Base(model, data) {
260  // Check that proper shared data has been passed
261  bool is_contact = true;
263  dynamic_cast<DataCollectorContactTpl<Scalar>*>(shared);
265  dynamic_cast<DataCollectorImpulseTpl<Scalar>*>(shared);
266  if (d1 == NULL && d2 == NULL) {
267  throw_pretty(
268  "Invalid argument: the shared data should be derived from "
269  "DataCollectorContact or DataCollectorImpulse");
270  }
271  if (d2 != NULL) {
272  is_contact = false;
273  }
274 
275  // Avoids data casting at runtime
276  const pinocchio::FrameIndex id = model->get_id();
277  const boost::shared_ptr<StateMultibody>& state =
278  boost::static_pointer_cast<StateMultibody>(model->get_state());
279  std::string frame_name = state->get_pinocchio()->frames[id].name;
280  bool found_contact = false;
281  if (is_contact) {
282  for (typename ContactModelMultiple::ContactDataContainer::iterator it =
283  d1->contacts->contacts.begin();
284  it != d1->contacts->contacts.end(); ++it) {
285  if (it->second->frame == id) {
287  dynamic_cast<ContactData3DTpl<Scalar>*>(it->second.get());
288  if (d3d != NULL) {
289  found_contact = true;
290  contact = it->second;
291  throw_pretty(
292  "Domain error: there isn't defined at least a 6d contact for " +
293  frame_name);
294  break;
295  }
297  dynamic_cast<ContactData6DTpl<Scalar>*>(it->second.get());
298  if (d6d != NULL) {
299  found_contact = true;
300  contact = it->second;
301  break;
302  }
303  throw_pretty(
304  "Domain error: there isn't defined at least a 6d contact for " +
305  frame_name);
306  break;
307  }
308  }
309  } else {
310  for (typename ImpulseModelMultiple::ImpulseDataContainer::iterator it =
311  d2->impulses->impulses.begin();
312  it != d2->impulses->impulses.end(); ++it) {
313  if (it->second->frame == id) {
315  dynamic_cast<ImpulseData3DTpl<Scalar>*>(it->second.get());
316  if (d3d != NULL) {
317  found_contact = true;
318  contact = it->second;
319  throw_pretty(
320  "Domain error: there isn't defined at least a 6d contact for " +
321  frame_name);
322  break;
323  }
325  dynamic_cast<ImpulseData6DTpl<Scalar>*>(it->second.get());
326  if (d6d != NULL) {
327  found_contact = true;
328  contact = it->second;
329  break;
330  }
331  throw_pretty(
332  "Domain error: there isn't defined at least a 6d contact for " +
333  frame_name);
334  break;
335  }
336  }
337  }
338  if (!found_contact) {
339  throw_pretty("Domain error: there isn't defined contact data for " +
340  frame_name);
341  }
342  }
343 
344  pinocchio::DataTpl<Scalar>* pinocchio;
345  boost::shared_ptr<ForceDataAbstractTpl<Scalar> > contact;
346  using Base::r;
347  using Base::Ru;
348  using Base::Rx;
349  using Base::shared;
350 };
351 
352 } // namespace crocoddyl
353 
354 /* --- Details -------------------------------------------------------------- */
355 /* --- Details -------------------------------------------------------------- */
356 /* --- Details -------------------------------------------------------------- */
357 #include "crocoddyl/multibody/residuals/contact-cop-position.hxx"
358 
359 #endif // CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_COP_POSITION_HPP_
Abstract class for residual models.
boost::shared_ptr< StateAbstract > state_
State description.
std::size_t nu_
Control dimension.
Center of pressure residual function.
DEPRECATED("Do not use set_id, instead create a new model", void set_id(pinocchio::FrameIndex id);) void set_reference(const CoPSupport &reference)
Modify the reference frame id.
ResidualModelContactCoPPositionTpl(boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const CoPSupport &cref, const std::size_t nu, const bool fwddyn=true)
Initialize the contact CoP residual model.
virtual void calc(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the residual vector for nodes that depends only on the state.
bool is_fwddyn() const
Indicates if we are using the forward-dynamics (true) or inverse-dynamics (false)
virtual void calcDiff(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the Jacobian of the residual functions with respect to the state only.
virtual void print(std::ostream &os) const
Print relevant information of the cop-position residual.
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.
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Create the contact CoP residual data.
ResidualModelContactCoPPositionTpl(boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const CoPSupport &cref)
Initialize the contact CoP residual model.
const CoPSupport & get_reference() const
Return the reference support region of CoP.
void updateJacobians(const boost::shared_ptr< ResidualDataAbstract > &data)
Update the Jacobians of the contact friction cone residual.
State multibody representation.
Definition: multibody.hpp:35
MatrixXs Ru
Jacobian of the residual vector with respect the control.
MatrixXs Rx
Jacobian of the residual vector with respect the state.
DataCollectorAbstract * shared
Shared data allocated by the action model.
boost::shared_ptr< ForceDataAbstractTpl< Scalar > > contact
Contact force.
DataCollectorAbstract * shared
Shared data allocated by the action model.
pinocchio::DataTpl< Scalar > * pinocchio
Pinocchio data.