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);
118 virtual boost::shared_ptr<CostDataAbstract>
createData(DataCollectorAbstract *
const data);
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)");
161 StateMultibody *sm =
static_cast<StateMultibody *
>(model->get_state().get());
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_ Abstract class for cost models.
virtual boost::shared_ptr< CostDataAbstract > createData(DataCollectorAbstract *const data)
Create the cost data.
boost::shared_ptr< ActivationModelAbstract > activation_
Activation model.
State multibody representation.
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.
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.
boost::shared_ptr< StateAbstract > state_
State description.