crocoddyl  1.7.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_CORE_COSTS_CONTROL_GRAVITY_HPP_
10 #define CROCODDYL_CORE_COSTS_CONTROL_GRAVITY_HPP_
11 
12 #include "crocoddyl/core/cost-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 
35 template <typename _Scalar>
37  public:
38  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
39 
40  typedef _Scalar Scalar;
50  typedef typename MathBase::VectorXs VectorXs;
51  typedef typename MathBase::MatrixXs MatrixXs;
52 
60  CostModelControlGravTpl(boost::shared_ptr<StateMultibody> state,
61  boost::shared_ptr<ActivationModelAbstract> activation, const std::size_t nu);
62 
71  CostModelControlGravTpl(boost::shared_ptr<StateMultibody> state,
72  boost::shared_ptr<ActivationModelAbstract> activation);
73 
83  CostModelControlGravTpl(boost::shared_ptr<StateMultibody> state, const std::size_t nu);
84 
94  explicit CostModelControlGravTpl(boost::shared_ptr<StateMultibody> state);
95 
96  virtual ~CostModelControlGravTpl();
97 
105  virtual void calc(const boost::shared_ptr<CostDataAbstract> &data, const Eigen::Ref<const VectorXs> &x,
106  const Eigen::Ref<const VectorXs> &u);
107 
115  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract> &data, const Eigen::Ref<const VectorXs> &x,
116  const Eigen::Ref<const VectorXs> &u);
117 
118  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract *const data);
119 
120  protected:
121  using Base::activation_;
122  using Base::nu_;
123  using Base::state_;
124  using Base::unone_;
125 
126  private:
127  typename StateMultibody::PinocchioModel pin_model_;
128 };
129 
130 template <typename _Scalar>
131 struct CostDataControlGravTpl : public CostDataAbstractTpl<_Scalar> {
132  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
133 
134  typedef _Scalar Scalar;
139  typedef pinocchio::DataTpl<Scalar> PinocchioData;
140  typedef typename MathBase::MatrixXs MatrixXs;
141 
142  template <template <typename Scalar> class Model>
143  CostDataControlGravTpl(Model<Scalar> *const model, DataCollectorAbstract *const data)
144  : Base(model, data),
145  dg_dq(model->get_state()->get_nv(), model->get_state()->get_nv()),
146  Arr_dgdq(model->get_state()->get_nv(), model->get_state()->get_nv()),
147  Arr_dtaudu(model->get_state()->get_nv(), model->get_nu()) {
148  dg_dq.setZero();
149  Arr_dgdq.setZero();
150  Arr_dtaudu.setZero();
151  // Check that proper shared data has been passed
153  if (d == NULL) {
154  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorActMultibodyTpl");
155  }
156  if (static_cast<std::size_t>(d->actuation->dtau_du.cols()) != model->get_state()->get_nv()) {
157  throw_pretty(
158  "Invalid argument: the actuation model should consider all the control dimensions (i.e., nu == state.nv)");
159  }
160  // Avoids data casting at runtime
161  StateMultibody *sm = static_cast<StateMultibody *>(model->get_state().get());
162  pinocchio = PinocchioData(*(sm->get_pinocchio().get()));
163  actuation = d->actuation;
164  }
165 
166  PinocchioData pinocchio;
167  boost::shared_ptr<ActuationDataAbstractTpl<Scalar> > actuation;
168  MatrixXs dg_dq;
169  MatrixXs Arr_dgdq;
170  MatrixXs Arr_dtaudu;
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::shared;
180 };
181 
182 } // namespace crocoddyl
183 
184 /* --- Details -------------------------------------------------------------- */
185 /* --- Details -------------------------------------------------------------- */
186 /* --- Details -------------------------------------------------------------- */
187 #include "crocoddyl/multibody/costs/control-gravity.hxx"
188 
189 #endif // CROCODDYL_MULTIBODY_COSTS_CONTROL_GRAVITY_HPP_
Abstract class for cost models.
Definition: cost-base.hpp:47
virtual boost::shared_ptr< CostDataAbstract > createData(DataCollectorAbstract *const data)
Create the cost data.
boost::shared_ptr< ActivationModelAbstract > activation_
Activation model.
Definition: cost-base.hpp:194
State multibody representation.
Definition: fwd.hpp:215
CostModelControlGravTpl(boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ActivationModelAbstract > activation, const std::size_t nu)
Initialize the control gravity cost model.
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 control gravity cost.
const boost::shared_ptr< PinocchioModel > & get_pinocchio() const
Return the Pinocchio model (i.e., model of the rigid body system)
VectorXs unone_
No control vector.
Definition: cost-base.hpp:196
virtual void calc(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the control gravity cost.
std::size_t nu_
Control dimension.
Definition: cost-base.hpp:195
boost::shared_ptr< StateAbstract > state_
State description.
Definition: cost-base.hpp:193