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