9 #ifndef CROCODDYL_CORE_COSTS_CONTROL_GRAVITY_HPP_
10 #define CROCODDYL_CORE_COSTS_CONTROL_GRAVITY_HPP_
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"
35 template <
typename _Scalar>
38 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
40 typedef _Scalar Scalar;
50 typedef typename MathBase::VectorXs VectorXs;
51 typedef typename MathBase::MatrixXs MatrixXs;
61 boost::shared_ptr<ActivationModelAbstract> activation,
const std::size_t nu);
72 boost::shared_ptr<ActivationModelAbstract> activation);
105 virtual void calc(
const boost::shared_ptr<CostDataAbstract> &data,
const Eigen::Ref<const VectorXs> &x,
106 const Eigen::Ref<const VectorXs> &u);
115 virtual void calcDiff(
const boost::shared_ptr<CostDataAbstract> &data,
const Eigen::Ref<const VectorXs> &x,
116 const Eigen::Ref<const VectorXs> &u);
127 typename StateMultibody::PinocchioModel pin_model_;
130 template <
typename _Scalar>
132 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
134 typedef _Scalar Scalar;
139 typedef pinocchio::DataTpl<Scalar> PinocchioData;
140 typedef typename MathBase::MatrixXs MatrixXs;
142 template <
template <
typename Scalar>
class Model>
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()) {
150 Arr_dtaudu.setZero();
154 throw_pretty(
"Invalid argument: the shared data should be derived from DataCollectorActMultibodyTpl");
156 if (
static_cast<std::size_t
>(d->actuation->dtau_du.cols()) != model->get_state()->get_nv()) {
158 "Invalid argument: the actuation model should consider all the control dimensions (i.e., nu == state.nv)");
163 actuation = d->actuation;
166 PinocchioData pinocchio;
167 boost::shared_ptr<ActuationDataAbstractTpl<Scalar> > actuation;
171 using Base::activation;
187 #include "crocoddyl/multibody/costs/control-gravity.hxx"
189 #endif // CROCODDYL_MULTIBODY_COSTS_CONTROL_GRAVITY_HPP_