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)
120 const boost::shared_ptr<StateMultibody>& state = boost::static_pointer_cast<StateMultibody>(model->get_state());
121 pinocchio_internal = pinocchio::DataTpl<Scalar>(*state->get_pinocchio().get());
123 DataCollectorMultibodyInImpulseTpl<Scalar>* d =
dynamic_cast<DataCollectorMultibodyInImpulseTpl<Scalar>*
>(shared);
125 throw_pretty(
"Invalid argument: the shared data should be derived from DataCollectorMultibodyInImpulse");
127 pinocchio = d->pinocchio;
128 impulses = d->impulses;
132 boost::shared_ptr<crocoddyl::ImpulseDataMultipleTpl<Scalar> >
impulses;
147 #include "crocoddyl/multibody/residuals/impulse-com.hxx" 149 #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.