quadruped_augmented_time.hpp
Go to the documentation of this file.
1 #ifndef __quadruped_walkgen_quadruped_augmented_time_hpp__
2 #define __quadruped_walkgen_quadruped_augmented_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_force_weights() const;
34  void set_force_weights(const typename MathBase::VectorXs& weights);
35 
36  const typename Eigen::Matrix<Scalar, 12, 1>& get_state_weights() const;
37  void set_state_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  const typename Eigen::Matrix<Scalar, 8, 1>& get_stop_weights() const;
43  void set_stop_weights(const typename MathBase::VectorXs& weights);
44 
45  const Scalar& get_friction_weight() const;
46  void set_friction_weight(const Scalar& weight);
47 
48  const Scalar& get_mu() const;
49  void set_mu(const Scalar& mu_coeff);
50 
51  const Scalar& get_mass() const;
52  void set_mass(const Scalar& m);
53 
54  const Scalar& get_dt_ref() const;
55  void set_dt_ref(const Scalar& dt);
56 
57  const Scalar& get_dt_min() const;
58  void set_dt_min(const Scalar& dt);
59 
60  const Scalar& get_dt_max() const;
61  void set_dt_max(const Scalar& dt);
62 
63  const typename Eigen::Matrix<Scalar, 3, 3>& get_gI() const;
64  void set_gI(const typename MathBase::Matrix3s& inertia_matrix);
65 
66  const Scalar& get_min_fz_contact() const;
67  void set_min_fz_contact(const Scalar& min_fz);
68 
69  const Scalar& get_max_fz_contact() const;
70  void set_max_fz_contact(const Scalar& max_fz_);
71 
72  const typename Eigen::Matrix<Scalar, 8, 1>& get_shoulder_position() const;
73  void set_shoulder_position(const typename MathBase::VectorXs& weights);
74 
75  const bool& get_symmetry_term() const;
76  void set_symmetry_term(const bool& sym_term);
77 
78  const bool& get_centrifugal_term() const;
79  void set_centrifugal_term(const bool& cent_term);
80 
81  const Scalar& get_T_gait() const;
82  void set_T_gait(const Scalar& T_gait_);
83 
84  const Scalar& get_dt_weight() const;
85  void set_dt_weight(const Scalar& weight_);
86 
87  const Scalar& get_dt_bound_weight() const;
88  void set_dt_bound_weight(const Scalar& weight_);
89 
90  const bool& get_relative_forces() const;
91  void set_relative_forces(const bool& rel_forces);
92 
93  // Set parameter relative to the shoulder height cost
94  const Scalar& get_shoulder_hlim() const;
95  void set_shoulder_hlim(const Scalar& hlim);
96 
97  const Scalar& get_shoulder_contact_weight() const;
98  void set_shoulder_contact_weight(const Scalar& weight);
99 
100  // Update the model depending if the foot in contact with the ground
101  // or the new lever arms
102  void update_model(const Eigen::Ref<const typename MathBase::MatrixXs>& l_feet,
103  const Eigen::Ref<const typename MathBase::MatrixXs>& l_stop,
104  const Eigen::Ref<const typename MathBase::MatrixXs>& xref,
105  const Eigen::Ref<const typename MathBase::MatrixXs>& S);
106 
107  // Get A & B matrix
108  const typename Eigen::Matrix<Scalar, 12, 12>& get_A() const;
109  const typename Eigen::Matrix<Scalar, 12, 12>& get_B() const;
110 
111  // get cost
112  const typename Eigen::Matrix<Scalar, 7, 1>& get_cost() const;
113 
114  protected:
115  using Base::has_control_limits_;
116  using Base::nr_;
118  using Base::nu_;
119  using Base::state_;
120  using Base::u_lb_;
121  using Base::u_ub_;
122  using Base::unone_;
123 
124  private:
125  Scalar dt_weight_;
126  Scalar mass;
127  Scalar mu;
128  Scalar friction_weight_;
129  Scalar min_fz_in_contact;
130  Scalar max_fz;
131  Scalar T_gait;
132  Scalar dt_bound_weight;
133  bool centrifugal_term;
134  bool symmetry_term;
135 
136  bool relative_forces;
137  typename Eigen::Matrix<Scalar, 12, 1> uref_;
138 
139  typename Eigen::Matrix<Scalar, 12, 1> force_weights_;
140  typename Eigen::Matrix<Scalar, 12, 1> state_weights_;
141  typename Eigen::Matrix<Scalar, 8, 1> heuristicWeights;
142  typename Eigen::Matrix<Scalar, 8, 1> last_position_weights_;
143 
144  typename Eigen::Matrix<Scalar, 12, 12> A;
145  typename Eigen::Matrix<Scalar, 12, 12> B;
146  typename Eigen::Matrix<Scalar, 12, 1> g;
147  typename Eigen::Matrix<Scalar, 3, 3> R;
148  typename MathBase::Matrix3s R_tmp;
149  typename Eigen::Matrix<Scalar, 3, 3> gI;
150 
151  typename Eigen::Matrix<Scalar, 3, 4> lever_arms;
152  typename MathBase::Vector3s lever_tmp;
153  typename MathBase::MatrixXs xref_;
154 
155  typename Eigen::Matrix<Scalar, 8, 1> pshoulder_;
156  typename Eigen::Matrix<Scalar, 8, 1> pheuristic_;
157  typename Eigen::Matrix<Scalar, 2, 4> pshoulder_0;
158  typename Eigen::Matrix<Scalar, 2, 4> pshoulder_tmp;
159 
160  typename Eigen::Matrix<Scalar, 3, 1> pcentrifugal_tmp;
161  typename Eigen::Matrix<Scalar, 3, 1> pcentrifugal_tmp_1;
162  typename Eigen::Matrix<Scalar, 3, 1> pcentrifugal_tmp_2;
163 
164  typename Eigen::Matrix<Scalar, 8, 1> pref_;
165 
166  typename Eigen::Matrix<Scalar, 24, 1> ub;
167 
168  typename Eigen::Matrix<Scalar, 24, 1> Fa_x_u;
169  typename Eigen::Matrix<Scalar, 24, 1> rub_max_;
170  typename Eigen::Matrix<Scalar, 24, 24> Arr;
171  typename Eigen::Matrix<Scalar, 6, 1> r;
172  typename Eigen::Matrix<Scalar, 4, 1> gait;
173  typename Eigen::Matrix<Scalar, 8, 1> gait_double;
174 
175  typename Eigen::Matrix<Scalar, 3, 1> base_vector_x;
176  typename Eigen::Matrix<Scalar, 3, 1> base_vector_y;
177  typename Eigen::Matrix<Scalar, 3, 1> base_vector_z;
178  typename Eigen::Matrix<Scalar, 3, 1> forces_3d;
179 
180  typename Eigen::Matrix<Scalar, 1, 1> dt_min_;
181  typename Eigen::Matrix<Scalar, 1, 1> dt_max_;
182 
183  typename Eigen::Matrix<Scalar, 2, 1> rub_max_dt;
184  typename Eigen::Matrix<Scalar, 2, 1> rub_max_dt_bool;
185 
186  // Log cost
187  bool log_cost;
188  typename Eigen::Matrix<Scalar, 7, 1> cost_;
189 
190  // Cost relative to the shoulder height
191  typename Eigen::Matrix<Scalar, 3, 4> psh;
192  typename Eigen::Matrix<Scalar, 4, 1> sh_ub_max_;
193  Scalar sh_weight;
194  Scalar sh_hlim;
195 };
196 
197 template <typename _Scalar>
199  : public crocoddyl::ActionDataAbstractTpl<_Scalar> {
200  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
201 
202  typedef _Scalar Scalar;
203  typedef crocoddyl::MathBaseTpl<Scalar> MathBase;
204  typedef crocoddyl::ActionDataAbstractTpl<Scalar> Base;
205  using Base::cost;
206  using Base::Fu;
207  using Base::Fx;
208  using Base::Lu;
209  using Base::Luu;
210  using Base::Lx;
211  using Base::Lxu;
212  using Base::Lxx;
213  using Base::r;
214  using Base::xnext;
215 
216  template <template <typename Scalar> class Model>
217  explicit ActionDataQuadrupedAugmentedTimeTpl(Model<Scalar>* const model)
218  : crocoddyl::ActionDataAbstractTpl<Scalar>(model) {}
219 };
220 
221 /* --- Details -------------------------------------------------------------- */
222 /* --- Details -------------------------------------------------------------- */
223 /* --- Details -------------------------------------------------------------- */
224 
225 typedef ActionModelQuadrupedAugmentedTimeTpl<double>
229 
230 } // namespace quadruped_walkgen
231 
233 
234 #endif
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::set_T_gait
void set_T_gait(const Scalar &T_gait_)
Definition: quadruped_augmented_time.hxx:646
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::set_state_weights
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented_time.hxx:437
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::set_force_weights
void set_force_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented_time.hxx:422
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::get_shoulder_position
const Eigen::Matrix< Scalar, 8, 1 > & get_shoulder_position() const
Definition: quadruped_augmented_time.hxx:463
quadruped_augmented_time.hxx
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::set_dt_bound_weight
void set_dt_bound_weight(const Scalar &weight_)
Definition: quadruped_augmented_time.hxx:662
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::get_symmetry_term
const bool & get_symmetry_term() const
Definition: quadruped_augmented_time.hxx:617
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl
Definition: quadruped_augmented_time.hpp:13
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::set_min_fz_contact
void set_min_fz_contact(const Scalar &min_fz)
Definition: quadruped_augmented_time.hxx:567
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::get_centrifugal_term
const bool & get_centrifugal_term() const
Definition: quadruped_augmented_time.hxx:629
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::get_friction_weight
const Scalar & get_friction_weight() const
Definition: quadruped_augmented_time.hxx:493
quadruped_walkgen::ActionModelQuadrupedAugmentedTime
ActionModelQuadrupedAugmentedTimeTpl< double > ActionModelQuadrupedAugmentedTime
Definition: quadruped_augmented_time.hpp:226
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::set_symmetry_term
void set_symmetry_term(const bool &sym_term)
Definition: quadruped_augmented_time.hxx:622
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::get_mu
const Scalar & get_mu() const
Definition: quadruped_augmented_time.hxx:503
quadruped_walkgen
Definition: quadruped.hpp:11
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::get_mass
const Scalar & get_mass() const
Definition: quadruped_augmented_time.hxx:513
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::createData
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition: quadruped_augmented_time.hxx:408
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::get_heuristic_weights
const Eigen::Matrix< Scalar, 8, 1 > & get_heuristic_weights() const
Definition: quadruped_augmented_time.hxx:448
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::ActionDataAbstract
crocoddyl::ActionDataAbstractTpl< Scalar > ActionDataAbstract
Definition: quadruped_augmented_time.hpp:17
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::set_shoulder_contact_weight
void set_shoulder_contact_weight(const Scalar &weight)
Definition: quadruped_augmented_time.hxx:607
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::set_stop_weights
void set_stop_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented_time.hxx:482
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::set_friction_weight
void set_friction_weight(const Scalar &weight)
Definition: quadruped_augmented_time.hxx:497
quadruped_walkgen::ActionDataQuadrupedAugmentedTimeTpl::ActionDataQuadrupedAugmentedTimeTpl
ActionDataQuadrupedAugmentedTimeTpl(Model< Scalar > *const model)
Definition: quadruped_augmented_time.hpp:217
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::set_dt_min
void set_dt_min(const Scalar &dt)
Definition: quadruped_augmented_time.hxx:527
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::get_shoulder_contact_weight
const Scalar & get_shoulder_contact_weight() const
Definition: quadruped_augmented_time.hxx:603
quadruped_walkgen::ActionDataQuadrupedAugmentedTimeTpl::MathBase
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped_augmented_time.hpp:203
quadruped_walkgen::ActionDataQuadrupedAugmentedTimeTpl::Scalar
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: quadruped_augmented_time.hpp:202
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::get_dt_weight
const Scalar & get_dt_weight() const
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::set_mass
void set_mass(const Scalar &m)
Definition: quadruped_augmented_time.hxx:517
quadruped_walkgen::ActionDataQuadrupedAugmentedTimeTpl::Base
crocoddyl::ActionDataAbstractTpl< Scalar > Base
Definition: quadruped_augmented_time.hpp:204
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::set_dt_ref
void set_dt_ref(const Scalar &dt)
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::set_shoulder_position
void set_shoulder_position(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented_time.hxx:467
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::set_mu
void set_mu(const Scalar &mu_coeff)
Definition: quadruped_augmented_time.hxx:507
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::~ActionModelQuadrupedAugmentedTimeTpl
~ActionModelQuadrupedAugmentedTimeTpl()
Definition: quadruped_augmented_time.hxx:98
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::set_gI
void set_gI(const typename MathBase::Matrix3s &inertia_matrix)
Definition: quadruped_augmented_time.hxx:550
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::get_state_weights
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition: quadruped_augmented_time.hxx:433
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::get_relative_forces
const bool & get_relative_forces() const
Definition: quadruped_augmented_time.hxx:692
quadruped_walkgen::ActionDataQuadrupedAugmentedTimeTpl
Definition: quadruped_augmented_time.hpp:198
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::Scalar
_Scalar Scalar
Definition: quadruped_augmented_time.hpp:16
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::get_stop_weights
const Eigen::Matrix< Scalar, 8, 1 > & get_stop_weights() const
Definition: quadruped_augmented_time.hxx:478
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::get_dt_bound_weight
const Scalar & get_dt_bound_weight() const
Definition: quadruped_augmented_time.hxx:657
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::set_shoulder_hlim
void set_shoulder_hlim(const Scalar &hlim)
Definition: quadruped_augmented_time.hxx:595
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::get_dt_ref
const Scalar & get_dt_ref() const
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::set_centrifugal_term
void set_centrifugal_term(const bool &cent_term)
Definition: quadruped_augmented_time.hxx:634
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::get_min_fz_contact
const Scalar & get_min_fz_contact() const
Definition: quadruped_augmented_time.hxx:561
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::MathBase
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped_augmented_time.hpp:19
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::set_max_fz_contact
void set_max_fz_contact(const Scalar &max_fz_)
Definition: quadruped_augmented_time.hxx:580
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::Base
crocoddyl::ActionModelAbstractTpl< Scalar > Base
Definition: quadruped_augmented_time.hpp:18
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::set_heuristic_weights
void set_heuristic_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented_time.hxx:452
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::get_B
const Eigen::Matrix< Scalar, 12, 12 > & get_B() const
Definition: quadruped_augmented_time.hxx:685
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::get_dt_min
const Scalar & get_dt_min() const
Definition: quadruped_augmented_time.hxx:523
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::get_dt_max
const Scalar & get_dt_max() const
Definition: quadruped_augmented_time.hxx:534
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::set_relative_forces
void set_relative_forces(const bool &rel_forces)
Definition: quadruped_augmented_time.hxx:697
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::get_max_fz_contact
const Scalar & get_max_fz_contact() const
Definition: quadruped_augmented_time.hxx:574
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::set_dt_max
void set_dt_max(const Scalar &dt)
Definition: quadruped_augmented_time.hxx:538
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::update_model
void update_model(const Eigen::Ref< const typename MathBase::MatrixXs > &l_feet, const Eigen::Ref< const typename MathBase::MatrixXs > &l_stop, const Eigen::Ref< const typename MathBase::MatrixXs > &xref, const Eigen::Ref< const typename MathBase::MatrixXs > &S)
Definition: quadruped_augmented_time.hxx:715
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::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_augmented_time.hxx:231
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::get_A
const Eigen::Matrix< Scalar, 12, 12 > & get_A() const
Definition: quadruped_augmented_time.hxx:680
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::get_shoulder_hlim
const Scalar & get_shoulder_hlim() const
Definition: quadruped_augmented_time.hxx:590
quadruped_walkgen::ActionDataQuadrupedAugmentedTime
ActionDataQuadrupedAugmentedTimeTpl< double > ActionDataQuadrupedAugmentedTime
Definition: quadruped_augmented_time.hpp:228
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::get_gI
const Eigen::Matrix< Scalar, 3, 3 > & get_gI() const
Definition: quadruped_augmented_time.hxx:546
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::get_cost
const Eigen::Matrix< Scalar, 7, 1 > & get_cost() const
Definition: quadruped_augmented_time.hxx:671
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::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_augmented_time.hxx:101
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::get_T_gait
const Scalar & get_T_gait() const
Definition: quadruped_augmented_time.hxx:641
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::get_force_weights
const Eigen::Matrix< Scalar, 12, 1 > & get_force_weights() const
Definition: quadruped_augmented_time.hxx:418
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::set_dt_weight
void set_dt_weight(const Scalar &weight_)
quadruped_walkgen::ActionModelQuadrupedAugmentedTimeTpl::ActionModelQuadrupedAugmentedTimeTpl
ActionModelQuadrupedAugmentedTimeTpl()
Definition: quadruped_augmented_time.hxx:9