9 #ifndef CROCODDYL_MULTIBODY_NUMDIFF_CONTACT_HPP_
10 #define CROCODDYL_MULTIBODY_NUMDIFF_CONTACT_HPP_
12 #include <boost/function.hpp>
13 #include "crocoddyl/multibody/fwd.hpp"
14 #include "crocoddyl/multibody/contact-base.hpp"
18 template <
typename _Scalar>
19 class ContactModelNumDiffTpl :
public ContactModelAbstractTpl<_Scalar> {
21 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
23 typedef _Scalar Scalar;
24 typedef ContactDataAbstractTpl<Scalar> ContactDataAbstract;
25 typedef ContactModelAbstractTpl<Scalar> Base;
26 typedef ContactDataNumDiffTpl<Scalar> Data;
27 typedef MathBaseTpl<Scalar> MathBase;
28 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
29 typedef boost::function<void(
const VectorXs&,
const VectorXs&)> ReevaluationFunction;
46 void calc(
const boost::shared_ptr<ContactDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x);
51 void calcDiff(
const boost::shared_ptr<ContactDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x);
56 void updateForce(
const boost::shared_ptr<ContactDataAbstract>& data,
const VectorXs& force);
64 boost::shared_ptr<ContactDataAbstract>
createData(pinocchio::DataTpl<Scalar>*
const data);
71 const boost::shared_ptr<Base>&
get_model()
const;
93 void set_reevals(
const std::vector<ReevaluationFunction>& reevals);
121 void assertStableStateFD(
const Eigen::Ref<const VectorXs>& );
124 template <
typename _Scalar>
126 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
128 typedef _Scalar Scalar;
133 template <
template <
typename Scalar>
class Model>
135 : Base(model, data),
dx(model->get_state()->get_ndx()),
xp(model->get_state()->get_nx()) {
139 const std::size_t ndx = model->get_model()->get_state()->get_ndx();
140 data_0 = model->get_model()->createData(data);
141 for (std::size_t i = 0; i < ndx; ++i) {
142 data_x.push_back(model->get_model()->createData(data));
146 virtual ~ContactDataNumDiffTpl() {}
151 using Base::pinocchio;
156 std::vector<boost::shared_ptr<Base> >
data_x;
164 #include "crocoddyl/multibody/numdiff/contact.hxx"
166 #endif // CROCODDYL_MULTIBODY_NUMDIFF_CONTACT_HPP_