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>
34  public:
35  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
36 
37  typedef _Scalar Scalar;
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>
118  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
119 
120  typedef _Scalar Scalar;
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
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_
Abstract class for residual models.
boost::shared_ptr< StateAbstract > state_
State description.
std::size_t nu_
Control dimension.
VectorXs unone_
No control vector.
bool v_dependent_
Label that indicates if the residual function depends on v.
virtual void calc(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
virtual void calcDiff(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
virtual void print(std::ostream &os) const
Print relevant information of the control-grav residual.
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 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.
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Create the residual data.
ResidualModelControlGravTpl(boost::shared_ptr< StateMultibody > state, const std::size_t nu)
Initialize the control gravity residual model.
ResidualModelControlGravTpl(boost::shared_ptr< StateMultibody > state)
Initialize the control gravity residual model.
State multibody representation.
Definition: multibody.hpp:31
const boost::shared_ptr< PinocchioModel > & get_pinocchio() const
Return the Pinocchio model (i.e., model of the rigid body system)
MatrixXs Ru
Jacobian of the residual vector with respect the control.
MatrixXs Rx
Jacobian of the residual vector with respect the state.
DataCollectorAbstract * shared
Shared data allocated by the action model.
PinocchioData pinocchio
Pinocchio data.
boost::shared_ptr< ActuationDataAbstractTpl< Scalar > > actuation
Actuation data.
DataCollectorAbstract * shared
Shared data allocated by the action model.