quadruped_step.hpp
Go to the documentation of this file.
1 #ifndef __quadruped_walkgen_quadruped_step_hpp__
2 #define __quadruped_walkgen_quadruped_step_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, 8, 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>& xref,
47  const Eigen::Ref<const typename MathBase::VectorXs>& S,
48  const Eigen::Ref<const typename MathBase::MatrixXs>& position,
49  const Eigen::Ref<const typename MathBase::MatrixXs>& velocity,
50  const Eigen::Ref<const typename MathBase::MatrixXs>& acceleration,
51  const Eigen::Ref<const typename MathBase::MatrixXs>& jerk,
52  const Eigen::Ref<const typename MathBase::MatrixXs>& oRh,
53  const Eigen::Ref<const typename MathBase::MatrixXs>& oTh,
54  const Scalar& delta_T);
55 
56  const bool& get_symmetry_term() const;
57  void set_symmetry_term(const bool& sym_term);
58 
59  const bool& get_centrifugal_term() const;
60  void set_centrifugal_term(const bool& cent_term);
61 
62  const Scalar& get_T_gait() const;
63  void set_T_gait(const Scalar& T_gait_);
64 
65  const bool& get_acc_activated() const;
66  void set_acc_activated(const bool& is_activated);
67 
68  const typename Eigen::Matrix<Scalar, 2, 1>& get_acc_lim() const;
69  void set_acc_lim(const typename MathBase::VectorXs& acceleration_lim_);
70 
71  const Scalar& get_acc_weight() const;
72  void set_acc_weight(const Scalar& weight_);
73 
74  const bool& get_vel_activated() const;
75  void set_vel_activated(const bool& is_activated);
76 
77  const typename Eigen::Matrix<Scalar, 2, 1>& get_vel_lim() const;
78  void set_vel_lim(const typename MathBase::VectorXs& velocity_lim_);
79 
80  const Scalar& get_vel_weight() const;
81  void set_vel_weight(const Scalar& weight_);
82 
83  void set_sample_feet_traj(const int& n_sample);
84 
85  const bool& get_jerk_activated() const;
86  void set_jerk_activated(const bool& is_activated);
87 
88  const Scalar& get_jerk_weight() const;
89  void set_jerk_weight(const Scalar& weight_);
90 
91  protected:
92  using Base::has_control_limits_;
93  using Base::nr_;
95  using Base::nu_;
96  using Base::state_;
97  using Base::u_lb_;
98  using Base::u_ub_;
99  using Base::unone_;
100 
101  private:
102  Scalar T_gait;
103  bool centrifugal_term;
104  bool symmetry_term;
105 
106  typename Eigen::Matrix<Scalar, 12, 1> state_weights_;
107  typename Eigen::Matrix<Scalar, 8, 1> step_weights_;
108  typename Eigen::Matrix<Scalar, 8, 1> heuristic_weights_;
109  typename MathBase::Matrix3s R_tmp;
110 
111  typename Eigen::Matrix<Scalar, 8, 8> B;
112 
113  typename MathBase::MatrixXs xref_;
114  typename Eigen::Matrix<Scalar, 8, 1> pheuristic_;
115 
116  // typename Eigen::Matrix<Scalar, 2, 4> pshoulder_0;
117  // typename Eigen::Matrix<Scalar, 2, 4> pshoulder_tmp;
118  // typename Eigen::Matrix<Scalar, 3, 1> pcentrifugal_tmp;
119  // typename Eigen::Matrix<Scalar, 3, 1> pcentrifugal_tmp_1;
120  // typename Eigen::Matrix<Scalar, 3, 1> pcentrifugal_tmp_2;
121 
122  // Cost on the acceleration of the feet :
123  int N_sampling;
124  bool is_acc_activated_; // Boolean to activate the cost on the acceleration
125  // of the feet
126  Scalar acc_weight_; // Weight on the acceleration cost
127  typename Eigen::Matrix<Scalar, 2, 1>
128  acc_lim_; // Maximum acceleration allowed on x and y axis
129  typename Eigen::Matrix<Scalar, 4, 1> S_; // Containing the moving feet
130  typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> delta_;
131  typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> gamma_;
132  typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> alpha_;
133  typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> beta_x_;
134  typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> beta_y_;
135  typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> tmp_ones_;
136  typename Eigen::Array<Scalar, 3, 4> position_;
137 
138  typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> rb_accx_max_;
139  typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> rb_accy_max_;
140  typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>
141  rb_accx_max_bool_;
142  typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>
143  rb_accy_max_bool_;
144 
145  // Cost on the velocity of the feet :
146  bool is_vel_activated_; // Boolean to activate the cost on the velocity of
147  // the feet
148  Scalar vel_weight_; // Weight on the velocity cost
149  typename Eigen::Matrix<Scalar, 2, 1>
150  vel_lim_; // Maximum velocity allowed on x and y axis
151  typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> gamma_v;
152  typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> alpha_v;
153  typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> beta_x_v;
154  typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> beta_y_v;
155 
156  typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> rb_velx_max_;
157  typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> rb_vely_max_;
158  typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>
159  rb_velx_max_bool_;
160  typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>
161  rb_vely_max_bool_;
162 
163  // Cost on the jerk of the feet
164  bool is_jerk_activated_;
165  Scalar jerk_weight_;
166  Scalar alpha_j;
167  typename Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic> beta_j;
168  typename Eigen::Array<Scalar, 3, 4> jerk_;
169 
170  typename Eigen::Matrix<Scalar, 2, 4> rb_jerk_;
171 
172  typename Eigen::Matrix<Scalar, 3, 3> oRh_;
173  typename Eigen::Matrix<Scalar, 3, 1> oTh_;
174 };
175 
176 template <typename _Scalar>
178  : public crocoddyl::ActionDataAbstractTpl<_Scalar> {
179  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
180 
181  typedef _Scalar Scalar;
182  typedef crocoddyl::MathBaseTpl<Scalar> MathBase;
183  typedef crocoddyl::ActionDataAbstractTpl<Scalar> Base;
184  using Base::cost;
185  using Base::Fu;
186  using Base::Fx;
187  using Base::Lu;
188  using Base::Luu;
189  using Base::Lx;
190  using Base::Lxu;
191  using Base::Lxx;
192  using Base::r;
193  using Base::xnext;
194 
195  template <template <typename Scalar> class Model>
196  explicit ActionDataQuadrupedStepTpl(Model<Scalar>* const model)
197  : crocoddyl::ActionDataAbstractTpl<Scalar>(model) {}
198 };
199 
200 /* --- Details -------------------------------------------------------------- */
201 /* --- Details -------------------------------------------------------------- */
202 /* --- Details -------------------------------------------------------------- */
203 
206 
207 } // namespace quadruped_walkgen
208 
209 #include "quadruped_step.hxx"
210 
211 #endif
quadruped_walkgen::ActionModelQuadrupedStepTpl::get_vel_weight
const Scalar & get_vel_weight() const
Definition: quadruped_step.hxx:1051
quadruped_walkgen::ActionModelQuadrupedStepTpl::set_heuristic_weights
void set_heuristic_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_step.hxx:946
quadruped_walkgen::ActionModelQuadrupedStepTpl::set_jerk_weight
void set_jerk_weight(const Scalar &weight_)
Definition: quadruped_step.hxx:1065
quadruped_walkgen::ActionModelQuadrupedStepTpl::get_heuristic_weights
const Eigen::Matrix< Scalar, 8, 1 > & get_heuristic_weights() const
Definition: quadruped_step.hxx:942
quadruped_walkgen::ActionModelQuadrupedStepTpl::set_acc_lim
void set_acc_lim(const typename MathBase::VectorXs &acceleration_lim_)
Definition: quadruped_step.hxx:1004
quadruped_step.hxx
quadruped_walkgen::ActionModelQuadrupedStepTpl::Scalar
_Scalar Scalar
Definition: quadruped_step.hpp:16
quadruped_walkgen::ActionModelQuadrupedStepTpl::get_state_weights
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition: quadruped_step.hxx:912
quadruped_walkgen::ActionModelQuadrupedStepTpl::Base
crocoddyl::ActionModelAbstractTpl< Scalar > Base
Definition: quadruped_step.hpp:18
quadruped_walkgen
Definition: quadruped.hpp:11
quadruped_walkgen::ActionModelQuadrupedStepTpl::set_sample_feet_traj
void set_sample_feet_traj(const int &n_sample)
Definition: quadruped_step.hxx:1081
quadruped_walkgen::ActionModelQuadrupedStepTpl::~ActionModelQuadrupedStepTpl
~ActionModelQuadrupedStepTpl()
Definition: quadruped_step.hxx:125
quadruped_walkgen::ActionModelQuadrupedStepTpl::get_vel_activated
const bool & get_vel_activated() const
Definition: quadruped_step.hxx:1025
quadruped_walkgen::ActionModelQuadrupedStepTpl::set_vel_weight
void set_vel_weight(const Scalar &weight_)
Definition: quadruped_step.hxx:1055
quadruped_walkgen::ActionModelQuadrupedStepTpl::set_state_weights
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_step.hxx:916
quadruped_walkgen::ActionModelQuadrupedStepTpl::set_acc_weight
void set_acc_weight(const Scalar &weight_)
Definition: quadruped_step.hxx:1019
quadruped_walkgen::ActionModelQuadrupedStepTpl::createData
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition: quadruped_step.hxx:902
quadruped_walkgen::ActionModelQuadrupedStepTpl::get_jerk_activated
const bool & get_jerk_activated() const
Definition: quadruped_step.hxx:1071
quadruped_walkgen::ActionModelQuadrupedStepTpl::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.hxx:345
quadruped_walkgen::ActionModelQuadrupedStepTpl::get_step_weights
const Eigen::Matrix< Scalar, 8, 1 > & get_step_weights() const
Definition: quadruped_step.hxx:927
quadruped_walkgen::ActionDataQuadrupedStepTpl
Definition: quadruped_step.hpp:177
quadruped_walkgen::ActionDataQuadrupedStepTpl::MathBase
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped_step.hpp:182
quadruped_walkgen::ActionDataQuadrupedStep
ActionDataQuadrupedStepTpl< double > ActionDataQuadrupedStep
Definition: quadruped_step.hpp:205
quadruped_walkgen::ActionModelQuadrupedStepTpl::set_symmetry_term
void set_symmetry_term(const bool &sym_term)
Definition: quadruped_step.hxx:960
quadruped_walkgen::ActionModelQuadrupedStepTpl::set_step_weights
void set_step_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_step.hxx:931
quadruped_walkgen::ActionModelQuadrupedStepTpl::get_centrifugal_term
const bool & get_centrifugal_term() const
Definition: quadruped_step.hxx:967
quadruped_walkgen::ActionModelQuadrupedStepTpl::set_T_gait
void set_T_gait(const Scalar &T_gait_)
Definition: quadruped_step.hxx:983
quadruped_walkgen::ActionModelQuadrupedStepTpl::set_vel_activated
void set_vel_activated(const bool &is_activated)
Definition: quadruped_step.hxx:1029
quadruped_walkgen::ActionModelQuadrupedStepTpl::set_acc_activated
void set_acc_activated(const bool &is_activated)
Definition: quadruped_step.hxx:993
quadruped_walkgen::ActionModelQuadrupedStepTpl::ActionModelQuadrupedStepTpl
ActionModelQuadrupedStepTpl()
Definition: quadruped_step.hxx:8
quadruped_walkgen::ActionDataQuadrupedStepTpl::Base
crocoddyl::ActionDataAbstractTpl< Scalar > Base
Definition: quadruped_step.hpp:183
quadruped_walkgen::ActionModelQuadrupedStepTpl::get_acc_lim
const Eigen::Matrix< Scalar, 2, 1 > & get_acc_lim() const
Definition: quadruped_step.hxx:1000
quadruped_walkgen::ActionModelQuadrupedStepTpl::MathBase
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped_step.hpp:19
quadruped_walkgen::ActionModelQuadrupedStepTpl::get_jerk_weight
const Scalar & get_jerk_weight() const
Definition: quadruped_step.hxx:1061
quadruped_walkgen::ActionModelQuadrupedStepTpl::set_centrifugal_term
void set_centrifugal_term(const bool &cent_term)
Definition: quadruped_step.hxx:971
quadruped_walkgen::ActionModelQuadrupedStepTpl::set_jerk_activated
void set_jerk_activated(const bool &is_activated)
Definition: quadruped_step.hxx:1075
quadruped_walkgen::ActionModelQuadrupedStepTpl::get_symmetry_term
const bool & get_symmetry_term() const
Definition: quadruped_step.hxx:956
quadruped_walkgen::ActionModelQuadrupedStepTpl::ActionDataAbstract
crocoddyl::ActionDataAbstractTpl< Scalar > ActionDataAbstract
Definition: quadruped_step.hpp:17
quadruped_walkgen::ActionModelQuadrupedStepTpl::get_acc_weight
const Scalar & get_acc_weight() const
Definition: quadruped_step.hxx:1015
quadruped_walkgen::ActionModelQuadrupedStepTpl::get_vel_lim
const Eigen::Matrix< Scalar, 2, 1 > & get_vel_lim() const
Definition: quadruped_step.hxx:1036
quadruped_walkgen::ActionModelQuadrupedStepTpl::get_T_gait
const Scalar & get_T_gait() const
Definition: quadruped_step.hxx:978
quadruped_walkgen::ActionModelQuadrupedStepTpl::get_acc_activated
const bool & get_acc_activated() const
Definition: quadruped_step.hxx:989
quadruped_walkgen::ActionModelQuadrupedStepTpl
Definition: quadruped_step.hpp:13
quadruped_walkgen::ActionDataQuadrupedStepTpl::Scalar
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: quadruped_step.hpp:181
quadruped_walkgen::ActionModelQuadrupedStepTpl::set_vel_lim
void set_vel_lim(const typename MathBase::VectorXs &velocity_lim_)
Definition: quadruped_step.hxx:1040
quadruped_walkgen::ActionModelQuadrupedStepTpl::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::VectorXs > &S, const Eigen::Ref< const typename MathBase::MatrixXs > &position, const Eigen::Ref< const typename MathBase::MatrixXs > &velocity, const Eigen::Ref< const typename MathBase::MatrixXs > &acceleration, const Eigen::Ref< const typename MathBase::MatrixXs > &jerk, const Eigen::Ref< const typename MathBase::MatrixXs > &oRh, const Eigen::Ref< const typename MathBase::MatrixXs > &oTh, const Scalar &delta_T)
Definition: quadruped_step.hxx:1157
quadruped_walkgen::ActionModelQuadrupedStep
ActionModelQuadrupedStepTpl< double > ActionModelQuadrupedStep
Definition: quadruped_step.hpp:204
quadruped_walkgen::ActionModelQuadrupedStepTpl::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.hxx:128
quadruped_walkgen::ActionDataQuadrupedStepTpl::ActionDataQuadrupedStepTpl
ActionDataQuadrupedStepTpl(Model< Scalar > *const model)
Definition: quadruped_step.hpp:196