crocoddyl  1.7.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
contact-friction-cone.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-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/core/cost-base.hpp"
14 #include "crocoddyl/multibody/states/multibody.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/data/contacts.hpp"
19 #include "crocoddyl/multibody/frames.hpp"
20 #include "crocoddyl/multibody/friction-cone.hpp"
21 #include "crocoddyl/core/utils/exception.hpp"
22 #include "crocoddyl/core/utils/deprecate.hpp"
23 
24 namespace crocoddyl {
25 
48 template <typename _Scalar>
50  public:
51  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
52 
53  typedef _Scalar Scalar;
62  typedef typename MathBase::VectorXs VectorXs;
63  typedef typename MathBase::MatrixXs MatrixXs;
64  typedef typename MathBase::MatrixX3s MatrixX3s;
65 
74  CostModelContactFrictionConeTpl(boost::shared_ptr<StateMultibody> state,
75  boost::shared_ptr<ActivationModelAbstract> activation, const FrameFrictionCone& fref,
76  const std::size_t nu);
77 
87  CostModelContactFrictionConeTpl(boost::shared_ptr<StateMultibody> state,
88  boost::shared_ptr<ActivationModelAbstract> activation,
89  const FrameFrictionCone& fref);
90 
100  CostModelContactFrictionConeTpl(boost::shared_ptr<StateMultibody> state, const FrameFrictionCone& fref,
101  const std::size_t nu);
102 
112  CostModelContactFrictionConeTpl(boost::shared_ptr<StateMultibody> state, const FrameFrictionCone& fref);
114 
122  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
123  const Eigen::Ref<const VectorXs>& u);
124 
132  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
133  const Eigen::Ref<const VectorXs>& u);
134 
138  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
139 
140  protected:
144  virtual void set_referenceImpl(const std::type_info& ti, const void* pv);
145 
149  virtual void get_referenceImpl(const std::type_info& ti, void* pv) const;
150 
151  using Base::activation_;
152  using Base::nu_;
153  using Base::state_;
154  using Base::unone_;
155 
156  private:
157  FrameFrictionCone fref_;
158 };
159 
160 template <typename _Scalar>
162  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
163 
164  typedef _Scalar Scalar;
171  typedef typename MathBase::MatrixXs MatrixXs;
172 
173  template <template <typename Scalar> class Model>
174  CostDataContactFrictionConeTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
175  : Base(model, data),
176  Arr_Ru(model->get_activation()->get_nr(), model->get_state()->get_nv()),
177  more_than_3_constraints(false) {
178  Arr_Ru.setZero();
179 
180  // Check that proper shared data has been passed
182  if (d == NULL) {
183  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorContact");
184  }
185 
186  // Avoids data casting at runtime
187  FrameFrictionCone fref = model->template get_reference<FrameFrictionCone>();
188  const boost::shared_ptr<StateMultibody>& state = boost::static_pointer_cast<StateMultibody>(model->get_state());
189  std::string frame_name = state->get_pinocchio()->frames[fref.id].name;
190  bool found_contact = false;
191  for (typename ContactModelMultiple::ContactDataContainer::iterator it = d->contacts->contacts.begin();
192  it != d->contacts->contacts.end(); ++it) {
193  if (it->second->frame == fref.id) {
194  ContactData3DTpl<Scalar>* d3d = dynamic_cast<ContactData3DTpl<Scalar>*>(it->second.get());
195  if (d3d != NULL) {
196  found_contact = true;
197  contact = it->second;
198  break;
199  }
200  ContactData6DTpl<Scalar>* d6d = dynamic_cast<ContactData6DTpl<Scalar>*>(it->second.get());
201  if (d6d != NULL) {
202  more_than_3_constraints = true;
203  found_contact = true;
204  contact = it->second;
205  break;
206  }
207  throw_pretty("Domain error: there isn't defined at least a 3d contact for " + frame_name);
208  break;
209  }
210  }
211  if (!found_contact) {
212  throw_pretty("Domain error: there isn't defined contact data for " + frame_name);
213  }
214  }
215 
216  boost::shared_ptr<ContactDataAbstractTpl<Scalar> > contact;
217  MatrixXs Arr_Ru;
218  MatrixXs Arr_Rx;
219  bool more_than_3_constraints;
220  using Base::activation;
221  using Base::cost;
222  using Base::Lu;
223  using Base::Luu;
224  using Base::Lx;
225  using Base::Lxu;
226  using Base::Lxx;
227  using Base::r;
228  using Base::Ru;
229  using Base::Rx;
230  using Base::shared;
231 };
232 
233 } // namespace crocoddyl
234 
235 /* --- Details -------------------------------------------------------------- */
236 /* --- Details -------------------------------------------------------------- */
237 /* --- Details -------------------------------------------------------------- */
238 #include "crocoddyl/multibody/costs/contact-friction-cone.hxx"
239 
240 #endif // CROCODDYL_MULTIBODY_COSTS_CONTACT_FRICTION_CONE_HPP_
virtual void calcDiff(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the derivatives of the contact friction cone cost.
Abstract class for cost models.
Definition: cost-base.hpp:47
virtual void set_referenceImpl(const std::type_info &ti, const void *pv)
Modify the contact friction cone reference.
Define a stack of contact models.
virtual boost::shared_ptr< CostDataAbstract > createData(DataCollectorAbstract *const data)
Create the contact friction cone cost data.
boost::shared_ptr< ActivationModelAbstract > activation_
Activation model.
Definition: cost-base.hpp:194
State multibody representation.
Definition: fwd.hpp:215
virtual void calc(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the contact friction cone cost.
virtual void get_referenceImpl(const std::type_info &ti, void *pv) const
Return the contact friction cone reference.
CostModelContactFrictionConeTpl(boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ActivationModelAbstract > activation, const FrameFrictionCone &fref, const std::size_t nu)
Initialize the contact friction cone cost model.
VectorXs unone_
No control vector.
Definition: cost-base.hpp:196
std::size_t nu_
Control dimension.
Definition: cost-base.hpp:195
boost::shared_ptr< StateAbstract > state_
State description.
Definition: cost-base.hpp:193