9 #ifndef CROCODDYL_MULTIBODY_COSTS_CONTACT_FORCE_HPP_ 10 #define CROCODDYL_MULTIBODY_COSTS_CONTACT_FORCE_HPP_ 12 #include "crocoddyl/multibody/fwd.hpp" 13 #include "crocoddyl/multibody/cost-base.hpp" 14 #include "crocoddyl/multibody/contact-base.hpp" 15 #include "crocoddyl/multibody/data/contacts.hpp" 16 #include "crocoddyl/multibody/frames.hpp" 17 #include "crocoddyl/core/utils/exception.hpp" 21 template <
typename _Scalar>
24 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
26 typedef _Scalar Scalar;
36 typedef typename MathBase::Vector6s Vector6s;
37 typedef typename MathBase::VectorXs VectorXs;
38 typedef typename MathBase::MatrixXs MatrixXs;
41 boost::shared_ptr<ActivationModelAbstract> activation,
const FrameForce& fref,
42 const std::size_t& nu);
44 boost::shared_ptr<ActivationModelAbstract> activation,
const FrameForce& fref);
49 virtual void calc(
const boost::shared_ptr<CostDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
50 const Eigen::Ref<const VectorXs>& u);
51 virtual void calcDiff(
const boost::shared_ptr<CostDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
52 const Eigen::Ref<const VectorXs>& u);
53 virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract*
const data);
55 const FrameForce& get_fref()
const;
56 void set_fref(
const FrameForce& fref);
59 virtual void set_referenceImpl(
const std::type_info& ti,
const void* pv);
60 virtual void get_referenceImpl(
const std::type_info& ti,
void* pv);
62 using Base::activation_;
71 template <
typename _Scalar>
73 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
75 typedef _Scalar Scalar;
80 typedef typename MathBase::VectorXs VectorXs;
81 typedef typename MathBase::MatrixXs MatrixXs;
82 typedef typename MathBase::Matrix6xs Matrix6xs;
84 template <
template <
typename Scalar>
class Model>
86 : Base(model, data), Arr_Ru(model->get_activation()->get_nr(), model->get_state()->get_nv()) {
92 throw_pretty(
"Invalid argument: the shared data should be derived from DataCollectorContact");
96 std::string frame_name = model->get_state()->get_pinocchio()->frames[model->get_fref().frame].name;
97 bool found_contact =
false;
98 for (
typename ContactModelMultiple::ContactDataContainer::iterator it = d->contacts->contacts.begin();
99 it != d->contacts->contacts.end(); ++it) {
100 if (it->second->frame == model->get_fref().frame) {
101 found_contact =
true;
102 contact = it->second;
106 if (!found_contact) {
107 throw_pretty(
"Domain error: there isn't defined contact data for " + frame_name);
111 boost::shared_ptr<ContactDataAbstractTpl<Scalar> > contact;
113 using Base::activation;
130 #include "crocoddyl/multibody/costs/contact-force.hxx" 132 #endif // CROCODDYL_MULTIBODY_COSTS_CONTACT_FORCE_HPP_