quadruped_step_period.hpp
Go to the documentation of this file.
1 #ifndef __quadruped_walkgen_quadruped_step_period_hpp__
2 #define __quadruped_walkgen_quadruped_step_period_hpp__
3 #include <stdexcept>
4 
5 #include "crocoddyl/core/action-base.hpp"
6 #include "crocoddyl/core/fwd.hpp"
7 #include "crocoddyl/core/states/euclidean.hpp"
8 #include "crocoddyl/core/utils/timer.hpp"
9 #include "crocoddyl/multibody/friction-cone.hpp"
10 
11 namespace quadruped_walkgen {
12 template <typename _Scalar>
14  : public crocoddyl::ActionModelAbstractTpl<_Scalar> {
15  public:
16  typedef _Scalar Scalar;
17  typedef crocoddyl::ActionDataAbstractTpl<Scalar> ActionDataAbstract;
18  typedef crocoddyl::ActionModelAbstractTpl<Scalar> Base;
19  typedef crocoddyl::MathBaseTpl<Scalar> MathBase;
20 
23 
24  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data,
25  const Eigen::Ref<const typename MathBase::VectorXs>& x,
26  const Eigen::Ref<const typename MathBase::VectorXs>& u);
27  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data,
28  const Eigen::Ref<const typename MathBase::VectorXs>& x,
29  const Eigen::Ref<const typename MathBase::VectorXs>& u);
30  virtual boost::shared_ptr<ActionDataAbstract> createData();
31 
32  // Get and Set weights vectors : state , force & friction cone :
33  const typename Eigen::Matrix<Scalar, 12, 1>& get_state_weights() const;
34  void set_state_weights(const typename MathBase::VectorXs& weights);
35 
36  const typename Eigen::Matrix<Scalar, 4, 1>& get_step_weights() const;
37  void set_step_weights(const typename MathBase::VectorXs& weights);
38 
39  const typename Eigen::Matrix<Scalar, 8, 1>& get_shoulder_weights() const;
40  void set_shoulder_weights(const typename MathBase::VectorXs& weights);
41 
42  const typename Eigen::Matrix<Scalar, 8, 1>& get_shoulder_position() const;
43  void set_shoulder_position(const typename MathBase::VectorXs& weights);
44 
45  // Update the model depending if the foot in contact with the ground
46  // or the new lever arms
47  void update_model(const Eigen::Ref<const typename MathBase::MatrixXs>& l_feet,
48  const Eigen::Ref<const typename MathBase::MatrixXs>& xref,
49  const Eigen::Ref<const typename MathBase::MatrixXs>& S);
50 
51  const bool& get_symmetry_term() const;
52  void set_symmetry_term(const bool& sym_term);
53 
54  const bool& get_centrifugal_term() const;
55  void set_centrifugal_term(const bool& cent_term);
56 
57  const Scalar& get_T_gait() const;
58  void set_T_gait(const Scalar& T_gait_);
59 
60  const Scalar& get_dt_ref() const;
61  void set_dt_ref(const Scalar& dt);
62 
63  const Scalar& get_dt_min() const;
64  void set_dt_min(const Scalar& dt);
65 
66  const Scalar& get_dt_max() const;
67  void set_dt_max(const Scalar& dt);
68 
69  const Scalar& get_dt_weight() const;
70  void set_dt_weight(const Scalar& weight_);
71 
72  const Scalar& get_dt_bound_weight() const;
73  void set_dt_bound_weight(const Scalar& weight_);
74 
75  const Scalar& get_nb_nodes() const;
76  void set_nb_nodes(const Scalar& nodes_);
77 
78  const Scalar& get_vlim() const;
79  void set_vlim(const Scalar& vlim_);
80 
81  const Scalar& get_speed_weight() const;
82  void set_speed_weight(const Scalar& weight_);
83 
84  protected:
85  using Base::has_control_limits_;
86  using Base::nr_;
88  using Base::nu_;
89  using Base::state_;
90  using Base::u_lb_;
91  using Base::u_ub_;
92  using Base::unone_;
93 
94  private:
95  Scalar T_gait;
96  Scalar dt_weight_;
97  Scalar dt_bound_weight;
98  Scalar speed_weight;
99  bool centrifugal_term;
100  bool symmetry_term;
101 
102  Scalar nb_nodes;
103  Scalar vlim;
104  Scalar beta_lim;
105 
106  typename Eigen::Matrix<Scalar, 12, 1> state_weights_;
107  typename Eigen::Matrix<Scalar, 4, 1> step_weights_;
108  typename Eigen::Matrix<Scalar, 8, 1> shoulder_weights_;
109  typename MathBase::Matrix3s R_tmp;
110 
111  typename Eigen::Matrix<Scalar, 8, 4> B;
112 
113  typename MathBase::MatrixXs xref_;
114 
115  typename Eigen::Matrix<Scalar, 8, 1> pshoulder_;
116  typename Eigen::Matrix<Scalar, 2, 4> pshoulder_0;
117  typename Eigen::Matrix<Scalar, 2, 4> pshoulder_tmp;
118 
119  typename Eigen::Matrix<Scalar, 3, 1> pcentrifugal_tmp;
120  typename Eigen::Matrix<Scalar, 3, 1> pcentrifugal_tmp_1;
121  typename Eigen::Matrix<Scalar, 3, 1> pcentrifugal_tmp_2;
122 
123  typename Eigen::Matrix<Scalar, 1, 1> dt_ref_;
124  typename Eigen::Matrix<Scalar, 1, 1> dt_min_;
125  typename Eigen::Matrix<Scalar, 1, 1> dt_max_;
126 
127  typename Eigen::Matrix<Scalar, 4, 1> rub_max_;
128  typename Eigen::Matrix<Scalar, 4, 1> rub_max_bool;
129 };
130 
131 template <typename _Scalar>
133  : public crocoddyl::ActionDataAbstractTpl<_Scalar> {
134  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
135 
136  typedef _Scalar Scalar;
137  typedef crocoddyl::MathBaseTpl<Scalar> MathBase;
138  typedef crocoddyl::ActionDataAbstractTpl<Scalar> Base;
139  using Base::cost;
140  using Base::Fu;
141  using Base::Fx;
142  using Base::Lu;
143  using Base::Luu;
144  using Base::Lx;
145  using Base::Lxu;
146  using Base::Lxx;
147  using Base::r;
148  using Base::xnext;
149 
150  template <template <typename Scalar> class Model>
151  explicit ActionDataQuadrupedStepPeriodTpl(Model<Scalar>* const model)
152  : crocoddyl::ActionDataAbstractTpl<Scalar>(model) {}
153 };
154 
155 /* --- Details -------------------------------------------------------------- */
156 /* --- Details -------------------------------------------------------------- */
157 /* --- Details -------------------------------------------------------------- */
158 
159 typedef ActionModelQuadrupedStepPeriodTpl<double>
162 
163 } // namespace quadruped_walkgen
164 
165 #include "quadruped_step_period.hxx"
166 
167 #endif
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::get_dt_bound_weight
const Scalar & get_dt_bound_weight() const
Definition: quadruped_step_period.hxx:338
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::get_state_weights
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition: quadruped_step_period.hxx:218
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::get_dt_ref
const Scalar & get_dt_ref() const
Definition: quadruped_step_period.hxx:378
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::set_nb_nodes
void set_nb_nodes(const Scalar &nodes_)
Definition: quadruped_step_period.hxx:356
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::get_speed_weight
const Scalar & get_speed_weight() const
Definition: quadruped_step_period.hxx:325
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::createData
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition: quadruped_step_period.hxx:208
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::update_model
void update_model(const Eigen::Ref< const typename MathBase::MatrixXs > &l_feet, const Eigen::Ref< const typename MathBase::MatrixXs > &xref, const Eigen::Ref< const typename MathBase::MatrixXs > &S)
Definition: quadruped_step_period.hxx:412
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::set_dt_bound_weight
void set_dt_bound_weight(const Scalar &weight_)
Definition: quadruped_step_period.hxx:344
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::set_state_weights
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_step_period.hxx:222
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::set_dt_max
void set_dt_max(const Scalar &dt)
Definition: quadruped_step_period.hxx:402
quadruped_walkgen::ActionDataQuadrupedStepPeriodTpl::Scalar
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: quadruped_step_period.hpp:136
quadruped_walkgen::ActionModelQuadrupedStepPeriod
ActionModelQuadrupedStepPeriodTpl< double > ActionModelQuadrupedStepPeriod
Definition: quadruped_step_period.hpp:160
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::set_vlim
void set_vlim(const Scalar &vlim_)
Definition: quadruped_step_period.hxx:370
quadruped_walkgen
Definition: quadruped.hpp:11
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::set_shoulder_position
void set_shoulder_position(const typename MathBase::VectorXs &weights)
Definition: quadruped_step_period.hxx:267
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::get_shoulder_weights
const Eigen::Matrix< Scalar, 8, 1 > & get_shoulder_weights() const
Definition: quadruped_step_period.hxx:248
quadruped_step_period.hxx
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::set_dt_min
void set_dt_min(const Scalar &dt)
Definition: quadruped_step_period.hxx:392
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::MathBase
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped_step_period.hpp:19
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::calc
virtual void calc(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const typename MathBase::VectorXs > &x, const Eigen::Ref< const typename MathBase::VectorXs > &u)
Definition: quadruped_step_period.hxx:52
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::ActionModelQuadrupedStepPeriodTpl
ActionModelQuadrupedStepPeriodTpl()
Definition: quadruped_step_period.hxx:8
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::set_T_gait
void set_T_gait(const Scalar &T_gait_)
Definition: quadruped_step_period.hxx:306
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::get_T_gait
const Scalar & get_T_gait() const
Definition: quadruped_step_period.hxx:301
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl
Definition: quadruped_step_period.hpp:13
quadruped_walkgen::ActionDataQuadrupedStepPeriod
ActionDataQuadrupedStepPeriodTpl< double > ActionDataQuadrupedStepPeriod
Definition: quadruped_step_period.hpp:161
quadruped_walkgen::ActionDataQuadrupedStepPeriodTpl::Base
crocoddyl::ActionDataAbstractTpl< Scalar > Base
Definition: quadruped_step_period.hpp:138
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::get_vlim
const Scalar & get_vlim() const
Definition: quadruped_step_period.hxx:365
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::get_nb_nodes
const Scalar & get_nb_nodes() const
Definition: quadruped_step_period.hxx:351
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::set_step_weights
void set_step_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_step_period.hxx:237
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::ActionDataAbstract
crocoddyl::ActionDataAbstractTpl< Scalar > ActionDataAbstract
Definition: quadruped_step_period.hpp:17
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::Base
crocoddyl::ActionModelAbstractTpl< Scalar > Base
Definition: quadruped_step_period.hpp:18
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::set_dt_weight
void set_dt_weight(const Scalar &weight_)
Definition: quadruped_step_period.hxx:318
quadruped_walkgen::ActionDataQuadrupedStepPeriodTpl::ActionDataQuadrupedStepPeriodTpl
ActionDataQuadrupedStepPeriodTpl(Model< Scalar > *const model)
Definition: quadruped_step_period.hpp:151
quadruped_walkgen::ActionDataQuadrupedStepPeriodTpl
Definition: quadruped_step_period.hpp:132
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::~ActionModelQuadrupedStepPeriodTpl
~ActionModelQuadrupedStepPeriodTpl()
Definition: quadruped_step_period.hxx:49
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::set_speed_weight
void set_speed_weight(const Scalar &weight_)
Definition: quadruped_step_period.hxx:331
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::get_step_weights
const Eigen::Matrix< Scalar, 4, 1 > & get_step_weights() const
Definition: quadruped_step_period.hxx:233
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::set_shoulder_weights
void set_shoulder_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_step_period.hxx:252
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::get_centrifugal_term
const bool & get_centrifugal_term() const
Definition: quadruped_step_period.hxx:289
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::get_dt_max
const Scalar & get_dt_max() const
Definition: quadruped_step_period.hxx:398
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::get_dt_min
const Scalar & get_dt_min() const
Definition: quadruped_step_period.hxx:388
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::calcDiff
virtual void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const typename MathBase::VectorXs > &x, const Eigen::Ref< const typename MathBase::VectorXs > &u)
Definition: quadruped_step_period.hxx:99
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::set_centrifugal_term
void set_centrifugal_term(const bool &cent_term)
Definition: quadruped_step_period.hxx:294
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::set_dt_ref
void set_dt_ref(const Scalar &dt)
Definition: quadruped_step_period.hxx:382
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::Scalar
_Scalar Scalar
Definition: quadruped_step_period.hpp:16
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::set_symmetry_term
void set_symmetry_term(const bool &sym_term)
Definition: quadruped_step_period.hxx:282
quadruped_walkgen::ActionDataQuadrupedStepPeriodTpl::MathBase
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped_step_period.hpp:137
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::get_symmetry_term
const bool & get_symmetry_term() const
Definition: quadruped_step_period.hxx:277
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::get_dt_weight
const Scalar & get_dt_weight() const
Definition: quadruped_step_period.hxx:313
quadruped_walkgen::ActionModelQuadrupedStepPeriodTpl::get_shoulder_position
const Eigen::Matrix< Scalar, 8, 1 > & get_shoulder_position() const
Definition: quadruped_step_period.hxx:263