9 #ifndef CROCODDYL_MULTIBODY_COSTS_CONTACT_FRICTION_CONE_HPP_ 10 #define CROCODDYL_MULTIBODY_COSTS_CONTACT_FRICTION_CONE_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/contacts/contact-3d.hpp" 16 #include "crocoddyl/multibody/contacts/contact-6d.hpp" 17 #include "crocoddyl/multibody/data/contacts.hpp" 18 #include "crocoddyl/multibody/frames.hpp" 19 #include "crocoddyl/multibody/friction-cone.hpp" 20 #include "crocoddyl/core/utils/exception.hpp" 24 template <
typename _Scalar>
27 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
29 typedef _Scalar Scalar;
41 typedef typename MathBase::Vector6s Vector6s;
42 typedef typename MathBase::VectorXs VectorXs;
43 typedef typename MathBase::MatrixXs MatrixXs;
44 typedef typename MathBase::MatrixX3s MatrixX3s;
47 boost::shared_ptr<ActivationModelAbstract> activation,
const FrameFrictionCone& fref,
48 const std::size_t& nu);
50 boost::shared_ptr<ActivationModelAbstract> activation,
51 const FrameFrictionCone& fref);
53 const std::size_t& nu);
57 virtual void calc(
const boost::shared_ptr<CostDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
58 const Eigen::Ref<const VectorXs>& u);
59 virtual void calcDiff(
const boost::shared_ptr<CostDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
60 const Eigen::Ref<const VectorXs>& u);
61 virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract*
const data);
63 const FrameFrictionCone& get_fref()
const;
64 void set_fref(
const FrameFrictionCone& fref);
67 virtual void set_referenceImpl(
const std::type_info& ti,
const void* pv);
68 virtual void get_referenceImpl(
const std::type_info& ti,
void* pv);
70 using Base::activation_;
76 FrameFrictionCone fref_;
79 template <
typename _Scalar>
81 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
83 typedef _Scalar Scalar;
89 typedef typename MathBase::VectorXs VectorXs;
90 typedef typename MathBase::MatrixXs MatrixXs;
91 typedef typename MathBase::Matrix6xs Matrix6xs;
93 template <
template <
typename Scalar>
class Model>
96 Arr_Ru(model->get_activation()->get_nr(), model->get_state()->get_nv()),
97 more_than_3_constraints(
false) {
103 throw_pretty(
"Invalid argument: the shared data should be derived from DataCollectorContact");
107 const FrameFrictionCone& fref = model->get_fref();
108 std::string frame_name = model->get_state()->get_pinocchio()->frames[fref.frame].name;
109 bool found_contact =
false;
110 for (
typename ContactModelMultiple::ContactDataContainer::iterator it = d->contacts->contacts.begin();
111 it != d->contacts->contacts.end(); ++it) {
112 if (it->second->frame == fref.frame) {
115 found_contact =
true;
116 contact = it->second;
121 more_than_3_constraints =
true;
122 found_contact =
true;
123 contact = it->second;
126 throw_pretty(
"Domain error: there isn't defined at least a 3d contact for " + frame_name);
130 if (!found_contact) {
131 throw_pretty(
"Domain error: there isn't defined contact data for " + frame_name);
135 boost::shared_ptr<ContactDataAbstractTpl<Scalar> > contact;
137 bool more_than_3_constraints;
138 using Base::activation;
156 #include "crocoddyl/multibody/costs/contact-friction-cone.hxx" 158 #endif // CROCODDYL_MULTIBODY_COSTS_CONTACT_FRICTION_CONE_HPP_