9 #include "crocoddyl/core/utils/exception.hpp" 10 #include "crocoddyl/multibody/costs/contact-friction-cone.hpp" 14 template <
typename Scalar>
15 CostModelContactFrictionConeTpl<Scalar>::CostModelContactFrictionConeTpl(
16 boost::shared_ptr<StateMultibody> state, boost::shared_ptr<ActivationModelAbstract> activation,
17 const FrameFrictionCone& fref,
const std::size_t& nu)
18 : Base(state, activation, nu), fref_(fref) {
19 if (activation_->get_nr() != fref_.oRf.get_nf() + 1) {
20 throw_pretty(
"Invalid argument: " 21 <<
"nr is equals to " << fref_.oRf.get_nf() + 1);
25 template <
typename Scalar>
26 CostModelContactFrictionConeTpl<Scalar>::CostModelContactFrictionConeTpl(
27 boost::shared_ptr<StateMultibody> state, boost::shared_ptr<ActivationModelAbstract> activation,
28 const FrameFrictionCone& fref)
29 : Base(state, activation), fref_(fref) {
30 if (activation_->get_nr() != fref_.oRf.get_nf() + 1) {
31 throw_pretty(
"Invalid argument: " 32 <<
"nr is equals to " << fref_.oRf.get_nf() + 1);
36 template <
typename Scalar>
37 CostModelContactFrictionConeTpl<Scalar>::CostModelContactFrictionConeTpl(boost::shared_ptr<StateMultibody> state,
38 const FrameFrictionCone& fref,
39 const std::size_t& nu)
40 : Base(state, fref.oRf.get_nf() + 1, nu), fref_(fref) {}
42 template <
typename Scalar>
43 CostModelContactFrictionConeTpl<Scalar>::CostModelContactFrictionConeTpl(boost::shared_ptr<StateMultibody> state,
44 const FrameFrictionCone& fref)
45 : Base(state, fref.oRf.get_nf() + 1), fref_(fref) {}
47 template <
typename Scalar>
48 CostModelContactFrictionConeTpl<Scalar>::~CostModelContactFrictionConeTpl() {}
50 template <
typename Scalar>
51 void CostModelContactFrictionConeTpl<Scalar>::calc(
const boost::shared_ptr<CostDataAbstract>& data,
52 const Eigen::Ref<const VectorXs>&,
53 const Eigen::Ref<const VectorXs>&) {
54 CostDataContactFrictionConeTpl<Scalar>* d =
static_cast<CostDataContactFrictionConeTpl<Scalar>*
>(data.get());
58 data->r.noalias() = fref_.oRf.get_A() * d->contact->jMf.actInv(d->contact->f).linear();
61 activation_->calc(data->activation, data->r);
62 data->cost = data->activation->a_value;
65 template <
typename Scalar>
66 void CostModelContactFrictionConeTpl<Scalar>::calcDiff(
const boost::shared_ptr<CostDataAbstract>& data,
67 const Eigen::Ref<const VectorXs>&,
68 const Eigen::Ref<const VectorXs>&) {
69 CostDataContactFrictionConeTpl<Scalar>* d =
static_cast<CostDataContactFrictionConeTpl<Scalar>*
>(data.get());
71 const MatrixXs& df_dx = d->contact->df_dx;
72 const MatrixXs& df_du = d->contact->df_du;
73 const MatrixX3s& A = fref_.oRf.get_A();
75 activation_->calcDiff(data->activation, data->r);
76 if (d->more_than_3_constraints) {
77 data->Rx.noalias() = A * df_dx.template topRows<3>();
78 data->Ru.noalias() = A * df_du.template topRows<3>();
80 data->Rx.noalias() = A * df_dx;
81 data->Ru.noalias() = A * df_du;
83 data->Lx.noalias() = data->Rx.transpose() * data->activation->Ar;
84 data->Lu.noalias() = data->Ru.transpose() * data->activation->Ar;
86 d->Arr_Ru.noalias() = data->activation->Arr * data->Ru;
88 data->Lxx.noalias() = data->Rx.transpose() * data->activation->Arr * data->Rx;
89 data->Lxu.noalias() = data->Rx.transpose() * d->Arr_Ru;
90 data->Luu.noalias() = data->Ru.transpose() * d->Arr_Ru;
93 template <
typename Scalar>
94 boost::shared_ptr<CostDataAbstractTpl<Scalar> > CostModelContactFrictionConeTpl<Scalar>::createData(
95 DataCollectorAbstract*
const data) {
96 return boost::make_shared<CostDataContactFrictionConeTpl<Scalar> >(
this, data);
99 template <
typename Scalar>
100 void CostModelContactFrictionConeTpl<Scalar>::set_referenceImpl(
const std::type_info& ti,
const void* pv) {
101 if (ti ==
typeid(FrameFrictionCone)) {
102 fref_ = *
static_cast<const FrameFrictionCone*
>(pv);
104 throw_pretty(
"Invalid argument: incorrect type (it should be FrameFrictionCone)");
108 template <
typename Scalar>
109 void CostModelContactFrictionConeTpl<Scalar>::get_referenceImpl(
const std::type_info& ti,
void* pv) {
110 if (ti ==
typeid(FrameFrictionCone)) {
111 FrameFrictionCone& ref_map = *
static_cast<FrameFrictionCone*
>(pv);
114 throw_pretty(
"Invalid argument: incorrect type (it should be FrameFrictionCone)");
118 template <
typename Scalar>
119 const FrameFrictionConeTpl<Scalar>& CostModelContactFrictionConeTpl<Scalar>::get_fref()
const {
123 template <
typename Scalar>
124 void CostModelContactFrictionConeTpl<Scalar>::set_fref(
const FrameFrictionCone& fref_in) {
Definition: action-base.hxx:11