crocoddyl  1.8.1
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
centroidal-momentum.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_MULTIBODY_RESIDUALS_MOMENTUM_HPP_
10 #define CROCODDYL_MULTIBODY_RESIDUALS_MOMENTUM_HPP_
11 
12 #include "crocoddyl/multibody/fwd.hpp"
13 #include "crocoddyl/core/residual-base.hpp"
14 #include "crocoddyl/multibody/states/multibody.hpp"
15 #include "crocoddyl/multibody/data/multibody.hpp"
16 
17 namespace crocoddyl {
18 
32 template <typename _Scalar>
33 class ResidualModelCentroidalMomentumTpl : public ResidualModelAbstractTpl<_Scalar> {
34  public:
35  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
36 
37  typedef _Scalar Scalar;
38  typedef MathBaseTpl<Scalar> MathBase;
39  typedef ResidualModelAbstractTpl<Scalar> Base;
40  typedef ResidualDataCentroidalMomentumTpl<Scalar> Data;
41  typedef StateMultibodyTpl<Scalar> StateMultibody;
42  typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
43  typedef ActivationModelAbstractTpl<Scalar> ActivationModelAbstract;
44  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
45  typedef typename MathBase::Vector6s Vector6s;
46  typedef typename MathBase::VectorXs VectorXs;
47  typedef typename MathBase::Matrix6xs Matrix6xs;
48 
56  ResidualModelCentroidalMomentumTpl(boost::shared_ptr<StateMultibody> state, const Vector6s& href,
57  const std::size_t nu);
58 
67  ResidualModelCentroidalMomentumTpl(boost::shared_ptr<StateMultibody> state, const Vector6s& href);
69 
77  virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
78  const Eigen::Ref<const VectorXs>& u);
79 
87  virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
88  const Eigen::Ref<const VectorXs>& u);
89 
93  virtual boost::shared_ptr<ResidualDataAbstract> createData(DataCollectorAbstract* const data);
94 
98  const Vector6s& get_reference() const;
99 
103  void set_reference(const Vector6s& href);
104 
110  virtual void print(std::ostream& os) const;
111 
112  protected:
113  using Base::nu_;
114  using Base::state_;
115  using Base::u_dependent_;
116  using Base::unone_;
117 
118  private:
119  Vector6s href_;
120  boost::shared_ptr<typename StateMultibody::PinocchioModel> pin_model_;
121 };
122 
123 template <typename _Scalar>
124 struct ResidualDataCentroidalMomentumTpl : public ResidualDataAbstractTpl<_Scalar> {
125  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
126 
127  typedef _Scalar Scalar;
128  typedef MathBaseTpl<Scalar> MathBase;
129  typedef ResidualDataAbstractTpl<Scalar> Base;
130  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
131  typedef typename MathBase::Matrix6xs Matrix6xs;
132 
133  template <template <typename Scalar> class Model>
134  ResidualDataCentroidalMomentumTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
135  : Base(model, data), dhd_dq(6, model->get_state()->get_nv()), dhd_dv(6, model->get_state()->get_nv()) {
136  dhd_dq.setZero();
137  dhd_dv.setZero();
138 
139  // Check that proper shared data has been passed
140  DataCollectorMultibodyTpl<Scalar>* d = dynamic_cast<DataCollectorMultibodyTpl<Scalar>*>(shared);
141  if (d == NULL) {
142  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorMultibody");
143  }
144 
145  // Avoids data casting at runtime
146  pinocchio = d->pinocchio;
147  }
148 
149  pinocchio::DataTpl<Scalar>* pinocchio;
150  Matrix6xs dhd_dq;
151  Matrix6xs dhd_dv;
152  using Base::r;
153  using Base::Ru;
154  using Base::Rx;
155  using Base::shared;
156 };
157 
158 } // namespace crocoddyl
159 
160 /* --- Details -------------------------------------------------------------- */
161 /* --- Details -------------------------------------------------------------- */
162 /* --- Details -------------------------------------------------------------- */
163 #include "crocoddyl/multibody/residuals/centroidal-momentum.hxx"
164 
165 #endif // CROCODDYL_MULTIBODY_RESIDUALS_MOMENTUM_HPP_
pinocchio::DataTpl< Scalar > * pinocchio
Pinocchio data.
Matrix6xs dhd_dq
Jacobian of the centroidal momentum.
bool u_dependent_
Label that indicates if the residual function depends on u.
const boost::shared_ptr< StateAbstract > & get_state() const
Return the state.
void set_reference(const Vector6s &href)
Modify the reference centroidal momentum.
virtual void print(std::ostream &os) const
Print relevant information of the centroidal-momentum residual.
std::size_t nu_
Control dimension.
virtual void calcDiff(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the derivatives of the centroidal momentum residual.
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Create the centroidal momentum residual data.
virtual void calc(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the centroidal momentum residual.
ResidualModelCentroidalMomentumTpl(boost::shared_ptr< StateMultibody > state, const Vector6s &href, const std::size_t nu)
Initialize the centroidal momentum residual model.
boost::shared_ptr< StateAbstract > state_
State description.
VectorXs unone_
No control vector.
const Vector6s & get_reference() const
Return the reference centroidal momentum.
Matrix6xs dhd_dv
Jacobian of the centroidal momentum.