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 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);
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);
156 void stabilizeApproximateAcceleration(
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);
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);
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);
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);
198 std::array<eVector3, 3> getStableCoMs(
const double &com_height);
202 void setPCCgains(
const double cop_pcc_gains);
204 void setIntegralGains(
const eVector2 &integral_gains);
207 void computeSupportPolygon(
const eMatrixHoms &stance_poses,
208 Polygon2D &convex_hull);
210 void projectCOPinSupportPolygon(
const eVector2 &target_cop_unclamped,
211 const Polygon2D &polygon,
212 eVector2 &target_cop);
214 bool isPointInPolygon(
const eVector2 &point,
const Polygon2D &polygon);
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,
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);
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);
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);
259 void getNonLinearPart(
const eVector3 &com,
const eVector3 &com_acc,
260 const eVector3 &cop, eVector3 &n);
262 void getNonLinearPart(eVector3 &n);
264 double estimateCopDisturbance(
const eVector2 ¤tTrackingError,
265 eVector2 &oldTrackingError,
266 const eVector2 &c_gainK);
268 double estimateJerkDisturbance(
const eVector3 ¤tTrackingError,
269 eVector3 &oldTrackingError,
270 const eVector3 &c_gainK);
272 bool configured_, first_iter_;
275 Eigen::Matrix2d A22_;
277 eVector3 cx_gainK_, cy_gainK_;
278 eVector2 cx_gainK2_, cy_gainK2_;
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_;
290 eVector3s jerk_ma_queue_;
291 eVector2 oldTrackingError2_x_, oldTrackingError2_y_;
292 eVector3 oldTrackingError_x_, oldTrackingError_y_;
294 eVector3 actualState3d_x_, actualState3d_y_;
295 eVector2 actualState2d_x_, actualState2d_y_;
296 eVector2 actual_command_;
299 eVector3s local_foot_description_;
300 eVector3s foot_description_;
301 Polygon2D support_polygon_;
302 std::vector<wykobi::point2d<double>> wykobi_foot_description_;
317 target_com_jerk_, non_linear_;
324 #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:319
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:320
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:321
double dt
Definition: cop_stabilizer.hpp:39
eVector3 cop_y_gains
Definition: cop_stabilizer.hpp:41
eVector2 target_cop_
Definition: cop_stabilizer.hpp:318
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:316
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