10 #define SOBEC_LPF_HPP_ 12 #include <crocoddyl/core/action-base.hpp> 13 #include <crocoddyl/core/activations/quadratic-barrier.hpp> 14 #include <crocoddyl/core/diff-action-base.hpp> 15 #include <crocoddyl/core/fwd.hpp> 16 #include <crocoddyl/multibody/states/multibody.hpp> 17 #include <pinocchio/multibody/model.hpp> 24 template <
typename _Scalar>
27 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
31 typedef ActionModelAbstractTpl<Scalar>
Base;
34 typedef DifferentialActionModelAbstractTpl<Scalar>
41 typedef ActivationModelQuadraticBarrierTpl<Scalar>
46 boost::shared_ptr<DifferentialActionModelAbstract> model,
47 std::vector<std::string> lpf_joint_names = {},
48 const Scalar& time_step = Scalar(1e-3),
49 const bool& with_cost_residual =
true,
const Scalar& fc = 0,
50 const bool& tau_plus_integration =
true,
const int& filter = 0);
53 virtual void calc(
const boost::shared_ptr<ActionDataAbstract>& data,
54 const Eigen::Ref<const VectorXs>&
y,
55 const Eigen::Ref<const VectorXs>& w);
57 virtual void calc(
const boost::shared_ptr<ActionDataAbstract>& data,
58 const Eigen::Ref<const VectorXs>&
y);
60 virtual void calcDiff(
const boost::shared_ptr<ActionDataAbstract>& data,
61 const Eigen::Ref<const VectorXs>&
y,
62 const Eigen::Ref<const VectorXs>& w);
64 virtual void calcDiff(
const boost::shared_ptr<ActionDataAbstract>& data,
65 const Eigen::Ref<const VectorXs>&
y);
67 virtual boost::shared_ptr<ActionDataAbstract> createData();
68 virtual bool checkData(
const boost::shared_ptr<ActionDataAbstract>& data);
70 virtual void quasiStatic(
const boost::shared_ptr<ActionDataAbstract>& data,
71 Eigen::Ref<VectorXs> u,
72 const Eigen::Ref<const VectorXs>&
x,
73 const std::size_t& maxiter = 100,
74 const Scalar& tol = Scalar(1e-9));
76 const boost::shared_ptr<DifferentialActionModelAbstract>& get_differential()
78 const Scalar& get_dt()
const;
79 const Scalar& get_fc()
const;
82 const std::size_t&
get_nw()
const {
return nw_; };
83 const std::size_t&
get_ntau()
const {
return ntau_; };
84 const std::size_t&
get_ny()
const {
return ny_; };
87 return lpf_joint_names_;
91 return lpf_torque_ids_;
94 return non_lpf_torque_ids_;
97 void set_dt(
const Scalar& dt);
98 void set_fc(
const Scalar& fc);
99 void set_alpha(
const Scalar& alpha);
100 void set_differential(
101 boost::shared_ptr<DifferentialActionModelAbstract> model);
104 void set_control_reg_cost(
const Scalar& cost_weight_w_reg,
105 const VectorXs& cost_ref_w_reg);
106 void set_control_lim_cost(
const Scalar& cost_weight_w_lim);
108 void compute_alpha(
const Scalar& fc);
111 using Base::has_control_limits_;
124 boost::shared_ptr<ActivationModelQuadraticBarrier>
128 boost::shared_ptr<DifferentialActionModelAbstract> differential_;
132 bool with_cost_residual_;
135 Scalar tauReg_weight_;
142 Scalar tauLim_weight_;
143 bool tau_plus_integration_;
146 boost::shared_ptr<PinocchioModel> pin_model_;
148 std::vector<std::string>
158 std::vector<int> non_lpf_joint_ids_;
159 std::vector<int> non_lpf_torque_ids_;
164 template <
typename _Scalar>
166 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
170 typedef ActionDataAbstractTpl<Scalar>
Base;
174 typedef DifferentialActionDataAbstractTpl<Scalar>
176 typedef ActivationDataQuadraticBarrierTpl<Scalar>
179 template <
template <
typename Scalar>
class Model>
181 :
Base(model), tau_tmp(model->get_nu()) {
183 differential = model->get_differential()->createData();
184 const std::size_t& ndy = model->get_state()->get_ndx();
185 dy = VectorXs::Zero(ndy);
188 model->activation_model_tauLim_->createData());
192 boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> >
differential;
197 boost::shared_ptr<ActivationDataQuadraticBarrier>
activation;
220 #endif // SOBEC_LPF_HPP_ DifferentialActionDataAbstractTpl< Scalar > DifferentialActionDataAbstract
Definition: action.hpp:175
StateLPFTpl< Scalar > StateLPF
Definition: action.hpp:38
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: action.hpp:29
virtual ~IntegratedActionDataLPFTpl()
Definition: action.hpp:190
IntegratedActionDataLPFTpl< Scalar > Data
Definition: action.hpp:32
IntegratedActionDataLPFTpl(Model< Scalar > *const model)
Definition: action.hpp:180
ActivationModelQuadraticBarrierTpl< Scalar > ActivationModelQuadraticBarrier
Definition: action.hpp:42
MathBase::VectorXs VectorXs
Definition: action.hpp:171
std::size_t nw_
< Neutral state
Definition: action.hpp:118
boost::shared_ptr< ActivationModelQuadraticBarrier > activation_model_tauLim_
< Model of the state
Definition: action.hpp:125
std::size_t ny_
Augmented state dimension : nq+nv+ntau.
Definition: action.hpp:120
VectorXs tau_tmp
Definition: action.hpp:201
pinocchio::ModelTpl< Scalar > PinocchioModel
Definition: action.hpp:40
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: action.hpp:168
StateMultibodyTpl< Scalar > StateMultibody
Definition: action.hpp:39
ActivationDataQuadraticBarrierTpl< Scalar > ActivationDataQuadraticBarrier
Definition: action.hpp:177
VectorXs dy
Definition: action.hpp:193
const std::vector< int > & get_lpf_torque_ids() const
Definition: action.hpp:90
boost::shared_ptr< ActivationDataQuadraticBarrier > activation
Definition: action.hpp:197
MathBase::MatrixXs MatrixXs
Definition: action.hpp:37
pinocchio::DataTpl< Scalar > PinocchioData
Definition: action.hpp:173
Definition: activation-quad-ref.hpp:19
const std::size_t & get_ny() const
Definition: action.hpp:84
ActivationBoundsTpl< Scalar > ActivationBounds
Definition: action.hpp:43
MathBaseTpl< Scalar > MathBase
Definition: action.hpp:30
MathBaseTpl< Scalar > MathBase
Definition: action.hpp:169
boost::shared_ptr< DifferentialActionDataAbstractTpl< Scalar > > differential
Definition: action.hpp:192
Definition: action.hpp:165
std::size_t ntau_
Filtered torque dimension ("lpf" dimension)
Definition: action.hpp:119
const std::size_t & get_ntau() const
Definition: action.hpp:83
DifferentialActionModelAbstractTpl< Scalar > DifferentialActionModelAbstract
Definition: action.hpp:35
MathBase::MatrixXs MatrixXs
Definition: action.hpp:172
Definition: action.hpp:25
ActionDataAbstractTpl< Scalar > ActionDataAbstract
Definition: action.hpp:33
const std::size_t & get_nw() const
Definition: action.hpp:82
ActionDataAbstractTpl< Scalar > Base
Definition: action.hpp:170
const std::vector< int > & get_non_lpf_torque_ids() const
Definition: action.hpp:93
const Scalar & get_alpha() const
Definition: action.hpp:80
ActionModelAbstractTpl< Scalar > Base
Definition: action.hpp:31
const std::vector< std::string > & get_lpf_joint_names() const
Definition: action.hpp:86
MathBase::VectorXs VectorXs
Definition: action.hpp:36