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);
87 virtual void calc(
const boost::shared_ptr<ResidualDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x);
96 virtual void calcDiff(
const boost::shared_ptr<ResidualDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
97 const Eigen::Ref<const VectorXs>& u);
108 virtual void calcDiff(
const boost::shared_ptr<ResidualDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x);
113 virtual boost::shared_ptr<ResidualDataAbstract>
createData(DataCollectorAbstract*
const data);
120 virtual void print(std::ostream& os)
const;
129 typename StateMultibody::PinocchioModel pin_model_;
132 template <
typename _Scalar>
133 struct ResidualDataContactControlGravTpl :
public ResidualDataAbstractTpl<_Scalar> {
134 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
136 typedef _Scalar Scalar;
137 typedef MathBaseTpl<Scalar> MathBase;
138 typedef ResidualDataAbstractTpl<Scalar> Base;
139 typedef StateMultibodyTpl<Scalar> StateMultibody;
140 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
141 typedef pinocchio::DataTpl<Scalar> PinocchioData;
143 template <
template <
typename Scalar>
class Model>
144 ResidualDataContactControlGravTpl(Model<Scalar>*
const model, DataCollectorAbstract*
const data)
145 : Base(model, data) {
146 StateMultibody* sm =
static_cast<StateMultibody*
>(model->get_state().get());
147 pinocchio = PinocchioData(*(sm->get_pinocchio().get()));
150 DataCollectorActMultibodyInContactTpl<Scalar>* d =
151 dynamic_cast<DataCollectorActMultibodyInContactTpl<Scalar>*
>(
shared);
154 "Invalid argument: the shared data should be derived from " 155 "DataCollectorActMultibodyInContactTpl");
159 fext = d->contacts->fext;
164 boost::shared_ptr<ActuationDataAbstractTpl<Scalar> >
actuation;
165 pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >
fext;
177 #include "crocoddyl/multibody/residuals/contact-control-gravity.hxx" 179 #endif // CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_CONTROL_GRAVITY_HPP_
std::size_t nu_
Control dimension.
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.
DataCollectorAbstract * shared
Shared data allocated by the action model.