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/core/utils/exception.hpp" 35 template <
typename _Scalar>
36 class ResidualModelImpulseCoMTpl :
public ResidualModelAbstractTpl<_Scalar> {
38 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
40 typedef _Scalar Scalar;
41 typedef MathBaseTpl<Scalar> MathBase;
42 typedef ResidualModelAbstractTpl<Scalar> Base;
43 typedef ResidualDataImpulseCoMTpl<Scalar> Data;
44 typedef StateMultibodyTpl<Scalar> StateMultibody;
45 typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
46 typedef ActivationModelAbstractTpl<Scalar> ActivationModelAbstract;
47 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
48 typedef typename MathBase::VectorXs VectorXs;
65 virtual void calc(
const boost::shared_ptr<ResidualDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
66 const Eigen::Ref<const VectorXs>& u);
75 virtual void calcDiff(
const boost::shared_ptr<ResidualDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
76 const Eigen::Ref<const VectorXs>& u);
81 virtual boost::shared_ptr<ResidualDataAbstract>
createData(DataCollectorAbstract*
const data);
88 virtual void print(std::ostream& os)
const;
97 boost::shared_ptr<typename StateMultibody::PinocchioModel> pin_model_;
100 template <
typename _Scalar>
101 struct ResidualDataImpulseCoMTpl :
public ResidualDataAbstractTpl<_Scalar> {
102 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
104 typedef _Scalar Scalar;
105 typedef MathBaseTpl<Scalar> MathBase;
106 typedef ResidualDataAbstractTpl<Scalar> Base;
107 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
108 typedef StateMultibodyTpl<Scalar> StateMultibody;
109 typedef typename MathBase::MatrixXs MatrixXs;
110 typedef typename MathBase::Matrix3xs Matrix3xs;
112 template <
template <
typename Scalar>
class Model>
113 ResidualDataImpulseCoMTpl(Model<Scalar>*
const model, DataCollectorAbstract*
const data)
119 const boost::shared_ptr<StateMultibody>& state = boost::static_pointer_cast<StateMultibody>(model->get_state());
120 pinocchio_internal = pinocchio::DataTpl<Scalar>(*state->get_pinocchio().get());
122 DataCollectorMultibodyInImpulseTpl<Scalar>* d =
dynamic_cast<DataCollectorMultibodyInImpulseTpl<Scalar>*
>(shared);
124 throw_pretty(
"Invalid argument: the shared data should be derived from DataCollectorMultibodyInImpulse");
126 pinocchio = d->pinocchio;
127 impulses = d->impulses;
131 boost::shared_ptr<crocoddyl::ImpulseDataMultipleTpl<Scalar> >
impulses;
146 #include "crocoddyl/multibody/residuals/impulse-com.hxx" 148 #endif // CROCODDYL_MULTIBODY_RESIDUALS_IMPULSE_COM_HPP_ virtual void calc(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the impulse CoM residual.
bool u_dependent_
Label that indicates if the residual function depends on u.
const boost::shared_ptr< StateAbstract > & get_state() const
Return the state.
virtual void calcDiff(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the Jacobians of the impulse CoM residual.
pinocchio::DataTpl< Scalar > * pinocchio
Pinocchio data.
std::size_t nu_
Control dimension.
ResidualModelImpulseCoMTpl(boost::shared_ptr< StateMultibody > state)
Initialize the impulse CoM residual model.
virtual void print(std::ostream &os) const
Print relevant information of the impulse-com residual.
pinocchio::DataTpl< Scalar > pinocchio_internal
Pinocchio data for internal computation.
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Create the impulse CoM residual data.
boost::shared_ptr< crocoddyl::ImpulseDataMultipleTpl< Scalar > > impulses
Impulses data.
MatrixXs ddv_dv
Jacobian of the CoM velocity.
boost::shared_ptr< StateAbstract > state_
State description.
VectorXs unone_
No control vector.
Matrix3xs dvc_dq
Jacobian of the CoM velocity.