cop_stabilizer.hpp
Go to the documentation of this file.
1 /*
2  * Copyright 2021 PAL Robotics SL. All Rights Reserved
3  *
4  * Unauthorized copying of this file, via any medium is strictly prohibited,
5  * unless it was supplied under the terms of a license agreement or
6  * nondisclosure agreement with PAL Robotics SL. In this case it may not be
7  * copied or disclosed except in accordance with the terms of that agreement.
8  */
9 
10 #ifndef BIPED_STABILIZER_COP_STABILIZER
11 #define BIPED_STABILIZER_COP_STABILIZER
12 
13 #include <Eigen/Dense>
14 #include <vector>
15 
17 
18 namespace biped_stabilizer {
19 
21 typedef Eigen::Matrix<double, 2, 1> eVector2;
22 typedef Eigen::Matrix<double, 3, 1> eVector3;
23 typedef Eigen::Matrix<double, 6, 1> eVector6;
24 typedef Eigen::Isometry3d eMatrixHom;
25 typedef Eigen::Matrix3d eMatrixRot;
26 typedef std::vector<eMatrixHom, Eigen::aligned_allocator<eMatrixHom>>
28 typedef std::vector<eVector2, Eigen::aligned_allocator<eVector2>> eVector2s;
29 typedef std::vector<eVector3, Eigen::aligned_allocator<eVector3>> eVector3s;
30 
32  public:
33  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
34 
35  double height = 0.0;
36  double foot_length = 0.0;
37  double foot_width = 0.0;
38  double robot_mass = 0.0;
39  double dt = 0.0;
40  eVector3 cop_x_gains = eVector3::Zero();
41  eVector3 cop_y_gains = eVector3::Zero();
42  double cop_p_cc_gain = 0.0;
43  eVector2 integral_gain = eVector2::Zero();
44 
45  // Meaningfull defaults.
46  double g = 9.81;
47  std::string cop_control_type = "p_cc";
48  bool saturate_cop = true;
49  bool use_rate_limited_dcm = false;
50 
51  bool operator==(const CopStabilizerSettings &rhs) {
52  bool test = true;
53  test &= this->height == rhs.height;
54  test &= this->foot_length == rhs.foot_length;
55  test &= this->foot_width == rhs.foot_width;
56  test &= this->robot_mass == rhs.robot_mass;
57  test &= this->dt == rhs.dt;
58  test &= this->cop_x_gains == rhs.cop_x_gains;
59  test &= this->cop_y_gains == rhs.cop_y_gains;
60  test &= this->cop_p_cc_gain == rhs.cop_p_cc_gain;
61  test &= this->integral_gain == rhs.integral_gain;
62  test &= this->g == rhs.g;
63  test &= this->saturate_cop == rhs.saturate_cop;
64  test &= this->use_rate_limited_dcm == rhs.use_rate_limited_dcm;
65  return test;
66  }
67 
68  bool operator!=(const CopStabilizerSettings &rhs) { return !(*this == rhs); }
69 
70  std::string to_string() {
71  std::ostringstream oss;
72  oss << "CopStabilizerSettings:" << std::endl;
73  oss << " - height = " << this->height << std::endl;
74  oss << " - foot_length = " << this->foot_length << std::endl;
75  oss << " - foot_width = " << this->foot_width << std::endl;
76  oss << " - robot_mass = " << this->robot_mass << std::endl;
77  oss << " - dt = " << this->dt << std::endl;
78  oss << " - cop_x_gains = " << this->cop_x_gains.transpose() << std::endl;
79  oss << " - cop_y_gains = " << this->cop_y_gains.transpose() << std::endl;
80  oss << " - cop_p_cc_gain = " << this->cop_p_cc_gain << std::endl;
81  oss << " - integral_gain = " << this->integral_gain.transpose()
82  << std::endl;
83  oss << " - g = " << this->g << std::endl;
84  oss << " - cop_control_type = " << this->cop_control_type << std::endl;
85  oss << " - saturate_cop = " << this->saturate_cop << std::endl;
86  oss << " - use_rate_limited_dcm = " << this->use_rate_limited_dcm
87  << std::endl;
88  return oss.str();
89  }
90 
91  std::ostream &operator<<(std::ostream &out) {
92  out << this->to_string();
93  return out;
94  }
95 };
96 
98  public:
99  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
100 
101  CopStabilizer();
102 
110  CopStabilizer(const CopStabilizerSettings &settings);
111 
112  virtual ~CopStabilizer();
113 
114  void configure(const CopStabilizerSettings &settings);
115 
116  const CopStabilizerSettings &getSettings() { return settings_; }
117 
118  void stabilize(const eVector3 &actual_com, const eVector3 &actual_com_vel,
119  const eVector3 &actual_com_acc, const eVector3 &actual_cop,
120  const eMatrixHoms &actual_stance_poses,
121  const eVector3 &reference_com,
122  const eVector3 &reference_com_vel,
123  const eVector3 &reference_com_acc,
124  const eVector3 &reference_com_jerk, eVector3 &desired_com,
125  eVector3 &desired_com_vel, eVector3 &desired_com_acc,
126  eVector3 &desired_icp, // ???
127  eVector3 &actual_icp, // ???
128  eVector3 &desired_cop_reference, // ???
129  eVector3 &desired_cop_computed);
130 
131  void stabilize(const eVector3 &actual_com, const eVector3 &actual_com_vel,
132  const eVector3 &actual_com_acc, const eVector3 &actual_cop,
133  const Polygon2D &support_polygon,
134  const eVector3 &reference_com,
135  const eVector3 &reference_com_vel,
136  const eVector3 &reference_com_acc,
137  const eVector3 &reference_com_jerk, eVector3 &desired_com,
138  eVector3 &desired_com_vel, eVector3 &desired_com_acc,
139  eVector3 &desired_icp, // ???
140  eVector3 &actual_icp, // ???
141  eVector3 &desired_cop_reference, // ???
142  eVector3 &desired_cop_computed);
143 
144  void stabilizeCOP(const eVector3 &actual_com, const eVector3 &actual_com_vel,
145  const eVector3 &actual_com_acc, const eVector3 &actual_cop,
146  const Polygon2D &support_polygon,
147  const eVector3 &reference_com,
148  const eVector3 &reference_com_vel,
149  const eVector3 &reference_com_acc, eVector3 &desired_com,
150  eVector3 &desired_com_vel, eVector3 &desired_com_acc,
151  eVector3 &desired_icp, // ???
152  eVector3 &actual_icp, // ???
153  eVector3 &desired_cop_reference, // ???
154  eVector3 &desired_cop_computed);
155 
157  const eVector3 &actual_com, const eVector3 &actual_com_vel,
158  const eVector3 &actual_com_acc, const eVector3 &actual_cop,
159  const Polygon2D &support_polygon, const eVector3 &reference_com,
160  const eVector3 &reference_com_vel, const eVector3 &reference_com_acc,
161  eVector3 &desired_com, eVector3 &desired_com_vel,
162  eVector3 &desired_com_acc,
163  eVector3 &desired_icp, // ???
164  eVector3 &actual_icp, // ???
165  eVector3 &desired_cop_reference, // ???
166  eVector3 &desired_cop_computed);
167 
168  void stabilizeP_CC(const eVector3 &actual_com, const eVector3 &actual_com_vel,
169  const eVector3 &actual_com_acc, const eVector3 &actual_cop,
170  const Polygon2D &support_polygon,
171  const eVector3 &reference_com,
172  const eVector3 &reference_com_vel,
173  const eVector3 &reference_com_acc, eVector3 &desired_com,
174  eVector3 &desired_com_vel, eVector3 &desired_com_acc,
175  eVector3 &desired_icp, // ???
176  eVector3 &actual_icp, // ???
177  eVector3 &desired_cop_reference, // ???
178  eVector3 &desired_cop_computed);
179 
180  void stabilizeJerk(const eVector3 &actual_com, const eVector3 &actual_com_vel,
181  const eVector3 &actual_com_acc, const eVector3 &actual_cop,
182  const Polygon2D &support_polygon,
183  const eVector3 &reference_com,
184  const eVector3 &reference_com_vel,
185  const eVector3 &reference_com_acc,
186  const eVector3 &reference_com_jerk, eVector3 &desired_com,
187  eVector3 &desired_com_vel, eVector3 &desired_com_acc,
188  eVector3 &desired_icp, // ???
189  eVector3 &actual_icp, // ???
190  eVector3 &desired_cop_reference, // ???
191  eVector3 &desired_cop_computed);
192 
193  double distributeForces(const eVector2 &desired_cop, const eVector2 LF_xy,
194  const double LF_force_z, const eVector2 LF_torque_xy,
195  const eVector2 RF_xy, const double RF_force_z,
196  const eVector2 RF_torque_xy);
197 
198  std::array<eVector3, 3> getStableCoMs(const double &com_height);
199 
200  void setCOPgains(const eVector3 &cop_x_gains, eVector3 &cop_y_gains);
201 
202  void setPCCgains(const double cop_pcc_gains);
203 
204  void setIntegralGains(const eVector2 &integral_gains);
205 
206  private:
207  void computeSupportPolygon(const eMatrixHoms &stance_poses,
208  Polygon2D &convex_hull);
209 
210  void projectCOPinSupportPolygon(const eVector2 &target_cop_unclamped,
211  const Polygon2D &polygon,
212  eVector2 &target_cop);
213 
214  bool isPointInPolygon(const eVector2 &point, const Polygon2D &polygon);
215 
224  Eigen::Vector3d getActualCOM_acc(const Eigen::Vector3d &externalForce);
225 
235  template <typename T, typename vec_T>
236  T movingAverage(const T x, const unsigned long nb_samples, vec_T &queue);
237 
238  void getNonLinearPart(const eVector6 &leftFootWrench,
239  const eVector6 &rightFootWrench,
240  const Eigen::Vector2d &leftFootPlace,
241  const Eigen::Vector2d &rightFootPlace,
242  const Eigen::Vector2d &CoM,
243  const Eigen::Vector2d &lateral_gravity,
244  const Eigen::Vector2d &externalForce, eVector3 &n);
245 
246  void getNonLinearPart(const eVector6 &leftFootWrench,
247  const eVector6 &rightFootWrench,
248  const Eigen::Vector2d &leftFootPlace,
249  const Eigen::Vector2d &rightFootPlace,
250  const Eigen::Vector2d &CoM,
251  const Eigen::Vector2d &CoM_acc, eVector3 &n);
252 
253  void getNonLinearPart(const eVector6 &leftFootWrench,
254  const eVector6 &rightFootWrench,
255  const Eigen::Vector2d &leftFootPlace_c,
256  const Eigen::Vector2d &rightFootPlace_c,
257  const Eigen::Vector2d &CoM_acc, eVector3 &n);
258 
259  void getNonLinearPart(const eVector3 &com, const eVector3 &com_acc,
260  const eVector3 &cop, eVector3 &n);
261 
262  void getNonLinearPart(eVector3 &n);
263 
264  double estimateCopDisturbance(const eVector2 &currentTrackingError,
265  eVector2 &oldTrackingError,
266  const eVector2 &c_gainK);
267 
268  double estimateJerkDisturbance(const eVector3 &currentTrackingError,
269  eVector3 &oldTrackingError,
270  const eVector3 &c_gainK);
271 
272  bool configured_, first_iter_;
273  eMatrixRot A_, Aj_;
274  eVector3 B_, Bj_;
275  Eigen::Matrix2d A22_;
276  eVector2 B2_;
277  eVector3 cx_gainK_, cy_gainK_;
278  eVector2 cx_gainK2_, cy_gainK2_;
279  bool saturate_cop_;
280  Eigen::Matrix2d RotPi_2_;
281 
282  // sqrt(g/h), g/h
283  double w_, w2_;
284 
285  Eigen::Matrix3d S_coms_j_;
286  Eigen::Matrix<double, 3, 2> S_coms_;
287  Eigen::Vector3d U_coms_, U_coms_j_;
288 
289  Eigen::Vector3d old_reference_com_acc_;
290  eVector3s jerk_ma_queue_;
291  eVector2 oldTrackingError2_x_, oldTrackingError2_y_;
292  eVector3 oldTrackingError_x_, oldTrackingError_y_;
293  // Storing data for stable CoMs computations.
294  eVector3 actualState3d_x_, actualState3d_y_;
295  eVector2 actualState2d_x_, actualState2d_y_;
296  eVector2 actual_command_;
297 
298  // Support polygone internal variables.
299  eVector3s local_foot_description_;
300  eVector3s foot_description_;
301  Polygon2D support_polygon_;
302  std::vector<wykobi::point2d<double>> wykobi_foot_description_;
303  wykobi::point2d<double> wykobi_point2d_;
304 
305  // projectCOPinSupportPolygon internal variables.
306  wykobi::point2d<double> wykobi_cop_unclamped_;
307  wykobi::point2d<double> wykobi_cop_clamped_;
308 
309  // isPointInPolygon internal variables.
310  wykobi::point2d<double> wykobi_2d_point_;
311 
312  // Input settings.
313  CopStabilizerSettings settings_;
314 
315  protected:
322 };
323 } // namespace biped_stabilizer
324 #endif // BIPED_STABILIZER_COP_STABILIZER
biped_stabilizer::CopStabilizerSettings::cop_p_cc_gain
double cop_p_cc_gain
Definition: cop_stabilizer.hpp:42
biped_stabilizer::CopStabilizer::distributeForces
double distributeForces(const eVector2 &desired_cop, const eVector2 LF_xy, const double LF_force_z, const eVector2 LF_torque_xy, const eVector2 RF_xy, const double RF_force_z, const eVector2 RF_torque_xy)
Definition: cop_stabilizer.cpp:677
biped_stabilizer::CopStabilizer::target_com_
Eigen::Vector3d target_com_
Definition: cop_stabilizer.hpp:316
biped_stabilizer::CopStabilizer::estimated_disturbance_
eVector2 estimated_disturbance_
Definition: cop_stabilizer.hpp:321
biped_stabilizer::eVector3
Eigen::Matrix< double, 3, 1 > eVector3
Definition: cop_stabilizer.hpp:22
biped_stabilizer::CopStabilizerSettings::cop_y_gains
eVector3 cop_y_gains
Definition: cop_stabilizer.hpp:41
biped_stabilizer
Definition: cop_stabilizer.hpp:18
biped_stabilizer::CopStabilizerSettings::height
EIGEN_MAKE_ALIGNED_OPERATOR_NEW double height
Definition: cop_stabilizer.hpp:35
biped_stabilizer::Polygon2D
wykobi::polygon< double, 2 > Polygon2D
Definition: cop_stabilizer.hpp:20
biped_stabilizer::eVector2
Eigen::Matrix< double, 2, 1 > eVector2
Definition: cop_stabilizer.hpp:21
biped_stabilizer::CopStabilizer
Definition: cop_stabilizer.hpp:97
wykobi::polygon< double, 2 >
biped_stabilizer::CopStabilizerSettings::cop_x_gains
eVector3 cop_x_gains
Definition: cop_stabilizer.hpp:40
biped_stabilizer::CopStabilizer::stabilizeCOP
void stabilizeCOP(const eVector3 &actual_com, const eVector3 &actual_com_vel, const eVector3 &actual_com_acc, const eVector3 &actual_cop, const Polygon2D &support_polygon, const eVector3 &reference_com, const eVector3 &reference_com_vel, const eVector3 &reference_com_acc, eVector3 &desired_com, eVector3 &desired_com_vel, eVector3 &desired_com_acc, eVector3 &desired_icp, eVector3 &actual_icp, eVector3 &desired_cop_reference, eVector3 &desired_cop_computed)
Definition: cop_stabilizer.cpp:236
biped_stabilizer::CopStabilizer::getStableCoMs
std::array< eVector3, 3 > getStableCoMs(const double &com_height)
Definition: cop_stabilizer.cpp:687
biped_stabilizer::CopStabilizer::errorSum_
eVector2 errorSum_
Definition: cop_stabilizer.hpp:319
biped_stabilizer::CopStabilizerSettings::dt
double dt
Definition: cop_stabilizer.hpp:39
biped_stabilizer::CopStabilizerSettings::operator!=
bool operator!=(const CopStabilizerSettings &rhs)
Definition: cop_stabilizer.hpp:68
biped_stabilizer::CopStabilizerSettings::foot_width
double foot_width
Definition: cop_stabilizer.hpp:37
biped_stabilizer::CopStabilizerSettings::g
double g
Definition: cop_stabilizer.hpp:46
biped_stabilizer::CopStabilizerSettings::operator<<
std::ostream & operator<<(std::ostream &out)
Definition: cop_stabilizer.hpp:91
biped_stabilizer::CopStabilizerSettings::integral_gain
eVector2 integral_gain
Definition: cop_stabilizer.hpp:43
biped_stabilizer::CopStabilizer::desired_uncampled_cop_
eVector2 desired_uncampled_cop_
Definition: cop_stabilizer.hpp:318
biped_stabilizer::eMatrixHoms
std::vector< eMatrixHom, Eigen::aligned_allocator< eMatrixHom > > eMatrixHoms
Definition: cop_stabilizer.hpp:27
biped_stabilizer::CopStabilizer::target_com_acc_
Eigen::Vector3d target_com_acc_
Definition: cop_stabilizer.hpp:316
biped_stabilizer::CopStabilizer::setPCCgains
void setPCCgains(const double cop_pcc_gains)
Definition: cop_stabilizer.cpp:836
biped_stabilizer::eMatrixHom
Eigen::Isometry3d eMatrixHom
Definition: cop_stabilizer.hpp:24
biped_stabilizer::CopStabilizer::stabilize
void stabilize(const eVector3 &actual_com, const eVector3 &actual_com_vel, const eVector3 &actual_com_acc, const eVector3 &actual_cop, const eMatrixHoms &actual_stance_poses, const eVector3 &reference_com, const eVector3 &reference_com_vel, const eVector3 &reference_com_acc, const eVector3 &reference_com_jerk, eVector3 &desired_com, eVector3 &desired_com_vel, eVector3 &desired_com_acc, eVector3 &desired_icp, eVector3 &actual_icp, eVector3 &desired_cop_reference, eVector3 &desired_cop_computed)
Definition: cop_stabilizer.cpp:174
biped_stabilizer::CopStabilizerSettings
Definition: cop_stabilizer.hpp:31
wykobi::point2d< double >
biped_stabilizer::CopStabilizer::target_com_jerk_
Eigen::Vector3d target_com_jerk_
Definition: cop_stabilizer.hpp:317
biped_stabilizer::CopStabilizer::cop_clamped
eVector2 cop_clamped
Definition: cop_stabilizer.hpp:320
biped_stabilizer::CopStabilizer::setIntegralGains
void setIntegralGains(const eVector2 &integral_gains)
Definition: cop_stabilizer.cpp:842
biped_stabilizer::eVector3s
std::vector< eVector3, Eigen::aligned_allocator< eVector3 > > eVector3s
Definition: cop_stabilizer.hpp:29
wykobi.hpp
biped_stabilizer::CopStabilizerSettings::robot_mass
double robot_mass
Definition: cop_stabilizer.hpp:38
biped_stabilizer::CopStabilizerSettings::to_string
std::string to_string()
Definition: cop_stabilizer.hpp:70
biped_stabilizer::CopStabilizer::stabilizeApproximateAcceleration
void stabilizeApproximateAcceleration(const eVector3 &actual_com, const eVector3 &actual_com_vel, const eVector3 &actual_com_acc, const eVector3 &actual_cop, const Polygon2D &support_polygon, const eVector3 &reference_com, const eVector3 &reference_com_vel, const eVector3 &reference_com_acc, eVector3 &desired_com, eVector3 &desired_com_vel, eVector3 &desired_com_acc, eVector3 &desired_icp, eVector3 &actual_icp, eVector3 &desired_cop_reference, eVector3 &desired_cop_computed)
Definition: cop_stabilizer.cpp:349
biped_stabilizer::CopStabilizer::target_cop_
eVector2 target_cop_
Definition: cop_stabilizer.hpp:318
biped_stabilizer::CopStabilizerSettings::cop_control_type
std::string cop_control_type
Definition: cop_stabilizer.hpp:47
biped_stabilizer::CopStabilizerSettings::operator==
bool operator==(const CopStabilizerSettings &rhs)
Definition: cop_stabilizer.hpp:51
biped_stabilizer::eVector2s
std::vector< eVector2, Eigen::aligned_allocator< eVector2 > > eVector2s
Definition: cop_stabilizer.hpp:28
biped_stabilizer::CopStabilizerSettings::saturate_cop
bool saturate_cop
Definition: cop_stabilizer.hpp:48
biped_stabilizer::CopStabilizer::CopStabilizer
EIGEN_MAKE_ALIGNED_OPERATOR_NEW CopStabilizer()
Definition: cop_stabilizer.cpp:9
biped_stabilizer::CopStabilizer::getSettings
const CopStabilizerSettings & getSettings()
Definition: cop_stabilizer.hpp:116
biped_stabilizer::CopStabilizer::setCOPgains
void setCOPgains(const eVector3 &cop_x_gains, eVector3 &cop_y_gains)
Definition: cop_stabilizer.cpp:828
biped_stabilizer::CopStabilizerSettings::foot_length
double foot_length
Definition: cop_stabilizer.hpp:36
biped_stabilizer::CopStabilizer::target_com_vel_
Eigen::Vector3d target_com_vel_
Definition: cop_stabilizer.hpp:316
biped_stabilizer::CopStabilizer::configure
void configure(const CopStabilizerSettings &settings)
Definition: cop_stabilizer.cpp:25
biped_stabilizer::CopStabilizerSettings::use_rate_limited_dcm
bool use_rate_limited_dcm
Definition: cop_stabilizer.hpp:49
biped_stabilizer::eMatrixRot
Eigen::Matrix3d eMatrixRot
Definition: cop_stabilizer.hpp:25
biped_stabilizer::CopStabilizer::~CopStabilizer
virtual ~CopStabilizer()
Definition: cop_stabilizer.cpp:23
biped_stabilizer::CopStabilizer::stabilizeP_CC
void stabilizeP_CC(const eVector3 &actual_com, const eVector3 &actual_com_vel, const eVector3 &actual_com_acc, const eVector3 &actual_cop, const Polygon2D &support_polygon, const eVector3 &reference_com, const eVector3 &reference_com_vel, const eVector3 &reference_com_acc, eVector3 &desired_com, eVector3 &desired_com_vel, eVector3 &desired_com_acc, eVector3 &desired_icp, eVector3 &actual_icp, eVector3 &desired_cop_reference, eVector3 &desired_cop_computed)
Definition: cop_stabilizer.cpp:464
biped_stabilizer::CopStabilizer::non_linear_
Eigen::Vector3d non_linear_
Definition: cop_stabilizer.hpp:317
biped_stabilizer::eVector6
Eigen::Matrix< double, 6, 1 > eVector6
Definition: cop_stabilizer.hpp:23
biped_stabilizer::CopStabilizer::stabilizeJerk
void stabilizeJerk(const eVector3 &actual_com, const eVector3 &actual_com_vel, const eVector3 &actual_com_acc, const eVector3 &actual_cop, const Polygon2D &support_polygon, const eVector3 &reference_com, const eVector3 &reference_com_vel, const eVector3 &reference_com_acc, const eVector3 &reference_com_jerk, eVector3 &desired_com, eVector3 &desired_com_vel, eVector3 &desired_com_acc, eVector3 &desired_icp, eVector3 &actual_icp, eVector3 &desired_cop_reference, eVector3 &desired_cop_computed)
Definition: cop_stabilizer.cpp:565