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