GCC Code Coverage Report

Directory: ./
File: include/quadruped-walkgen/quadruped_augmented_time.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_augmented_time_hpp__
2 #define __quadruped_walkgen_quadruped_augmented_time_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 ActionModelQuadrupedAugmentedTimeTpl
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 ActionModelQuadrupedAugmentedTimeTpl();
22 ~ActionModelQuadrupedAugmentedTimeTpl();
23
24 virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data,
25 const Eigen::Ref<const typename MathBase::VectorXs>& x,
26 const Eigen::Ref<const typename MathBase::VectorXs>& u);
27 virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data,
28 const Eigen::Ref<const typename MathBase::VectorXs>& x,
29 const Eigen::Ref<const typename MathBase::VectorXs>& u);
30 virtual boost::shared_ptr<ActionDataAbstract> createData();
31
32 // Get and Set weights vectors : state , force & friction cone :
33 const typename Eigen::Matrix<Scalar, 12, 1>& get_force_weights() const;
34 void set_force_weights(const typename MathBase::VectorXs& weights);
35
36 const typename Eigen::Matrix<Scalar, 12, 1>& get_state_weights() const;
37 void set_state_weights(const typename MathBase::VectorXs& weights);
38
39 const typename Eigen::Matrix<Scalar, 8, 1>& get_heuristic_weights() const;
40 void set_heuristic_weights(const typename MathBase::VectorXs& weights);
41
42 const typename Eigen::Matrix<Scalar, 8, 1>& get_stop_weights() const;
43 void set_stop_weights(const typename MathBase::VectorXs& weights);
44
45 const Scalar& get_friction_weight() const;
46 void set_friction_weight(const Scalar& weight);
47
48 const Scalar& get_mu() const;
49 void set_mu(const Scalar& mu_coeff);
50
51 const Scalar& get_mass() const;
52 void set_mass(const Scalar& m);
53
54 const Scalar& get_dt_ref() const;
55 void set_dt_ref(const Scalar& dt);
56
57 const Scalar& get_dt_min() const;
58 void set_dt_min(const Scalar& dt);
59
60 const Scalar& get_dt_max() const;
61 void set_dt_max(const Scalar& dt);
62
63 const typename Eigen::Matrix<Scalar, 3, 3>& get_gI() const;
64 void set_gI(const typename MathBase::Matrix3s& inertia_matrix);
65
66 const Scalar& get_min_fz_contact() const;
67 void set_min_fz_contact(const Scalar& min_fz);
68
69 const Scalar& get_max_fz_contact() const;
70 void set_max_fz_contact(const Scalar& max_fz_);
71
72 const typename Eigen::Matrix<Scalar, 8, 1>& get_shoulder_position() const;
73 void set_shoulder_position(const typename MathBase::VectorXs& weights);
74
75 const bool& get_symmetry_term() const;
76 void set_symmetry_term(const bool& sym_term);
77
78 const bool& get_centrifugal_term() const;
79 void set_centrifugal_term(const bool& cent_term);
80
81 const Scalar& get_T_gait() const;
82 void set_T_gait(const Scalar& T_gait_);
83
84 const Scalar& get_dt_weight() const;
85 void set_dt_weight(const Scalar& weight_);
86
87 const Scalar& get_dt_bound_weight() const;
88 void set_dt_bound_weight(const Scalar& weight_);
89
90 const bool& get_relative_forces() const;
91 void set_relative_forces(const bool& rel_forces);
92
93 // Set parameter relative to the shoulder height cost
94 const Scalar& get_shoulder_hlim() const;
95 void set_shoulder_hlim(const Scalar& hlim);
96
97 const Scalar& get_shoulder_contact_weight() const;
98 void set_shoulder_contact_weight(const Scalar& weight);
99
100 // Update the model depending if the foot in contact with the ground
101 // or the new lever arms
102 void update_model(const Eigen::Ref<const typename MathBase::MatrixXs>& l_feet,
103 const Eigen::Ref<const typename MathBase::MatrixXs>& l_stop,
104 const Eigen::Ref<const typename MathBase::MatrixXs>& xref,
105 const Eigen::Ref<const typename MathBase::MatrixXs>& S);
106
107 // Get A & B matrix
108 const typename Eigen::Matrix<Scalar, 12, 12>& get_A() const;
109 const typename Eigen::Matrix<Scalar, 12, 12>& get_B() const;
110
111 // get cost
112 const typename Eigen::Matrix<Scalar, 7, 1>& get_cost() const;
113
114 protected:
115 using Base::has_control_limits_; //!< Indicates whether any of the control
116 //!< limits
117 using Base::nr_; //!< Dimension of the cost residual
118 using Base::nu_; //!< Control dimension
119 using Base::state_; //!< Model of the state
120 using Base::u_lb_; //!< Lower control limits
121 using Base::u_ub_; //!< Upper control limits
122 using Base::unone_; //!< Neutral state
123
124 private:
125 Scalar dt_weight_;
126 Scalar mass;
127 Scalar mu;
128 Scalar friction_weight_;
129 Scalar min_fz_in_contact;
130 Scalar max_fz;
131 Scalar T_gait;
132 Scalar dt_bound_weight;
133 bool centrifugal_term;
134 bool symmetry_term;
135
136 bool relative_forces;
137 typename Eigen::Matrix<Scalar, 12, 1> uref_;
138
139 typename Eigen::Matrix<Scalar, 12, 1> force_weights_;
140 typename Eigen::Matrix<Scalar, 12, 1> state_weights_;
141 typename Eigen::Matrix<Scalar, 8, 1> heuristicWeights;
142 typename Eigen::Matrix<Scalar, 8, 1> last_position_weights_;
143
144 typename Eigen::Matrix<Scalar, 12, 12> A;
145 typename Eigen::Matrix<Scalar, 12, 12> B;
146 typename Eigen::Matrix<Scalar, 12, 1> g;
147 typename Eigen::Matrix<Scalar, 3, 3> R;
148 typename MathBase::Matrix3s R_tmp;
149 typename Eigen::Matrix<Scalar, 3, 3> gI;
150
151 typename Eigen::Matrix<Scalar, 3, 4> lever_arms;
152 typename MathBase::Vector3s lever_tmp;
153 typename MathBase::MatrixXs xref_;
154
155 typename Eigen::Matrix<Scalar, 8, 1> pshoulder_;
156 typename Eigen::Matrix<Scalar, 8, 1> pheuristic_;
157 typename Eigen::Matrix<Scalar, 2, 4> pshoulder_0;
158 typename Eigen::Matrix<Scalar, 2, 4> pshoulder_tmp;
159
160 typename Eigen::Matrix<Scalar, 3, 1> pcentrifugal_tmp;
161 typename Eigen::Matrix<Scalar, 3, 1> pcentrifugal_tmp_1;
162 typename Eigen::Matrix<Scalar, 3, 1> pcentrifugal_tmp_2;
163
164 typename Eigen::Matrix<Scalar, 8, 1> pref_;
165
166 typename Eigen::Matrix<Scalar, 24, 1> ub;
167
168 typename Eigen::Matrix<Scalar, 24, 1> Fa_x_u;
169 typename Eigen::Matrix<Scalar, 24, 1> rub_max_;
170 typename Eigen::Matrix<Scalar, 24, 24> Arr;
171 typename Eigen::Matrix<Scalar, 6, 1> r;
172 typename Eigen::Matrix<Scalar, 4, 1> gait;
173 typename Eigen::Matrix<Scalar, 8, 1> gait_double;
174
175 typename Eigen::Matrix<Scalar, 3, 1> base_vector_x;
176 typename Eigen::Matrix<Scalar, 3, 1> base_vector_y;
177 typename Eigen::Matrix<Scalar, 3, 1> base_vector_z;
178 typename Eigen::Matrix<Scalar, 3, 1> forces_3d;
179
180 typename Eigen::Matrix<Scalar, 1, 1> dt_min_;
181 typename Eigen::Matrix<Scalar, 1, 1> dt_max_;
182
183 typename Eigen::Matrix<Scalar, 2, 1> rub_max_dt;
184 typename Eigen::Matrix<Scalar, 2, 1> rub_max_dt_bool;
185
186 // Log cost
187 bool log_cost;
188 typename Eigen::Matrix<Scalar, 7, 1> cost_;
189
190 // Cost relative to the shoulder height
191 typename Eigen::Matrix<Scalar, 3, 4> psh;
192 typename Eigen::Matrix<Scalar, 4, 1> sh_ub_max_;
193 Scalar sh_weight;
194 Scalar sh_hlim;
195 };
196
197 template <typename _Scalar>
198 struct ActionDataQuadrupedAugmentedTimeTpl
199 : public crocoddyl::ActionDataAbstractTpl<_Scalar> {
200 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
201
202 typedef _Scalar Scalar;
203 typedef crocoddyl::MathBaseTpl<Scalar> MathBase;
204 typedef crocoddyl::ActionDataAbstractTpl<Scalar> Base;
205 using Base::cost;
206 using Base::Fu;
207 using Base::Fx;
208 using Base::Lu;
209 using Base::Luu;
210 using Base::Lx;
211 using Base::Lxu;
212 using Base::Lxx;
213 using Base::r;
214 using Base::xnext;
215
216 template <template <typename Scalar> class Model>
217 ✗ explicit ActionDataQuadrupedAugmentedTimeTpl(Model<Scalar>* const model)
218 ✗ : crocoddyl::ActionDataAbstractTpl<Scalar>(model) {}
219 };
220
221 /* --- Details -------------------------------------------------------------- */
222 /* --- Details -------------------------------------------------------------- */
223 /* --- Details -------------------------------------------------------------- */
224
225 typedef ActionModelQuadrupedAugmentedTimeTpl<double>
226 ActionModelQuadrupedAugmentedTime;
227 typedef ActionDataQuadrupedAugmentedTimeTpl<double>
228 ActionDataQuadrupedAugmentedTime;
229
230 } // namespace quadruped_walkgen
231
232 #include "quadruped_augmented_time.hxx"
233
234 #endif
235