Go to the documentation of this file.
10 #ifndef BIPED_STABILIZER_COP_STABILIZER
11 #define BIPED_STABILIZER_COP_STABILIZER
13 #include <Eigen/Dense>
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;
33 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
53 test &= this->height == rhs.
height;
57 test &= this->dt == rhs.
dt;
62 test &= this->g == rhs.
g;
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()
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
99 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
194 const double LF_force_z,
const eVector2 LF_torque_xy,
195 const eVector2 RF_xy,
const double RF_force_z,
198 std::array<eVector3, 3>
getStableCoMs(
const double &com_height);
207 void computeSupportPolygon(
const eMatrixHoms &stance_poses,
210 void projectCOPinSupportPolygon(
const eVector2 &target_cop_unclamped,
224 Eigen::Vector3d getActualCOM_acc(
const Eigen::Vector3d &externalForce);
235 template <
typename T,
typename vec_T>
236 T movingAverage(
const T x,
const unsigned long nb_samples, vec_T &queue);
238 void getNonLinearPart(
const eVector6 &leftFootWrench,
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);
246 void getNonLinearPart(
const eVector6 &leftFootWrench,
248 const Eigen::Vector2d &leftFootPlace,
249 const Eigen::Vector2d &rightFootPlace,
250 const Eigen::Vector2d &CoM,
251 const Eigen::Vector2d &CoM_acc,
eVector3 &n);
253 void getNonLinearPart(
const eVector6 &leftFootWrench,
255 const Eigen::Vector2d &leftFootPlace_c,
256 const Eigen::Vector2d &rightFootPlace_c,
257 const Eigen::Vector2d &CoM_acc,
eVector3 &n);
264 double estimateCopDisturbance(
const eVector2 ¤tTrackingError,
268 double estimateJerkDisturbance(
const eVector3 ¤tTrackingError,
272 bool configured_, first_iter_;
275 Eigen::Matrix2d A22_;
280 Eigen::Matrix2d RotPi_2_;
285 Eigen::Matrix3d S_coms_j_;
286 Eigen::Matrix<double, 3, 2> S_coms_;
287 Eigen::Vector3d U_coms_, U_coms_j_;
289 Eigen::Vector3d old_reference_com_acc_;
291 eVector2 oldTrackingError2_x_, oldTrackingError2_y_;
292 eVector3 oldTrackingError_x_, oldTrackingError_y_;
294 eVector3 actualState3d_x_, actualState3d_y_;
295 eVector2 actualState2d_x_, actualState2d_y_;
302 std::vector<wykobi::point2d<double>> wykobi_foot_description_;
324 #endif // BIPED_STABILIZER_COP_STABILIZER
double cop_p_cc_gain
Definition: cop_stabilizer.hpp:42
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
Eigen::Vector3d target_com_
Definition: cop_stabilizer.hpp:316
eVector2 estimated_disturbance_
Definition: cop_stabilizer.hpp:321
Eigen::Matrix< double, 3, 1 > eVector3
Definition: cop_stabilizer.hpp:22
eVector3 cop_y_gains
Definition: cop_stabilizer.hpp:41
Definition: cop_stabilizer.hpp:18
EIGEN_MAKE_ALIGNED_OPERATOR_NEW double height
Definition: cop_stabilizer.hpp:35
wykobi::polygon< double, 2 > Polygon2D
Definition: cop_stabilizer.hpp:20
Eigen::Matrix< double, 2, 1 > eVector2
Definition: cop_stabilizer.hpp:21
Definition: cop_stabilizer.hpp:97
eVector3 cop_x_gains
Definition: cop_stabilizer.hpp:40
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
std::array< eVector3, 3 > getStableCoMs(const double &com_height)
Definition: cop_stabilizer.cpp:687
eVector2 errorSum_
Definition: cop_stabilizer.hpp:319
double dt
Definition: cop_stabilizer.hpp:39
bool operator!=(const CopStabilizerSettings &rhs)
Definition: cop_stabilizer.hpp:68
double foot_width
Definition: cop_stabilizer.hpp:37
double g
Definition: cop_stabilizer.hpp:46
std::ostream & operator<<(std::ostream &out)
Definition: cop_stabilizer.hpp:91
eVector2 integral_gain
Definition: cop_stabilizer.hpp:43
eVector2 desired_uncampled_cop_
Definition: cop_stabilizer.hpp:318
std::vector< eMatrixHom, Eigen::aligned_allocator< eMatrixHom > > eMatrixHoms
Definition: cop_stabilizer.hpp:27
Eigen::Vector3d target_com_acc_
Definition: cop_stabilizer.hpp:316
void setPCCgains(const double cop_pcc_gains)
Definition: cop_stabilizer.cpp:836
Eigen::Isometry3d eMatrixHom
Definition: cop_stabilizer.hpp:24
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
Definition: cop_stabilizer.hpp:31
Eigen::Vector3d target_com_jerk_
Definition: cop_stabilizer.hpp:317
eVector2 cop_clamped
Definition: cop_stabilizer.hpp:320
void setIntegralGains(const eVector2 &integral_gains)
Definition: cop_stabilizer.cpp:842
std::vector< eVector3, Eigen::aligned_allocator< eVector3 > > eVector3s
Definition: cop_stabilizer.hpp:29
double robot_mass
Definition: cop_stabilizer.hpp:38
std::string to_string()
Definition: cop_stabilizer.hpp:70
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
eVector2 target_cop_
Definition: cop_stabilizer.hpp:318
std::string cop_control_type
Definition: cop_stabilizer.hpp:47
bool operator==(const CopStabilizerSettings &rhs)
Definition: cop_stabilizer.hpp:51
std::vector< eVector2, Eigen::aligned_allocator< eVector2 > > eVector2s
Definition: cop_stabilizer.hpp:28
bool saturate_cop
Definition: cop_stabilizer.hpp:48
EIGEN_MAKE_ALIGNED_OPERATOR_NEW CopStabilizer()
Definition: cop_stabilizer.cpp:9
const CopStabilizerSettings & getSettings()
Definition: cop_stabilizer.hpp:116
void setCOPgains(const eVector3 &cop_x_gains, eVector3 &cop_y_gains)
Definition: cop_stabilizer.cpp:828
double foot_length
Definition: cop_stabilizer.hpp:36
Eigen::Vector3d target_com_vel_
Definition: cop_stabilizer.hpp:316
void configure(const CopStabilizerSettings &settings)
Definition: cop_stabilizer.cpp:25
bool use_rate_limited_dcm
Definition: cop_stabilizer.hpp:49
Eigen::Matrix3d eMatrixRot
Definition: cop_stabilizer.hpp:25
virtual ~CopStabilizer()
Definition: cop_stabilizer.cpp:23
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
Eigen::Vector3d non_linear_
Definition: cop_stabilizer.hpp:317
Eigen::Matrix< double, 6, 1 > eVector6
Definition: cop_stabilizer.hpp:23
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