9 #include "crocoddyl/core/utils/exception.hpp" 10 #include "crocoddyl/multibody/costs/contact-force.hpp" 14 template <
typename Scalar>
15 CostModelContactForceTpl<Scalar>::CostModelContactForceTpl(boost::shared_ptr<StateMultibody> state,
16 boost::shared_ptr<ActivationModelAbstract> activation,
17 const FrameForce& fref,
const std::size_t& nu)
18 : Base(state, activation, nu), fref_(fref) {
19 if (activation_->get_nr() != 6) {
20 throw_pretty(
"Invalid argument: " 21 <<
"nr is equals to 6");
25 template <
typename Scalar>
26 CostModelContactForceTpl<Scalar>::CostModelContactForceTpl(boost::shared_ptr<StateMultibody> state,
27 boost::shared_ptr<ActivationModelAbstract> activation,
28 const FrameForce& fref)
29 : Base(state, activation), fref_(fref) {
30 if (activation_->get_nr() != 6) {
31 throw_pretty(
"Invalid argument: " 32 <<
"nr is equals to 6");
36 template <
typename Scalar>
37 CostModelContactForceTpl<Scalar>::CostModelContactForceTpl(boost::shared_ptr<StateMultibody> state,
38 const FrameForce& fref,
const std::size_t& nu)
39 : Base(state, 6, nu), fref_(fref) {}
41 template <
typename Scalar>
42 CostModelContactForceTpl<Scalar>::CostModelContactForceTpl(boost::shared_ptr<StateMultibody> state,
43 const FrameForce& fref)
44 : Base(state, 6), fref_(fref) {}
46 template <
typename Scalar>
47 CostModelContactForceTpl<Scalar>::~CostModelContactForceTpl() {}
49 template <
typename Scalar>
50 void CostModelContactForceTpl<Scalar>::calc(
const boost::shared_ptr<CostDataAbstract>& data,
51 const Eigen::Ref<const VectorXs>& ,
52 const Eigen::Ref<const VectorXs>& ) {
53 CostDataContactForceTpl<Scalar>* d =
static_cast<CostDataContactForceTpl<Scalar>*
>(data.get());
56 data->r = (d->contact->jMf.actInv(d->contact->f) - fref_.oFf).toVector();
59 activation_->calc(data->activation, data->r);
60 data->cost = data->activation->a_value;
63 template <
typename Scalar>
64 void CostModelContactForceTpl<Scalar>::calcDiff(
const boost::shared_ptr<CostDataAbstract>& data,
65 const Eigen::Ref<const VectorXs>&,
const Eigen::Ref<const VectorXs>&) {
66 CostDataContactForceTpl<Scalar>* d =
static_cast<CostDataContactForceTpl<Scalar>*
>(data.get());
68 const MatrixXs& df_dx = d->contact->df_dx;
69 const MatrixXs& df_du = d->contact->df_du;
71 activation_->calcDiff(data->activation, data->r);
74 data->Lx.noalias() = df_dx.transpose() * data->activation->Ar;
75 data->Lu.noalias() = df_du.transpose() * data->activation->Ar;
77 d->Arr_Ru.noalias() = data->activation->Arr * df_du;
79 data->Lxx.noalias() = df_dx.transpose() * data->activation->Arr * df_dx;
80 data->Lxu.noalias() = df_dx.transpose() * d->Arr_Ru;
81 data->Luu.noalias() = df_du.transpose() * d->Arr_Ru;
84 template <
typename Scalar>
85 boost::shared_ptr<CostDataAbstractTpl<Scalar> > CostModelContactForceTpl<Scalar>::createData(
86 DataCollectorAbstract*
const data) {
87 return boost::make_shared<CostDataContactForceTpl<Scalar> >(
this, data);
90 template <
typename Scalar>
91 void CostModelContactForceTpl<Scalar>::set_referenceImpl(
const std::type_info& ti,
const void* pv) {
92 if (ti ==
typeid(FrameForce)) {
93 fref_ = *
static_cast<const FrameForce*
>(pv);
95 throw_pretty(
"Invalid argument: incorrect type (it should be FrameForce)");
99 template <
typename Scalar>
100 void CostModelContactForceTpl<Scalar>::get_referenceImpl(
const std::type_info& ti,
void* pv) {
101 if (ti ==
typeid(FrameForce)) {
102 FrameForce& ref_map = *
static_cast<FrameForce*
>(pv);
105 throw_pretty(
"Invalid argument: incorrect type (it should be FrameForce)");
109 template <
typename Scalar>
110 const FrameForceTpl<Scalar>& CostModelContactForceTpl<Scalar>::get_fref()
const {
114 template <
typename Scalar>
115 void CostModelContactForceTpl<Scalar>::set_fref(
const FrameForce& fref_in) {
Definition: action-base.hxx:11