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_ ResidualModelControlGravTpl(boost::shared_ptr< StateMultibody > state, const std::size_t nu)
Initialize the control gravity residual model.
std::size_t nu_
Control dimension.
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 control gravity residual.
boost::shared_ptr< ActuationDataAbstractTpl< Scalar > > actuation
Actuation data.
virtual void calc(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the control gravity residual.
virtual void print(std::ostream &os) const
Print relevant information of the control-grav residual.
PinocchioData pinocchio
Pinocchio data.
MatrixXs Rx
Jacobian of the residual vector with respect the state.
MatrixXs Ru
Jacobian of the residual vector with respect the control.
boost::shared_ptr< StateAbstract > state_
State description.
VectorXs unone_
No control vector.
bool v_dependent_
Label that indicates if the residual function depends on v.
VectorXs r
Residual vector.
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Create the residual data.
DataCollectorAbstract * shared
Shared data allocated by the action model.