quadruped.hpp
Go to the documentation of this file.
1 #ifndef __quadruped_walkgen_quadruped_hpp__
2 #define __quadruped_walkgen_quadruped_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 
21  ActionModelQuadrupedTpl(typename Eigen::Matrix<Scalar, 3, 1> offset_CoM =
22  Eigen::Matrix<Scalar, 3, 1>::Zero());
24 
25  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data,
26  const Eigen::Ref<const typename MathBase::VectorXs>& x,
27  const Eigen::Ref<const typename MathBase::VectorXs>& u);
28  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data,
29  const Eigen::Ref<const typename MathBase::VectorXs>& x,
30  const Eigen::Ref<const typename MathBase::VectorXs>& u);
31  virtual boost::shared_ptr<ActionDataAbstract> createData();
32 
33  // Get and Set weights vectors : state , force & friction cone :
34  const typename Eigen::Matrix<Scalar, 12, 1>& get_force_weights() const;
35  void set_force_weights(const typename MathBase::VectorXs& weights);
36 
37  const typename Eigen::Matrix<Scalar, 12, 1>& get_state_weights() const;
38  void set_state_weights(const typename MathBase::VectorXs& weights);
39 
40  const Scalar& get_friction_weight() const;
41  void set_friction_weight(const Scalar& weight);
42 
43  const Scalar& get_mu() const;
44  void set_mu(const Scalar& mu_coeff);
45 
46  const Scalar& get_mass() const;
47  void set_mass(const Scalar& m);
48 
49  const Scalar& get_dt() const;
50  void set_dt(const Scalar& dt);
51 
52  const typename Eigen::Matrix<Scalar, 3, 3>& get_gI() const;
53  void set_gI(const typename MathBase::Matrix3s& inertia_matrix);
54 
55  const Scalar& get_min_fz_contact() const;
56  void set_min_fz_contact(const Scalar& min_fz);
57 
58  const Scalar& get_max_fz_contact() const;
59  void set_max_fz_contact(const Scalar& max_fz_);
60 
61  // Set parameter relative to the shoulder height cost
62  const Scalar& get_shoulder_hlim() const;
63  void set_shoulder_hlim(const Scalar& hlim);
64 
65  const Scalar& get_shoulder_weight() const;
66  void set_shoulder_weight(const Scalar& weight);
67 
68  const bool& get_relative_forces() const;
69  void set_relative_forces(const bool& rel_forces);
70 
71  const bool& get_implicit_integration() const;
72  void set_implicit_integration(const bool& implicit);
73 
74  // Update the model depending if the foot in contact with the ground
75  // or the new lever arms
76  void update_model(const Eigen::Ref<const typename MathBase::MatrixXs>& l_feet,
77  const Eigen::Ref<const typename MathBase::MatrixXs>& xref,
78  const Eigen::Ref<const typename MathBase::MatrixXs>& S);
79 
80  // Get A & B matrix
81  const typename Eigen::Matrix<Scalar, 12, 12>& get_A() const;
82  const typename Eigen::Matrix<Scalar, 12, 12>& get_B() const;
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 dt_;
96  Scalar mass;
97  Scalar mu;
98  Scalar friction_weight_;
99  Scalar min_fz_in_contact;
100  Scalar max_fz;
101  bool relative_forces;
102  bool implicit_integration;
103 
104  typename Eigen::Matrix<Scalar, 12, 1> uref_;
105 
106  typename Eigen::Matrix<Scalar, 12, 1> force_weights_;
107  typename Eigen::Matrix<Scalar, 12, 1> state_weights_;
108 
109  typename Eigen::Matrix<Scalar, 12, 12> A;
110  typename Eigen::Matrix<Scalar, 12, 12> B;
111  typename Eigen::Matrix<Scalar, 12, 1> g;
112  typename Eigen::Matrix<Scalar, 3, 3> I_inv;
113  typename MathBase::Matrix3s R_tmp;
114  typename Eigen::Matrix<Scalar, 3, 3> gI;
115 
116  typename Eigen::Matrix<Scalar, 3, 4> lever_arms;
117  typename MathBase::Vector3s lever_tmp;
118  typename MathBase::MatrixXs xref_;
119 
120  typename Eigen::Matrix<Scalar, 24, 1> ub;
121 
122  typename Eigen::Matrix<Scalar, 24, 1> Fa_x_u;
123  typename Eigen::Matrix<Scalar, 24, 1> rub_max_;
124  typename Eigen::Matrix<Scalar, 24, 24> Arr;
125  typename Eigen::Matrix<Scalar, 6, 1> r;
126 
127  // Cost relative to the shoulder height
128  typename Eigen::Matrix<Scalar, 2, 4> pshoulder_0;
129  typename Eigen::Matrix<Scalar, 3, 4> psh;
130  typename Eigen::Matrix<Scalar, 4, 1> sh_ub_max_;
131  typename Eigen::Matrix<Scalar, 4, 1> gait;
132  typename Eigen::Matrix<Scalar, 3, 1> offset_com;
133  Scalar sh_weight;
134  Scalar sh_hlim;
135 };
136 
137 template <typename _Scalar>
139  : public crocoddyl::ActionDataAbstractTpl<_Scalar> {
140  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
141 
142  typedef _Scalar Scalar;
143  typedef crocoddyl::MathBaseTpl<Scalar> MathBase;
144  typedef crocoddyl::ActionDataAbstractTpl<Scalar> Base;
145  using Base::cost;
146  using Base::Fu;
147  using Base::Fx;
148  using Base::Lu;
149  using Base::Luu;
150  using Base::Lx;
151  using Base::Lxu;
152  using Base::Lxx;
153  using Base::r;
154  using Base::xnext;
155 
156  template <template <typename Scalar> class Model>
157  explicit ActionDataQuadrupedTpl(Model<Scalar>* const model)
158  : crocoddyl::ActionDataAbstractTpl<Scalar>(model) {}
159 };
160 
161 /* --- Details -------------------------------------------------------------- */
162 /* --- Details -------------------------------------------------------------- */
163 /* --- Details -------------------------------------------------------------- */
164 
167 typedef crocoddyl::ActionModelAbstractTpl<double> ActionModelAbstract;
168 typedef crocoddyl::ActionDataAbstractTpl<double> ActionDataAbstract;
169 typedef crocoddyl::StateAbstractTpl<double> StateAbstract;
170 
171 typedef crocoddyl::ActionModelUnicycleTpl<double> ActionModelUnicycle;
172 typedef crocoddyl::ActionDataUnicycleTpl<double> ActionDataUnicycle;
173 } // namespace quadruped_walkgen
174 
175 #include "quadruped.hxx"
176 
177 #endif
quadruped_walkgen::ActionModelQuadrupedTpl::set_implicit_integration
void set_implicit_integration(const bool &implicit)
Definition: quadruped.hxx:431
quadruped_walkgen::ActionModelQuadrupedTpl::get_mu
const Scalar & get_mu() const
Definition: quadruped.hxx:298
quadruped_walkgen::ActionDataQuadrupedTpl::Scalar
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: quadruped.hpp:142
quadruped_walkgen::ActionModelQuadrupedTpl::get_A
const Eigen::Matrix< Scalar, 12, 12 > & get_A() const
Definition: quadruped.hxx:375
quadruped_walkgen::ActionModelQuadruped
ActionModelQuadrupedTpl< double > ActionModelQuadruped
Definition: quadruped.hpp:165
quadruped_walkgen::ActionModelQuadrupedTpl::set_force_weights
void set_force_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped.hxx:261
quadruped_walkgen::ActionModelQuadrupedTpl::~ActionModelQuadrupedTpl
~ActionModelQuadrupedTpl()
Definition: quadruped.hxx:72
quadruped_walkgen::ActionModelQuadrupedTpl::set_friction_weight
void set_friction_weight(const Scalar &weight)
Definition: quadruped.hxx:292
quadruped_walkgen::ActionModelQuadrupedTpl::get_B
const Eigen::Matrix< Scalar, 12, 12 > & get_B() const
Definition: quadruped.hxx:380
quadruped_walkgen::ActionModelQuadrupedTpl::get_dt
const Scalar & get_dt() const
Definition: quadruped.hxx:317
quadruped_walkgen::ActionDataQuadrupedTpl::ActionDataQuadrupedTpl
ActionDataQuadrupedTpl(Model< Scalar > *const model)
Definition: quadruped.hpp:157
quadruped_walkgen::ActionModelQuadrupedTpl::MathBase
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped.hpp:19
quadruped_walkgen::ActionDataAbstract
crocoddyl::ActionDataAbstractTpl< double > ActionDataAbstract
Definition: quadruped.hpp:168
quadruped_walkgen::ActionDataQuadruped
ActionDataQuadrupedTpl< double > ActionDataQuadruped
Definition: quadruped.hpp:166
quadruped_walkgen
Definition: quadruped.hpp:11
quadruped_walkgen::ActionModelQuadrupedTpl::get_max_fz_contact
const Scalar & get_max_fz_contact() const
Definition: quadruped.hxx:356
quadruped_walkgen::ActionModelQuadrupedTpl::get_state_weights
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition: quadruped.hxx:273
quadruped_walkgen::ActionModelQuadrupedTpl::ActionModelQuadrupedTpl
ActionModelQuadrupedTpl(typename Eigen::Matrix< Scalar, 3, 1 > offset_CoM=Eigen::Matrix< Scalar, 3, 1 >::Zero())
Definition: quadruped.hxx:8
quadruped_walkgen::ActionModelQuadrupedTpl::get_shoulder_hlim
const Scalar & get_shoulder_hlim() const
Definition: quadruped.hxx:385
quadruped_walkgen::ActionModelQuadrupedTpl::set_mass
void set_mass(const Scalar &m)
Definition: quadruped.hxx:311
quadruped_walkgen::ActionModelQuadrupedTpl::get_force_weights
const Eigen::Matrix< Scalar, 12, 1 > & get_force_weights() const
Definition: quadruped.hxx:257
quadruped_walkgen::ActionModelQuadrupedTpl::get_min_fz_contact
const Scalar & get_min_fz_contact() const
Definition: quadruped.hxx:345
quadruped_walkgen::ActionModelQuadrupedTpl::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.hxx:441
quadruped_walkgen::ActionModelQuadrupedTpl::set_shoulder_weight
void set_shoulder_weight(const Scalar &weight)
Definition: quadruped.hxx:399
quadruped_walkgen::ActionModelQuadrupedTpl::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.hxx:155
quadruped_walkgen::ActionModelQuadrupedTpl::ActionDataAbstract
crocoddyl::ActionDataAbstractTpl< Scalar > ActionDataAbstract
Definition: quadruped.hpp:17
quadruped_walkgen::ActionModelQuadrupedTpl::get_gI
const Eigen::Matrix< Scalar, 3, 3 > & get_gI() const
Definition: quadruped.hxx:330
quadruped_walkgen::ActionModelQuadrupedTpl::get_implicit_integration
const bool & get_implicit_integration() const
Definition: quadruped.hxx:427
quadruped_walkgen::StateAbstract
crocoddyl::StateAbstractTpl< double > StateAbstract
Definition: quadruped.hpp:169
quadruped_walkgen::ActionModelQuadrupedTpl::set_gI
void set_gI(const typename MathBase::Matrix3s &inertia_matrix)
Definition: quadruped.hxx:334
quadruped_walkgen::ActionModelQuadrupedTpl::set_dt
void set_dt(const Scalar &dt)
Definition: quadruped.hxx:321
quadruped_walkgen::ActionModelQuadrupedTpl::Scalar
_Scalar Scalar
Definition: quadruped.hpp:16
quadruped_walkgen::ActionDataQuadrupedTpl::MathBase
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped.hpp:143
quadruped_walkgen::ActionDataUnicycle
crocoddyl::ActionDataUnicycleTpl< double > ActionDataUnicycle
Definition: quadruped.hpp:172
quadruped_walkgen::ActionDataQuadrupedTpl::Base
crocoddyl::ActionDataAbstractTpl< Scalar > Base
Definition: quadruped.hpp:144
quadruped_walkgen::ActionModelQuadrupedTpl::get_mass
const Scalar & get_mass() const
Definition: quadruped.hxx:307
quadruped_walkgen::ActionModelQuadrupedTpl
Definition: quadruped.hpp:13
quadruped_walkgen::ActionDataQuadrupedTpl
Definition: quadruped.hpp:138
quadruped_walkgen::ActionModelQuadrupedTpl::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.hxx:75
quadruped_walkgen::ActionModelQuadrupedTpl::Base
crocoddyl::ActionModelAbstractTpl< Scalar > Base
Definition: quadruped.hpp:18
quadruped_walkgen::ActionModelUnicycle
crocoddyl::ActionModelUnicycleTpl< double > ActionModelUnicycle
Definition: quadruped.hpp:171
quadruped.hxx
quadruped_walkgen::ActionModelQuadrupedTpl::set_mu
void set_mu(const Scalar &mu_coeff)
Definition: quadruped.hxx:302
quadruped_walkgen::ActionModelQuadrupedTpl::createData
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition: quadruped.hxx:247
quadruped_walkgen::ActionModelQuadrupedTpl::get_shoulder_weight
const Scalar & get_shoulder_weight() const
Definition: quadruped.hxx:395
quadruped_walkgen::ActionModelQuadrupedTpl::set_shoulder_hlim
void set_shoulder_hlim(const Scalar &hlim)
Definition: quadruped.hxx:389
quadruped_walkgen::ActionModelQuadrupedTpl::set_max_fz_contact
void set_max_fz_contact(const Scalar &max_fz_)
Definition: quadruped.hxx:361
quadruped_walkgen::ActionModelQuadrupedTpl::set_relative_forces
void set_relative_forces(const bool &rel_forces)
Definition: quadruped.hxx:412
quadruped_walkgen::ActionModelQuadrupedTpl::set_min_fz_contact
void set_min_fz_contact(const Scalar &min_fz)
Definition: quadruped.hxx:350
quadruped_walkgen::ActionModelQuadrupedTpl::get_friction_weight
const Scalar & get_friction_weight() const
Definition: quadruped.hxx:288
quadruped_walkgen::ActionModelQuadrupedTpl::set_state_weights
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped.hxx:277
quadruped_walkgen::ActionModelAbstract
crocoddyl::ActionModelAbstractTpl< double > ActionModelAbstract
Definition: quadruped.hpp:167
quadruped_walkgen::ActionModelQuadrupedTpl::get_relative_forces
const bool & get_relative_forces() const
Definition: quadruped.hxx:408