9 #ifndef CROCODDYL_CORE_COSTS_CONTROL_GRAVITY_CONTACT_HPP_
10 #define CROCODDYL_CORE_COSTS_CONTROL_GRAVITY_CONTACT_HPP_
12 #include "crocoddyl/core/cost-base.hpp"
13 #include "crocoddyl/multibody/states/multibody.hpp"
14 #include "crocoddyl/multibody/data/contacts.hpp"
15 #include "crocoddyl/core/utils/exception.hpp"
40 template <
typename _Scalar>
43 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
45 typedef _Scalar Scalar;
55 typedef typename MathBase::VectorXs VectorXs;
56 typedef typename MathBase::MatrixXs MatrixXs;
66 boost::shared_ptr<ActivationModelAbstract> activation,
const std::size_t nu);
77 boost::shared_ptr<ActivationModelAbstract> activation);
109 virtual void calc(
const boost::shared_ptr<CostDataAbstract> &data,
const Eigen::Ref<const VectorXs> &x,
110 const Eigen::Ref<const VectorXs> &u);
119 virtual void calcDiff(
const boost::shared_ptr<CostDataAbstract> &data,
const Eigen::Ref<const VectorXs> &x,
120 const Eigen::Ref<const VectorXs> &u);
131 typename StateMultibody::PinocchioModel pin_model_;
134 template <
typename _Scalar>
136 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
138 typedef _Scalar Scalar;
143 typedef pinocchio::DataTpl<Scalar> PinocchioData;
144 typedef typename MathBase::MatrixXs MatrixXs;
146 template <
template <
typename Scalar>
class Model>
149 dg_dq(model->get_state()->get_nv(), model->get_state()->get_nv()),
150 Arr_dgdq(model->get_state()->get_nv(), model->get_state()->get_nv()),
151 Arr_dtaudu(model->get_state()->get_nv(), model->get_nu()) {
154 Arr_dtaudu.setZero();
164 "Invalid argument: the shared data should be derived from "
165 "DataCollectorActMultibodyInContactTpl");
169 fext = d->contacts->fext;
170 actuation = d->actuation;
173 PinocchioData pinocchio;
174 pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> > fext;
175 boost::shared_ptr<ActuationDataAbstractTpl<Scalar> > actuation;
179 using Base::activation;
195 #include "crocoddyl/multibody/costs/control-gravity-contact.hxx"
197 #endif // CROCODDYL_MULTIBODY_COSTS_CONTROL_GRAVITY_CONTACT_HPP_