quadruped_nl.hpp
Go to the documentation of this file.
1 #ifndef __quadruped_walkgen_quadruped_nl_hpp__
2 #define __quadruped_walkgen_quadruped_nl_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 Scalar& get_friction_weight() const;
42  void set_friction_weight(const Scalar& weight);
43 
44  const Scalar& get_mu() const;
45  void set_mu(const Scalar& mu_coeff);
46 
47  const Scalar& get_mass() const;
48  void set_mass(const Scalar& m);
49 
50  const Scalar& get_dt() const;
51  void set_dt(const Scalar& dt);
52 
53  const typename Eigen::Matrix<Scalar, 3, 3>& get_gI() const;
54  void set_gI(const typename MathBase::Matrix3s& inertia_matrix);
55 
56  const Scalar& get_min_fz_contact() const;
57  void set_min_fz_contact(const Scalar& min_fz);
58 
59  const Scalar& get_max_fz_contact() const;
60  void set_max_fz_contact(const Scalar& max_fz_);
61 
62  // Set parameter relative to the shoulder height cost
63  const Scalar& get_shoulder_hlim() const;
64  void set_shoulder_hlim(const Scalar& hlim);
65 
66  const Scalar& get_shoulder_weight() const;
67  void set_shoulder_weight(const Scalar& weight);
68 
69  const bool& get_relative_forces() const;
70  void set_relative_forces(const bool& rel_forces);
71 
72  const bool& get_implicit_integration() const;
73  void set_implicit_integration(const bool& implicit);
74 
75  // Update the model depending if the foot in contact with the ground
76  // or the new lever arms
77  void update_model(const Eigen::Ref<const typename MathBase::MatrixXs>& l_feet,
78  const Eigen::Ref<const typename MathBase::MatrixXs>& xref,
79  const Eigen::Ref<const typename MathBase::MatrixXs>& S);
80 
81  // Get A & B matrix
82  const typename Eigen::Matrix<Scalar, 12, 12>& get_A() const;
83  const typename Eigen::Matrix<Scalar, 12, 12>& get_B() const;
84 
85  protected:
86  using Base::has_control_limits_;
88  using Base::nr_;
89  using Base::nu_;
90  using Base::state_;
91  using Base::u_lb_;
92  using Base::u_ub_;
93  using Base::unone_;
94 
95  private:
96  Scalar dt_;
97  Scalar mass;
98  Scalar mu;
99  Scalar friction_weight_;
100  Scalar min_fz_in_contact;
101  Scalar max_fz;
102  bool relative_forces;
103  bool implicit_integration;
104 
105  typename Eigen::Matrix<Scalar, 12, 1> uref_;
106 
107  typename Eigen::Matrix<Scalar, 12, 1> force_weights_;
108  typename Eigen::Matrix<Scalar, 12, 1> state_weights_;
109 
110  typename Eigen::Matrix<Scalar, 12, 12> A;
111  typename Eigen::Matrix<Scalar, 12, 12> B;
112  typename Eigen::Matrix<Scalar, 12, 1> g;
113  typename Eigen::Matrix<Scalar, 3, 3> I_inv;
114  typename MathBase::Matrix3s R_tmp;
115  typename Eigen::Matrix<Scalar, 3, 3> gI;
116 
117  typename Eigen::Matrix<Scalar, 3, 4> lever_arms;
118  typename MathBase::Vector3s lever_tmp;
119  typename MathBase::MatrixXs xref_;
120 
121  typename Eigen::Matrix<Scalar, 24, 1> ub;
122 
123  typename Eigen::Matrix<Scalar, 24, 1> Fa_x_u;
124  typename Eigen::Matrix<Scalar, 24, 1> rub_max_;
125  typename Eigen::Matrix<Scalar, 24, 24> Arr;
126  typename Eigen::Matrix<Scalar, 6, 1> r;
127  typename Eigen::Matrix<Scalar, 4, 1> gait;
128 
129  typename Eigen::Matrix<Scalar, 3, 1> base_vector_x;
130  typename Eigen::Matrix<Scalar, 3, 1> base_vector_y;
131  typename Eigen::Matrix<Scalar, 3, 1> base_vector_z;
132  typename Eigen::Matrix<Scalar, 3, 1> forces_3d;
133 
134  // Cost relative to the shoulder height
135  typename Eigen::Matrix<Scalar, 2, 4> pshoulder_0;
136  typename Eigen::Matrix<Scalar, 3, 4> psh;
137  typename Eigen::Matrix<Scalar, 4, 1> sh_ub_max_;
138  typename Eigen::Matrix<Scalar, 3, 1> offset_com;
139  Scalar sh_weight;
140  Scalar sh_hlim;
141 };
142 
143 template <typename _Scalar>
145  : public crocoddyl::ActionDataAbstractTpl<_Scalar> {
146  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
147 
148  typedef _Scalar Scalar;
149  typedef crocoddyl::MathBaseTpl<Scalar> MathBase;
150  typedef crocoddyl::ActionDataAbstractTpl<Scalar> Base;
151  using Base::cost;
152  using Base::Fu;
153  using Base::Fx;
154  using Base::Lu;
155  using Base::Luu;
156  using Base::Lx;
157  using Base::Lxu;
158  using Base::Lxx;
159  using Base::r;
160  using Base::xnext;
161 
162  template <template <typename Scalar> class Model>
163  explicit ActionDataQuadrupedNonLinearTpl(Model<Scalar>* const model)
164  : crocoddyl::ActionDataAbstractTpl<Scalar>(model) {}
165 };
166 
167 /* --- Details -------------------------------------------------------------- */
168 /* --- Details -------------------------------------------------------------- */
169 /* --- Details -------------------------------------------------------------- */
170 
173 
174 } // namespace quadruped_walkgen
175 
176 #include "quadruped_nl.hxx"
177 
178 #endif
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition: quadruped_nl.hxx:260
void set_mass(const Scalar &m)
Definition: quadruped_nl.hxx:325
const Scalar & get_friction_weight() const
Definition: quadruped_nl.hxx:301
crocoddyl::ActionModelAbstractTpl< Scalar > Base
Definition: quadruped_nl.hpp:18
void set_mu(const Scalar &mu_coeff)
Definition: quadruped_nl.hxx:316
const Scalar & get_mu() const
Definition: quadruped_nl.hxx:312
void set_relative_forces(const bool &rel_forces)
Definition: quadruped_nl.hxx:433
const Eigen::Matrix< Scalar, 12, 12 > & get_B() const
Definition: quadruped_nl.hxx:421
void set_shoulder_weight(const Scalar &weight)
Definition: quadruped_nl.hxx:405
void set_max_fz_contact(const Scalar &max_fz_)
Definition: quadruped_nl.hxx:378
const Scalar & get_dt() const
Definition: quadruped_nl.hxx:331
const Scalar & get_min_fz_contact() const
Definition: quadruped_nl.hxx:359
const Scalar & get_shoulder_hlim() const
Definition: quadruped_nl.hxx:388
const Eigen::Matrix< Scalar, 12, 1 > & get_force_weights() const
Definition: quadruped_nl.hxx:270
ActionModelQuadrupedNonLinearTpl(typename Eigen::Matrix< Scalar, 3, 1 > offset_CoM=Eigen::Matrix< Scalar, 3, 1 >::Zero())
Definition: quadruped_nl.hxx:8
~ActionModelQuadrupedNonLinearTpl()
Definition: quadruped_nl.hxx:76
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_nl.hxx:451
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_nl.hxx:79
void set_min_fz_contact(const Scalar &min_fz)
Definition: quadruped_nl.hxx:365
const bool & get_relative_forces() const
Definition: quadruped_nl.hxx:428
void set_friction_weight(const Scalar &weight)
Definition: quadruped_nl.hxx:306
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_nl.hxx:156
const Eigen::Matrix< Scalar, 3, 3 > & get_gI() const
Definition: quadruped_nl.hxx:344
_Scalar Scalar
Definition: quadruped_nl.hpp:16
const Eigen::Matrix< Scalar, 12, 12 > & get_A() const
Definition: quadruped_nl.hxx:416
void set_gI(const typename MathBase::Matrix3s &inertia_matrix)
Definition: quadruped_nl.hxx:348
void set_force_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_nl.hxx:274
void set_dt(const Scalar &dt)
Definition: quadruped_nl.hxx:335
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped_nl.hpp:19
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_nl.hxx:290
const Scalar & get_max_fz_contact() const
Definition: quadruped_nl.hxx:372
crocoddyl::ActionDataAbstractTpl< Scalar > ActionDataAbstract
Definition: quadruped_nl.hpp:17
void set_shoulder_hlim(const Scalar &hlim)
Definition: quadruped_nl.hxx:393
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition: quadruped_nl.hxx:286
const Scalar & get_mass() const
Definition: quadruped_nl.hxx:321
const Scalar & get_shoulder_weight() const
Definition: quadruped_nl.hxx:400
Definition: quadruped.hpp:11
ActionModelQuadrupedNonLinearTpl< double > ActionModelQuadrupedNonLinear
Definition: quadruped_nl.hpp:171
ActionDataQuadrupedNonLinearTpl< double > ActionDataQuadrupedNonLinear
Definition: quadruped_nl.hpp:172
ActionDataQuadrupedNonLinearTpl(Model< Scalar > *const model)
Definition: quadruped_nl.hpp:163
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: quadruped_nl.hpp:148
crocoddyl::ActionDataAbstractTpl< Scalar > Base
Definition: quadruped_nl.hpp:150
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition: quadruped_nl.hpp:149