crocoddyl  1.4.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
contact-cop-position.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020, University of Duisburg-Essen, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_MULTIBODY_COSTS_CONTACT_COP_POSITION_HPP_
10 #define CROCODDYL_MULTIBODY_COSTS_CONTACT_COP_POSITION_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/data/multibody.hpp"
20 #include "crocoddyl/core/activations/quadratic-barrier.hpp"
21 #include "crocoddyl/core/utils/exception.hpp"
22 
23 namespace crocoddyl {
24 
45 template <typename _Scalar>
47  public:
48  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
49 
50  typedef _Scalar Scalar;
61  typedef typename MathBase::Vector2s Vector2s;
62  typedef typename MathBase::Vector3s Vector3s;
63  typedef typename MathBase::VectorXs VectorXs;
64  typedef typename MathBase::MatrixXs MatrixXs;
65  typedef typename MathBase::MatrixX3s MatrixX3s;
66  typedef Eigen::Matrix<Scalar, 4, 6> Matrix46;
67 
76  CostModelContactCoPPositionTpl(boost::shared_ptr<StateMultibody> state,
77  boost::shared_ptr<ActivationModelAbstract> activation,
78  const FrameCoPSupport& cop_support, const std::size_t& nu);
79 
89  CostModelContactCoPPositionTpl(boost::shared_ptr<StateMultibody> state,
90  boost::shared_ptr<ActivationModelAbstract> activation,
91  const FrameCoPSupport& cop_support);
92 
103  CostModelContactCoPPositionTpl(boost::shared_ptr<StateMultibody> state, const FrameCoPSupport& cop_support,
104  const std::size_t& nu);
105 
115  CostModelContactCoPPositionTpl(boost::shared_ptr<StateMultibody> state, const FrameCoPSupport& cop_support);
117 
128  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
129  const Eigen::Ref<const VectorXs>& u);
130 
141  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
142  const Eigen::Ref<const VectorXs>& u);
143 
153  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
154 
155  protected:
159  virtual void set_referenceImpl(const std::type_info& ti, const void* pv);
160 
164  virtual void get_referenceImpl(const std::type_info& ti, void* pv) const;
165 
166  using Base::activation_;
167  using Base::nu_;
168  using Base::state_;
169  using Base::unone_;
170 
171  private:
172  FrameCoPSupport cop_support_;
173 };
174 
175 template <typename _Scalar>
177  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
178 
179  typedef _Scalar Scalar;
184  typedef typename MathBase::Vector3s Vector3s;
185  typedef typename MathBase::VectorXs VectorXs;
186  typedef typename MathBase::MatrixXs MatrixXs;
187  typedef typename MathBase::Matrix3s Matrix3s;
188  typedef typename MathBase::Matrix6xs Matrix6xs;
189  typedef typename MathBase::Matrix6s Matrix6s;
190  typedef typename MathBase::Vector6s Vector6s;
191 
192  template <template <typename Scalar> class Model>
193  CostDataContactCoPPositionTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
194  : Base(model, data), Arr_Ru(model->get_activation()->get_nr(), model->get_state()->get_nv()) {
195  Arr_Ru.setZero();
196 
197  // Check that proper shared data has been passed
199  if (d == NULL) {
200  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorContact");
201  }
202 
203  // Get the active 6d contact (avoids data casting at runtime)
204  FrameCoPSupport cop_support = model->template get_reference<FrameCoPSupport>();
205  std::string frame_name = model->get_state()->get_pinocchio()->frames[cop_support.get_frame()].name;
206  bool found_contact = false;
207  for (typename ContactModelMultiple::ContactDataContainer::iterator it = d->contacts->contacts.begin();
208  it != d->contacts->contacts.end(); ++it) {
209  if (it->second->frame == cop_support.get_frame()) {
210  ContactData3DTpl<Scalar>* d3d = dynamic_cast<ContactData3DTpl<Scalar>*>(it->second.get());
211  if (d3d != NULL) {
212  throw_pretty("Domain error: a 6d contact model is required in " + frame_name +
213  "in order to compute the CoP");
214  break;
215  }
216  ContactData6DTpl<Scalar>* d6d = dynamic_cast<ContactData6DTpl<Scalar>*>(it->second.get());
217  if (d6d != NULL) {
218  found_contact = true;
219  contact = it->second;
220  break;
221  }
222  }
223  }
224  if (!found_contact) {
225  throw_pretty("Domain error: there isn't defined contact data for " + frame_name);
226  }
227  }
228 
229  pinocchio::DataTpl<Scalar>* pinocchio;
230  MatrixXs Arr_Ru;
231  boost::shared_ptr<ContactDataAbstractTpl<Scalar> > contact;
232  using Base::activation;
233  using Base::cost;
234  using Base::Lu;
235  using Base::Luu;
236  using Base::Lx;
237  using Base::Lxu;
238  using Base::Lxx;
239  using Base::r;
240  using Base::Ru;
241  using Base::Rx;
242  using Base::shared;
243 };
244 
245 } // namespace crocoddyl
246 
247 /* --- Details -------------------------------------------------------------- */
248 /* --- Details -------------------------------------------------------------- */
249 /* --- Details -------------------------------------------------------------- */
250 #include "crocoddyl/multibody/costs/contact-cop-position.hxx"
251 
252 #endif // CROCODDYL_MULTIBODY_COSTS_CONTACT_COP_POSITION_HPP_
virtual boost::shared_ptr< CostDataAbstract > createData(DataCollectorAbstract *const data)
Create the cop cost data.
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 cop cost.
CostModelContactCoPPositionTpl(boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ActivationModelAbstract > activation, const FrameCoPSupport &cop_support, const std::size_t &nu)
Initialize the cop cost model.
boost::shared_ptr< ContactDataAbstractTpl< Scalar > > contact
contact force
virtual void calc(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the cop cost.
Define a center of pressure cost function.
virtual void get_referenceImpl(const std::type_info &ti, void *pv) const
Modify the cop.
virtual void set_referenceImpl(const std::type_info &ti, const void *pv)
Return the cop.