9 #ifndef CROCODDYL_CORE_ACTIVATIONS_WEIGHTED_QUADRATIC_HPP_
10 #define CROCODDYL_CORE_ACTIVATIONS_WEIGHTED_QUADRATIC_HPP_
14 #include "crocoddyl/core/fwd.hpp"
15 #include "crocoddyl/core/utils/exception.hpp"
16 #include "crocoddyl/core/activation-base.hpp"
20 template <
typename _Scalar>
23 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
25 typedef _Scalar Scalar;
30 typedef typename MathBase::VectorXs VectorXs;
31 typedef typename MathBase::MatrixXs MatrixXs;
34 :
Base(weights.size()), weights_(weights), new_weights_(
false){};
37 virtual void calc(
const boost::shared_ptr<ActivationDataAbstract>& data,
const Eigen::Ref<const VectorXs>& r) {
38 if (
static_cast<std::size_t
>(r.size()) != nr_) {
39 throw_pretty(
"Invalid argument: "
40 <<
"r has wrong dimension (it should be " + std::to_string(nr_) +
")");
42 boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
44 d->Wr = weights_.cwiseProduct(r);
45 data->a_value = Scalar(0.5) * r.dot(d->Wr);
48 virtual void calcDiff(
const boost::shared_ptr<ActivationDataAbstract>& data,
const Eigen::Ref<const VectorXs>& r) {
49 if (
static_cast<std::size_t
>(r.size()) != nr_) {
50 throw_pretty(
"Invalid argument: "
51 <<
"r has wrong dimension (it should be " + std::to_string(nr_) +
")");
54 boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data);
57 data->Arr.diagonal() = weights_;
62 assert_pretty(MatrixXs(data->Arr).isApprox(Arr_),
"Arr has wrong value");
66 virtual boost::shared_ptr<ActivationDataAbstract> createData() {
67 boost::shared_ptr<Data> data = boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(),
this);
68 data->Arr.diagonal() = weights_;
77 const VectorXs& get_weights()
const {
return weights_; };
78 void set_weights(
const VectorXs& weights) {
79 if (weights.size() != weights_.size()) {
80 throw_pretty(
"Invalid argument: "
81 <<
"weight vector has wrong dimension (it should be " + std::to_string(weights_.size()) +
")");
93 virtual void print(std::ostream& os)
const { os <<
"ActivationModelQuad {nr=" << nr_ <<
"}"; }
107 template <
typename _Scalar>
109 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
111 typedef _Scalar Scalar;
113 typedef typename MathBase::VectorXs VectorXs;
116 template <
typename Activation>
118 :
Base(activation), Wr(VectorXs::Zero(activation->get_nr())) {}
125 #endif // CROCODDYL_CORE_ACTIVATIONS_WEIGHTED_QUADRATIC_HPP_