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/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/core/utils/exception.hpp" 20 #include "crocoddyl/core/utils/deprecate.hpp" 24 enum ContactType { Contact3D, Contact6D, Undefined };
46 template <
typename _Scalar>
49 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
51 typedef _Scalar Scalar;
61 typedef typename MathBase::Vector6s Vector6s;
62 typedef typename MathBase::VectorXs VectorXs;
63 typedef typename MathBase::MatrixXs MatrixXs;
77 boost::shared_ptr<ActivationModelAbstract> activation,
const FrameForce& fref,
78 const std::size_t& nu);
92 boost::shared_ptr<ActivationModelAbstract> activation,
const FrameForce& fref);
107 const std::size_t& nu);
145 virtual void calc(
const boost::shared_ptr<CostDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
146 const Eigen::Ref<const VectorXs>& u);
158 virtual void calcDiff(
const boost::shared_ptr<CostDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
159 const Eigen::Ref<const VectorXs>& u);
170 virtual boost::shared_ptr<CostDataAbstract>
createData(DataCollectorAbstract*
const data);
172 DEPRECATED(
"Use set_reference<FrameForceTpl<Scalar> >()",
void set_fref(
const FrameForce& fref));
173 DEPRECATED(
"Use get_reference<FrameForceTpl<Scalar> >()",
const FrameForce& get_fref()
const);
188 using Base::activation_;
198 template <
typename _Scalar>
200 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
202 typedef _Scalar Scalar;
208 typedef typename MathBase::VectorXs VectorXs;
209 typedef typename MathBase::MatrixXs MatrixXs;
210 typedef typename MathBase::Matrix6xs Matrix6xs;
212 template <
template <
typename Scalar>
class Model>
214 : Base(model, data), Arr_Ru(model->get_activation()->get_nr(), model->get_state()->get_nv()) {
216 contact_type = Undefined;
221 throw_pretty(
"Invalid argument: the shared data should be derived from DataCollectorContact");
225 FrameForce fref = model->template get_reference<FrameForce>();
226 std::string frame_name = model->get_state()->get_pinocchio()->frames[fref.frame].name;
227 bool found_contact =
false;
228 for (
typename ContactModelMultiple::ContactDataContainer::iterator it = d->contacts->contacts.begin();
229 it != d->contacts->contacts.end(); ++it) {
230 if (it->second->frame == fref.frame) {
233 contact_type = Contact3D;
234 if (model->get_activation()->get_nr() != 3) {
235 throw_pretty(
"Domain error: nr isn't defined as 3 in the activation model for the 3d contact in " +
238 found_contact =
true;
239 contact = it->second;
244 contact_type = Contact6D;
245 if (model->get_activation()->get_nr() != 6) {
246 throw_pretty(
"Domain error: nr isn't defined as 6 in the activation model for the 3d contact in " +
249 found_contact =
true;
250 contact = it->second;
253 throw_pretty(
"Domain error: there isn't defined at least a 3d contact for " + frame_name);
257 if (!found_contact) {
258 throw_pretty(
"Domain error: there isn't defined contact data for " + frame_name);
262 boost::shared_ptr<ContactDataAbstractTpl<Scalar> > contact;
264 ContactType contact_type;
265 using Base::activation;
283 #include "crocoddyl/multibody/costs/contact-force.hxx" 285 #endif // CROCODDYL_MULTIBODY_COSTS_CONTACT_FORCE_HPP_