crocoddyl  1.7.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
com-position.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_COM_POSITION_HPP_
10 #define CROCODDYL_MULTIBODY_COSTS_COM_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/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::Vector3s Vector3s;
51  typedef typename MathBase::VectorXs VectorXs;
52 
61  CostModelCoMPositionTpl(boost::shared_ptr<StateMultibody> state,
62  boost::shared_ptr<ActivationModelAbstract> activation, const Vector3s& cref,
63  const std::size_t nu);
64 
74  CostModelCoMPositionTpl(boost::shared_ptr<StateMultibody> state,
75  boost::shared_ptr<ActivationModelAbstract> activation, const Vector3s& cref);
76 
86  CostModelCoMPositionTpl(boost::shared_ptr<StateMultibody> state, const Vector3s& cref, const std::size_t nu);
87 
97  CostModelCoMPositionTpl(boost::shared_ptr<StateMultibody> state, const Vector3s& cref);
98  virtual ~CostModelCoMPositionTpl();
99 
107  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
108  const Eigen::Ref<const VectorXs>& u);
109 
117  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
118  const Eigen::Ref<const VectorXs>& u);
119  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
120 
121  DEPRECATED("Use set_reference<MathBaseTpl<Scalar>::Vector3s>()", void set_cref(const Vector3s& cref_in));
122  DEPRECATED("Use get_reference<MathBaseTpl<Scalar>::Vector3s>()", const Vector3s& get_cref() const);
123 
124  protected:
128  virtual void set_referenceImpl(const std::type_info& ti, const void* pv);
129 
133  virtual void get_referenceImpl(const std::type_info& ti, void* pv) const;
134 
135  using Base::activation_;
136  using Base::nu_;
137  using Base::state_;
138  using Base::unone_;
139 
140  private:
141  Vector3s cref_;
142 };
143 
144 template <typename _Scalar>
145 struct CostDataCoMPositionTpl : public CostDataAbstractTpl<_Scalar> {
146  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
147 
148  typedef _Scalar Scalar;
152  typedef typename MathBase::Matrix3xs Matrix3xs;
153 
154  template <template <typename Scalar> class Model>
155  CostDataCoMPositionTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
156  : Base(model, data), Arr_Jcom(3, model->get_state()->get_nv()) {
157  Arr_Jcom.setZero();
158  // Check that proper shared data has been passed
160  if (d == NULL) {
161  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorMultibody");
162  }
163 
164  // Avoids data casting at runtime
165  pinocchio = d->pinocchio;
166  }
167 
168  pinocchio::DataTpl<Scalar>* pinocchio;
169  Matrix3xs Arr_Jcom;
170 
171  using Base::activation;
172  using Base::cost;
173  using Base::Lu;
174  using Base::Luu;
175  using Base::Lx;
176  using Base::Lxu;
177  using Base::Lxx;
178  using Base::r;
179  using Base::Ru;
180  using Base::Rx;
181  using Base::shared;
182 };
183 
184 } // namespace crocoddyl
185 
186 /* --- Details -------------------------------------------------------------- */
187 /* --- Details -------------------------------------------------------------- */
188 /* --- Details -------------------------------------------------------------- */
189 #include "crocoddyl/multibody/costs/com-position.hxx"
190 
191 #endif // CROCODDYL_MULTIBODY_COSTS_COM_POSITION_HPP_
Abstract class for cost models.
Definition: cost-base.hpp:47
boost::shared_ptr< ActivationModelAbstract > activation_
Activation model.
Definition: cost-base.hpp:194
CostModelCoMPositionTpl(boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ActivationModelAbstract > activation, const Vector3s &cref, const std::size_t nu)
Initialize the CoM position cost model.
virtual boost::shared_ptr< CostDataAbstract > createData(DataCollectorAbstract *const data)
Create the cost data.
virtual void get_referenceImpl(const std::type_info &ti, void *pv) const
Return the CoM position reference.
State multibody representation.
Definition: fwd.hpp:215
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 CoM position cost.
VectorXs unone_
No control vector.
Definition: cost-base.hpp:196
virtual void set_referenceImpl(const std::type_info &ti, const void *pv)
Modify the CoM position reference.
virtual void calc(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the CoM position cost.
std::size_t nu_
Control dimension.
Definition: cost-base.hpp:195
boost::shared_ptr< StateAbstract > state_
State description.
Definition: cost-base.hpp:193