crocoddyl  1.6.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/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/data/multibody.hpp"
21 #include "crocoddyl/core/activations/quadratic-barrier.hpp"
22 #include "crocoddyl/core/utils/exception.hpp"
23 
24 namespace crocoddyl {
25 
56 template <typename _Scalar>
58  public:
59  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
60 
61  typedef _Scalar Scalar;
72  typedef typename MathBase::Vector2s Vector2s;
73  typedef typename MathBase::Vector3s Vector3s;
74  typedef typename MathBase::VectorXs VectorXs;
75  typedef typename MathBase::MatrixXs MatrixXs;
76  typedef Eigen::Matrix<Scalar, 4, 6> Matrix46;
77 
86  CostModelContactCoPPositionTpl(boost::shared_ptr<StateMultibody> state,
87  boost::shared_ptr<ActivationModelAbstract> activation,
88  const FrameCoPSupport& cop_support, const std::size_t& nu);
89 
99  CostModelContactCoPPositionTpl(boost::shared_ptr<StateMultibody> state,
100  boost::shared_ptr<ActivationModelAbstract> activation,
101  const FrameCoPSupport& cop_support);
102 
113  CostModelContactCoPPositionTpl(boost::shared_ptr<StateMultibody> state, const FrameCoPSupport& cop_support,
114  const std::size_t& nu);
115 
126  CostModelContactCoPPositionTpl(boost::shared_ptr<StateMultibody> state, const FrameCoPSupport& cop_support);
128 
139  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
140  const Eigen::Ref<const VectorXs>& u);
141 
152  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
153  const Eigen::Ref<const VectorXs>& u);
154 
164  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
165 
166  protected:
170  virtual void set_referenceImpl(const std::type_info& ti, const void* pv);
171 
175  virtual void get_referenceImpl(const std::type_info& ti, void* pv) const;
176 
177  using Base::activation_;
178  using Base::nu_;
179  using Base::state_;
180  using Base::unone_;
181 
182  private:
183  FrameCoPSupport cop_support_;
184 };
185 
186 template <typename _Scalar>
188  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
189 
190  typedef _Scalar Scalar;
196  typedef typename MathBase::MatrixXs MatrixXs;
197 
198  template <template <typename Scalar> class Model>
199  CostDataContactCoPPositionTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
200  : Base(model, data), Arr_Ru(model->get_activation()->get_nr(), model->get_state()->get_nv()) {
201  Arr_Ru.setZero();
202 
203  // Check that proper shared data has been passed
205  if (d == NULL) {
206  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorContact");
207  }
208 
209  // Get the active 6d contact (avoids data casting at runtime)
210  FrameCoPSupport cop_support = model->template get_reference<FrameCoPSupport>();
211  const boost::shared_ptr<StateMultibody>& state = boost::static_pointer_cast<StateMultibody>(model->get_state());
212  std::string frame_name = state->get_pinocchio()->frames[cop_support.get_id()].name;
213  bool found_contact = false;
214  for (typename ContactModelMultiple::ContactDataContainer::iterator it = d->contacts->contacts.begin();
215  it != d->contacts->contacts.end(); ++it) {
216  if (it->second->frame == cop_support.get_id()) {
217  ContactData3DTpl<Scalar>* d3d = dynamic_cast<ContactData3DTpl<Scalar>*>(it->second.get());
218  if (d3d != NULL) {
219  throw_pretty("Domain error: a 6d contact model is required in " + frame_name +
220  "in order to compute the CoP");
221  break;
222  }
223  ContactData6DTpl<Scalar>* d6d = dynamic_cast<ContactData6DTpl<Scalar>*>(it->second.get());
224  if (d6d != NULL) {
225  found_contact = true;
226  contact = it->second;
227  break;
228  }
229  }
230  }
231  if (!found_contact) {
232  throw_pretty("Domain error: there isn't defined contact data for " + frame_name);
233  }
234  }
235 
236  pinocchio::DataTpl<Scalar>* pinocchio;
237  MatrixXs Arr_Ru;
238  boost::shared_ptr<ContactDataAbstractTpl<Scalar> > contact;
239  using Base::activation;
240  using Base::cost;
241  using Base::Lu;
242  using Base::Luu;
243  using Base::Lx;
244  using Base::Lxu;
245  using Base::Lxx;
246  using Base::r;
247  using Base::Ru;
248  using Base::Rx;
249  using Base::shared;
250 };
251 
252 } // namespace crocoddyl
253 
254 /* --- Details -------------------------------------------------------------- */
255 /* --- Details -------------------------------------------------------------- */
256 /* --- Details -------------------------------------------------------------- */
257 #include "crocoddyl/multibody/costs/contact-cop-position.hxx"
258 
259 #endif // CROCODDYL_MULTIBODY_COSTS_CONTACT_COP_POSITION_HPP_
virtual boost::shared_ptr< CostDataAbstract > createData(DataCollectorAbstract *const data)
Create the contact CoP cost data.
Abstract class for cost models.
Definition: cost-base.hpp:47
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 CoP cost.
boost::shared_ptr< ActivationModelAbstract > activation_
Activation model.
Definition: cost-base.hpp:194
CostModelContactCoPPositionTpl(boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ActivationModelAbstract > activation, const FrameCoPSupport &cop_support, const std::size_t &nu)
Initialize the contact 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 contact CoP cost.
VectorXs unone_
No control vector.
Definition: cost-base.hpp:196
Define a center of pressure cost function.
virtual void get_referenceImpl(const std::type_info &ti, void *pv) const
Modify the frame CoP support.
virtual void set_referenceImpl(const std::type_info &ti, const void *pv)
Return the frame CoP support.
std::size_t nu_
Control dimension.
Definition: cost-base.hpp:195
boost::shared_ptr< StateAbstract > state_
State description.
Definition: cost-base.hpp:193