quadruped_augmented.hpp
Go to the documentation of this file.
1 #ifndef __quadruped_walkgen_quadruped_augmented_hpp__
2 #define __quadruped_walkgen_quadruped_augmented_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 
22  typename Eigen::Matrix<Scalar, 3, 1> offset_CoM =
23  Eigen::Matrix<Scalar, 3, 1>::Zero());
25 
26  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data,
27  const Eigen::Ref<const typename MathBase::VectorXs>& x,
28  const Eigen::Ref<const typename MathBase::VectorXs>& u);
29  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data,
30  const Eigen::Ref<const typename MathBase::VectorXs>& x,
31  const Eigen::Ref<const typename MathBase::VectorXs>& u);
32  virtual boost::shared_ptr<ActionDataAbstract> createData();
33 
34  // Get and Set weights vectors : state , force & friction cone :
35  const typename Eigen::Matrix<Scalar, 12, 1>& get_force_weights() const;
36  void set_force_weights(const typename MathBase::VectorXs& weights);
37 
38  const typename Eigen::Matrix<Scalar, 12, 1>& get_state_weights() const;
39  void set_state_weights(const typename MathBase::VectorXs& weights);
40 
41  const typename Eigen::Matrix<Scalar, 8, 1>& get_heuristic_weights() const;
42  void set_heuristic_weights(const typename MathBase::VectorXs& weights);
43 
44  const typename Eigen::Matrix<Scalar, 8, 1>& get_stop_weights() const;
45  void set_stop_weights(const typename MathBase::VectorXs& weights);
46 
47  const Scalar& get_friction_weight() const;
48  void set_friction_weight(const Scalar& weight);
49 
50  const Scalar& get_mu() const;
51  void set_mu(const Scalar& mu_coeff);
52 
53  const Scalar& get_mass() const;
54  void set_mass(const Scalar& m);
55 
56  const Scalar& get_dt() const;
57  void set_dt(const Scalar& dt);
58 
59  const typename Eigen::Matrix<Scalar, 3, 3>& get_gI() const;
60  void set_gI(const typename MathBase::Matrix3s& inertia_matrix);
61 
62  const Scalar& get_min_fz_contact() const;
63  void set_min_fz_contact(const Scalar& min_fz);
64 
65  const Scalar& get_max_fz_contact() const;
66  void set_max_fz_contact(const Scalar& max_fz);
67 
68  const bool& get_symmetry_term() const;
69  void set_symmetry_term(const bool& sym_term);
70 
71  const bool& get_centrifugal_term() const;
72  void set_centrifugal_term(const bool& cent_term);
73 
74  const Scalar& get_T_gait() const;
75  void set_T_gait(const Scalar& T_gait_);
76 
77  // Set parameter relative to the shoulder height cost
78  const Scalar& get_shoulder_hlim() const;
79  void set_shoulder_hlim(const Scalar& hlim);
80 
81  const typename Eigen::Matrix<Scalar, 4, 1>& get_shoulder_contact_weight()
82  const;
84  const typename Eigen::Matrix<Scalar, 4, 1>& weight);
85 
86  const bool& get_shoulder_reference_position() const;
87  void set_shoulder_reference_position(const bool& reference);
88 
89  // Update the model depending if the foot in contact with the ground
90  // or the new lever arms
91  void update_model(const Eigen::Ref<const typename MathBase::MatrixXs>& l_feet,
92  const Eigen::Ref<const typename MathBase::MatrixXs>& l_stop,
93  const Eigen::Ref<const typename MathBase::MatrixXs>& xref,
94  const Eigen::Ref<const typename MathBase::MatrixXs>& S);
95 
96  // Get A & B matrix
97  const typename Eigen::Matrix<Scalar, 12, 12>& get_A() const;
98  const typename Eigen::Matrix<Scalar, 12, 12>& get_B() const;
99 
100  const bool& get_relative_forces() const;
101  void set_relative_forces(const bool& rel_forces);
102 
103  protected:
104  using Base::has_control_limits_;
106  using Base::nr_;
107  using Base::nu_;
108  using Base::state_;
109  using Base::u_lb_;
110  using Base::u_ub_;
111  using Base::unone_;
112 
113  private:
114  Scalar dt_;
115  Scalar mass;
116  Scalar mu;
117  Scalar friction_weight_;
118  Scalar min_fz_in_contact;
119  Scalar max_fz_in_contact;
120  Scalar T_gait;
121  bool centrifugal_term;
122  bool symmetry_term;
123  bool relative_forces;
124 
125  // Using the reference trajectory (true) or the predicted trajectory (false)
126  // of the CoM to compute the distance shoulder / contact point.
127  bool shoulder_reference_position;
128 
129  typename Eigen::Matrix<Scalar, 12, 1> uref_;
130 
131  typename Eigen::Matrix<Scalar, 12, 1> force_weights_;
132  typename Eigen::Matrix<Scalar, 12, 1> state_weights_;
133  typename Eigen::Matrix<Scalar, 8, 1> heuristic_weights_;
134  typename Eigen::Matrix<Scalar, 8, 1> stop_weights_;
135 
136  typename Eigen::Matrix<Scalar, 12, 12> A;
137  typename Eigen::Matrix<Scalar, 12, 12> B;
138  typename Eigen::Matrix<Scalar, 12, 1> g;
139  typename Eigen::Matrix<Scalar, 3, 3> R;
140  typename MathBase::Matrix3s R_tmp;
141  typename Eigen::Matrix<Scalar, 3, 3> gI;
142 
143  typename Eigen::Matrix<Scalar, 3, 4> lever_arms;
144  typename MathBase::Vector3s lever_tmp;
145  typename MathBase::MatrixXs xref_;
146 
147  // typename Eigen::Matrix<Scalar, 8, 1> pshoulder_;
148  typename Eigen::Matrix<Scalar, 2, 4> pshoulder_0;
149  // typename Eigen::Matrix<Scalar, 2, 4> pshoulder_tmp;
150 
151  // typename Eigen::Matrix<Scalar, 3, 1> pcentrifugal_tmp;
152  // typename Eigen::Matrix<Scalar, 3, 1> pcentrifugal_tmp_1;
153  // typename Eigen::Matrix<Scalar, 3, 1> pcentrifugal_tmp_2;
154 
155  typename Eigen::Matrix<Scalar, 8, 1> pstop_;
156  typename Eigen::Matrix<Scalar, 8, 1> pheuristic_;
157 
158  typename Eigen::Matrix<Scalar, 24, 1> ub;
159 
160  typename Eigen::Matrix<Scalar, 24, 1> Fa_x_u;
161  typename Eigen::Matrix<Scalar, 20, 1> rub_;
162  typename Eigen::Matrix<Scalar, 24, 1> rub_max_;
163  typename Eigen::Matrix<Scalar, 24, 24> Arr;
164  typename Eigen::Matrix<Scalar, 6, 1> r;
165  typename Eigen::Matrix<Scalar, 4, 1> gait;
166  typename Eigen::Matrix<Scalar, 8, 1> gait_double;
167 
168  typename Eigen::Matrix<Scalar, 3, 1> base_vector_x;
169  typename Eigen::Matrix<Scalar, 3, 1> base_vector_y;
170  typename Eigen::Matrix<Scalar, 3, 1> base_vector_z;
171  typename Eigen::Matrix<Scalar, 3, 1> forces_3d;
172 
173  // Cost relative to the shoulder height
174  typename Eigen::Matrix<Scalar, 3, 4> psh;
175  typename Eigen::Matrix<Scalar, 4, 1> sh_ub_max_;
176  typename Eigen::Matrix<Scalar, 4, 1> sh_weight;
177  typename Eigen::Matrix<Scalar, 3, 1> offset_com;
178  Scalar sh_hlim;
179 };
180 
181 template <typename _Scalar>
183  : public crocoddyl::ActionDataAbstractTpl<_Scalar> {
184  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
185 
186  typedef _Scalar Scalar;
187  typedef crocoddyl::MathBaseTpl<Scalar> MathBase;
188  typedef crocoddyl::ActionDataAbstractTpl<Scalar> Base;
189  using Base::cost;
190  using Base::Fu;
191  using Base::Fx;
192  using Base::Lu;
193  using Base::Luu;
194  using Base::Lx;
195  using Base::Lxu;
196  using Base::Lxx;
197  using Base::r;
198  using Base::xnext;
199 
200  template <template <typename Scalar> class Model>
201  explicit ActionDataQuadrupedAugmentedTpl(Model<Scalar>* const model)
202  : crocoddyl::ActionDataAbstractTpl<Scalar>(model) {}
203 };
204 
205 /* --- Details -------------------------------------------------------------- */
206 /* --- Details -------------------------------------------------------------- */
207 /* --- Details -------------------------------------------------------------- */
208 
211 
212 } // namespace quadruped_walkgen
213 
214 #include "quadruped_augmented.hxx"
215 
216 #endif
Definition: quadruped_augmented.hpp:14
const bool & get_centrifugal_term() const
Definition: quadruped_augmented.hxx:610
const Scalar & get_max_fz_contact() const
Definition: quadruped_augmented.hxx:582
void set_T_gait(const Scalar &T_gait_)
Definition: quadruped_augmented.hxx:638
const Eigen::Matrix< Scalar, 8, 1 > & get_heuristic_weights() const
Definition: quadruped_augmented.hxx:482
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.hxx:96
void set_heuristic_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented.hxx:486
const Scalar & get_shoulder_hlim() const
Definition: quadruped_augmented.hxx:645
void set_min_fz_contact(const Scalar &min_fz)
Definition: quadruped_augmented.hxx:575
void set_stop_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented.hxx:501
const Scalar & get_T_gait() const
Definition: quadruped_augmented.hxx:633
crocoddyl::ActionDataAbstractTpl< Scalar > ActionDataAbstract
Definition: quadruped_augmented.hpp:17
const Eigen::Matrix< Scalar, 12, 12 > & get_A() const
Definition: quadruped_augmented.hxx:673
void set_friction_weight(const Scalar &weight)
Definition: quadruped_augmented.hxx:516
const Scalar & get_min_fz_contact() const
Definition: quadruped_augmented.hxx:569
const Eigen::Matrix< Scalar, 4, 1 > & get_shoulder_contact_weight() const
Definition: quadruped_augmented.hxx:658
~ActionModelQuadrupedAugmentedTpl()
Definition: quadruped_augmented.hxx:93
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.hxx:708
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition: quadruped_augmented.hxx:467
const Scalar & get_friction_weight() const
Definition: quadruped_augmented.hxx:511
const Scalar & get_mu() const
Definition: quadruped_augmented.hxx:522
const bool & get_shoulder_reference_position() const
Definition: quadruped_augmented.hxx:623
void set_max_fz_contact(const Scalar &max_fz)
Definition: quadruped_augmented.hxx:588
void set_relative_forces(const bool &rel_forces)
Definition: quadruped_augmented.hxx:690
void set_gI(const typename MathBase::Matrix3s &inertia_matrix)
Definition: quadruped_augmented.hxx:558
const Scalar & get_dt() const
Definition: quadruped_augmented.hxx:541
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition: quadruped_augmented.hxx:442
void set_centrifugal_term(const bool &cent_term)
Definition: quadruped_augmented.hxx:615
void set_dt(const Scalar &dt)
Definition: quadruped_augmented.hxx:545
const bool & get_symmetry_term() const
Definition: quadruped_augmented.hxx:598
const Eigen::Matrix< Scalar, 3, 3 > & get_gI() const
Definition: quadruped_augmented.hxx:554
void set_shoulder_hlim(const Scalar &hlim)
Definition: quadruped_augmented.hxx:650
void set_force_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented.hxx:456
void set_mu(const Scalar &mu_coeff)
Definition: quadruped_augmented.hxx:526
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented.hxx:471
_Scalar Scalar
Definition: quadruped_augmented.hpp:16
crocoddyl::ActionModelAbstractTpl< Scalar > Base
Definition: quadruped_augmented.hpp:18
ActionModelQuadrupedAugmentedTpl(typename Eigen::Matrix< Scalar, 3, 1 > offset_CoM=Eigen::Matrix< Scalar, 3, 1 >::Zero())
Definition: quadruped_augmented.hxx:8
void set_mass(const Scalar &m)
Definition: quadruped_augmented.hxx:535
void set_symmetry_term(const bool &sym_term)
Definition: quadruped_augmented.hxx:603
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.hxx:215
void set_shoulder_contact_weight(const typename Eigen::Matrix< Scalar, 4, 1 > &weight)
Definition: quadruped_augmented.hxx:662
const Eigen::Matrix< Scalar, 12, 12 > & get_B() const
Definition: quadruped_augmented.hxx:678
void set_shoulder_reference_position(const bool &reference)
Definition: quadruped_augmented.hxx:627
const Eigen::Matrix< Scalar, 12, 1 > & get_force_weights() const
Definition: quadruped_augmented.hxx:452
const bool & get_relative_forces() const
Definition: quadruped_augmented.hxx:685
const Scalar & get_mass() const
Definition: quadruped_augmented.hxx:531
const Eigen::Matrix< Scalar, 8, 1 > & get_stop_weights() const
Definition: quadruped_augmented.hxx:497
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped_augmented.hpp:19
Definition: quadruped.hpp:11
ActionModelQuadrupedAugmentedTpl< double > ActionModelQuadrupedAugmented
Definition: quadruped_augmented.hpp:209
ActionDataQuadrupedAugmentedTpl< double > ActionDataQuadrupedAugmented
Definition: quadruped_augmented.hpp:210
Definition: quadruped_augmented.hpp:183
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped_augmented.hpp:187
ActionDataQuadrupedAugmentedTpl(Model< Scalar > *const model)
Definition: quadruped_augmented.hpp:201
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: quadruped_augmented.hpp:186
crocoddyl::ActionDataAbstractTpl< Scalar > Base
Definition: quadruped_augmented.hpp:188