frame-velocity.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-velocity.hpp"
11 
12 #include <pinocchio/algorithm/frames.hpp>
13 #include <pinocchio/algorithm/kinematics-derivatives.hpp>
14 
15 namespace crocoddyl {
16 
17 template <typename Scalar>
18 CostModelFrameVelocityTpl<Scalar>::CostModelFrameVelocityTpl(boost::shared_ptr<StateMultibody> state,
19  boost::shared_ptr<ActivationModelAbstract> activation,
20  const FrameMotion& vref, const std::size_t& nu)
21  : Base(state, activation, nu), vref_(vref) {
22  if (activation_->get_nr() != 6) {
23  throw_pretty("Invalid argument: "
24  << "nr is equals to 6");
25  }
26 }
27 
28 template <typename Scalar>
29 CostModelFrameVelocityTpl<Scalar>::CostModelFrameVelocityTpl(boost::shared_ptr<StateMultibody> state,
30  boost::shared_ptr<ActivationModelAbstract> activation,
31  const FrameMotion& vref)
32  : Base(state, activation), vref_(vref) {
33  if (activation_->get_nr() != 6) {
34  throw_pretty("Invalid argument: "
35  << "nr is equals to 6");
36  }
37 }
38 
39 template <typename Scalar>
40 CostModelFrameVelocityTpl<Scalar>::CostModelFrameVelocityTpl(boost::shared_ptr<StateMultibody> state,
41  const FrameMotion& vref, const std::size_t& nu)
42  : Base(state, 6, nu), vref_(vref) {}
43 
44 template <typename Scalar>
45 CostModelFrameVelocityTpl<Scalar>::CostModelFrameVelocityTpl(boost::shared_ptr<StateMultibody> state,
46  const FrameMotion& vref)
47  : Base(state, 6), vref_(vref) {}
48 
49 template <typename Scalar>
50 CostModelFrameVelocityTpl<Scalar>::~CostModelFrameVelocityTpl() {}
51 
52 template <typename Scalar>
53 void CostModelFrameVelocityTpl<Scalar>::calc(const boost::shared_ptr<CostDataAbstract>& data,
54  const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
55  CostDataFrameVelocityTpl<Scalar>* d = static_cast<CostDataFrameVelocityTpl<Scalar>*>(data.get());
56 
57  // Compute the frame velocity w.r.t. the reference frame
58  d->vr = pinocchio::getFrameVelocity(*state_->get_pinocchio().get(), *d->pinocchio, vref_.frame) - vref_.oMf;
59  data->r = d->vr.toVector();
60 
61  // Compute the cost
62  activation_->calc(data->activation, data->r);
63  data->cost = data->activation->a_value;
64 }
65 
66 template <typename Scalar>
67 void CostModelFrameVelocityTpl<Scalar>::calcDiff(const boost::shared_ptr<CostDataAbstract>& data,
68  const Eigen::Ref<const VectorXs>&,
69  const Eigen::Ref<const VectorXs>&) {
70  // Get the partial derivatives of the local frame velocity
71  CostDataFrameVelocityTpl<Scalar>* d = static_cast<CostDataFrameVelocityTpl<Scalar>*>(data.get());
72  pinocchio::getJointVelocityDerivatives(*state_->get_pinocchio().get(), *d->pinocchio, d->joint, pinocchio::LOCAL,
73  d->dv_dq, d->dv_dv);
74 
75  // Compute the derivatives of the frame velocity
76  const std::size_t& nv = state_->get_nv();
77  activation_->calcDiff(data->activation, data->r);
78  data->Rx.leftCols(nv).noalias() = d->fXj * d->dv_dq;
79  data->Rx.rightCols(nv).noalias() = d->fXj * d->dv_dv;
80  data->Lx.noalias() = data->Rx.transpose() * data->activation->Ar;
81  d->Arr_Rx.noalias() = data->activation->Arr * data->Rx;
82  data->Lxx.noalias() = data->Rx.transpose() * d->Arr_Rx;
83 }
84 
85 template <typename Scalar>
86 boost::shared_ptr<CostDataAbstractTpl<Scalar> > CostModelFrameVelocityTpl<Scalar>::createData(
87  DataCollectorAbstract* const data) {
88  return boost::make_shared<CostDataFrameVelocityTpl<Scalar> >(this, data);
89 }
90 
91 template <typename Scalar>
92 void CostModelFrameVelocityTpl<Scalar>::set_referenceImpl(const std::type_info& ti, const void* pv) {
93  if (ti == typeid(FrameMotion)) {
94  vref_ = *static_cast<const FrameMotion*>(pv);
95  } else {
96  throw_pretty("Invalid argument: incorrect type (it should be FrameMotion)");
97  }
98 }
99 
100 template <typename Scalar>
101 void CostModelFrameVelocityTpl<Scalar>::get_referenceImpl(const std::type_info& ti, void* pv) {
102  if (ti == typeid(FrameMotion)) {
103  FrameMotion& ref_map = *static_cast<FrameMotion*>(pv);
104  ref_map = vref_;
105  } else {
106  throw_pretty("Invalid argument: incorrect type (it should be FrameMotion)");
107  }
108 }
109 
110 template <typename Scalar>
111 const FrameMotionTpl<Scalar>& CostModelFrameVelocityTpl<Scalar>::get_vref() const {
112  return vref_;
113 }
114 
115 template <typename Scalar>
116 void CostModelFrameVelocityTpl<Scalar>::set_vref(const FrameMotion& vref_in) {
117  vref_ = vref_in;
118 }
119 
120 } // namespace crocoddyl
Definition: action-base.hxx:11