Loading...
Searching...
No Matches
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
11namespace quadruped_walkgen {
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
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;
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;
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;
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;
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
143template <typename _Scalar>
145 : public crocoddyl::ActionDataAbstractTpl<_Scalar> {
147
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>
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
Definition quadruped_augmented_time.hpp:14
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