Loading...
Searching...
No Matches
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
12template <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_;
87 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
137template <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
167typedef crocoddyl::ActionModelAbstractTpl<double> ActionModelAbstract;
168typedef crocoddyl::ActionDataAbstractTpl<double> ActionDataAbstract;
169typedef crocoddyl::StateAbstractTpl<double> StateAbstract;
170
171typedef crocoddyl::ActionModelUnicycleTpl<double> ActionModelUnicycle;
172typedef crocoddyl::ActionDataUnicycleTpl<double> ActionDataUnicycle;
173} // namespace quadruped_walkgen
174
175#include "quadruped.hxx"
176
177#endif
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition quadruped.hpp:19
void set_max_fz_contact(const Scalar &max_fz_)
Definition quadruped.hxx:361
const Scalar & get_mass() const
Definition quadruped.hxx:307
const Scalar & get_shoulder_weight() const
Definition quadruped.hxx:395
const Eigen::Matrix< Scalar, 3, 3 > & get_gI() const
Definition quadruped.hxx:330
void set_force_weights(const typename MathBase::VectorXs &weights)
Definition quadruped.hxx:261
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
const bool & get_implicit_integration() const
Definition quadruped.hxx:427
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition quadruped.hxx:273
const Scalar & get_friction_weight() const
Definition quadruped.hxx:288
const Eigen::Matrix< Scalar, 12, 1 > & get_force_weights() const
Definition quadruped.hxx:257
void set_mu(const Scalar &mu_coeff)
Definition quadruped.hxx:302
void set_friction_weight(const Scalar &weight)
Definition quadruped.hxx:292
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
const Scalar & get_shoulder_hlim() const
Definition quadruped.hxx:385
const bool & get_relative_forces() const
Definition quadruped.hxx:408
void set_shoulder_weight(const Scalar &weight)
Definition quadruped.hxx:399
_Scalar Scalar
Definition quadruped.hpp:16
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition quadruped.hxx:277
void set_mass(const Scalar &m)
Definition quadruped.hxx:311
void set_implicit_integration(const bool &implicit)
Definition quadruped.hxx:431
const Scalar & get_min_fz_contact() const
Definition quadruped.hxx:345
const Scalar & get_dt() const
Definition quadruped.hxx:317
void set_dt(const Scalar &dt)
Definition quadruped.hxx:321
crocoddyl::ActionModelAbstractTpl< Scalar > Base
Definition quadruped.hpp:18
void set_min_fz_contact(const Scalar &min_fz)
Definition quadruped.hxx:350
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
~ActionModelQuadrupedTpl()
Definition quadruped.hxx:72
const Eigen::Matrix< Scalar, 12, 12 > & get_A() const
Definition quadruped.hxx:375
void set_relative_forces(const bool &rel_forces)
Definition quadruped.hxx:412
const Scalar & get_max_fz_contact() const
Definition quadruped.hxx:356
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition quadruped.hxx:247
void set_gI(const typename MathBase::Matrix3s &inertia_matrix)
Definition quadruped.hxx:334
const Eigen::Matrix< Scalar, 12, 12 > & get_B() const
Definition quadruped.hxx:380
const Scalar & get_mu() const
Definition quadruped.hxx:298
void set_shoulder_hlim(const Scalar &hlim)
Definition quadruped.hxx:389
crocoddyl::ActionDataAbstractTpl< Scalar > ActionDataAbstract
Definition quadruped.hpp:17
Definition quadruped.hpp:11
crocoddyl::ActionModelAbstractTpl< double > ActionModelAbstract
Definition quadruped.hpp:167
ActionModelQuadrupedTpl< double > ActionModelQuadruped
Definition quadruped.hpp:165
crocoddyl::ActionModelUnicycleTpl< double > ActionModelUnicycle
Definition quadruped.hpp:171
crocoddyl::ActionDataUnicycleTpl< double > ActionDataUnicycle
Definition quadruped.hpp:172
crocoddyl::ActionDataAbstractTpl< double > ActionDataAbstract
Definition quadruped.hpp:168
crocoddyl::StateAbstractTpl< double > StateAbstract
Definition quadruped.hpp:169
ActionDataQuadrupedTpl< double > ActionDataQuadruped
Definition quadruped.hpp:166
Definition quadruped.hpp:139
crocoddyl::MathBaseTpl< Scalar > MathBase
Definition quadruped.hpp:143
ActionDataQuadrupedTpl(Model< Scalar > *const model)
Definition quadruped.hpp:157
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition quadruped.hpp:142
crocoddyl::ActionDataAbstractTpl< Scalar > Base
Definition quadruped.hpp:144