crocoddyl  1.9.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 
79  virtual void calc(const boost::shared_ptr<ResidualDataAbstract> &data, const Eigen::Ref<const VectorXs> &x);
80 
88  virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract> &data, const Eigen::Ref<const VectorXs> &x,
89  const Eigen::Ref<const VectorXs> &u);
90 
95  virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract> &data, const Eigen::Ref<const VectorXs> &x);
96 
97  virtual boost::shared_ptr<ResidualDataAbstract> createData(DataCollectorAbstract *const data);
98 
104  virtual void print(std::ostream &os) const;
105 
106  protected:
107  using Base::nu_;
108  using Base::state_;
109  using Base::unone_;
110  using Base::v_dependent_;
111 
112  private:
113  typename StateMultibody::PinocchioModel pin_model_;
114 };
115 
116 template <typename _Scalar>
117 struct ResidualDataControlGravTpl : public ResidualDataAbstractTpl<_Scalar> {
118  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
119 
120  typedef _Scalar Scalar;
121  typedef MathBaseTpl<Scalar> MathBase;
122  typedef ResidualDataAbstractTpl<Scalar> Base;
123  typedef StateMultibodyTpl<Scalar> StateMultibody;
124  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
125  typedef pinocchio::DataTpl<Scalar> PinocchioData;
126 
127  template <template <typename Scalar> class Model>
128  ResidualDataControlGravTpl(Model<Scalar> *const model, DataCollectorAbstract *const data) : Base(model, data) {
129  // Check that proper shared data has been passed
130  DataCollectorActMultibodyTpl<Scalar> *d = dynamic_cast<DataCollectorActMultibodyTpl<Scalar> *>(shared);
131  if (d == NULL) {
132  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorActMultibodyTpl");
133  }
134  // Avoids data casting at runtime
135  StateMultibody *sm = static_cast<StateMultibody *>(model->get_state().get());
136  pinocchio = PinocchioData(*(sm->get_pinocchio().get()));
137  actuation = d->actuation;
138  }
139 
140  PinocchioData pinocchio;
141  boost::shared_ptr<ActuationDataAbstractTpl<Scalar> > actuation;
142  using Base::r;
143  using Base::Ru;
144  using Base::Rx;
145  using Base::shared;
146 };
147 
148 } // namespace crocoddyl
149 
150 /* --- Details -------------------------------------------------------------- */
151 /* --- Details -------------------------------------------------------------- */
152 /* --- Details -------------------------------------------------------------- */
153 #include "crocoddyl/multibody/residuals/control-gravity.hxx"
154 
155 #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.
MatrixXs Rx
Jacobian of the residual vector with respect the state.
MatrixXs Ru
Jacobian of the residual vector with respect the control.
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.
VectorXs r
Residual vector.
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Create the residual data.
DataCollectorAbstract * shared
Shared data allocated by the action model.