9 #ifndef CROCODDYL_MULTIBODY_RESIDUALS_IMPULSE_COM_HPP_
10 #define CROCODDYL_MULTIBODY_RESIDUALS_IMPULSE_COM_HPP_
12 #include "crocoddyl/multibody/fwd.hpp"
13 #include "crocoddyl/core/residual-base.hpp"
14 #include "crocoddyl/multibody/states/multibody.hpp"
15 #include "crocoddyl/multibody/impulse-base.hpp"
16 #include "crocoddyl/multibody/data/impulses.hpp"
17 #include "crocoddyl/multibody/frames.hpp"
18 #include "crocoddyl/core/utils/exception.hpp"
36 template <
typename _Scalar>
37 class ResidualModelImpulseCoMTpl :
public ResidualModelAbstractTpl<_Scalar> {
39 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
41 typedef _Scalar Scalar;
42 typedef MathBaseTpl<Scalar> MathBase;
43 typedef ResidualModelAbstractTpl<Scalar> Base;
44 typedef ResidualDataImpulseCoMTpl<Scalar> Data;
45 typedef StateMultibodyTpl<Scalar> StateMultibody;
46 typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
47 typedef ActivationModelAbstractTpl<Scalar> ActivationModelAbstract;
48 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
49 typedef typename MathBase::VectorXs VectorXs;
66 virtual void calc(
const boost::shared_ptr<ResidualDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
67 const Eigen::Ref<const VectorXs>& u);
76 virtual void calcDiff(
const boost::shared_ptr<ResidualDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
77 const Eigen::Ref<const VectorXs>& u);
82 virtual boost::shared_ptr<ResidualDataAbstract>
createData(DataCollectorAbstract*
const data);
89 virtual void print(std::ostream& os)
const;
98 boost::shared_ptr<typename StateMultibody::PinocchioModel> pin_model_;
101 template <
typename _Scalar>
102 struct ResidualDataImpulseCoMTpl :
public ResidualDataAbstractTpl<_Scalar> {
103 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
105 typedef _Scalar Scalar;
106 typedef MathBaseTpl<Scalar> MathBase;
107 typedef ResidualDataAbstractTpl<Scalar> Base;
108 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
109 typedef StateMultibodyTpl<Scalar> StateMultibody;
110 typedef typename MathBase::MatrixXs MatrixXs;
111 typedef typename MathBase::Matrix3xs Matrix3xs;
113 template <
template <
typename Scalar>
class Model>
114 ResidualDataImpulseCoMTpl(Model<Scalar>*
const model, DataCollectorAbstract*
const data)
116 dvc_dq(3, model->get_state()->get_nv()),
117 ddv_dv(model->get_state()->get_nv(), model->get_state()->get_nv()) {
120 const boost::shared_ptr<StateMultibody>& state = boost::static_pointer_cast<StateMultibody>(model->get_state());
123 DataCollectorMultibodyInImpulseTpl<Scalar>* d =
dynamic_cast<DataCollectorMultibodyInImpulseTpl<Scalar>*
>(
shared);
125 throw_pretty(
"Invalid argument: the shared data should be derived from DataCollectorMultibodyInImpulse");
132 boost::shared_ptr<crocoddyl::ImpulseDataMultipleTpl<Scalar> >
impulses;
147 #include "crocoddyl/multibody/residuals/impulse-com.hxx"
149 #endif // CROCODDYL_MULTIBODY_RESIDUALS_IMPULSE_COM_HPP_