contact-friction-cone.hxx
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #include "crocoddyl/core/utils/exception.hpp"
10 #include "crocoddyl/multibody/costs/contact-friction-cone.hpp"
11 
12 namespace crocoddyl {
13 
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);
22  }
23 }
24 
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);
33  }
34 }
35 
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) {}
41 
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) {}
46 
47 template <typename Scalar>
48 CostModelContactFrictionConeTpl<Scalar>::~CostModelContactFrictionConeTpl() {}
49 
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());
55 
56  // Compute the residual of the friction cone. Note that we need to transform the force
57  // to the contact frame
58  data->r.noalias() = fref_.oRf.get_A() * d->contact->jMf.actInv(d->contact->f).linear();
59 
60  // Compute the cost
61  activation_->calc(data->activation, data->r);
62  data->cost = data->activation->a_value;
63 }
64 
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());
70 
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();
74 
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>();
79  } else {
80  data->Rx.noalias() = A * df_dx;
81  data->Ru.noalias() = A * df_du;
82  }
83  data->Lx.noalias() = data->Rx.transpose() * data->activation->Ar;
84  data->Lu.noalias() = data->Ru.transpose() * data->activation->Ar;
85 
86  d->Arr_Ru.noalias() = data->activation->Arr * data->Ru;
87 
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;
91 }
92 
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);
97 }
98 
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);
103  } else {
104  throw_pretty("Invalid argument: incorrect type (it should be FrameFrictionCone)");
105  }
106 }
107 
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);
112  ref_map = fref_;
113  } else {
114  throw_pretty("Invalid argument: incorrect type (it should be FrameFrictionCone)");
115  }
116 }
117 
118 template <typename Scalar>
119 const FrameFrictionConeTpl<Scalar>& CostModelContactFrictionConeTpl<Scalar>::get_fref() const {
120  return fref_;
121 }
122 
123 template <typename Scalar>
124 void CostModelContactFrictionConeTpl<Scalar>::set_fref(const FrameFrictionCone& fref_in) {
125  fref_ = fref_in;
126 }
127 
128 } // namespace crocoddyl
Definition: action-base.hxx:11