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
112 virtual ~CopStabilizer();
118 void stabilize(
const eVector3 &actual_com,
const eVector3 &actual_com_vel,
119 const eVector3 &actual_com_acc,
const eVector3 &actual_cop,
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);
131 void stabilizeCOP(
const eVector3 &actual_com,
const eVector3 &actual_com_vel,
132 const eVector3 &actual_com_acc,
const eVector3 &actual_cop,
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);
143 void stabilizeApproximateAcceleration(
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);
155 void stabilizeP_CC(
const eVector3 &actual_com,
const eVector3 &actual_com_vel,
156 const eVector3 &actual_com_acc,
const eVector3 &actual_cop,
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);
167 void stabilizeJerk(
const eVector3 &actual_com,
const eVector3 &actual_com_vel,
168 const eVector3 &actual_com_acc,
const eVector3 &actual_cop,
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);
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);
185 std::array<eVector3, 3> getStableCoMs(
const double &com_height);
188 void computeSupportPolygon(
const eMatrixHoms &stance_poses,
189 Polygon2D &convex_hull);
191 void projectCOPinSupportPolygon(
const eVector2 &target_cop_unclamped,
192 const Polygon2D &polygon,
193 eVector2 &target_cop);
195 bool isPointInPolygon(eVector2 &point, Polygon2D &polygon);
205 Eigen::Vector3d getActualCOM_acc(
const Eigen::Vector3d &externalForce);
216 template <
typename T,
typename vec_T>
217 T movingAverage(
const T x,
const unsigned long nb_samples, vec_T &queue);
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);
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);
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);
240 void getNonLinearPart(
const eVector3 &com,
const eVector3 &com_acc,
241 const eVector3 &cop, eVector3 &n);
243 void getNonLinearPart(eVector3 &n);
245 double estimateCopDisturbance(
const eVector2 ¤tTrackingError,
246 eVector2 &oldTrackingError,
247 const eVector2 &c_gainK);
249 double estimateJerkDisturbance(
const eVector3 ¤tTrackingError,
250 eVector3 &oldTrackingError,
251 const eVector3 &c_gainK);
253 bool configured_, first_iter_;
256 Eigen::Matrix2d A22_;
258 eVector3 cx_gainK_, cy_gainK_;
259 eVector2 cx_gainK2_, cy_gainK2_;
261 Eigen::Matrix2d RotPi_2_;
266 Eigen::Matrix3d S_coms_j_;
267 Eigen::Matrix<double, 3, 2> S_coms_;
268 Eigen::Vector3d U_coms_, U_coms_j_;
270 Eigen::Vector3d old_reference_com_acc_;
271 eVector3s jerk_ma_queue_;
272 eVector2 oldTrackingError2_x_, oldTrackingError2_y_;
273 eVector3 oldTrackingError_x_, oldTrackingError_y_;
275 eVector3 actualState3d_x_, actualState3d_y_;
276 eVector2 actualState2d_x_, actualState2d_y_;
277 eVector2 actual_command_;
280 eVector3s local_foot_description_;
281 eVector3s foot_description_;
282 Polygon2D support_polygon_;
283 std::vector<wykobi::point2d<double>> wykobi_foot_description_;
298 target_com_jerk_, non_linear_;
305 #endif // BIPED_STABILIZER_COP_STABILIZER std::vector< eVector3, Eigen::aligned_allocator< eVector3 > > eVector3s
Definition: cop_stabilizer.hpp:29
Definition: cop_stabilizer.hpp:18
std::ostream & operator<<(std::ostream &out)
Definition: cop_stabilizer.hpp:91
std::vector< eVector2, Eigen::aligned_allocator< eVector2 > > eVector2s
Definition: cop_stabilizer.hpp:28
Eigen::Matrix< double, 3, 1 > eVector3
Definition: cop_stabilizer.hpp:22
EIGEN_MAKE_ALIGNED_OPERATOR_NEW double height
Definition: cop_stabilizer.hpp:35
eVector2 errorSum_
Definition: cop_stabilizer.hpp:300
const CopStabilizerSettings & getSettings()
Definition: cop_stabilizer.hpp:116
wykobi::polygon< double, 2 > Polygon2D
Definition: cop_stabilizer.hpp:20
bool operator==(const CopStabilizerSettings &rhs)
Definition: cop_stabilizer.hpp:51
bool saturate_cop
Definition: cop_stabilizer.hpp:48
Definition: cop_stabilizer.hpp:31
bool operator!=(const CopStabilizerSettings &rhs)
Definition: cop_stabilizer.hpp:68
Eigen::Isometry3d eMatrixHom
Definition: cop_stabilizer.hpp:24
eVector2 integral_gain
Definition: cop_stabilizer.hpp:43
double robot_mass
Definition: cop_stabilizer.hpp:38
Eigen::Matrix3d eMatrixRot
Definition: cop_stabilizer.hpp:25
eVector2 cop_clamped
Definition: cop_stabilizer.hpp:301
bool use_rate_limited_dcm
Definition: cop_stabilizer.hpp:49
eVector3 cop_x_gains
Definition: cop_stabilizer.hpp:40
eVector2 estimated_disturbance_
Definition: cop_stabilizer.hpp:302
double dt
Definition: cop_stabilizer.hpp:39
eVector3 cop_y_gains
Definition: cop_stabilizer.hpp:41
eVector2 target_cop_
Definition: cop_stabilizer.hpp:299
std::string to_string()
Definition: cop_stabilizer.hpp:70
Eigen::Matrix< double, 2, 1 > eVector2
Definition: cop_stabilizer.hpp:21
Eigen::Matrix< double, 6, 1 > eVector6
Definition: cop_stabilizer.hpp:23
std::string cop_control_type
Definition: cop_stabilizer.hpp:47
double g
Definition: cop_stabilizer.hpp:46
Definition: cop_stabilizer.hpp:97
double foot_length
Definition: cop_stabilizer.hpp:36
Eigen::Vector3d target_com_vel_
Definition: cop_stabilizer.hpp:297
double cop_p_cc_gain
Definition: cop_stabilizer.hpp:42
double foot_width
Definition: cop_stabilizer.hpp:37
std::vector< eMatrixHom, Eigen::aligned_allocator< eMatrixHom > > eMatrixHoms
Definition: cop_stabilizer.hpp:27