crocoddyl  1.8.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
control-gravity.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-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_CONTROL_GRAVITY_HPP_
10 #define CROCODDYL_MULTIBODY_RESIDUALS_CONTROL_GRAVITY_HPP_
11 
12 #include "crocoddyl/core/residual-base.hpp"
13 #include "crocoddyl/multibody/states/multibody.hpp"
14 #include "crocoddyl/multibody/data/multibody.hpp"
15 #include "crocoddyl/core/utils/exception.hpp"
16 
17 namespace crocoddyl {
18 
32 template <typename _Scalar>
33 class ResidualModelControlGravTpl : 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 ResidualDataControlGravTpl<Scalar> Data;
41  typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
42  typedef StateMultibodyTpl<Scalar> StateMultibody;
43  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
44  typedef typename MathBase::VectorXs VectorXs;
45  typedef typename MathBase::MatrixXs MatrixXs;
46 
53  ResidualModelControlGravTpl(boost::shared_ptr<StateMultibody> state, const std::size_t nu);
54 
62  ResidualModelControlGravTpl(boost::shared_ptr<StateMultibody> state);
63  virtual ~ResidualModelControlGravTpl();
64 
72  virtual void calc(const boost::shared_ptr<ResidualDataAbstract> &data, const Eigen::Ref<const VectorXs> &x,
73  const Eigen::Ref<const VectorXs> &u);
74 
82  virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract> &data, const Eigen::Ref<const VectorXs> &x,
83  const Eigen::Ref<const VectorXs> &u);
84 
85  virtual boost::shared_ptr<ResidualDataAbstract> createData(DataCollectorAbstract *const data);
86 
92  virtual void print(std::ostream &os) const;
93 
94  protected:
95  using Base::nu_;
96  using Base::state_;
97  using Base::unone_;
98  using Base::v_dependent_;
99 
100  private:
101  typename StateMultibody::PinocchioModel pin_model_;
102 };
103 
104 template <typename _Scalar>
105 struct ResidualDataControlGravTpl : public ResidualDataAbstractTpl<_Scalar> {
106  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
107 
108  typedef _Scalar Scalar;
109  typedef MathBaseTpl<Scalar> MathBase;
110  typedef ResidualDataAbstractTpl<Scalar> Base;
111  typedef StateMultibodyTpl<Scalar> StateMultibody;
112  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
113  typedef pinocchio::DataTpl<Scalar> PinocchioData;
114 
115  template <template <typename Scalar> class Model>
116  ResidualDataControlGravTpl(Model<Scalar> *const model, DataCollectorAbstract *const data) : Base(model, data) {
117  // Check that proper shared data has been passed
118  DataCollectorActMultibodyTpl<Scalar> *d = dynamic_cast<DataCollectorActMultibodyTpl<Scalar> *>(shared);
119  if (d == NULL) {
120  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorActMultibodyTpl");
121  }
122  // Avoids data casting at runtime
123  StateMultibody *sm = static_cast<StateMultibody *>(model->get_state().get());
124  pinocchio = PinocchioData(*(sm->get_pinocchio().get()));
125  actuation = d->actuation;
126  }
127 
128  PinocchioData pinocchio;
129  boost::shared_ptr<ActuationDataAbstractTpl<Scalar> > actuation;
130  using Base::r;
131  using Base::Ru;
132  using Base::Rx;
133  using Base::shared;
134 };
135 
136 } // namespace crocoddyl
137 
138 /* --- Details -------------------------------------------------------------- */
139 /* --- Details -------------------------------------------------------------- */
140 /* --- Details -------------------------------------------------------------- */
141 #include "crocoddyl/multibody/residuals/control-gravity.hxx"
142 
143 #endif // CROCODDYL_MULTIBODY_RESIDUALS_CONTROL_GRAVITY_HPP_
ResidualModelControlGravTpl(boost::shared_ptr< StateMultibody > state, const std::size_t nu)
Initialize the control gravity residual model.
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 Jacobians of the control gravity residual.
boost::shared_ptr< ActuationDataAbstractTpl< Scalar > > actuation
Actuation data.
virtual void calc(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the control gravity residual.
virtual void print(std::ostream &os) const
Print relevant information of the control-grav residual.
PinocchioData pinocchio
Pinocchio data.
boost::shared_ptr< StateAbstract > state_
State description.
VectorXs unone_
No control vector.
bool v_dependent_
Label that indicates if the residual function depends on v.
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Create the residual data.