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