crocoddyl  1.6.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
centroidal-momentum.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_MOMENTUM_HPP_
10 #define CROCODDYL_MULTIBODY_COSTS_MOMENTUM_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/data/multibody.hpp"
16 #include "crocoddyl/core/utils/exception.hpp"
17 #include "crocoddyl/core/utils/deprecate.hpp"
18 
19 namespace crocoddyl {
20 
37 template <typename _Scalar>
39  public:
40  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
41 
42  typedef _Scalar Scalar;
50  typedef typename MathBase::Vector6s Vector6s;
51  typedef typename MathBase::VectorXs VectorXs;
52  typedef typename MathBase::Matrix6xs Matrix6xs;
53 
62  CostModelCentroidalMomentumTpl(boost::shared_ptr<StateMultibody> state,
63  boost::shared_ptr<ActivationModelAbstract> activation, const Vector6s& mref,
64  const std::size_t& nu);
65 
75  CostModelCentroidalMomentumTpl(boost::shared_ptr<StateMultibody> state,
76  boost::shared_ptr<ActivationModelAbstract> activation, const Vector6s& mref);
77 
87  CostModelCentroidalMomentumTpl(boost::shared_ptr<StateMultibody> state, const Vector6s& mref, const std::size_t& nu);
88 
98  CostModelCentroidalMomentumTpl(boost::shared_ptr<StateMultibody> state, const Vector6s& mref);
100 
108  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
109  const Eigen::Ref<const VectorXs>& u);
110 
118  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
119  const Eigen::Ref<const VectorXs>& u);
120 
124  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
125 
126  DEPRECATED("Use set_reference<MathBaseTpl<Scalar>::Vector6s>()", void set_href(const Vector6s& mref_in));
127  DEPRECATED("Use get_reference<MathBaseTpl<Scalar>::Vector6s>()", const Vector6s& get_href() const);
128 
129  protected:
133  virtual void set_referenceImpl(const std::type_info& ti, const void* pv);
134 
138  virtual void get_referenceImpl(const std::type_info& ti, void* pv) const;
139 
140  using Base::activation_;
141  using Base::nu_;
142  using Base::state_;
143  using Base::unone_;
144 
145  private:
146  Vector6s href_;
147  boost::shared_ptr<typename StateMultibody::PinocchioModel> pin_model_;
148 };
149 
150 template <typename _Scalar>
152  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
153 
154  typedef _Scalar Scalar;
158  typedef typename MathBase::Matrix6xs Matrix6xs;
159 
160  template <template <typename Scalar> class Model>
161  CostDataCentroidalMomentumTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
162  : Base(model, data), dhd_dq(6, model->get_state()->get_nv()), dhd_dv(6, model->get_state()->get_nv()) {
163  dhd_dq.setZero();
164  dhd_dv.setZero();
165 
166  // Check that proper shared data has been passed
168  if (d == NULL) {
169  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorMultibody");
170  }
171 
172  // Avoids data casting at runtime
173  pinocchio = d->pinocchio;
174  }
175 
176  pinocchio::DataTpl<Scalar>* pinocchio;
177  Matrix6xs dhd_dq;
178  Matrix6xs dhd_dv;
179  Matrix6xs Arr_Rx;
180  using Base::activation;
181  using Base::cost;
182  using Base::Lu;
183  using Base::Luu;
184  using Base::Lx;
185  using Base::Lxu;
186  using Base::Lxx;
187  using Base::r;
188  using Base::Ru;
189  using Base::Rx;
190  using Base::shared;
191 };
192 
193 } // namespace crocoddyl
194 
195 /* --- Details -------------------------------------------------------------- */
196 /* --- Details -------------------------------------------------------------- */
197 /* --- Details -------------------------------------------------------------- */
198 #include "crocoddyl/multibody/costs/centroidal-momentum.hxx"
199 
200 #endif // CROCODDYL_MULTIBODY_COSTS_MOMENTUM_HPP_
Abstract class for cost models.
Definition: cost-base.hpp:47
CostModelCentroidalMomentumTpl(boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ActivationModelAbstract > activation, const Vector6s &mref, const std::size_t &nu)
Initialize the centroidal momentum cost model.
virtual void get_referenceImpl(const std::type_info &ti, void *pv) const
Return the centroidal momentum reference.
boost::shared_ptr< ActivationModelAbstract > activation_
Activation model.
Definition: cost-base.hpp:194
virtual boost::shared_ptr< CostDataAbstract > createData(DataCollectorAbstract *const data)
Create the centroidal momentum cost data.
virtual void calc(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the centroidal momentum cost.
VectorXs unone_
No control vector.
Definition: cost-base.hpp:196
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 centroidal momentum cost.
virtual void set_referenceImpl(const std::type_info &ti, const void *pv)
Modify the centroidal momentum reference.
std::size_t nu_
Control dimension.
Definition: cost-base.hpp:195
boost::shared_ptr< StateAbstract > state_
State description.
Definition: cost-base.hpp:193