9 #ifndef CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_CONTROL_GRAVITY_HPP_ 10 #define CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_CONTROL_GRAVITY_HPP_ 12 #include "crocoddyl/core/residual-base.hpp" 13 #include "crocoddyl/multibody/states/multibody.hpp" 14 #include "crocoddyl/multibody/data/contacts.hpp" 15 #include "crocoddyl/core/utils/exception.hpp" 34 template <
typename _Scalar>
35 class ResidualModelContactControlGravTpl :
public ResidualModelAbstractTpl<_Scalar> {
37 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
39 typedef _Scalar Scalar;
40 typedef MathBaseTpl<Scalar> MathBase;
41 typedef ResidualModelAbstractTpl<Scalar> Base;
42 typedef ResidualDataContactControlGravTpl<Scalar> Data;
43 typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
44 typedef StateMultibodyTpl<Scalar> StateMultibody;
45 typedef ActuationModelAbstractTpl<Scalar> ActuationModelAbstract;
46 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
47 typedef typename MathBase::VectorXs VectorXs;
48 typedef typename MathBase::MatrixXs MatrixXs;
75 virtual void calc(
const boost::shared_ptr<ResidualDataAbstract> &data,
const Eigen::Ref<const VectorXs> &x,
76 const Eigen::Ref<const VectorXs> &u);
85 virtual void calcDiff(
const boost::shared_ptr<ResidualDataAbstract> &data,
const Eigen::Ref<const VectorXs> &x,
86 const Eigen::Ref<const VectorXs> &u);
88 virtual boost::shared_ptr<ResidualDataAbstract>
createData(DataCollectorAbstract *
const data);
95 virtual void print(std::ostream &os)
const;
104 typename StateMultibody::PinocchioModel pin_model_;
107 template <
typename _Scalar>
108 struct ResidualDataContactControlGravTpl :
public ResidualDataAbstractTpl<_Scalar> {
109 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
111 typedef _Scalar Scalar;
112 typedef MathBaseTpl<Scalar> MathBase;
113 typedef ResidualDataAbstractTpl<Scalar> Base;
114 typedef StateMultibodyTpl<Scalar> StateMultibody;
115 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
116 typedef pinocchio::DataTpl<Scalar> PinocchioData;
118 template <
template <
typename Scalar>
class Model>
119 ResidualDataContactControlGravTpl(Model<Scalar> *
const model, DataCollectorAbstract *
const data)
120 : Base(model, data) {
121 StateMultibody *sm =
static_cast<StateMultibody *
>(model->get_state().get());
122 pinocchio = PinocchioData(*(sm->get_pinocchio().get()));
125 DataCollectorActMultibodyInContactTpl<Scalar> *d =
126 dynamic_cast<DataCollectorActMultibodyInContactTpl<Scalar> *
>(shared);
129 "Invalid argument: the shared data should be derived from " 130 "DataCollectorActMultibodyInContactTpl");
134 fext = d->contacts->fext;
135 actuation = d->actuation;
139 boost::shared_ptr<ActuationDataAbstractTpl<Scalar> >
actuation;
140 pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >
fext;
152 #include "crocoddyl/multibody/residuals/contact-control-gravity.hxx" 154 #endif // CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_CONTROL_GRAVITY_HPP_
std::size_t nu_
Control dimension.
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.