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);
82 virtual void calcDiff(
const boost::shared_ptr<ResidualDataAbstract> &data,
const Eigen::Ref<const VectorXs> &x,
83 const Eigen::Ref<const VectorXs> &u);
85 virtual boost::shared_ptr<ResidualDataAbstract> createData(DataCollectorAbstract *
const data);
92 virtual void print(std::ostream &os)
const;
101 typename StateMultibody::PinocchioModel pin_model_;
104 template <
typename _Scalar>
105 struct ResidualDataControlGravTpl :
public ResidualDataAbstractTpl<_Scalar> {
106 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
108 typedef _Scalar Scalar;
109 typedef MathBaseTpl<Scalar> MathBase;
110 typedef ResidualDataAbstractTpl<Scalar> Base;
111 typedef StateMultibodyTpl<Scalar> StateMultibody;
112 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
113 typedef pinocchio::DataTpl<Scalar> PinocchioData;
115 template <
template <
typename Scalar>
class Model>
116 ResidualDataControlGravTpl(Model<Scalar> *
const model, DataCollectorAbstract *
const data) : Base(model, data) {
118 DataCollectorActMultibodyTpl<Scalar> *d =
dynamic_cast<DataCollectorActMultibodyTpl<Scalar> *
>(
shared);
120 throw_pretty(
"Invalid argument: the shared data should be derived from DataCollectorActMultibodyTpl");
123 StateMultibody *sm =
static_cast<StateMultibody *
>(model->get_state().get());
124 pinocchio = PinocchioData(*(sm->get_pinocchio().get()));
129 boost::shared_ptr<ActuationDataAbstractTpl<Scalar> >
actuation;
141 #include "crocoddyl/multibody/residuals/control-gravity.hxx"
143 #endif // CROCODDYL_MULTIBODY_RESIDUALS_CONTROL_GRAVITY_HPP_