9 #include "crocoddyl/core/utils/exception.hpp" 10 #include "crocoddyl/multibody/costs/centroidal-momentum.hpp" 12 #include <pinocchio/algorithm/centroidal-derivatives.hpp> 16 template <
typename Scalar>
17 CostModelCentroidalMomentumTpl<Scalar>::CostModelCentroidalMomentumTpl(
18 boost::shared_ptr<StateMultibody> state, boost::shared_ptr<ActivationModelAbstract> activation,
19 const Vector6s& href,
const std::size_t& nu)
20 : Base(state, activation, nu), href_(href) {
21 if (activation_->get_nr() != 6) {
22 throw_pretty(
"Invalid argument: " 23 <<
"nr is equals to 6");
27 template <
typename Scalar>
28 CostModelCentroidalMomentumTpl<Scalar>::CostModelCentroidalMomentumTpl(
29 boost::shared_ptr<StateMultibody> state, boost::shared_ptr<ActivationModelAbstract> activation,
31 : Base(state, activation), href_(href) {
32 if (activation_->get_nr() != 6) {
33 throw_pretty(
"Invalid argument: " 34 <<
"nr is equals to 6");
38 template <
typename Scalar>
39 CostModelCentroidalMomentumTpl<Scalar>::CostModelCentroidalMomentumTpl(boost::shared_ptr<StateMultibody> state,
40 const Vector6s& href,
const std::size_t& nu)
41 : Base(state, 6, nu), href_(href) {}
43 template <
typename Scalar>
44 CostModelCentroidalMomentumTpl<Scalar>::CostModelCentroidalMomentumTpl(boost::shared_ptr<StateMultibody> state,
46 : Base(state, 6), href_(href) {}
48 template <
typename Scalar>
49 CostModelCentroidalMomentumTpl<Scalar>::~CostModelCentroidalMomentumTpl() {}
51 template <
typename Scalar>
52 void CostModelCentroidalMomentumTpl<Scalar>::calc(
const boost::shared_ptr<CostDataAbstract>& data,
53 const Eigen::Ref<const VectorXs>&,
54 const Eigen::Ref<const VectorXs>&) {
56 CostDataCentroidalMomentumTpl<Scalar>* d =
static_cast<CostDataCentroidalMomentumTpl<Scalar>*
>(data.get());
57 data->r = d->pinocchio->hg.toVector() - href_;
59 activation_->calc(data->activation, data->r);
60 data->cost = data->activation->a_value;
63 template <
typename Scalar>
64 void CostModelCentroidalMomentumTpl<Scalar>::calcDiff(
const boost::shared_ptr<CostDataAbstract>& data,
65 const Eigen::Ref<const VectorXs>&,
66 const Eigen::Ref<const VectorXs>&) {
67 CostDataCentroidalMomentumTpl<Scalar>* d =
static_cast<CostDataCentroidalMomentumTpl<Scalar>*
>(data.get());
68 const std::size_t& nv = state_->get_nv();
69 Eigen::Ref<Matrix6xs> Rq = data->Rx.leftCols(nv);
70 Eigen::Ref<Matrix6xs> Rv = data->Rx.rightCols(nv);
72 activation_->calcDiff(data->activation, data->r);
73 pinocchio::getCentroidalDynamicsDerivatives(*state_->get_pinocchio().get(), *d->pinocchio, Rq, d->dhd_dq, d->dhd_dv,
78 for (
int i = 0; i < d->pinocchio->Jcom.cols(); ++i) {
79 data->Rx.template block<3, 1>(3, i) -= d->pinocchio->Jcom.col(i).cross(d->pinocchio->hg.linear());
82 d->Arr_Rx.noalias() = data->activation->Arr * data->Rx;
83 data->Lx.noalias() = data->Rx.transpose() * data->activation->Ar;
84 data->Lxx.noalias() = data->Rx.transpose() * d->Arr_Rx;
87 template <
typename Scalar>
88 boost::shared_ptr<CostDataAbstractTpl<Scalar> > CostModelCentroidalMomentumTpl<Scalar>::createData(
89 DataCollectorAbstract*
const data) {
90 return boost::make_shared<CostDataCentroidalMomentumTpl<Scalar> >(
this, data);
93 template <
typename Scalar>
94 void CostModelCentroidalMomentumTpl<Scalar>::set_referenceImpl(
const std::type_info& ti,
const void* pv) {
95 if (ti ==
typeid(Vector6s)) {
96 href_ = *
static_cast<const Vector6s*
>(pv);
98 throw_pretty(
"Invalid argument: incorrect type (it should be Vector6s)");
102 template <
typename Scalar>
103 void CostModelCentroidalMomentumTpl<Scalar>::get_referenceImpl(
const std::type_info& ti,
void* pv) {
104 if (ti ==
typeid(Vector6s)) {
105 Eigen::Map<Vector6s> ref_map(static_cast<Vector6s*>(pv)->data());
106 ref_map[0] = href_[0];
107 ref_map[1] = href_[1];
108 ref_map[2] = href_[2];
109 ref_map[3] = href_[3];
110 ref_map[4] = href_[4];
111 ref_map[5] = href_[5];
113 throw_pretty(
"Invalid argument: incorrect type (it should be Vector6s)");
117 template <
typename Scalar>
118 const typename MathBaseTpl<Scalar>::Vector6s& CostModelCentroidalMomentumTpl<Scalar>::get_href()
const {
122 template <
typename Scalar>
123 void CostModelCentroidalMomentumTpl<Scalar>::set_href(
const Vector6s& href_in) {
Definition: action-base.hxx:11