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 stabilizeCOP(const eVector3 &actual_com, const eVector3 &actual_com_vel,
132  const eVector3 &actual_com_acc, const eVector3 &actual_cop,
133  const eMatrixHoms &actual_stance_poses,
134  const eVector3 &reference_com,
135  const eVector3 &reference_com_vel,
136  const eVector3 &reference_com_acc, eVector3 &desired_com,
137  eVector3 &desired_com_vel, eVector3 &desired_com_acc,
138  eVector3 &desired_icp, // ???
139  eVector3 &actual_icp, // ???
140  eVector3 &desired_cop_reference, // ???
141  eVector3 &desired_cop_computed);
142 
144  const eVector3 &actual_com, const eVector3 &actual_com_vel,
145  const eVector3 &actual_com_acc, const eVector3 &actual_cop,
146  const eMatrixHoms &actual_stance_poses, const eVector3 &reference_com,
147  const eVector3 &reference_com_vel, const eVector3 &reference_com_acc,
148  eVector3 &desired_com, eVector3 &desired_com_vel,
149  eVector3 &desired_com_acc,
150  eVector3 &desired_icp, // ???
151  eVector3 &actual_icp, // ???
152  eVector3 &desired_cop_reference, // ???
153  eVector3 &desired_cop_computed);
154 
155  void stabilizeP_CC(const eVector3 &actual_com, const eVector3 &actual_com_vel,
156  const eVector3 &actual_com_acc, const eVector3 &actual_cop,
157  const eMatrixHoms &actual_stance_poses,
158  const eVector3 &reference_com,
159  const eVector3 &reference_com_vel,
160  const eVector3 &reference_com_acc, eVector3 &desired_com,
161  eVector3 &desired_com_vel, eVector3 &desired_com_acc,
162  eVector3 &desired_icp, // ???
163  eVector3 &actual_icp, // ???
164  eVector3 &desired_cop_reference, // ???
165  eVector3 &desired_cop_computed);
166 
167  void stabilizeJerk(const eVector3 &actual_com, const eVector3 &actual_com_vel,
168  const eVector3 &actual_com_acc, const eVector3 &actual_cop,
169  const eMatrixHoms &actual_stance_poses,
170  const eVector3 &reference_com,
171  const eVector3 &reference_com_vel,
172  const eVector3 &reference_com_acc,
173  const eVector3 &reference_com_jerk, 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  double distributeForces(const eVector2 &desired_cop, const eVector2 LF_xy,
181  const double LF_force_z, const eVector2 LF_torque_xy,
182  const eVector2 RF_xy, const double RF_force_z,
183  const eVector2 RF_torque_xy);
184 
185  std::array<eVector3, 3> getStableCoMs(const double &com_height);
186 
187  private:
188  void computeSupportPolygon(const eMatrixHoms &stance_poses,
189  Polygon2D &convex_hull);
190 
191  void projectCOPinSupportPolygon(const eVector2 &target_cop_unclamped,
192  const Polygon2D &polygon,
193  eVector2 &target_cop);
194 
195  bool isPointInPolygon(eVector2 &point, Polygon2D &polygon);
196 
205  Eigen::Vector3d getActualCOM_acc(const Eigen::Vector3d &externalForce);
206 
216  template <typename T, typename vec_T>
217  T movingAverage(const T x, const unsigned long nb_samples, vec_T &queue);
218 
219  void getNonLinearPart(const eVector6 &leftFootWrench,
220  const eVector6 &rightFootWrench,
221  const Eigen::Vector2d &leftFootPlace,
222  const Eigen::Vector2d &rightFootPlace,
223  const Eigen::Vector2d &CoM,
224  const Eigen::Vector2d &lateral_gravity,
225  const Eigen::Vector2d &externalForce, eVector3 &n);
226 
227  void getNonLinearPart(const eVector6 &leftFootWrench,
228  const eVector6 &rightFootWrench,
229  const Eigen::Vector2d &leftFootPlace,
230  const Eigen::Vector2d &rightFootPlace,
231  const Eigen::Vector2d &CoM,
232  const Eigen::Vector2d &CoM_acc, eVector3 &n);
233 
234  void getNonLinearPart(const eVector6 &leftFootWrench,
235  const eVector6 &rightFootWrench,
236  const Eigen::Vector2d &leftFootPlace_c,
237  const Eigen::Vector2d &rightFootPlace_c,
238  const Eigen::Vector2d &CoM_acc, eVector3 &n);
239 
240  void getNonLinearPart(const eVector3 &com, const eVector3 &com_acc,
241  const eVector3 &cop, eVector3 &n);
242 
243  void getNonLinearPart(eVector3 &n);
244 
245  double estimateCopDisturbance(const eVector2 &currentTrackingError,
246  eVector2 &oldTrackingError,
247  const eVector2 &c_gainK);
248 
249  double estimateJerkDisturbance(const eVector3 &currentTrackingError,
250  eVector3 &oldTrackingError,
251  const eVector3 &c_gainK);
252 
253  bool configured_, first_iter_;
254  eMatrixRot A_, Aj_;
255  eVector3 B_, Bj_;
256  Eigen::Matrix2d A22_;
257  eVector2 B2_;
258  eVector3 cx_gainK_, cy_gainK_;
259  eVector2 cx_gainK2_, cy_gainK2_;
260  bool saturate_cop_;
261  Eigen::Matrix2d RotPi_2_;
262 
263  // sqrt(g/h), g/h
264  double w_, w2_;
265 
266  Eigen::Matrix3d S_coms_j_;
267  Eigen::Matrix<double, 3, 2> S_coms_;
268  Eigen::Vector3d U_coms_, U_coms_j_;
269 
270  Eigen::Vector3d old_reference_com_acc_;
271  eVector3s jerk_ma_queue_;
272  eVector2 oldTrackingError2_x_, oldTrackingError2_y_;
273  eVector3 oldTrackingError_x_, oldTrackingError_y_;
274  // Storing data for stable CoMs computations.
275  eVector3 actualState3d_x_, actualState3d_y_;
276  eVector2 actualState2d_x_, actualState2d_y_;
277  eVector2 actual_command_;
278 
279  // Support polygone internal variables.
280  eVector3s local_foot_description_;
281  eVector3s foot_description_;
282  Polygon2D support_polygon_;
283  std::vector<wykobi::point2d<double>> wykobi_foot_description_;
284  wykobi::point2d<double> wykobi_point2d_;
285 
286  // projectCOPinSupportPolygon internal variables.
287  wykobi::point2d<double> wykobi_cop_unclamped_;
288  wykobi::point2d<double> wykobi_cop_clamped_;
289 
290  // isPointInPolygon internal variables.
291  wykobi::point2d<double> wykobi_2d_point_;
292 
293  // Input settings.
294  CopStabilizerSettings settings_;
295 
296  protected:
303 };
304 } // namespace biped_stabilizer
305 #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:667
biped_stabilizer::CopStabilizer::target_com_
Eigen::Vector3d target_com_
Definition: cop_stabilizer.hpp:297
biped_stabilizer::CopStabilizer::estimated_disturbance_
eVector2 estimated_disturbance_
Definition: cop_stabilizer.hpp:302
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::CopStabilizer::stabilizeCOP
void stabilizeCOP(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, 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:217
biped_stabilizer::CopStabilizerSettings::cop_x_gains
eVector3 cop_x_gains
Definition: cop_stabilizer.hpp:40
biped_stabilizer::CopStabilizer::getStableCoMs
std::array< eVector3, 3 > getStableCoMs(const double &com_height)
Definition: cop_stabilizer.cpp:677
biped_stabilizer::CopStabilizer::errorSum_
eVector2 errorSum_
Definition: cop_stabilizer.hpp:300
biped_stabilizer::CopStabilizerSettings::dt
double dt
Definition: cop_stabilizer.hpp:39
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 eMatrixHoms &actual_stance_poses, 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:334
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:299
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:297
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:176
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:298
biped_stabilizer::CopStabilizer::cop_clamped
eVector2 cop_clamped
Definition: cop_stabilizer.hpp:301
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::target_cop_
eVector2 target_cop_
Definition: cop_stabilizer.hpp:299
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:10
biped_stabilizer::CopStabilizer::getSettings
const CopStabilizerSettings & getSettings()
Definition: cop_stabilizer.hpp:116
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:297
biped_stabilizer::CopStabilizer::configure
void configure(const CopStabilizerSettings &settings)
Definition: cop_stabilizer.cpp:26
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 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:553
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:24
biped_stabilizer::CopStabilizer::non_linear_
Eigen::Vector3d non_linear_
Definition: cop_stabilizer.hpp:298
biped_stabilizer::eVector6
Eigen::Matrix< double, 6, 1 > eVector6
Definition: cop_stabilizer.hpp: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 eMatrixHoms &actual_stance_poses, 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:450