frame-translation.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/frame-translation.hpp"
11 
12 #include <pinocchio/algorithm/frames.hpp>
13 
14 namespace crocoddyl {
15 
16 template <typename Scalar>
17 CostModelFrameTranslationTpl<Scalar>::CostModelFrameTranslationTpl(
18  boost::shared_ptr<StateMultibody> state, boost::shared_ptr<ActivationModelAbstract> activation,
19  const FrameTranslation& xref, const std::size_t& nu)
20  : Base(state, activation, nu), xref_(xref) {
21  if (activation_->get_nr() != 3) {
22  throw_pretty("Invalid argument: "
23  << "nr is equals to 3");
24  }
25 }
26 
27 template <typename Scalar>
28 CostModelFrameTranslationTpl<Scalar>::CostModelFrameTranslationTpl(
29  boost::shared_ptr<StateMultibody> state, boost::shared_ptr<ActivationModelAbstract> activation,
30  const FrameTranslation& xref)
31  : Base(state, activation), xref_(xref) {
32  if (activation_->get_nr() != 3) {
33  throw_pretty("Invalid argument: "
34  << "nr is equals to 3");
35  }
36 }
37 
38 template <typename Scalar>
39 CostModelFrameTranslationTpl<Scalar>::CostModelFrameTranslationTpl(boost::shared_ptr<StateMultibody> state,
40  const FrameTranslation& xref, const std::size_t& nu)
41  : Base(state, 3, nu), xref_(xref) {}
42 
43 template <typename Scalar>
44 CostModelFrameTranslationTpl<Scalar>::CostModelFrameTranslationTpl(boost::shared_ptr<StateMultibody> state,
45  const FrameTranslation& xref)
46  : Base(state, 3), xref_(xref) {}
47 
48 template <typename Scalar>
49 CostModelFrameTranslationTpl<Scalar>::~CostModelFrameTranslationTpl() {}
50 
51 template <typename Scalar>
52 void CostModelFrameTranslationTpl<Scalar>::calc(const boost::shared_ptr<CostDataAbstract>& data,
53  const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
54  // Compute the frame translation w.r.t. the reference frame
55  CostDataFrameTranslationTpl<Scalar>* d = static_cast<CostDataFrameTranslationTpl<Scalar>*>(data.get());
56  pinocchio::updateFramePlacement(*state_->get_pinocchio().get(), *d->pinocchio, xref_.frame);
57  data->r = d->pinocchio->oMf[xref_.frame].translation() - xref_.oxf;
58 
59  // Compute the cost
60  activation_->calc(data->activation, data->r);
61  data->cost = data->activation->a_value;
62 }
63 
64 template <typename Scalar>
65 void CostModelFrameTranslationTpl<Scalar>::calcDiff(const boost::shared_ptr<CostDataAbstract>& data,
66  const Eigen::Ref<const VectorXs>&,
67  const Eigen::Ref<const VectorXs>&) {
68  // Update the frame placements
69  CostDataFrameTranslationTpl<Scalar>* d = static_cast<CostDataFrameTranslationTpl<Scalar>*>(data.get());
70 
71  // Compute the frame Jacobian at the error point
72  pinocchio::getFrameJacobian(*state_->get_pinocchio().get(), *d->pinocchio, xref_.frame, pinocchio::LOCAL, d->fJf);
73  d->J = d->pinocchio->oMf[xref_.frame].rotation() * d->fJf.template topRows<3>();
74 
75  // Compute the derivatives of the frame placement
76  const std::size_t& nv = state_->get_nv();
77  activation_->calcDiff(d->activation, d->r);
78  d->Rx.leftCols(nv) = d->J;
79  d->Lx.head(nv) = d->J.transpose() * d->activation->Ar;
80  d->Lxx.topLeftCorner(nv, nv) = d->J.transpose() * d->activation->Arr * d->J;
81 }
82 
83 template <typename Scalar>
84 boost::shared_ptr<CostDataAbstractTpl<Scalar> > CostModelFrameTranslationTpl<Scalar>::createData(
85  DataCollectorAbstract* const data) {
86  return boost::make_shared<CostDataFrameTranslationTpl<Scalar> >(this, data);
87 }
88 
89 template <typename Scalar>
90 void CostModelFrameTranslationTpl<Scalar>::set_referenceImpl(const std::type_info& ti, const void* pv) {
91  if (ti == typeid(FrameTranslation)) {
92  xref_ = *static_cast<const FrameTranslation*>(pv);
93  } else {
94  throw_pretty("Invalid argument: incorrect type (it should be FrameTranslation)");
95  }
96 }
97 
98 template <typename Scalar>
99 void CostModelFrameTranslationTpl<Scalar>::get_referenceImpl(const std::type_info& ti, void* pv) {
100  if (ti == typeid(FrameTranslation)) {
101  FrameTranslation& ref_map = *static_cast<FrameTranslation*>(pv);
102  ref_map = xref_;
103  } else {
104  throw_pretty("Invalid argument: incorrect type (it should be FrameTranslation)");
105  }
106 }
107 
108 template <typename Scalar>
109 const FrameTranslationTpl<Scalar>& CostModelFrameTranslationTpl<Scalar>::get_xref() const {
110  return xref_;
111 }
112 
113 template <typename Scalar>
114 void CostModelFrameTranslationTpl<Scalar>::set_xref(const FrameTranslation& xref_in) {
115  xref_ = xref_in;
116 }
117 
118 } // namespace crocoddyl
Definition: action-base.hxx:11