crocoddyl  1.4.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
contact-friction-cone.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_MULTIBODY_COSTS_CONTACT_FRICTION_CONE_HPP_
10 #define CROCODDYL_MULTIBODY_COSTS_CONTACT_FRICTION_CONE_HPP_
11 
12 #include "crocoddyl/multibody/fwd.hpp"
13 #include "crocoddyl/multibody/cost-base.hpp"
14 #include "crocoddyl/multibody/contact-base.hpp"
15 #include "crocoddyl/multibody/contacts/contact-3d.hpp"
16 #include "crocoddyl/multibody/contacts/contact-6d.hpp"
17 #include "crocoddyl/multibody/data/contacts.hpp"
18 #include "crocoddyl/multibody/frames.hpp"
19 #include "crocoddyl/multibody/friction-cone.hpp"
20 #include "crocoddyl/core/utils/exception.hpp"
21 #include "crocoddyl/core/utils/deprecate.hpp"
22 
23 namespace crocoddyl {
24 
25 template <typename _Scalar>
27  public:
28  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
29 
30  typedef _Scalar Scalar;
42  typedef typename MathBase::Vector6s Vector6s;
43  typedef typename MathBase::VectorXs VectorXs;
44  typedef typename MathBase::MatrixXs MatrixXs;
45  typedef typename MathBase::MatrixX3s MatrixX3s;
46 
47  CostModelContactFrictionConeTpl(boost::shared_ptr<StateMultibody> state,
48  boost::shared_ptr<ActivationModelAbstract> activation, const FrameFrictionCone& fref,
49  const std::size_t& nu);
50  CostModelContactFrictionConeTpl(boost::shared_ptr<StateMultibody> state,
51  boost::shared_ptr<ActivationModelAbstract> activation,
52  const FrameFrictionCone& fref);
53  CostModelContactFrictionConeTpl(boost::shared_ptr<StateMultibody> state, const FrameFrictionCone& fref,
54  const std::size_t& nu);
55  CostModelContactFrictionConeTpl(boost::shared_ptr<StateMultibody> state, const FrameFrictionCone& fref);
57 
58  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
59  const Eigen::Ref<const VectorXs>& u);
60  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
61  const Eigen::Ref<const VectorXs>& u);
62  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
63 
64  DEPRECATED("Use set_reference<FrameFrictionConeTpl<Scalar> >()", void set_fref(const FrameFrictionCone& fref));
65  DEPRECATED("Use get_reference<FrameFrictionConeTpl<Scalar> >()", const FrameFrictionCone& get_fref() const);
66 
67  protected:
68  virtual void set_referenceImpl(const std::type_info& ti, const void* pv);
69  virtual void get_referenceImpl(const std::type_info& ti, void* pv) const;
70 
71  using Base::activation_;
72  using Base::nu_;
73  using Base::state_;
74  using Base::unone_;
75 
76  private:
77  FrameFrictionCone fref_;
78 };
79 
80 template <typename _Scalar>
82  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
83 
84  typedef _Scalar Scalar;
90  typedef typename MathBase::VectorXs VectorXs;
91  typedef typename MathBase::MatrixXs MatrixXs;
92  typedef typename MathBase::Matrix6xs Matrix6xs;
93 
94  template <template <typename Scalar> class Model>
95  CostDataContactFrictionConeTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
96  : Base(model, data),
97  Arr_Ru(model->get_activation()->get_nr(), model->get_state()->get_nv()),
98  more_than_3_constraints(false) {
99  Arr_Ru.setZero();
100 
101  // Check that proper shared data has been passed
103  if (d == NULL) {
104  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorContact");
105  }
106 
107  // Avoids data casting at runtime
108  FrameFrictionCone fref = model->template get_reference<FrameFrictionCone>();
109  std::string frame_name = model->get_state()->get_pinocchio()->frames[fref.frame].name;
110  bool found_contact = false;
111  for (typename ContactModelMultiple::ContactDataContainer::iterator it = d->contacts->contacts.begin();
112  it != d->contacts->contacts.end(); ++it) {
113  if (it->second->frame == fref.frame) {
114  ContactData3DTpl<Scalar>* d3d = dynamic_cast<ContactData3DTpl<Scalar>*>(it->second.get());
115  if (d3d != NULL) {
116  found_contact = true;
117  contact = it->second;
118  break;
119  }
120  ContactData6DTpl<Scalar>* d6d = dynamic_cast<ContactData6DTpl<Scalar>*>(it->second.get());
121  if (d6d != NULL) {
122  more_than_3_constraints = true;
123  found_contact = true;
124  contact = it->second;
125  break;
126  }
127  throw_pretty("Domain error: there isn't defined at least a 3d contact for " + frame_name);
128  break;
129  }
130  }
131  if (!found_contact) {
132  throw_pretty("Domain error: there isn't defined contact data for " + frame_name);
133  }
134  }
135 
136  boost::shared_ptr<ContactDataAbstractTpl<Scalar> > contact;
137  MatrixXs Arr_Ru;
138  bool more_than_3_constraints;
139  using Base::activation;
140  using Base::cost;
141  using Base::Lu;
142  using Base::Luu;
143  using Base::Lx;
144  using Base::Lxu;
145  using Base::Lxx;
146  using Base::r;
147  using Base::Ru;
148  using Base::Rx;
149  using Base::shared;
150 };
151 
152 } // namespace crocoddyl
153 
154 /* --- Details -------------------------------------------------------------- */
155 /* --- Details -------------------------------------------------------------- */
156 /* --- Details -------------------------------------------------------------- */
157 #include "crocoddyl/multibody/costs/contact-friction-cone.hxx"
158 
159 #endif // CROCODDYL_MULTIBODY_COSTS_CONTACT_FRICTION_CONE_HPP_
Define a stack of contact models.