contact-force.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-force.hpp"
11 
12 namespace crocoddyl {
13 
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");
22  }
23 }
24 
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");
33  }
34 }
35 
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) {}
40 
41 template <typename Scalar>
42 CostModelContactForceTpl<Scalar>::CostModelContactForceTpl(boost::shared_ptr<StateMultibody> state,
43  const FrameForce& fref)
44  : Base(state, 6), fref_(fref) {}
45 
46 template <typename Scalar>
47 CostModelContactForceTpl<Scalar>::~CostModelContactForceTpl() {}
48 
49 template <typename Scalar>
50 void CostModelContactForceTpl<Scalar>::calc(const boost::shared_ptr<CostDataAbstract>& data,
51  const Eigen::Ref<const VectorXs>& /*x*/,
52  const Eigen::Ref<const VectorXs>& /*u*/) {
53  CostDataContactForceTpl<Scalar>* d = static_cast<CostDataContactForceTpl<Scalar>*>(data.get());
54 
55  // We transform the force to the contact frame
56  data->r = (d->contact->jMf.actInv(d->contact->f) - fref_.oFf).toVector();
57 
58  // Compute the cost
59  activation_->calc(data->activation, data->r);
60  data->cost = data->activation->a_value;
61 }
62 
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());
67 
68  const MatrixXs& df_dx = d->contact->df_dx;
69  const MatrixXs& df_du = d->contact->df_du;
70 
71  activation_->calcDiff(data->activation, data->r);
72  data->Rx = df_dx;
73  data->Ru = df_du;
74  data->Lx.noalias() = df_dx.transpose() * data->activation->Ar;
75  data->Lu.noalias() = df_du.transpose() * data->activation->Ar;
76 
77  d->Arr_Ru.noalias() = data->activation->Arr * df_du;
78 
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;
82 }
83 
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);
88 }
89 
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);
94  } else {
95  throw_pretty("Invalid argument: incorrect type (it should be FrameForce)");
96  }
97 }
98 
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);
103  ref_map = fref_;
104  } else {
105  throw_pretty("Invalid argument: incorrect type (it should be FrameForce)");
106  }
107 }
108 
109 template <typename Scalar>
110 const FrameForceTpl<Scalar>& CostModelContactForceTpl<Scalar>::get_fref() const {
111  return fref_;
112 }
113 
114 template <typename Scalar>
115 void CostModelContactForceTpl<Scalar>::set_fref(const FrameForce& fref_in) {
116  fref_ = fref_in;
117 }
118 
119 } // namespace crocoddyl
Definition: action-base.hxx:11