9 #include "crocoddyl/core/utils/exception.hpp" 10 #include "crocoddyl/multibody/costs/com-position.hpp" 14 template <
typename Scalar>
15 CostModelCoMPositionTpl<Scalar>::CostModelCoMPositionTpl(boost::shared_ptr<StateMultibody> state,
16 boost::shared_ptr<ActivationModelAbstract> activation,
17 const Vector3s& cref,
const std::size_t& nu)
18 : Base(state, activation, nu), cref_(cref) {
19 if (activation_->get_nr() != 3) {
20 throw_pretty(
"Invalid argument: " 21 <<
"nr is equals to 3");
25 template <
typename Scalar>
26 CostModelCoMPositionTpl<Scalar>::CostModelCoMPositionTpl(boost::shared_ptr<StateMultibody> state,
27 boost::shared_ptr<ActivationModelAbstract> activation,
29 : Base(state, activation), cref_(cref) {
30 if (activation_->get_nr() != 3) {
31 throw_pretty(
"Invalid argument: " 32 <<
"nr is equals to 3");
36 template <
typename Scalar>
37 CostModelCoMPositionTpl<Scalar>::CostModelCoMPositionTpl(boost::shared_ptr<StateMultibody> state,
const Vector3s& cref,
38 const std::size_t& nu)
39 : Base(state, 3, nu), cref_(cref) {}
41 template <
typename Scalar>
42 CostModelCoMPositionTpl<Scalar>::CostModelCoMPositionTpl(boost::shared_ptr<StateMultibody> state,
const Vector3s& cref)
43 : Base(state, 3), cref_(cref) {}
45 template <
typename Scalar>
46 CostModelCoMPositionTpl<Scalar>::~CostModelCoMPositionTpl() {}
48 template <
typename Scalar>
49 void CostModelCoMPositionTpl<Scalar>::calc(
const boost::shared_ptr<CostDataAbstract>& data,
50 const Eigen::Ref<const VectorXs>&,
const Eigen::Ref<const VectorXs>&) {
52 CostDataCoMPositionTpl<Scalar>* d =
static_cast<CostDataCoMPositionTpl<Scalar>*
>(data.get());
53 data->r = d->pinocchio->com[0] - cref_;
56 activation_->calc(data->activation, data->r);
57 data->cost = data->activation->a_value;
60 template <
typename Scalar>
61 void CostModelCoMPositionTpl<Scalar>::calcDiff(
const boost::shared_ptr<CostDataAbstract>& data,
62 const Eigen::Ref<const VectorXs>&,
const Eigen::Ref<const VectorXs>&) {
63 CostDataCoMPositionTpl<Scalar>* d =
static_cast<CostDataCoMPositionTpl<Scalar>*
>(data.get());
66 const std::size_t& nv = state_->get_nv();
67 activation_->calcDiff(data->activation, data->r);
68 data->Rx.leftCols(nv) = d->pinocchio->Jcom;
69 data->Lx.head(nv).noalias() = d->pinocchio->Jcom.transpose() * data->activation->Ar;
70 d->Arr_Jcom.noalias() = data->activation->Arr * d->pinocchio->Jcom;
71 data->Lxx.topLeftCorner(nv, nv).noalias() = d->pinocchio->Jcom.transpose() * d->Arr_Jcom;
74 template <
typename Scalar>
75 boost::shared_ptr<CostDataAbstractTpl<Scalar> > CostModelCoMPositionTpl<Scalar>::createData(
76 DataCollectorAbstract*
const data) {
77 return boost::make_shared<CostDataCoMPositionTpl<Scalar> >(
this, data);
80 template <
typename Scalar>
81 void CostModelCoMPositionTpl<Scalar>::set_referenceImpl(
const std::type_info& ti,
const void* pv) {
82 if (ti ==
typeid(Vector3s)) {
83 cref_ = *
static_cast<const Vector3s*
>(pv);
85 throw_pretty(
"Invalid argument: incorrect type (it should be Vector3s)");
89 template <
typename Scalar>
90 void CostModelCoMPositionTpl<Scalar>::get_referenceImpl(
const std::type_info& ti,
void* pv) {
91 if (ti ==
typeid(Vector3s)) {
92 Eigen::Map<Vector3s> ref_map(static_cast<Vector3s*>(pv)->data());
93 ref_map[0] = cref_[0];
94 ref_map[1] = cref_[1];
95 ref_map[2] = cref_[2];
97 throw_pretty(
"Invalid argument: incorrect type (it should be Vector3s)");
101 template <
typename Scalar>
102 const typename MathBaseTpl<Scalar>::Vector3s& CostModelCoMPositionTpl<Scalar>::get_cref()
const {
106 template <
typename Scalar>
107 void CostModelCoMPositionTpl<Scalar>::set_cref(
const Vector3s& cref_in) {
Definition: action-base.hxx:11