quadruped_step_time.hpp
Go to the documentation of this file.
1 #ifndef __quadruped_walkgen_quadruped_step_time_hpp__
2 #define __quadruped_walkgen_quadruped_step_time_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_heuristic_weights() const;
40  void set_heuristic_weights(const typename MathBase::VectorXs& weights);
41 
42  // Update the model depending if the foot in contact with the ground
43  // or the new lever arms
44  void update_model(
45  const Eigen::Ref<const typename MathBase::MatrixXs>& l_feet,
46  const Eigen::Ref<const typename MathBase::MatrixXs>& velocity,
47  const Eigen::Ref<const typename MathBase::MatrixXs>& acceleration,
48  const Eigen::Ref<const typename MathBase::MatrixXs>& xref,
49  const Eigen::Ref<const typename MathBase::VectorXs>& 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_nb_nodes() const;
61  void set_nb_nodes(const Scalar& nodes_);
62 
63  const Scalar& get_vlim() const;
64  void set_vlim(const Scalar& vlim_);
65 
66  const Scalar& get_speed_weight() const;
67  void set_speed_weight(const Scalar& weight_);
68 
69  const bool& get_first_step() const;
70  void set_first_step(const bool& first);
71 
72  // get cost
73  const typename Eigen::Matrix<Scalar, 7, 1>& get_cost() const;
74 
75  protected:
76  using Base::has_control_limits_;
77  using Base::nr_;
79  using Base::nu_;
80  using Base::state_;
81  using Base::u_lb_;
82  using Base::u_ub_;
83  using Base::unone_;
84 
85  private:
86  Scalar T_gait;
87  Scalar speed_weight;
88  Scalar nb_nodes;
89  Scalar vlim;
90  Scalar beta_lim;
91  int nb_alpha_;
92  bool centrifugal_term;
93  bool symmetry_term;
94  // indicates whether it t the 1st step, otherwise the cost function is much
95  // simpler (acc, speed = 0)
96  bool first_step;
97 
98  typename Eigen::Matrix<Scalar, 12, 1> state_weights_;
99  typename Eigen::Matrix<Scalar, 4, 1> step_weights_;
100  typename Eigen::Matrix<Scalar, 8, 1> heuristicWeights;
101  typename MathBase::Matrix3s R_tmp;
102 
103  // typename Eigen::Array<Scalar, 3, 1 > alpha ;
104  // typename Eigen::Array<Scalar, 3, 4 > alpha2 ;
105  // typename Eigen::Array<Scalar, 3, 3 > b_coeff ;
106  typename MathBase::ArrayXs alpha;
107  typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> alpha2;
108  typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> b_coeff;
109  typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> b_coeff_x0;
110  typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> b_coeff_y0;
111  typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> b_coeff_x1;
112  typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> b_coeff_y1;
113  typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> b_coeff_x2;
114  typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> b_coeff_y2;
115 
116  typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> rub_max_first_x;
117  typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> rub_max_first_y;
118  typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> rub_max_first_2;
119  typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>
120  rub_max_first_bool;
121 
122  // typename Eigen::Array<Scalar, 3, 12 > b_coeff2 ;
123  typename Eigen::Matrix<Scalar, 3, 4> lfeet;
124  // typename Eigen::Array<Scalar, 3, 4 > rub_max_first ;
125  // typename Eigen::Array<Scalar, 3, 2 > rub_max_first_2 ;
126  // typename Eigen::Array<Scalar, 3, 2 > rub_max_bool_first ;
127 
128  typename Eigen::Matrix<Scalar, 8, 8> B;
129 
130  typename MathBase::MatrixXs xref_;
131  typename MathBase::VectorXs S_; // Containing the flying feet
132  typename Eigen::Matrix<Scalar, 8, 1> pheuristic_;
133 
134  // Compute heuristic inside update Model
135  // typename Eigen::Matrix<Scalar, 2 , 4 > pshoulder_0;
136  // typename Eigen::Matrix<Scalar, 2 , 4 > pshoulder_tmp;
137  // typename Eigen::Matrix<Scalar, 3 , 1 > pcentrifugal_tmp;
138  // typename Eigen::Matrix<Scalar, 3 , 1 > pcentrifugal_tmp_1;
139  // typename Eigen::Matrix<Scalar, 3 , 1 > pcentrifugal_tmp_2;
140 
141  typename Eigen::Matrix<Scalar, 4, 1> rub_max_;
142  typename Eigen::Matrix<Scalar, 4, 1> rub_max_bool;
143 
144  // Log cost
145  bool log_cost;
146  typename Eigen::Matrix<Scalar, 7, 1> cost_;
147 };
148 
149 template <typename _Scalar>
151  : public crocoddyl::ActionDataAbstractTpl<_Scalar> {
152  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
153 
154  typedef _Scalar Scalar;
155  typedef crocoddyl::MathBaseTpl<Scalar> MathBase;
156  typedef crocoddyl::ActionDataAbstractTpl<Scalar> Base;
157  using Base::cost;
158  using Base::Fu;
159  using Base::Fx;
160  using Base::Lu;
161  using Base::Luu;
162  using Base::Lx;
163  using Base::Lxu;
164  using Base::Lxx;
165  using Base::r;
166  using Base::xnext;
167 
168  template <template <typename Scalar> class Model>
169  explicit ActionDataQuadrupedStepTimeTpl(Model<Scalar>* const model)
170  : crocoddyl::ActionDataAbstractTpl<Scalar>(model) {}
171 };
172 
173 /* --- Details -------------------------------------------------------------- */
174 /* --- Details -------------------------------------------------------------- */
175 /* --- Details -------------------------------------------------------------- */
176 
179 
180 } // namespace quadruped_walkgen
181 
182 #include "quadruped_step_time.hxx"
183 
184 #endif
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::set_vlim
void set_vlim(const Scalar &vlim_)
Definition: quadruped_step_time.hxx:432
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::set_centrifugal_term
void set_centrifugal_term(const bool &cent_term)
Definition: quadruped_step_time.hxx:383
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::get_nb_nodes
const Scalar & get_nb_nodes() const
Definition: quadruped_step_time.hxx:416
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::update_model
void update_model(const Eigen::Ref< const typename MathBase::MatrixXs > &l_feet, const Eigen::Ref< const typename MathBase::MatrixXs > &velocity, const Eigen::Ref< const typename MathBase::MatrixXs > &acceleration, const Eigen::Ref< const typename MathBase::MatrixXs > &xref, const Eigen::Ref< const typename MathBase::VectorXs > &S)
Definition: quadruped_step_time.hxx:465
quadruped_walkgen::ActionDataQuadrupedStepTimeTpl::MathBase
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped_step_time.hpp:155
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::get_state_weights
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition: quadruped_step_time.hxx:323
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::Scalar
_Scalar Scalar
Definition: quadruped_step_time.hpp:16
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::~ActionModelQuadrupedStepTimeTpl
~ActionModelQuadrupedStepTimeTpl()
Definition: quadruped_step_time.hxx:100
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::get_vlim
const Scalar & get_vlim() const
Definition: quadruped_step_time.hxx:428
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::set_T_gait
void set_T_gait(const Scalar &T_gait_)
Definition: quadruped_step_time.hxx:395
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::get_first_step
const bool & get_first_step() const
Definition: quadruped_step_time.hxx:450
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::set_speed_weight
void set_speed_weight(const Scalar &weight_)
Definition: quadruped_step_time.hxx:410
quadruped_walkgen
Definition: quadruped.hpp:11
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::get_step_weights
const Eigen::Matrix< Scalar, 4, 1 > & get_step_weights() const
Definition: quadruped_step_time.hxx:338
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::get_heuristic_weights
const Eigen::Matrix< Scalar, 8, 1 > & get_heuristic_weights() const
Definition: quadruped_step_time.hxx:353
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::set_nb_nodes
void set_nb_nodes(const Scalar &nodes_)
Definition: quadruped_step_time.hxx:420
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::createData
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition: quadruped_step_time.hxx:313
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::ActionModelQuadrupedStepTimeTpl
ActionModelQuadrupedStepTimeTpl()
Definition: quadruped_step_time.hxx:8
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::set_symmetry_term
void set_symmetry_term(const bool &sym_term)
Definition: quadruped_step_time.hxx:371
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::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_time.hxx:103
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::set_heuristic_weights
void set_heuristic_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_step_time.hxx:357
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::Base
crocoddyl::ActionModelAbstractTpl< Scalar > Base
Definition: quadruped_step_time.hpp:18
quadruped_walkgen::ActionDataQuadrupedStepTime
ActionDataQuadrupedStepTimeTpl< double > ActionDataQuadrupedStepTime
Definition: quadruped_step_time.hpp:178
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl
Definition: quadruped_step_time.hpp:13
quadruped_walkgen::ActionModelQuadrupedStepTime
ActionModelQuadrupedStepTimeTpl< double > ActionModelQuadrupedStepTime
Definition: quadruped_step_time.hpp:177
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::get_speed_weight
const Scalar & get_speed_weight() const
Definition: quadruped_step_time.hxx:405
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::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_time.hxx:194
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::get_cost
const Eigen::Matrix< Scalar, 7, 1 > & get_cost() const
Definition: quadruped_step_time.hxx:443
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::set_state_weights
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_step_time.hxx:327
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::set_step_weights
void set_step_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_step_time.hxx:342
quadruped_walkgen::ActionDataQuadrupedStepTimeTpl::Base
crocoddyl::ActionDataAbstractTpl< Scalar > Base
Definition: quadruped_step_time.hpp:156
quadruped_walkgen::ActionDataQuadrupedStepTimeTpl
Definition: quadruped_step_time.hpp:150
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::ActionDataAbstract
crocoddyl::ActionDataAbstractTpl< Scalar > ActionDataAbstract
Definition: quadruped_step_time.hpp:17
quadruped_walkgen::ActionDataQuadrupedStepTimeTpl::ActionDataQuadrupedStepTimeTpl
ActionDataQuadrupedStepTimeTpl(Model< Scalar > *const model)
Definition: quadruped_step_time.hpp:169
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::get_centrifugal_term
const bool & get_centrifugal_term() const
Definition: quadruped_step_time.hxx:378
quadruped_walkgen::ActionDataQuadrupedStepTimeTpl::Scalar
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: quadruped_step_time.hpp:154
quadruped_step_time.hxx
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::set_first_step
void set_first_step(const bool &first)
Definition: quadruped_step_time.hxx:454
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::get_symmetry_term
const bool & get_symmetry_term() const
Definition: quadruped_step_time.hxx:367
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::MathBase
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped_step_time.hpp:19
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::get_T_gait
const Scalar & get_T_gait() const
Definition: quadruped_step_time.hxx:390