action.hpp
Go to the documentation of this file.
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef SOBEC_LPF_HPP_
10 #define SOBEC_LPF_HPP_
11 
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>
18 
19 #include "state.hpp"
20 
21 namespace sobec {
22 using namespace crocoddyl;
23 
24 template <typename _Scalar>
25 class IntegratedActionModelLPFTpl : public ActionModelAbstractTpl<_Scalar> {
26  public:
27  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
28 
29  typedef _Scalar Scalar;
30  typedef MathBaseTpl<Scalar> MathBase;
31  typedef ActionModelAbstractTpl<Scalar> Base;
33  typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract;
34  typedef DifferentialActionModelAbstractTpl<Scalar>
36  typedef typename MathBase::VectorXs VectorXs;
37  typedef typename MathBase::MatrixXs MatrixXs;
39  typedef StateMultibodyTpl<Scalar> StateMultibody;
40  typedef pinocchio::ModelTpl<Scalar> PinocchioModel;
41  typedef ActivationModelQuadraticBarrierTpl<Scalar>
43  typedef ActivationBoundsTpl<Scalar> ActivationBounds;
44 
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);
51  virtual ~IntegratedActionModelLPFTpl();
52 
53  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data,
54  const Eigen::Ref<const VectorXs>& y,
55  const Eigen::Ref<const VectorXs>& w);
56 
57  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data,
58  const Eigen::Ref<const VectorXs>& y);
59 
60  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data,
61  const Eigen::Ref<const VectorXs>& y,
62  const Eigen::Ref<const VectorXs>& w);
63 
64  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data,
65  const Eigen::Ref<const VectorXs>& y);
66 
67  virtual boost::shared_ptr<ActionDataAbstract> createData();
68  virtual bool checkData(const boost::shared_ptr<ActionDataAbstract>& data);
69 
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));
75 
76  const boost::shared_ptr<DifferentialActionModelAbstract>& get_differential()
77  const;
78  const Scalar& get_dt() const;
79  const Scalar& get_fc() const;
80  const Scalar& get_alpha() const { return alpha_; };
81 
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_; };
85 
86  const std::vector<std::string>& get_lpf_joint_names() const {
87  return lpf_joint_names_;
88  };
89 
90  const std::vector<int>& get_lpf_torque_ids() const {
91  return lpf_torque_ids_;
92  };
93  const std::vector<int>& get_non_lpf_torque_ids() const {
94  return non_lpf_torque_ids_;
95  };
96 
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);
102 
103  // hard-coded costs
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);
107 
108  void compute_alpha(const Scalar& fc);
109 
110  protected:
111  using Base::has_control_limits_;
113  using Base::nr_;
114  using Base::nu_;
115  using Base::u_lb_;
116  using Base::u_ub_;
117  using Base::unone_;
118  std::size_t nw_;
119  std::size_t ntau_;
120  std::size_t ny_;
121  using Base::state_;
122 
123  public:
124  boost::shared_ptr<ActivationModelQuadraticBarrier>
126 
127  private:
128  boost::shared_ptr<DifferentialActionModelAbstract> differential_;
129  Scalar time_step_;
130  Scalar time_step2_;
131  Scalar alpha_;
132  bool with_cost_residual_;
133  Scalar fc_;
134  // bool enable_integration_;
135  Scalar tauReg_weight_;
136  VectorXs tauReg_reference_;
138  VectorXs tauReg_residual_,
139  tauLim_residual_;
140  // bool gravity_reg_; //!< Use gravity torque for
141  // unfiltered torque reg, or user-provided reference?
142  Scalar tauLim_weight_;
143  bool tau_plus_integration_;
145  int filter_;
146  boost::shared_ptr<PinocchioModel> pin_model_;
147  bool is_terminal_;
149  std::vector<std::string>
150  lpf_joint_names_;
151  std::vector<int>
152  lpf_joint_ids_;
153  std::vector<int>
154  lpf_torque_ids_;
155 
156  // std::vector<std::string> non_lpf_joint_names_; //!< Vector of joint
157  // names that are NOT low-pass filtered
158  std::vector<int> non_lpf_joint_ids_;
160  std::vector<int> non_lpf_torque_ids_;
162 };
163 
164 template <typename _Scalar>
165 struct IntegratedActionDataLPFTpl : public ActionDataAbstractTpl<_Scalar> {
166  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
167 
168  typedef _Scalar Scalar;
169  typedef MathBaseTpl<Scalar> MathBase;
170  typedef ActionDataAbstractTpl<Scalar> Base;
171  typedef typename MathBase::VectorXs VectorXs;
172  typedef typename MathBase::MatrixXs MatrixXs;
173  typedef pinocchio::DataTpl<Scalar> PinocchioData;
174  typedef DifferentialActionDataAbstractTpl<Scalar>
176  typedef ActivationDataQuadraticBarrierTpl<Scalar>
178 
179  template <template <typename Scalar> class Model>
180  explicit IntegratedActionDataLPFTpl(Model<Scalar>* const model)
181  : Base(model), tau_tmp(model->get_nu()) {
182  tau_tmp.setZero();
183  differential = model->get_differential()->createData();
184  const std::size_t& ndy = model->get_state()->get_ndx();
185  dy = VectorXs::Zero(ndy);
186  // for wlim cost
187  activation = boost::static_pointer_cast<ActivationDataQuadraticBarrier>(
188  model->activation_model_tauLim_->createData());
189  }
191 
192  boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> > differential;
194 
195  // PinocchioData pinocchio; // for reg
196  // cost
197  boost::shared_ptr<ActivationDataQuadraticBarrier> activation; // for lim cost
198 
199  using Base::cost;
200  using Base::r;
202  // use refs to "alias" base class member names
203  VectorXs& ynext = Base::xnext;
204  MatrixXs& Fy = Base::Fx;
205  MatrixXs& Fw = Base::Fu;
206  VectorXs& Ly = Base::Lx;
207  VectorXs& Lw = Base::Lu;
208  MatrixXs& Lyy = Base::Lxx;
209  MatrixXs& Lyw = Base::Lxu;
210  MatrixXs& Lww = Base::Luu;
211 };
212 
213 } // namespace sobec
214 
215 /* --- Details -------------------------------------------------------------- */
216 /* --- Details -------------------------------------------------------------- */
217 /* --- Details -------------------------------------------------------------- */
219 
220 #endif // SOBEC_LPF_HPP_
Definition: action.hpp:25
StateMultibodyTpl< Scalar > StateMultibody
Definition: action.hpp:39
IntegratedActionDataLPFTpl< Scalar > Data
Definition: action.hpp:32
DifferentialActionModelAbstractTpl< Scalar > DifferentialActionModelAbstract
Definition: action.hpp:35
ActivationBoundsTpl< Scalar > ActivationBounds
Definition: action.hpp:43
const std::vector< std::string > & get_lpf_joint_names() const
Definition: action.hpp:86
MathBase::MatrixXs MatrixXs
Definition: action.hpp:37
const std::size_t & get_nw() const
Definition: action.hpp:82
pinocchio::ModelTpl< Scalar > PinocchioModel
Definition: action.hpp:40
const std::vector< int > & get_non_lpf_torque_ids() const
Definition: action.hpp:93
ActionDataAbstractTpl< Scalar > ActionDataAbstract
Definition: action.hpp:33
const std::size_t & get_ny() const
Definition: action.hpp:84
const std::size_t & get_ntau() const
Definition: action.hpp:83
const std::vector< int > & get_lpf_torque_ids() const
Definition: action.hpp:90
std::size_t nw_
< Neutral state
Definition: action.hpp:118
const Scalar & get_alpha() const
Definition: action.hpp:80
StateLPFTpl< Scalar > StateLPF
Definition: action.hpp:38
ActivationModelQuadraticBarrierTpl< Scalar > ActivationModelQuadraticBarrier
Definition: action.hpp:42
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: action.hpp:29
std::size_t ntau_
Filtered torque dimension ("lpf" dimension)
Definition: action.hpp:119
boost::shared_ptr< ActivationModelQuadraticBarrier > activation_model_tauLim_
< Model of the state
Definition: action.hpp:125
MathBaseTpl< Scalar > MathBase
Definition: action.hpp:30
ActionModelAbstractTpl< Scalar > Base
Definition: action.hpp:31
MathBase::VectorXs VectorXs
Definition: action.hpp:36
std::size_t ny_
Augmented state dimension : nq+nv+ntau.
Definition: action.hpp:120
Definition: state.hpp:19
@ y
Definition: contact1d.hpp:26
@ x
Definition: contact1d.hpp:26
Definition: activation-quad-ref.hpp:19
Definition: action.hpp:165
VectorXs tau_tmp
Definition: action.hpp:201
boost::shared_ptr< DifferentialActionDataAbstractTpl< Scalar > > differential
Definition: action.hpp:192
ActivationDataQuadraticBarrierTpl< Scalar > ActivationDataQuadraticBarrier
Definition: action.hpp:177
pinocchio::DataTpl< Scalar > PinocchioData
Definition: action.hpp:173
VectorXs dy
Definition: action.hpp:193
IntegratedActionDataLPFTpl(Model< Scalar > *const model)
Definition: action.hpp:180
MathBase::MatrixXs MatrixXs
Definition: action.hpp:172
DifferentialActionDataAbstractTpl< Scalar > DifferentialActionDataAbstract
Definition: action.hpp:175
ActionDataAbstractTpl< Scalar > Base
Definition: action.hpp:170
MathBase::VectorXs VectorXs
Definition: action.hpp:171
virtual ~IntegratedActionDataLPFTpl()
Definition: action.hpp:190
MathBaseTpl< Scalar > MathBase
Definition: action.hpp:169
boost::shared_ptr< ActivationDataQuadraticBarrier > activation
Definition: action.hpp:197
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: action.hpp:168