frame-rotation.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-rotation.hpp"
11 
12 #include <pinocchio/algorithm/frames.hpp>
13 
14 namespace crocoddyl {
15 
16 template <typename Scalar>
17 CostModelFrameRotationTpl<Scalar>::CostModelFrameRotationTpl(boost::shared_ptr<StateMultibody> state,
18  boost::shared_ptr<ActivationModelAbstract> activation,
19  const FrameRotation& Rref, const std::size_t& nu)
20  : Base(state, activation, nu), Rref_(Rref), oRf_inv_(Rref.oRf.transpose()) {
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 CostModelFrameRotationTpl<Scalar>::CostModelFrameRotationTpl(boost::shared_ptr<StateMultibody> state,
29  boost::shared_ptr<ActivationModelAbstract> activation,
30  const FrameRotation& Rref)
31  : Base(state, activation), Rref_(Rref), oRf_inv_(Rref.oRf.transpose()) {
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 CostModelFrameRotationTpl<Scalar>::CostModelFrameRotationTpl(boost::shared_ptr<StateMultibody> state,
40  const FrameRotation& Rref, const std::size_t& nu)
41  : Base(state, 3, nu), Rref_(Rref), oRf_inv_(Rref.oRf.transpose()) {}
42 
43 template <typename Scalar>
44 CostModelFrameRotationTpl<Scalar>::CostModelFrameRotationTpl(boost::shared_ptr<StateMultibody> state,
45  const FrameRotation& Rref)
46  : Base(state, 3), Rref_(Rref), oRf_inv_(Rref.oRf.transpose()) {}
47 
48 template <typename Scalar>
49 CostModelFrameRotationTpl<Scalar>::~CostModelFrameRotationTpl() {}
50 
51 template <typename Scalar>
52 void CostModelFrameRotationTpl<Scalar>::calc(const boost::shared_ptr<CostDataAbstract>& data,
53  const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
54  CostDataFrameRotationTpl<Scalar>* d = static_cast<CostDataFrameRotationTpl<Scalar>*>(data.get());
55 
56  // Compute the frame placement w.r.t. the reference frame
57  pinocchio::updateFramePlacement(*state_->get_pinocchio().get(), *d->pinocchio, Rref_.frame);
58  d->rRf.noalias() = oRf_inv_ * d->pinocchio->oMf[Rref_.frame].rotation();
59  d->r = pinocchio::log3(d->rRf);
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 CostModelFrameRotationTpl<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  CostDataFrameRotationTpl<Scalar>* d = static_cast<CostDataFrameRotationTpl<Scalar>*>(data.get());
73 
74  // // Compute the frame Jacobian at the error point
75  pinocchio::Jlog3(d->rRf, d->rJf);
76  pinocchio::getFrameJacobian(*state_->get_pinocchio().get(), *d->pinocchio, Rref_.frame, pinocchio::LOCAL, d->fJf);
77  d->J.noalias() = d->rJf * d->fJf.template bottomRows<3>();
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> > CostModelFrameRotationTpl<Scalar>::createData(
90  DataCollectorAbstract* const data) {
91  return boost::make_shared<CostDataFrameRotationTpl<Scalar> >(this, data);
92 }
93 
94 template <typename Scalar>
95 void CostModelFrameRotationTpl<Scalar>::set_referenceImpl(const std::type_info& ti, const void* pv) {
96  if (ti == typeid(FrameRotation)) {
97  Rref_ = *static_cast<const FrameRotation*>(pv);
98  } else {
99  throw_pretty("Invalid argument: incorrect type (it should be FrameRotation)");
100  }
101 }
102 
103 template <typename Scalar>
104 void CostModelFrameRotationTpl<Scalar>::get_referenceImpl(const std::type_info& ti, void* pv) {
105  if (ti == typeid(FrameRotation)) {
106  FrameRotation& ref_map = *static_cast<FrameRotation*>(pv);
107  ref_map = Rref_;
108  } else {
109  throw_pretty("Invalid argument: incorrect type (it should be FrameRotation)");
110  }
111 }
112 
113 template <typename Scalar>
114 const FrameRotationTpl<Scalar>& CostModelFrameRotationTpl<Scalar>::get_Rref() const {
115  return Rref_;
116 }
117 
118 template <typename Scalar>
119 void CostModelFrameRotationTpl<Scalar>::set_Rref(const FrameRotation& Rref_in) {
120  Rref_ = Rref_in;
121  oRf_inv_ = Rref_.oRf.transpose();
122 }
123 
124 } // namespace crocoddyl
Definition: action-base.hxx:11