GCC Code Coverage Report

Directory: ./
File: include/quadruped-walkgen/quadruped_nl.hpp
Date: 2024-12-02 00:24:10
Exec Total Coverage
Lines: 0 2 0.0%
Branches: 0 0 -%
Line Branch Exec Source
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>
13 class ActionModelQuadrupedNonLinearTpl
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 ActionModelQuadrupedNonLinearTpl(
22 typename Eigen::Matrix<Scalar, 3, 1> offset_CoM =
23 Eigen::Matrix<Scalar, 3, 1>::Zero());
24 ~ActionModelQuadrupedNonLinearTpl();
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_; //!< Indicates whether any of the control
87 //!< limits
88 using Base::nr_; //!< Dimension of the cost residual
89 using Base::nu_; //!< Control dimension
90 using Base::state_; //!< Model of the state
91 using Base::u_lb_; //!< Lower control limits
92 using Base::u_ub_; //!< Upper control limits
93 using Base::unone_; //!< Neutral state
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>
144 struct ActionDataQuadrupedNonLinearTpl
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
171 typedef ActionModelQuadrupedNonLinearTpl<double> ActionModelQuadrupedNonLinear;
172 typedef ActionDataQuadrupedNonLinearTpl<double> ActionDataQuadrupedNonLinear;
173
174 } // namespace quadruped_walkgen
175
176 #include "quadruped_nl.hxx"
177
178 #endif
179