9 #include "crocoddyl/core/utils/exception.hpp" 10 #include "crocoddyl/multibody/costs/control.hpp" 13 template <
typename Scalar>
14 CostModelControlTpl<Scalar>::CostModelControlTpl(boost::shared_ptr<StateMultibody> state,
15 boost::shared_ptr<ActivationModelAbstract> activation,
17 : Base(state, activation, static_cast<std::size_t>(uref.size())), uref_(uref) {
18 if (activation_->get_nr() != nu_) {
19 throw_pretty(
"Invalid argument: " 20 <<
"nr is equals to " + std::to_string(nu_));
24 template <
typename Scalar>
25 CostModelControlTpl<Scalar>::CostModelControlTpl(boost::shared_ptr<StateMultibody> state,
26 boost::shared_ptr<ActivationModelAbstract> activation)
27 : Base(state, activation), uref_(VectorXs::Zero(activation->get_nr())) {}
29 template <
typename Scalar>
30 CostModelControlTpl<Scalar>::CostModelControlTpl(boost::shared_ptr<StateMultibody> state,
31 boost::shared_ptr<ActivationModelAbstract> activation,
32 const std::size_t& nu)
33 : Base(state, activation, nu), uref_(VectorXs::Zero(nu)) {
34 if (activation_->get_nr() != nu_) {
35 throw_pretty(
"Invalid argument: " 36 <<
"nr is equals to " + std::to_string(nu_));
40 template <
typename Scalar>
41 CostModelControlTpl<Scalar>::CostModelControlTpl(boost::shared_ptr<StateMultibody> state,
const VectorXs& uref)
42 : Base(state, static_cast<std::size_t>(uref.size()), static_cast<std::size_t>(uref.size())), uref_(uref) {}
44 template <
typename Scalar>
45 CostModelControlTpl<Scalar>::CostModelControlTpl(boost::shared_ptr<StateMultibody> state)
46 : Base(state, state->get_nv()), uref_(VectorXs::Zero(state->get_nv())) {}
48 template <
typename Scalar>
49 CostModelControlTpl<Scalar>::CostModelControlTpl(boost::shared_ptr<StateMultibody> state,
const std::size_t& nu)
50 : Base(state, nu, nu), uref_(VectorXs::Zero(nu)) {}
52 template <
typename Scalar>
53 CostModelControlTpl<Scalar>::~CostModelControlTpl() {}
55 template <
typename Scalar>
56 void CostModelControlTpl<Scalar>::calc(
const boost::shared_ptr<CostDataAbstract>& data,
57 const Eigen::Ref<const VectorXs>&,
const Eigen::Ref<const VectorXs>& u) {
59 throw_pretty(
"Invalid argument: " 60 <<
"it seems to be an autonomous system, if so, don't add this cost function");
62 if (static_cast<std::size_t>(u.size()) != nu_) {
63 throw_pretty(
"Invalid argument: " 64 <<
"u has wrong dimension (it should be " + std::to_string(nu_) +
")");
68 activation_->calc(data->activation, data->r);
69 data->cost = data->activation->a_value;
72 template <
typename Scalar>
73 void CostModelControlTpl<Scalar>::calcDiff(
const boost::shared_ptr<CostDataAbstract>& data,
74 const Eigen::Ref<const VectorXs>&,
const Eigen::Ref<const VectorXs>& u) {
76 throw_pretty(
"Invalid argument: " 77 <<
"it seems to be an autonomous system, if so, don't add this cost function");
79 if (static_cast<std::size_t>(u.size()) != nu_) {
80 throw_pretty(
"Invalid argument: " 81 <<
"u has wrong dimension (it should be " + std::to_string(nu_) +
")");
84 activation_->calcDiff(data->activation, data->r);
85 data->Lu = data->activation->Ar;
86 data->Luu.diagonal() = data->activation->Arr.diagonal();
89 template <
typename Scalar>
90 void CostModelControlTpl<Scalar>::set_referenceImpl(
const std::type_info& ti,
const void* pv) {
91 if (ti ==
typeid(VectorXs)) {
92 if (static_cast<std::size_t>(static_cast<const VectorXs*>(pv)->size()) != nu_) {
93 throw_pretty(
"Invalid argument: " 94 <<
"reference has wrong dimension (it should be " + std::to_string(nu_) +
")");
96 uref_ = *
static_cast<const VectorXs*
>(pv);
98 throw_pretty(
"Invalid argument: incorrect type (it should be VectorXs)");
102 template <
typename Scalar>
103 void CostModelControlTpl<Scalar>::get_referenceImpl(
const std::type_info& ti,
void* pv) {
104 if (ti ==
typeid(VectorXs)) {
105 Eigen::VectorXd& tmp = *
static_cast<VectorXs*
>(pv);
107 Eigen::Map<VectorXs> ref_map(static_cast<VectorXs*>(pv)->data(), nu_);
108 for (std::size_t i = 0; i < nu_; ++i) {
109 ref_map[i] = uref_[i];
112 throw_pretty(
"Invalid argument: incorrect type (it should be VectorXs)");
116 template <
typename Scalar>
117 const typename MathBaseTpl<Scalar>::VectorXs& CostModelControlTpl<Scalar>::get_uref()
const {
121 template <
typename Scalar>
122 void CostModelControlTpl<Scalar>::set_uref(
const VectorXs& uref_in) {
123 if (static_cast<std::size_t>(uref_in.size()) != nu_) {
124 throw_pretty(
"Invalid argument: " 125 <<
"uref has wrong dimension (it should be " + std::to_string(nu_) +
")");
Definition: action-base.hxx:11