9 #include "crocoddyl/core/utils/exception.hpp" 10 #include "crocoddyl/multibody/costs/state.hpp" 14 template <
typename Scalar>
15 CostModelStateTpl<Scalar>::CostModelStateTpl(boost::shared_ptr<StateMultibody> state,
16 boost::shared_ptr<ActivationModelAbstract> activation,
17 const VectorXs& xref,
const std::size_t& nu)
18 : Base(state, activation, nu), xref_(xref) {
19 if (static_cast<std::size_t>(xref_.size()) != state_->get_nx()) {
20 throw_pretty(
"Invalid argument: " 21 <<
"xref has wrong dimension (it should be " + std::to_string(state_->get_nx()) +
")");
23 if (activation_->get_nr() != state_->get_ndx()) {
24 throw_pretty(
"Invalid argument: " 25 <<
"nr is equals to " + std::to_string(state_->get_ndx()));
29 template <
typename Scalar>
30 CostModelStateTpl<Scalar>::CostModelStateTpl(boost::shared_ptr<StateMultibody> state,
31 boost::shared_ptr<ActivationModelAbstract> activation,
33 : Base(state, activation), xref_(xref) {
34 if (static_cast<std::size_t>(xref_.size()) != state_->get_nx()) {
35 throw_pretty(
"Invalid argument: " 36 <<
"xref has wrong dimension (it should be " + std::to_string(state_->get_nx()) +
")");
38 if (activation_->get_nr() != state_->get_ndx()) {
39 throw_pretty(
"Invalid argument: " 40 <<
"nr is equals to " + std::to_string(state_->get_ndx()));
44 template <
typename Scalar>
45 CostModelStateTpl<Scalar>::CostModelStateTpl(boost::shared_ptr<StateMultibody> state,
const VectorXs& xref,
46 const std::size_t& nu)
47 : Base(state, state->get_ndx(), nu), xref_(xref) {
48 if (static_cast<std::size_t>(xref_.size()) != state_->get_nx()) {
49 throw_pretty(
"Invalid argument: " 50 <<
"xref has wrong dimension (it should be " + std::to_string(state_->get_nx()) +
")");
52 if (activation_->get_nr() != state_->get_ndx()) {
53 throw_pretty(
"Invalid argument: " 54 <<
"nr is equals to " + std::to_string(state_->get_ndx()));
58 template <
typename Scalar>
59 CostModelStateTpl<Scalar>::CostModelStateTpl(boost::shared_ptr<StateMultibody> state,
const VectorXs& xref)
60 : Base(state, state->get_ndx()), xref_(xref) {
61 if (static_cast<std::size_t>(xref_.size()) != state_->get_nx()) {
62 throw_pretty(
"Invalid argument: " 63 <<
"xref has wrong dimension (it should be " + std::to_string(state_->get_nx()) +
")");
65 if (activation_->get_nr() != state_->get_ndx()) {
66 throw_pretty(
"Invalid argument: " 67 <<
"nr is equals to " + std::to_string(state_->get_ndx()));
71 template <
typename Scalar>
72 CostModelStateTpl<Scalar>::CostModelStateTpl(boost::shared_ptr<StateMultibody> state,
73 boost::shared_ptr<ActivationModelAbstract> activation,
74 const std::size_t& nu)
75 : Base(state, activation, nu), xref_(state->zero()) {
76 if (static_cast<std::size_t>(xref_.size()) != state_->get_nx()) {
77 throw_pretty(
"Invalid argument: " 78 <<
"xref has wrong dimension (it should be " + std::to_string(state_->get_nx()) +
")");
80 if (activation_->get_nr() != state_->get_ndx()) {
81 throw_pretty(
"Invalid argument: " 82 <<
"nr is equals to " + std::to_string(state_->get_ndx()));
86 template <
typename Scalar>
87 CostModelStateTpl<Scalar>::CostModelStateTpl(boost::shared_ptr<StateMultibody> state,
const std::size_t& nu)
88 : Base(state, state->get_ndx(), nu), xref_(state->zero()) {
89 if (static_cast<std::size_t>(xref_.size()) != state_->get_nx()) {
90 throw_pretty(
"Invalid argument: " 91 <<
"xref has wrong dimension (it should be " + std::to_string(state_->get_nx()) +
")");
93 if (activation_->get_nr() != state_->get_ndx()) {
94 throw_pretty(
"Invalid argument: " 95 <<
"nr is equals to " + std::to_string(state_->get_ndx()));
99 template <
typename Scalar>
100 CostModelStateTpl<Scalar>::CostModelStateTpl(boost::shared_ptr<StateMultibody> state,
101 boost::shared_ptr<ActivationModelAbstract> activation)
102 : Base(state, activation), xref_(state->zero()) {
103 if (static_cast<std::size_t>(xref_.size()) != state_->get_nx()) {
104 throw_pretty(
"Invalid argument: " 105 <<
"xref has wrong dimension (it should be " + std::to_string(state_->get_nx()) +
")");
107 if (activation_->get_nr() != state_->get_ndx()) {
108 throw_pretty(
"Invalid argument: " 109 <<
"nr is equals to " + std::to_string(state_->get_ndx()));
113 template <
typename Scalar>
114 CostModelStateTpl<Scalar>::CostModelStateTpl(boost::shared_ptr<StateMultibody> state)
115 : Base(state, state->get_ndx()), xref_(state->zero()) {
116 if (static_cast<std::size_t>(xref_.size()) != state_->get_nx()) {
117 throw_pretty(
"Invalid argument: " 118 <<
"xref has wrong dimension (it should be " + std::to_string(state_->get_nx()) +
")");
120 if (activation_->get_nr() != state_->get_ndx()) {
121 throw_pretty(
"Invalid argument: " 122 <<
"nr is equals to " + std::to_string(state_->get_ndx()));
126 template <
typename Scalar>
127 CostModelStateTpl<Scalar>::~CostModelStateTpl() {}
129 template <
typename Scalar>
130 void CostModelStateTpl<Scalar>::calc(
const boost::shared_ptr<CostDataAbstract>& data,
131 const Eigen::Ref<const VectorXs>& x,
const Eigen::Ref<const VectorXs>&) {
132 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
133 throw_pretty(
"Invalid argument: " 134 <<
"x has wrong dimension (it should be " + std::to_string(state_->get_nx()) +
")");
137 state_->diff(xref_, x, data->r);
138 activation_->calc(data->activation, data->r);
139 data->cost = data->activation->a_value;
142 template <
typename Scalar>
143 void CostModelStateTpl<Scalar>::calcDiff(
const boost::shared_ptr<CostDataAbstract>& data,
144 const Eigen::Ref<const VectorXs>& x,
const Eigen::Ref<const VectorXs>&) {
145 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
146 throw_pretty(
"Invalid argument: " 147 <<
"x has wrong dimension (it should be " + std::to_string(state_->get_nx()) +
")");
150 CostDataStateTpl<Scalar>* d =
static_cast<CostDataStateTpl<Scalar>*
>(data.get());
151 state_->Jdiff(xref_, x, data->Rx, data->Rx, second);
152 activation_->calcDiff(data->activation, data->r);
153 data->Lx.noalias() = data->Rx.transpose() * data->activation->Ar;
154 d->Arr_Rx.noalias() = data->activation->Arr * data->Rx;
155 data->Lxx.noalias() = data->Rx.transpose() * d->Arr_Rx;
158 template <
typename Scalar>
159 boost::shared_ptr<CostDataAbstractTpl<Scalar> > CostModelStateTpl<Scalar>::createData(
160 DataCollectorAbstract*
const data) {
161 return boost::make_shared<CostDataStateTpl<Scalar> >(
this, data);
164 template <
typename Scalar>
165 void CostModelStateTpl<Scalar>::set_referenceImpl(
const std::type_info& ti,
const void* pv) {
166 if (ti ==
typeid(VectorXs)) {
167 if (static_cast<std::size_t>(static_cast<const VectorXs*>(pv)->size()) != state_->get_nx()) {
168 throw_pretty(
"Invalid argument: " 169 <<
"reference has wrong dimension (it should be " + std::to_string(state_->get_nx()) +
")");
171 xref_ = *
static_cast<const VectorXs*
>(pv);
173 throw_pretty(
"Invalid argument: incorrect type (it should be VectorXs)");
177 template <
typename Scalar>
178 void CostModelStateTpl<Scalar>::get_referenceImpl(
const std::type_info& ti,
void* pv) {
179 if (ti ==
typeid(VectorXs)) {
180 Eigen::VectorXd& tmp = *
static_cast<VectorXs*
>(pv);
181 tmp.resize(state_->get_nx());
182 Eigen::Map<VectorXs> ref_map(static_cast<VectorXs*>(pv)->data(), state_->get_nx());
183 for (std::size_t i = 0; i < state_->get_nx(); ++i) {
184 ref_map[i] = xref_[i];
187 throw_pretty(
"Invalid argument: incorrect type (it should be VectorXs)");
191 template <
typename Scalar>
192 const typename MathBaseTpl<Scalar>::VectorXs& CostModelStateTpl<Scalar>::get_xref()
const {
196 template <
typename Scalar>
197 void CostModelStateTpl<Scalar>::set_xref(
const VectorXs& xref_in) {
198 if (static_cast<std::size_t>(xref_in.size()) != state_->get_nx()) {
199 throw_pretty(
"Invalid argument: " 200 <<
"xref has wrong dimension (it should be " + std::to_string(state_->get_nx()) +
")");
Definition: action-base.hxx:11