9 #ifndef CROCODDYL_MULTIBODY_COSTS_IMPULSE_WRENCH_CONE_HPP_ 10 #define CROCODDYL_MULTIBODY_COSTS_IMPULSE_WRENCH_CONE_HPP_ 12 #include "crocoddyl/multibody/fwd.hpp" 13 #include "crocoddyl/core/cost-base.hpp" 14 #include "crocoddyl/multibody/states/multibody.hpp" 15 #include "crocoddyl/multibody/impulse-base.hpp" 16 #include "crocoddyl/multibody/impulses/impulse-3d.hpp" 17 #include "crocoddyl/multibody/impulses/impulse-6d.hpp" 18 #include "crocoddyl/multibody/data/impulses.hpp" 19 #include "crocoddyl/multibody/frames.hpp" 20 #include "crocoddyl/multibody/wrench-cone.hpp" 21 #include "crocoddyl/core/utils/exception.hpp" 25 template <
typename _Scalar>
28 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
30 typedef _Scalar Scalar;
39 typedef typename MathBase::VectorXs VectorXs;
40 typedef typename MathBase::MatrixXs MatrixXs;
41 typedef typename MathBase::MatrixX6s MatrixX6s;
44 boost::shared_ptr<ActivationModelAbstract> activation,
const FrameWrenchCone& fref);
48 virtual void calc(
const boost::shared_ptr<CostDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
49 const Eigen::Ref<const VectorXs>& u);
50 virtual void calcDiff(
const boost::shared_ptr<CostDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
51 const Eigen::Ref<const VectorXs>& u);
52 virtual boost::shared_ptr<CostDataAbstract>
createData(DataCollectorAbstract*
const data);
62 FrameWrenchCone fref_;
65 template <
typename _Scalar>
67 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
69 typedef _Scalar Scalar;
76 typedef typename MathBase::MatrixXs MatrixXs;
77 typedef typename MathBase::Matrix6xs Matrix6xs;
79 template <
template <
typename Scalar>
class Model>
81 : Base(model, data), Arr_Rx(model->get_activation()->get_nr(), model->get_state()->get_ndx()) {
87 throw_pretty(
"Invalid argument: the shared data should be derived from DataCollectorImpulse");
91 FrameWrenchCone fref = model->template get_reference<FrameWrenchCone>();
92 const boost::shared_ptr<StateMultibody>& state = boost::static_pointer_cast<StateMultibody>(model->get_state());
93 std::string frame_name = state->get_pinocchio()->frames[fref.id].name;
94 bool found_impulse =
false;
95 for (
typename ImpulseModelMultiple::ImpulseDataContainer::iterator it = d->impulses->impulses.begin();
96 it != d->impulses->impulses.end(); ++it) {
97 if (it->second->frame == fref.id) {
100 found_impulse =
true;
101 impulse = it->second;
102 throw_pretty(
"Domain error: there isn't defined at least a 6d contact for " + frame_name);
106 found_impulse =
true;
107 impulse = it->second;
110 throw_pretty(
"Domain error: there isn't defined at least a 6d impulse for " + frame_name);
114 if (!found_impulse) {
115 throw_pretty(
"Domain error: there isn't defined impulse data for " + frame_name);
119 boost::shared_ptr<ImpulseDataAbstractTpl<Scalar> > impulse;
121 using Base::activation;
135 #include "crocoddyl/multibody/costs/impulse-wrench-cone.hxx" 137 #endif // CROCODDYL_MULTIBODY_COSTS_IMPULSE_WRENCH_CONE_HPP_ Abstract class for cost models.
virtual boost::shared_ptr< CostDataAbstract > createData(DataCollectorAbstract *const data)
Create the cost data.
boost::shared_ptr< ActivationModelAbstract > activation_
Activation model.
State multibody representation.
Define a stack of impulse models.
virtual void set_referenceImpl(const std::type_info &ti, const void *pv)
virtual void get_referenceImpl(const std::type_info &ti, void *pv) const
boost::shared_ptr< StateAbstract > state_
State description.