9 #ifndef CROCODDYL_MULTIBODY_RESIDUALS_CONTROL_GRAVITY_HPP_
10 #define CROCODDYL_MULTIBODY_RESIDUALS_CONTROL_GRAVITY_HPP_
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"
32 template <
typename _Scalar>
33 class ResidualModelControlGravTpl :
public ResidualModelAbstractTpl<_Scalar> {
35 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
37 typedef _Scalar Scalar;
38 typedef MathBaseTpl<Scalar> MathBase;
39 typedef ResidualModelAbstractTpl<Scalar> Base;
40 typedef ResidualDataControlGravTpl<Scalar> Data;
41 typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
42 typedef StateMultibodyTpl<Scalar> StateMultibody;
43 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
44 typedef typename MathBase::VectorXs VectorXs;
45 typedef typename MathBase::MatrixXs MatrixXs;
72 virtual void calc(
const boost::shared_ptr<ResidualDataAbstract> &data,
const Eigen::Ref<const VectorXs> &x,
73 const Eigen::Ref<const VectorXs> &u);
79 virtual void calc(
const boost::shared_ptr<ResidualDataAbstract> &data,
const Eigen::Ref<const VectorXs> &x);
88 virtual void calcDiff(
const boost::shared_ptr<ResidualDataAbstract> &data,
const Eigen::Ref<const VectorXs> &x,
89 const Eigen::Ref<const VectorXs> &u);
95 virtual void calcDiff(
const boost::shared_ptr<ResidualDataAbstract> &data,
const Eigen::Ref<const VectorXs> &x);
97 virtual boost::shared_ptr<ResidualDataAbstract> createData(DataCollectorAbstract *
const data);
104 virtual void print(std::ostream &os)
const;
113 typename StateMultibody::PinocchioModel pin_model_;
116 template <
typename _Scalar>
117 struct ResidualDataControlGravTpl :
public ResidualDataAbstractTpl<_Scalar> {
118 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
120 typedef _Scalar Scalar;
121 typedef MathBaseTpl<Scalar> MathBase;
122 typedef ResidualDataAbstractTpl<Scalar> Base;
123 typedef StateMultibodyTpl<Scalar> StateMultibody;
124 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
125 typedef pinocchio::DataTpl<Scalar> PinocchioData;
127 template <
template <
typename Scalar>
class Model>
128 ResidualDataControlGravTpl(Model<Scalar> *
const model, DataCollectorAbstract *
const data) : Base(model, data) {
130 DataCollectorActMultibodyTpl<Scalar> *d =
dynamic_cast<DataCollectorActMultibodyTpl<Scalar> *
>(
shared);
132 throw_pretty(
"Invalid argument: the shared data should be derived from DataCollectorActMultibodyTpl");
135 StateMultibody *sm =
static_cast<StateMultibody *
>(model->get_state().get());
136 pinocchio = PinocchioData(*(sm->get_pinocchio().get()));
141 boost::shared_ptr<ActuationDataAbstractTpl<Scalar> >
actuation;
153 #include "crocoddyl/multibody/residuals/control-gravity.hxx"
155 #endif // CROCODDYL_MULTIBODY_RESIDUALS_CONTROL_GRAVITY_HPP_