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,
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);
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_;
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");
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_