crocoddyl  1.3.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
com-position.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-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_COM_POSITION_HPP_
10 #define CROCODDYL_MULTIBODY_COSTS_COM_POSITION_HPP_
11 
12 #include "crocoddyl/multibody/fwd.hpp"
13 #include "crocoddyl/multibody/cost-base.hpp"
14 #include "crocoddyl/multibody/data/multibody.hpp"
15 #include "crocoddyl/core/utils/exception.hpp"
16 
17 namespace crocoddyl {
18 
19 template <typename _Scalar>
21  public:
22  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
23 
24  typedef _Scalar Scalar;
33  typedef typename MathBase::Vector3s Vector3s;
34  typedef typename MathBase::VectorXs VectorXs;
35  typedef typename MathBase::MatrixXs MatrixXs;
36 
37  CostModelCoMPositionTpl(boost::shared_ptr<StateMultibody> state,
38  boost::shared_ptr<ActivationModelAbstract> activation, const Vector3s& cref,
39  const std::size_t& nu);
40  CostModelCoMPositionTpl(boost::shared_ptr<StateMultibody> state,
41  boost::shared_ptr<ActivationModelAbstract> activation, const Vector3s& cref);
42  CostModelCoMPositionTpl(boost::shared_ptr<StateMultibody> state, const Vector3s& cref, const std::size_t& nu);
43  CostModelCoMPositionTpl(boost::shared_ptr<StateMultibody> state, const Vector3s& cref);
44  virtual ~CostModelCoMPositionTpl();
45 
46  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
47  const Eigen::Ref<const VectorXs>& u);
48  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
49  const Eigen::Ref<const VectorXs>& u);
50  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
51 
52  const Vector3s& get_cref() const;
53  void set_cref(const Vector3s& cref_in);
54 
55  protected:
56  virtual void set_referenceImpl(const std::type_info& ti, const void* pv);
57  virtual void get_referenceImpl(const std::type_info& ti, void* pv);
58 
59  using Base::activation_;
60  using Base::nu_;
61  using Base::state_;
62  using Base::unone_;
63 
64  private:
65  Vector3s cref_;
66 };
67 
68 template <typename _Scalar>
69 struct CostDataCoMPositionTpl : public CostDataAbstractTpl<_Scalar> {
70  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
71 
72  typedef _Scalar Scalar;
76  typedef typename MathBase::VectorXs VectorXs;
77  typedef typename MathBase::MatrixXs MatrixXs;
78  typedef typename MathBase::Matrix3xs Matrix3xs;
79  typedef typename MathBase::Matrix6xs Matrix6xs;
80  typedef typename MathBase::Matrix6s Matrix6s;
81  typedef typename MathBase::Vector6s Vector6s;
82 
83  template <template <typename Scalar> class Model>
84  CostDataCoMPositionTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
85  : Base(model, data), Arr_Jcom(3, model->get_state()->get_nv()) {
86  Arr_Jcom.setZero();
87  // Check that proper shared data has been passed
89  if (d == NULL) {
90  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorMultibody");
91  }
92 
93  // Avoids data casting at runtime
94  pinocchio = d->pinocchio;
95  }
96 
97  pinocchio::DataTpl<Scalar>* pinocchio;
98  Matrix3xs Arr_Jcom;
99 
100  using Base::activation;
101  using Base::cost;
102  using Base::Lu;
103  using Base::Luu;
104  using Base::Lx;
105  using Base::Lxu;
106  using Base::Lxx;
107  using Base::r;
108  using Base::Ru;
109  using Base::Rx;
110  using Base::shared;
111 };
112 
113 } // namespace crocoddyl
114 
115 /* --- Details -------------------------------------------------------------- */
116 /* --- Details -------------------------------------------------------------- */
117 /* --- Details -------------------------------------------------------------- */
118 #include "crocoddyl/multibody/costs/com-position.hxx"
119 
120 #endif // CROCODDYL_MULTIBODY_COSTS_COM_POSITION_HPP_