80 cut_off_freq_ = cut_off_frequency;
84 if(cut_off_frequency > 0)
95 cut_off_freq_ = cut_off_frequency;
99 if(cut_off_frequency > 0)
107 cut_off_freq_ = cut_off_frequency;
109 if(cut_off_frequency > 0)
117 return cut_off_freq_;
122 prev_output_ = alpha_*present_raw_value + (1.0 - alpha_)*prev_output_;
141 prev_err_ = curr_err_;
142 curr_err_ =
desired_ - present_sensor_output;
144 return (p_gain_*curr_err_ + d_gain_*(curr_err_ - prev_err_));
155 orientation_enable_ = 1.0;
158 desired_robot_to_cob_ = Eigen::MatrixXd::Identity(4,4);
159 desired_robot_to_right_foot_ = Eigen::MatrixXd::Identity(4,4);
160 desired_robot_to_left_foot_ = Eigen::MatrixXd::Identity(4,4);
162 gyro_cut_off_freq_ = 10.0;
164 gyro_roll_filtered_ = gyro_pitch_filtered_ = 0;
165 desired_gyro_roll_ = desired_gyro_pitch_ = 0;
168 current_gyro_roll_rad_per_sec_ = current_gyro_pitch_rad_per_sec_ = 0;
170 current_orientation_roll_rad_ = current_orientation_pitch_rad_ = 0;
172 current_right_fx_N_ = current_right_fy_N_ = current_right_fz_N_ = 0;
173 current_right_tx_Nm_ = current_right_ty_Nm_ = current_right_tz_Nm_ = 0;
174 current_left_fx_N_ = current_left_fy_N_ = current_left_fz_N_ = 0;
175 current_left_tx_Nm_ = current_left_ty_Nm_ = current_left_tz_Nm_ = 0;
178 foot_roll_adjustment_by_gyro_roll_ = 0;
179 foot_pitch_adjustment_by_gyro_pitch_ = 0;
181 foot_roll_adjustment_by_orientation_roll_ = 0;
182 foot_pitch_adjustment_by_orientation_pitch_ = 0;
184 foot_z_adjustment_by_force_z_difference_ = 0;
185 r_foot_z_adjustment_by_force_z_ = 0;
186 l_foot_z_adjustment_by_force_z_ = 0;
188 r_foot_x_adjustment_by_force_x_ = 0;
189 r_foot_y_adjustment_by_force_y_ = 0;
190 r_foot_roll_adjustment_by_torque_roll_ = 0;
191 r_foot_pitch_adjustment_by_torque_pitch_ = 0;
193 l_foot_x_adjustment_by_force_x_ = 0;
194 l_foot_y_adjustment_by_force_y_ = 0;
195 l_foot_roll_adjustment_by_torque_roll_ = 0;
196 l_foot_pitch_adjustment_by_torque_pitch_ = 0;
199 cob_x_manual_adjustment_m_ = 0;
200 cob_y_manual_adjustment_m_ = 0;
201 cob_z_manual_adjustment_m_ = 0;
204 gyro_balance_gain_ratio_ = 0.0;
205 gyro_balance_roll_gain_ = -0.10*0.75*gyro_balance_gain_ratio_;
206 gyro_balance_pitch_gain_ = -0.10*0.5 *gyro_balance_gain_ratio_;
210 cob_x_adjustment_abs_max_m_ = 0.05;
211 cob_y_adjustment_abs_max_m_ = 0.05;
212 cob_z_adjustment_abs_max_m_ = 0.05;
216 foot_x_adjustment_abs_max_m_ = 0.05;
217 foot_y_adjustment_abs_max_m_ = 0.05;
218 foot_z_adjustment_abs_max_m_ = 0.05;
223 mat_robot_to_cob_modified_ = Eigen::MatrixXd::Identity(4,4);
224 mat_robot_to_right_foot_modified_ = Eigen::MatrixXd::Identity(4,4);
225 mat_robot_to_left_foot_modified_ = Eigen::MatrixXd::Identity(4,4);
226 pose_cob_adjustment_ = Eigen::VectorXd::Zero(6);
227 pose_right_foot_adjustment_ = Eigen::VectorXd::Zero(6);;
228 pose_left_foot_adjustment_ = Eigen::VectorXd::Zero(6);;
240 gyro_cut_off_freq_ = 10.0;
243 pose_cob_adjustment_.fill(0);
244 pose_right_foot_adjustment_.fill(0);
245 pose_left_foot_adjustment_.fill(0);
276 orientation_enable_ = 1.0;
278 orientation_enable_ = 0.0;
293 pose_cob_adjustment_.fill(0);
294 pose_right_foot_adjustment_.fill(0);
295 pose_left_foot_adjustment_.fill(0);
298 gyro_roll_filtered_ = current_gyro_roll_rad_per_sec_*gyro_lpf_alpha_ + (1.0 - gyro_lpf_alpha_)*gyro_roll_filtered_;
299 gyro_pitch_filtered_ = current_gyro_pitch_rad_per_sec_*gyro_lpf_alpha_ + (1.0 - gyro_lpf_alpha_)*gyro_pitch_filtered_;
301 foot_roll_adjustment_by_gyro_roll_ = gyro_enable_ * (desired_gyro_roll_ - gyro_roll_filtered_) * gyro_balance_roll_gain_;
302 foot_pitch_adjustment_by_gyro_pitch_ = gyro_enable_ * (desired_gyro_pitch_ - gyro_pitch_filtered_) * gyro_balance_pitch_gain_;
305 foot_roll_adjustment_by_orientation_roll_ = orientation_enable_ * foot_roll_angle_ctrl_.getDampingControllerOutput(current_orientation_roll_rad_);
306 foot_pitch_adjustment_by_orientation_pitch_ = orientation_enable_ * foot_pitch_angle_ctrl_.getDampingControllerOutput(current_orientation_pitch_rad_);
308 Eigen::MatrixXd mat_orientation_adjustment_by_imu =
robotis_framework::getRotation4d(foot_roll_adjustment_by_gyro_roll_ + foot_roll_adjustment_by_orientation_roll_, foot_pitch_adjustment_by_gyro_pitch_ + foot_pitch_adjustment_by_orientation_pitch_, 0.0);
309 Eigen::MatrixXd mat_r_xy, mat_l_xy;
310 mat_r_xy.resize(4,1); mat_l_xy.resize(4,1);
311 mat_r_xy.coeffRef(0,0) = desired_robot_to_right_foot_.coeff(0,3) - 0.5*(desired_robot_to_right_foot_.coeff(0,3) + desired_robot_to_left_foot_.coeff(0,3));
312 mat_r_xy.coeffRef(1,0) = desired_robot_to_right_foot_.coeff(1,3) - 0.5*(desired_robot_to_right_foot_.coeff(1,3) + desired_robot_to_left_foot_.coeff(1,3));
313 mat_r_xy.coeffRef(2,0) = 0.0;
314 mat_r_xy.coeffRef(3,0) = 1;
316 mat_l_xy.coeffRef(0,0) = desired_robot_to_left_foot_.coeff(0,3) - 0.5*(desired_robot_to_right_foot_.coeff(0,3) + desired_robot_to_left_foot_.coeff(0,3));
317 mat_l_xy.coeffRef(1,0) = desired_robot_to_left_foot_.coeff(1,3) - 0.5*(desired_robot_to_right_foot_.coeff(1,3) + desired_robot_to_left_foot_.coeff(1,3));
318 mat_l_xy.coeffRef(2,0) = 0.0;
319 mat_l_xy.coeffRef(3,0) = 1;
321 mat_r_xy = mat_orientation_adjustment_by_imu * mat_r_xy;
322 mat_l_xy = mat_orientation_adjustment_by_imu * mat_l_xy;
325 foot_z_adjustment_by_force_z_difference_ = ft_enable_*0.001*foot_force_z_diff_ctrl_.getDampingControllerOutput(current_left_fz_N_ - current_right_fz_N_);
326 r_foot_z_adjustment_by_force_z_ = ft_enable_*0.001*right_foot_force_z_ctrl_.getDampingControllerOutput(current_right_fz_N_);
327 l_foot_z_adjustment_by_force_z_ = ft_enable_*0.001*left_foot_force_z_ctrl_.getDampingControllerOutput(current_left_fz_N_);
329 r_foot_x_adjustment_by_force_x_ = ft_enable_*0.001*right_foot_force_x_ctrl_.getDampingControllerOutput(current_right_fx_N_);
330 r_foot_y_adjustment_by_force_y_ = ft_enable_*0.001*right_foot_force_y_ctrl_.getDampingControllerOutput(current_right_fy_N_);
331 r_foot_roll_adjustment_by_torque_roll_ = ft_enable_*right_foot_torque_roll_ctrl_.getDampingControllerOutput(current_right_tx_Nm_);
332 r_foot_pitch_adjustment_by_torque_pitch_ = ft_enable_*right_foot_torque_pitch_ctrl_.getDampingControllerOutput(current_right_ty_Nm_);
334 l_foot_x_adjustment_by_force_x_ = ft_enable_*0.001*left_foot_force_x_ctrl_.getDampingControllerOutput(current_left_fx_N_);
335 l_foot_y_adjustment_by_force_y_ = ft_enable_*0.001*left_foot_force_y_ctrl_.getDampingControllerOutput(current_left_fy_N_);
336 l_foot_roll_adjustment_by_torque_roll_ = ft_enable_*left_foot_torque_roll_ctrl_.getDampingControllerOutput(current_left_tx_Nm_);
337 l_foot_pitch_adjustment_by_torque_pitch_ = ft_enable_*left_foot_torque_pitch_ctrl_.getDampingControllerOutput(current_left_ty_Nm_);
339 r_foot_z_adjustment_by_force_z_ += ft_enable_*0.001*0.0*(right_foot_force_z_ctrl_.desired_ - current_right_fz_N_);
340 l_foot_z_adjustment_by_force_z_ += ft_enable_*0.001*0.0*(left_foot_force_z_ctrl_.desired_ - current_left_fz_N_);
342 pose_cob_adjustment_.coeffRef(0) = cob_x_manual_adjustment_m_;
343 pose_cob_adjustment_.coeffRef(1) = cob_y_manual_adjustment_m_;
344 pose_cob_adjustment_.coeffRef(2) = cob_z_manual_adjustment_m_;
346 pose_right_foot_adjustment_.coeffRef(0) = r_foot_x_adjustment_by_force_x_;
347 pose_right_foot_adjustment_.coeffRef(1) = r_foot_y_adjustment_by_force_y_;
348 pose_right_foot_adjustment_.coeffRef(2) = 0.5*0.0*foot_z_adjustment_by_force_z_difference_ + mat_r_xy.coeff(2, 0) + r_foot_z_adjustment_by_force_z_*1.0;
349 pose_right_foot_adjustment_.coeffRef(3) = (foot_roll_adjustment_by_gyro_roll_ + foot_roll_adjustment_by_orientation_roll_ + r_foot_roll_adjustment_by_torque_roll_);
350 pose_right_foot_adjustment_.coeffRef(4) = (foot_pitch_adjustment_by_gyro_pitch_ + foot_pitch_adjustment_by_orientation_pitch_ + r_foot_pitch_adjustment_by_torque_pitch_);
352 pose_left_foot_adjustment_.coeffRef(0) = l_foot_x_adjustment_by_force_x_;
353 pose_left_foot_adjustment_.coeffRef(1) = l_foot_y_adjustment_by_force_y_;
354 pose_left_foot_adjustment_.coeffRef(2) = -0.5*0.0*foot_z_adjustment_by_force_z_difference_ + mat_l_xy.coeff(2, 0) + l_foot_z_adjustment_by_force_z_*1.0;
355 pose_left_foot_adjustment_.coeffRef(3) = (foot_roll_adjustment_by_gyro_roll_ + foot_roll_adjustment_by_orientation_roll_ + l_foot_roll_adjustment_by_torque_roll_);
356 pose_left_foot_adjustment_.coeffRef(4) = (foot_pitch_adjustment_by_gyro_pitch_ + foot_pitch_adjustment_by_orientation_pitch_ + l_foot_pitch_adjustment_by_torque_pitch_);
359 if((fabs(pose_cob_adjustment_.coeff(0)) == cob_x_adjustment_abs_max_m_ ) ||
360 (fabs(pose_cob_adjustment_.coeff(1)) == cob_y_adjustment_abs_max_m_ ) ||
361 (fabs(pose_cob_adjustment_.coeff(2)) == cob_z_adjustment_abs_max_m_ ) ||
362 (fabs(pose_cob_adjustment_.coeff(3)) == cob_roll_adjustment_abs_max_rad_ ) ||
363 (fabs(pose_cob_adjustment_.coeff(4)) == cob_pitch_adjustment_abs_max_rad_) ||
364 (fabs(pose_right_foot_adjustment_.coeff(0)) == foot_x_adjustment_abs_max_m_ ) ||
365 (fabs(pose_right_foot_adjustment_.coeff(1)) == foot_y_adjustment_abs_max_m_ ) ||
366 (fabs(pose_right_foot_adjustment_.coeff(2)) == foot_z_adjustment_abs_max_m_ ) ||
367 (fabs(pose_right_foot_adjustment_.coeff(3)) == foot_roll_adjustment_abs_max_rad_ ) ||
368 (fabs(pose_right_foot_adjustment_.coeff(4)) == foot_pitch_adjustment_abs_max_rad_) ||
369 (fabs(pose_left_foot_adjustment_.coeff(0)) == foot_x_adjustment_abs_max_m_ ) ||
370 (fabs(pose_left_foot_adjustment_.coeff(1)) == foot_y_adjustment_abs_max_m_ ) ||
371 (fabs(pose_left_foot_adjustment_.coeff(2)) == foot_z_adjustment_abs_max_m_ ) ||
372 (fabs(pose_left_foot_adjustment_.coeff(3)) == foot_roll_adjustment_abs_max_rad_ ) ||
373 (fabs(pose_left_foot_adjustment_.coeff(4)) == foot_pitch_adjustment_abs_max_rad_))
376 pose_cob_adjustment_.coeffRef(0) = copysign(fmin(fabs(pose_cob_adjustment_.coeff(0)), cob_x_adjustment_abs_max_m_ ), pose_cob_adjustment_.coeff(0));
377 pose_cob_adjustment_.coeffRef(1) = copysign(fmin(fabs(pose_cob_adjustment_.coeff(1)), cob_x_adjustment_abs_max_m_ ), pose_cob_adjustment_.coeff(1));
378 pose_cob_adjustment_.coeffRef(2) = copysign(fmin(fabs(pose_cob_adjustment_.coeff(2)), cob_x_adjustment_abs_max_m_ ), pose_cob_adjustment_.coeff(2));
379 pose_cob_adjustment_.coeffRef(3) = copysign(fmin(fabs(pose_cob_adjustment_.coeff(3)), cob_roll_adjustment_abs_max_rad_ ), pose_cob_adjustment_.coeff(3));
380 pose_cob_adjustment_.coeffRef(4) = copysign(fmin(fabs(pose_cob_adjustment_.coeff(4)), cob_pitch_adjustment_abs_max_rad_), pose_cob_adjustment_.coeff(4));
381 pose_cob_adjustment_.coeffRef(5) = 0;
383 pose_right_foot_adjustment_.coeffRef(0) = copysign(fmin(fabs(pose_right_foot_adjustment_.coeff(0)), foot_x_adjustment_abs_max_m_ ), pose_right_foot_adjustment_.coeff(0));
384 pose_right_foot_adjustment_.coeffRef(1) = copysign(fmin(fabs(pose_right_foot_adjustment_.coeff(1)), foot_y_adjustment_abs_max_m_ ), pose_right_foot_adjustment_.coeff(1));
385 pose_right_foot_adjustment_.coeffRef(2) = copysign(fmin(fabs(pose_right_foot_adjustment_.coeff(2)), foot_z_adjustment_abs_max_m_ ), pose_right_foot_adjustment_.coeff(2));
386 pose_right_foot_adjustment_.coeffRef(3) = copysign(fmin(fabs(pose_right_foot_adjustment_.coeff(3)), foot_roll_adjustment_abs_max_rad_ ), pose_right_foot_adjustment_.coeff(3));
387 pose_right_foot_adjustment_.coeffRef(4) = copysign(fmin(fabs(pose_right_foot_adjustment_.coeff(4)), foot_pitch_adjustment_abs_max_rad_), pose_right_foot_adjustment_.coeff(4));
388 pose_right_foot_adjustment_.coeffRef(5) = 0;
390 pose_left_foot_adjustment_.coeffRef(0) = copysign(fmin(fabs(pose_left_foot_adjustment_.coeff(0)), foot_x_adjustment_abs_max_m_ ), pose_left_foot_adjustment_.coeff(0));
391 pose_left_foot_adjustment_.coeffRef(1) = copysign(fmin(fabs(pose_left_foot_adjustment_.coeff(1)), foot_y_adjustment_abs_max_m_ ), pose_left_foot_adjustment_.coeff(1));
392 pose_left_foot_adjustment_.coeffRef(2) = copysign(fmin(fabs(pose_left_foot_adjustment_.coeff(2)), foot_z_adjustment_abs_max_m_ ), pose_left_foot_adjustment_.coeff(2));
393 pose_left_foot_adjustment_.coeffRef(3) = copysign(fmin(fabs(pose_left_foot_adjustment_.coeff(3)), foot_roll_adjustment_abs_max_rad_ ), pose_left_foot_adjustment_.coeff(3));
394 pose_left_foot_adjustment_.coeffRef(4) = copysign(fmin(fabs(pose_left_foot_adjustment_.coeff(4)), foot_pitch_adjustment_abs_max_rad_), pose_left_foot_adjustment_.coeff(4));
395 pose_left_foot_adjustment_.coeffRef(5) = 0;
400 mat_robot_to_cob_modified_.block<3,3>(0,0) = cob_rotation_adj * desired_robot_to_cob_.block<3,3>(0,0);
401 mat_robot_to_right_foot_modified_.block<3,3>(0,0) = rf_rotation_adj * desired_robot_to_right_foot_.block<3,3>(0,0);;
402 mat_robot_to_left_foot_modified_.block<3,3>(0,0) = lf_rotation_adj * desired_robot_to_left_foot_.block<3,3>(0,0);;
404 mat_robot_to_cob_modified_.coeffRef(0,3) = desired_robot_to_cob_.coeff(0,3) + pose_cob_adjustment_.coeff(0);
405 mat_robot_to_cob_modified_.coeffRef(1,3) = desired_robot_to_cob_.coeff(1,3) + pose_cob_adjustment_.coeff(1);
406 mat_robot_to_cob_modified_.coeffRef(2,3) = desired_robot_to_cob_.coeff(2,3) + pose_cob_adjustment_.coeff(2);
408 mat_robot_to_right_foot_modified_.coeffRef(0,3) = desired_robot_to_right_foot_.coeff(0,3) + pose_right_foot_adjustment_.coeff(0);
409 mat_robot_to_right_foot_modified_.coeffRef(1,3) = desired_robot_to_right_foot_.coeff(1,3) + pose_right_foot_adjustment_.coeff(1);
410 mat_robot_to_right_foot_modified_.coeffRef(2,3) = desired_robot_to_right_foot_.coeff(2,3) + pose_right_foot_adjustment_.coeff(2);
412 mat_robot_to_left_foot_modified_.coeffRef(0,3) = desired_robot_to_left_foot_.coeff(0,3) + pose_left_foot_adjustment_.coeff(0);
413 mat_robot_to_left_foot_modified_.coeffRef(1,3) = desired_robot_to_left_foot_.coeff(1,3) + pose_left_foot_adjustment_.coeff(1);
414 mat_robot_to_left_foot_modified_.coeffRef(2,3) = desired_robot_to_left_foot_.coeff(2,3) + pose_left_foot_adjustment_.coeff(2);
416 if(balance_error != 0)
417 *balance_error = balance_control_error_;
419 *robot_to_cob_modified = mat_robot_to_cob_modified_;
420 *robot_to_right_foot_modified = mat_robot_to_right_foot_modified_;
421 *robot_to_left_foot_modified = mat_robot_to_left_foot_modified_;
426 desired_robot_to_cob_ = robot_to_cob;
427 desired_robot_to_right_foot_ = robot_to_right_foot;
428 desired_robot_to_left_foot_ = robot_to_left_foot;
433 desired_gyro_roll_ = gyro_roll;
434 desired_gyro_pitch_ = gyro_pitch;
439 foot_roll_angle_ctrl_.desired_ = cob_orientation_roll;
440 foot_pitch_angle_ctrl_.desired_ = cob_orientation_pitch;
444 double r_torque_roll_Nm,
double r_torque_pitch_Nm,
double r_torque_yaw_Nm,
445 double l_force_x_N,
double l_force_y_N,
double l_force_z_N,
446 double l_torque_roll_Nm,
double l_torque_pitch_Nm,
double l_torque_yaw_Nm)
448 foot_force_z_diff_ctrl_.desired_ = l_force_z_N - r_force_z_N;
449 right_foot_force_z_ctrl_.desired_ = r_force_z_N;
450 left_foot_force_z_ctrl_.desired_ = l_force_z_N;
452 right_foot_force_x_ctrl_.desired_ = r_force_x_N;
453 right_foot_force_y_ctrl_.desired_ = r_force_y_N;
454 right_foot_torque_roll_ctrl_.desired_ = r_torque_roll_Nm;
455 right_foot_torque_pitch_ctrl_.desired_ = r_torque_pitch_Nm;
457 left_foot_force_x_ctrl_.desired_ = l_force_x_N;
458 left_foot_force_y_ctrl_.desired_ = l_force_y_N;
459 left_foot_torque_roll_ctrl_.desired_ = l_torque_roll_Nm;
460 left_foot_torque_pitch_ctrl_.desired_ = l_torque_pitch_Nm;
466 current_gyro_roll_rad_per_sec_ = gyro_roll;
467 current_gyro_pitch_rad_per_sec_ = gyro_pitch;
472 current_orientation_roll_rad_ = cob_orientation_roll;
473 current_orientation_pitch_rad_ = cob_orientation_pitch;
477 double r_torque_roll_Nm,
double r_torque_pitch_Nm,
double r_torque_yaw_Nm,
478 double l_force_x_N,
double l_force_y_N,
double l_force_z_N,
479 double l_torque_roll_Nm,
double l_torque_pitch_Nm,
double l_torque_yaw_Nm)
481 current_right_fx_N_ = r_force_x_N;
482 current_right_fy_N_ = r_force_y_N;
483 current_right_fz_N_ = r_force_z_N;
484 current_right_tx_Nm_ = r_torque_roll_Nm;
485 current_right_ty_Nm_ = r_torque_pitch_Nm;
486 current_right_tz_Nm_ = r_torque_yaw_Nm;
488 current_left_fx_N_ = l_force_x_N;
489 current_left_fy_N_ = l_force_y_N;
490 current_left_fz_N_ = l_force_z_N;
491 current_left_tx_Nm_ = l_torque_roll_Nm;
492 current_left_ty_Nm_ = l_torque_pitch_Nm;
493 current_left_tz_Nm_ = l_torque_yaw_Nm;
498 double cob_roll_max_adjustment_rad,
double cob_pitch_max_adjustment_rad,
double cob_yaw_max_adjustment_rad,
499 double foot_x_max_adjustment_m,
double foot_y_max_adjustment_m,
double foot_z_max_adjustment_m,
500 double foot_roll_max_adjustment_rad,
double foot_pitch_max_adjustment_rad,
double foot_yaw_max_adjustment_rad)
502 cob_x_adjustment_abs_max_m_ = cob_x_max_adjustment_m;
503 cob_y_adjustment_abs_max_m_ = cob_y_max_adjustment_m;
504 cob_z_adjustment_abs_max_m_ = cob_z_max_adjustment_m;
505 cob_roll_adjustment_abs_max_rad_ = cob_roll_max_adjustment_rad;
506 cob_pitch_adjustment_abs_max_rad_ = cob_pitch_max_adjustment_rad;
507 cob_yaw_adjustment_abs_max_rad_ = cob_yaw_max_adjustment_rad;
508 foot_x_adjustment_abs_max_m_ = foot_x_max_adjustment_m;
509 foot_y_adjustment_abs_max_m_ = foot_y_max_adjustment_m;
510 foot_z_adjustment_abs_max_m_ = foot_z_max_adjustment_m;
511 foot_roll_adjustment_abs_max_rad_ = foot_roll_max_adjustment_rad;
512 foot_pitch_adjustment_abs_max_rad_ = foot_pitch_max_adjustment_rad;
513 foot_yaw_adjustment_abs_max_rad_ = foot_yaw_max_adjustment_rad;
519 cob_x_manual_adjustment_m_ = cob_x_adjustment_m;
520 cob_y_manual_adjustment_m_ = cob_y_adjustment_m;
521 cob_z_manual_adjustment_m_ = cob_z_adjustment_m;
526 return cob_x_manual_adjustment_m_;
531 return cob_y_manual_adjustment_m_;
536 return cob_z_manual_adjustment_m_;
541 gyro_balance_gain_ratio_ = gyro_balance_gain_ratio;
542 gyro_balance_roll_gain_ = -0.10*0.75*gyro_balance_gain_ratio_;
543 gyro_balance_pitch_gain_ = -0.10*0.5 *gyro_balance_gain_ratio_;
548 return gyro_balance_gain_ratio_;
559 orientation_enable_ = 1.0;
562 desired_robot_to_cob_ = Eigen::MatrixXd::Identity(4,4);
563 desired_robot_to_right_foot_ = Eigen::MatrixXd::Identity(4,4);
564 desired_robot_to_left_foot_ = Eigen::MatrixXd::Identity(4,4);
567 current_gyro_roll_rad_per_sec_ = current_gyro_pitch_rad_per_sec_ = 0;
569 current_orientation_roll_rad_ = current_orientation_pitch_rad_ = 0;
571 current_right_fx_N_ = current_right_fy_N_ = current_right_fz_N_ = 0;
572 current_right_tx_Nm_ = current_right_ty_Nm_ = current_right_tz_Nm_ = 0;
573 current_left_fx_N_ = current_left_fy_N_ = current_left_fz_N_ = 0;
574 current_left_tx_Nm_ = current_left_ty_Nm_ = current_left_tz_Nm_ = 0;
577 foot_roll_adjustment_by_gyro_roll_ = 0;
578 foot_pitch_adjustment_by_gyro_pitch_ = 0;
580 foot_roll_adjustment_by_orientation_roll_ = 0;
581 foot_pitch_adjustment_by_orientation_pitch_ = 0;
583 r_foot_z_adjustment_by_force_z_ = 0;
584 l_foot_z_adjustment_by_force_z_ = 0;
586 r_foot_x_adjustment_by_force_x_ = 0;
587 r_foot_y_adjustment_by_force_y_ = 0;
588 r_foot_roll_adjustment_by_torque_roll_ = 0;
589 r_foot_pitch_adjustment_by_torque_pitch_ = 0;
591 l_foot_x_adjustment_by_force_x_ = 0;
592 l_foot_y_adjustment_by_force_y_ = 0;
593 l_foot_roll_adjustment_by_torque_roll_ = 0;
594 l_foot_pitch_adjustment_by_torque_pitch_ = 0;
597 cob_x_manual_adjustment_m_ = 0;
598 cob_y_manual_adjustment_m_ = 0;
599 cob_z_manual_adjustment_m_ = 0;
602 cob_x_adjustment_abs_max_m_ = 0.05;
603 cob_y_adjustment_abs_max_m_ = 0.05;
604 cob_z_adjustment_abs_max_m_ = 0.05;
608 foot_x_adjustment_abs_max_m_ = 0.05;
609 foot_y_adjustment_abs_max_m_ = 0.05;
610 foot_z_adjustment_abs_max_m_ = 0.05;
615 mat_robot_to_cob_modified_ = Eigen::MatrixXd::Identity(4,4);
616 mat_robot_to_right_foot_modified_ = Eigen::MatrixXd::Identity(4,4);
617 mat_robot_to_left_foot_modified_ = Eigen::MatrixXd::Identity(4,4);
618 pose_cob_adjustment_ = Eigen::VectorXd::Zero(6);
619 pose_right_foot_adjustment_ = Eigen::VectorXd::Zero(6);
620 pose_left_foot_adjustment_ = Eigen::VectorXd::Zero(6);
632 pose_cob_adjustment_.fill(0);
633 pose_right_foot_adjustment_.fill(0);
634 pose_left_foot_adjustment_.fill(0);
666 orientation_enable_ = 1.0;
668 orientation_enable_ = 0.0;
683 pose_cob_adjustment_.fill(0);
684 pose_right_foot_adjustment_.fill(0);
685 pose_left_foot_adjustment_.fill(0);
688 double roll_gyro_filtered = roll_gyro_lpf_.getFilteredOutput(current_gyro_roll_rad_per_sec_);
689 double pitch_gyro_filtered = pitch_gyro_lpf_.getFilteredOutput(current_gyro_pitch_rad_per_sec_);
691 double roll_angle_filtered = roll_angle_lpf_.getFilteredOutput(current_orientation_roll_rad_);
692 double pitch_angle_filtered = pitch_angle_lpf_.getFilteredOutput(current_orientation_pitch_rad_);
694 double right_foot_force_x_filtered = right_foot_force_x_lpf_.getFilteredOutput(current_right_fx_N_);
695 double right_foot_force_y_filtered = right_foot_force_y_lpf_.getFilteredOutput(current_right_fy_N_);
696 double right_foot_force_z_filtered = right_foot_force_z_lpf_.getFilteredOutput(current_right_fz_N_);
697 double right_foot_torque_roll_filtered = right_foot_torque_roll_lpf_.getFilteredOutput(current_right_tx_Nm_);
698 double right_foot_torque_pitch_filtered = right_foot_torque_pitch_lpf_.getFilteredOutput(current_right_ty_Nm_);;
700 double left_foot_force_x_filtered = left_foot_force_x_lpf_.getFilteredOutput(current_left_fx_N_);
701 double left_foot_force_y_filtered = left_foot_force_y_lpf_.getFilteredOutput(current_left_fy_N_);
702 double left_foot_force_z_filtered = left_foot_force_z_lpf_.getFilteredOutput(current_left_fz_N_);
703 double left_foot_torque_roll_filtered = left_foot_torque_roll_lpf_.getFilteredOutput(current_left_tx_Nm_);
704 double left_foot_torque_pitch_filtered = left_foot_torque_pitch_lpf_.getFilteredOutput(current_left_ty_Nm_);
708 foot_roll_adjustment_by_gyro_roll_ = -0.1*gyro_enable_*foot_roll_gyro_ctrl_.getFeedBack(roll_gyro_filtered);
709 foot_pitch_adjustment_by_gyro_pitch_ = -0.1*gyro_enable_*foot_pitch_gyro_ctrl_.getFeedBack(pitch_gyro_filtered);
712 foot_roll_adjustment_by_orientation_roll_ = -1.0*orientation_enable_ * foot_roll_angle_ctrl_.getFeedBack(roll_angle_filtered);
713 foot_pitch_adjustment_by_orientation_pitch_ = -1.0*orientation_enable_ * foot_pitch_angle_ctrl_.getFeedBack(pitch_angle_filtered);
715 Eigen::MatrixXd mat_orientation_adjustment_by_imu =
robotis_framework::getRotation4d(foot_roll_adjustment_by_gyro_roll_ + foot_roll_adjustment_by_orientation_roll_, foot_pitch_adjustment_by_gyro_pitch_ + foot_pitch_adjustment_by_orientation_pitch_, 0.0);
716 Eigen::MatrixXd mat_r_xy, mat_l_xy;
717 mat_r_xy.resize(4,1); mat_l_xy.resize(4,1);
718 mat_r_xy.coeffRef(0,0) = desired_robot_to_right_foot_.coeff(0,3) - 0.5*(desired_robot_to_right_foot_.coeff(0,3) + desired_robot_to_left_foot_.coeff(0,3));
719 mat_r_xy.coeffRef(1,0) = desired_robot_to_right_foot_.coeff(1,3) - 0.5*(desired_robot_to_right_foot_.coeff(1,3) + desired_robot_to_left_foot_.coeff(1,3));
720 mat_r_xy.coeffRef(2,0) = 0.0;
721 mat_r_xy.coeffRef(3,0) = 1;
723 mat_l_xy.coeffRef(0,0) = desired_robot_to_left_foot_.coeff(0,3) - 0.5*(desired_robot_to_right_foot_.coeff(0,3) + desired_robot_to_left_foot_.coeff(0,3));
724 mat_l_xy.coeffRef(1,0) = desired_robot_to_left_foot_.coeff(1,3) - 0.5*(desired_robot_to_right_foot_.coeff(1,3) + desired_robot_to_left_foot_.coeff(1,3));
725 mat_l_xy.coeffRef(2,0) = 0.0;
726 mat_l_xy.coeffRef(3,0) = 1;
728 mat_r_xy = mat_orientation_adjustment_by_imu * mat_r_xy;
729 mat_l_xy = mat_orientation_adjustment_by_imu * mat_l_xy;
732 r_foot_x_adjustment_by_force_x_ = ft_enable_*0.001*right_foot_force_x_ctrl_.getFeedBack(right_foot_force_x_filtered);
733 r_foot_y_adjustment_by_force_y_ = ft_enable_*0.001*right_foot_force_y_ctrl_.getFeedBack(right_foot_force_y_filtered);
734 r_foot_z_adjustment_by_force_z_ = ft_enable_*0.001*right_foot_force_z_ctrl_.getFeedBack(right_foot_force_z_filtered);
735 r_foot_roll_adjustment_by_torque_roll_ = ft_enable_*right_foot_torque_roll_ctrl_.getFeedBack(right_foot_torque_roll_filtered);
736 r_foot_pitch_adjustment_by_torque_pitch_ = ft_enable_*right_foot_torque_pitch_ctrl_.getFeedBack(right_foot_torque_pitch_filtered);
738 l_foot_x_adjustment_by_force_x_ = ft_enable_*0.001*left_foot_force_x_ctrl_.getFeedBack(left_foot_force_x_filtered);
739 l_foot_y_adjustment_by_force_y_ = ft_enable_*0.001*left_foot_force_y_ctrl_.getFeedBack(left_foot_force_y_filtered);
740 l_foot_z_adjustment_by_force_z_ = ft_enable_*0.001*left_foot_force_z_ctrl_.getFeedBack(left_foot_force_z_filtered);
741 l_foot_roll_adjustment_by_torque_roll_ = ft_enable_*left_foot_torque_roll_ctrl_.getFeedBack(left_foot_torque_roll_filtered);
742 l_foot_pitch_adjustment_by_torque_pitch_ = ft_enable_*left_foot_torque_pitch_ctrl_.getFeedBack(left_foot_torque_pitch_filtered);
745 pose_cob_adjustment_.coeffRef(0) = cob_x_manual_adjustment_m_;
746 pose_cob_adjustment_.coeffRef(1) = cob_y_manual_adjustment_m_;
747 pose_cob_adjustment_.coeffRef(2) = cob_z_manual_adjustment_m_;
749 pose_right_foot_adjustment_.coeffRef(0) = r_foot_x_adjustment_by_force_x_;
750 pose_right_foot_adjustment_.coeffRef(1) = r_foot_y_adjustment_by_force_y_;
751 pose_right_foot_adjustment_.coeffRef(2) = mat_r_xy.coeff(2, 0) + r_foot_z_adjustment_by_force_z_*1.0;
752 pose_right_foot_adjustment_.coeffRef(3) = (foot_roll_adjustment_by_gyro_roll_ + foot_roll_adjustment_by_orientation_roll_ + r_foot_roll_adjustment_by_torque_roll_);
753 pose_right_foot_adjustment_.coeffRef(4) = (foot_pitch_adjustment_by_gyro_pitch_ + foot_pitch_adjustment_by_orientation_pitch_ + r_foot_pitch_adjustment_by_torque_pitch_);
755 pose_left_foot_adjustment_.coeffRef(0) = l_foot_x_adjustment_by_force_x_;
756 pose_left_foot_adjustment_.coeffRef(1) = l_foot_y_adjustment_by_force_y_;
757 pose_left_foot_adjustment_.coeffRef(2) = mat_l_xy.coeff(2, 0) + l_foot_z_adjustment_by_force_z_*1.0;
758 pose_left_foot_adjustment_.coeffRef(3) = (foot_roll_adjustment_by_gyro_roll_ + foot_roll_adjustment_by_orientation_roll_ + l_foot_roll_adjustment_by_torque_roll_);
759 pose_left_foot_adjustment_.coeffRef(4) = (foot_pitch_adjustment_by_gyro_pitch_ + foot_pitch_adjustment_by_orientation_pitch_ + l_foot_pitch_adjustment_by_torque_pitch_);
762 if((fabs(pose_cob_adjustment_.coeff(0)) == cob_x_adjustment_abs_max_m_ ) ||
763 (fabs(pose_cob_adjustment_.coeff(1)) == cob_y_adjustment_abs_max_m_ ) ||
764 (fabs(pose_cob_adjustment_.coeff(2)) == cob_z_adjustment_abs_max_m_ ) ||
765 (fabs(pose_cob_adjustment_.coeff(3)) == cob_roll_adjustment_abs_max_rad_ ) ||
766 (fabs(pose_cob_adjustment_.coeff(4)) == cob_pitch_adjustment_abs_max_rad_) ||
767 (fabs(pose_right_foot_adjustment_.coeff(0)) == foot_x_adjustment_abs_max_m_ ) ||
768 (fabs(pose_right_foot_adjustment_.coeff(1)) == foot_y_adjustment_abs_max_m_ ) ||
769 (fabs(pose_right_foot_adjustment_.coeff(2)) == foot_z_adjustment_abs_max_m_ ) ||
770 (fabs(pose_right_foot_adjustment_.coeff(3)) == foot_roll_adjustment_abs_max_rad_ ) ||
771 (fabs(pose_right_foot_adjustment_.coeff(4)) == foot_pitch_adjustment_abs_max_rad_) ||
772 (fabs(pose_left_foot_adjustment_.coeff(0)) == foot_x_adjustment_abs_max_m_ ) ||
773 (fabs(pose_left_foot_adjustment_.coeff(1)) == foot_y_adjustment_abs_max_m_ ) ||
774 (fabs(pose_left_foot_adjustment_.coeff(2)) == foot_z_adjustment_abs_max_m_ ) ||
775 (fabs(pose_left_foot_adjustment_.coeff(3)) == foot_roll_adjustment_abs_max_rad_ ) ||
776 (fabs(pose_left_foot_adjustment_.coeff(4)) == foot_pitch_adjustment_abs_max_rad_))
779 pose_cob_adjustment_.coeffRef(0) = copysign(fmin(fabs(pose_cob_adjustment_.coeff(0)), cob_x_adjustment_abs_max_m_ ), pose_cob_adjustment_.coeff(0));
780 pose_cob_adjustment_.coeffRef(1) = copysign(fmin(fabs(pose_cob_adjustment_.coeff(1)), cob_x_adjustment_abs_max_m_ ), pose_cob_adjustment_.coeff(1));
781 pose_cob_adjustment_.coeffRef(2) = copysign(fmin(fabs(pose_cob_adjustment_.coeff(2)), cob_x_adjustment_abs_max_m_ ), pose_cob_adjustment_.coeff(2));
782 pose_cob_adjustment_.coeffRef(3) = copysign(fmin(fabs(pose_cob_adjustment_.coeff(3)), cob_roll_adjustment_abs_max_rad_ ), pose_cob_adjustment_.coeff(3));
783 pose_cob_adjustment_.coeffRef(4) = copysign(fmin(fabs(pose_cob_adjustment_.coeff(4)), cob_pitch_adjustment_abs_max_rad_), pose_cob_adjustment_.coeff(4));
784 pose_cob_adjustment_.coeffRef(5) = 0;
786 pose_right_foot_adjustment_.coeffRef(0) = copysign(fmin(fabs(pose_right_foot_adjustment_.coeff(0)), foot_x_adjustment_abs_max_m_ ), pose_right_foot_adjustment_.coeff(0));
787 pose_right_foot_adjustment_.coeffRef(1) = copysign(fmin(fabs(pose_right_foot_adjustment_.coeff(1)), foot_y_adjustment_abs_max_m_ ), pose_right_foot_adjustment_.coeff(1));
788 pose_right_foot_adjustment_.coeffRef(2) = copysign(fmin(fabs(pose_right_foot_adjustment_.coeff(2)), foot_z_adjustment_abs_max_m_ ), pose_right_foot_adjustment_.coeff(2));
789 pose_right_foot_adjustment_.coeffRef(3) = copysign(fmin(fabs(pose_right_foot_adjustment_.coeff(3)), foot_roll_adjustment_abs_max_rad_ ), pose_right_foot_adjustment_.coeff(3));
790 pose_right_foot_adjustment_.coeffRef(4) = copysign(fmin(fabs(pose_right_foot_adjustment_.coeff(4)), foot_pitch_adjustment_abs_max_rad_), pose_right_foot_adjustment_.coeff(4));
791 pose_right_foot_adjustment_.coeffRef(5) = 0;
793 pose_left_foot_adjustment_.coeffRef(0) = copysign(fmin(fabs(pose_left_foot_adjustment_.coeff(0)), foot_x_adjustment_abs_max_m_ ), pose_left_foot_adjustment_.coeff(0));
794 pose_left_foot_adjustment_.coeffRef(1) = copysign(fmin(fabs(pose_left_foot_adjustment_.coeff(1)), foot_y_adjustment_abs_max_m_ ), pose_left_foot_adjustment_.coeff(1));
795 pose_left_foot_adjustment_.coeffRef(2) = copysign(fmin(fabs(pose_left_foot_adjustment_.coeff(2)), foot_z_adjustment_abs_max_m_ ), pose_left_foot_adjustment_.coeff(2));
796 pose_left_foot_adjustment_.coeffRef(3) = copysign(fmin(fabs(pose_left_foot_adjustment_.coeff(3)), foot_roll_adjustment_abs_max_rad_ ), pose_left_foot_adjustment_.coeff(3));
797 pose_left_foot_adjustment_.coeffRef(4) = copysign(fmin(fabs(pose_left_foot_adjustment_.coeff(4)), foot_pitch_adjustment_abs_max_rad_), pose_left_foot_adjustment_.coeff(4));
798 pose_left_foot_adjustment_.coeffRef(5) = 0;
803 mat_robot_to_cob_modified_.block<3,3>(0,0) = cob_rotation_adj * desired_robot_to_cob_.block<3,3>(0,0);
804 mat_robot_to_right_foot_modified_.block<3,3>(0,0) = rf_rotation_adj * desired_robot_to_right_foot_.block<3,3>(0,0);;
805 mat_robot_to_left_foot_modified_.block<3,3>(0,0) = lf_rotation_adj * desired_robot_to_left_foot_.block<3,3>(0,0);;
807 mat_robot_to_cob_modified_.coeffRef(0,3) = desired_robot_to_cob_.coeff(0,3) + pose_cob_adjustment_.coeff(0);
808 mat_robot_to_cob_modified_.coeffRef(1,3) = desired_robot_to_cob_.coeff(1,3) + pose_cob_adjustment_.coeff(1);
809 mat_robot_to_cob_modified_.coeffRef(2,3) = desired_robot_to_cob_.coeff(2,3) + pose_cob_adjustment_.coeff(2);
811 mat_robot_to_right_foot_modified_.coeffRef(0,3) = desired_robot_to_right_foot_.coeff(0,3) + pose_right_foot_adjustment_.coeff(0);
812 mat_robot_to_right_foot_modified_.coeffRef(1,3) = desired_robot_to_right_foot_.coeff(1,3) + pose_right_foot_adjustment_.coeff(1);
813 mat_robot_to_right_foot_modified_.coeffRef(2,3) = desired_robot_to_right_foot_.coeff(2,3) + pose_right_foot_adjustment_.coeff(2);
815 mat_robot_to_left_foot_modified_.coeffRef(0,3) = desired_robot_to_left_foot_.coeff(0,3) + pose_left_foot_adjustment_.coeff(0);
816 mat_robot_to_left_foot_modified_.coeffRef(1,3) = desired_robot_to_left_foot_.coeff(1,3) + pose_left_foot_adjustment_.coeff(1);
817 mat_robot_to_left_foot_modified_.coeffRef(2,3) = desired_robot_to_left_foot_.coeff(2,3) + pose_left_foot_adjustment_.coeff(2);
819 if(balance_error != 0)
820 *balance_error = balance_control_error_;
822 *robot_to_cob_modified = mat_robot_to_cob_modified_;
823 *robot_to_right_foot_modified = mat_robot_to_right_foot_modified_;
824 *robot_to_left_foot_modified = mat_robot_to_left_foot_modified_;
829 desired_robot_to_cob_ = robot_to_cob;
830 desired_robot_to_right_foot_ = robot_to_right_foot;
831 desired_robot_to_left_foot_ = robot_to_left_foot;
836 foot_roll_gyro_ctrl_.desired_ = gyro_roll;
837 foot_pitch_gyro_ctrl_.desired_ = gyro_pitch;
842 foot_roll_angle_ctrl_.desired_ = cob_orientation_roll;
843 foot_pitch_angle_ctrl_.desired_ = cob_orientation_pitch;
847 double r_torque_roll_Nm,
double r_torque_pitch_Nm,
double r_torque_yaw_Nm,
848 double l_force_x_N,
double l_force_y_N,
double l_force_z_N,
849 double l_torque_roll_Nm,
double l_torque_pitch_Nm,
double l_torque_yaw_Nm)
851 right_foot_force_x_ctrl_.desired_ = r_force_x_N;
852 right_foot_force_y_ctrl_.desired_ = r_force_y_N;
853 right_foot_force_z_ctrl_.desired_ = r_force_z_N;
854 right_foot_torque_roll_ctrl_.desired_ = r_torque_roll_Nm;
855 right_foot_torque_pitch_ctrl_.desired_ = r_torque_pitch_Nm;
857 left_foot_force_x_ctrl_.desired_ = l_force_x_N;
858 left_foot_force_y_ctrl_.desired_ = l_force_y_N;
859 left_foot_force_z_ctrl_.desired_ = l_force_z_N;
860 left_foot_torque_roll_ctrl_.desired_ = l_torque_roll_Nm;
861 left_foot_torque_pitch_ctrl_.desired_ = l_torque_pitch_Nm;
867 current_gyro_roll_rad_per_sec_ = gyro_roll;
868 current_gyro_pitch_rad_per_sec_ = gyro_pitch;
873 current_orientation_roll_rad_ = cob_orientation_roll;
874 current_orientation_pitch_rad_ = cob_orientation_pitch;
878 double r_torque_roll_Nm,
double r_torque_pitch_Nm,
double r_torque_yaw_Nm,
879 double l_force_x_N,
double l_force_y_N,
double l_force_z_N,
880 double l_torque_roll_Nm,
double l_torque_pitch_Nm,
double l_torque_yaw_Nm)
882 current_right_fx_N_ = r_force_x_N;
883 current_right_fy_N_ = r_force_y_N;
884 current_right_fz_N_ = r_force_z_N;
885 current_right_tx_Nm_ = r_torque_roll_Nm;
886 current_right_ty_Nm_ = r_torque_pitch_Nm;
887 current_right_tz_Nm_ = r_torque_yaw_Nm;
889 current_left_fx_N_ = l_force_x_N;
890 current_left_fy_N_ = l_force_y_N;
891 current_left_fz_N_ = l_force_z_N;
892 current_left_tx_Nm_ = l_torque_roll_Nm;
893 current_left_ty_Nm_ = l_torque_pitch_Nm;
894 current_left_tz_Nm_ = l_torque_yaw_Nm;
899 double cob_roll_max_adjustment_rad,
double cob_pitch_max_adjustment_rad,
double cob_yaw_max_adjustment_rad,
900 double foot_x_max_adjustment_m,
double foot_y_max_adjustment_m,
double foot_z_max_adjustment_m,
901 double foot_roll_max_adjustment_rad,
double foot_pitch_max_adjustment_rad,
double foot_yaw_max_adjustment_rad)
903 cob_x_adjustment_abs_max_m_ = cob_x_max_adjustment_m;
904 cob_y_adjustment_abs_max_m_ = cob_y_max_adjustment_m;
905 cob_z_adjustment_abs_max_m_ = cob_z_max_adjustment_m;
906 cob_roll_adjustment_abs_max_rad_ = cob_roll_max_adjustment_rad;
907 cob_pitch_adjustment_abs_max_rad_ = cob_pitch_max_adjustment_rad;
908 cob_yaw_adjustment_abs_max_rad_ = cob_yaw_max_adjustment_rad;
909 foot_x_adjustment_abs_max_m_ = foot_x_max_adjustment_m;
910 foot_y_adjustment_abs_max_m_ = foot_y_max_adjustment_m;
911 foot_z_adjustment_abs_max_m_ = foot_z_max_adjustment_m;
912 foot_roll_adjustment_abs_max_rad_ = foot_roll_max_adjustment_rad;
913 foot_pitch_adjustment_abs_max_rad_ = foot_pitch_max_adjustment_rad;
914 foot_yaw_adjustment_abs_max_rad_ = foot_yaw_max_adjustment_rad;
920 cob_x_manual_adjustment_m_ = cob_x_adjustment_m;
921 cob_y_manual_adjustment_m_ = cob_y_adjustment_m;
922 cob_z_manual_adjustment_m_ = cob_z_adjustment_m;
927 return cob_x_manual_adjustment_m_;
932 return cob_y_manual_adjustment_m_;
937 return cob_z_manual_adjustment_m_;
void setForceTorqueBalanceEnable(bool enable)
void setCurrentOrientationSensorOutput(double cob_orientation_roll, double cob_orientation_pitch)
void setGyroBalanceEnable(bool enable)
Eigen::Matrix3d getRotationZ(double angle)
double getFilteredOutput(double present_raw_value)
double getCOBManualAdjustmentZ()
void setGyroBalanceEnable(bool enable)
void setForceTorqueBalanceEnable(bool enable)
void setDesiredCOBOrientation(double cob_orientation_roll, double cob_orientation_pitch)
void process(int *balance_error, Eigen::MatrixXd *robot_to_cob_modified, Eigen::MatrixXd *robot_to_right_foot_modified, Eigen::MatrixXd *robot_to_left_foot_modified)
void setDesiredCOBOrientation(double cob_orientation_roll, double cob_orientation_pitch)
double getCOBManualAdjustmentZ()
void initialize(const int control_cycle_msec)
double getCOBManualAdjustmentX()
void setCurrentGyroSensorOutput(double gyro_roll, double gyro_pitch)
double getDampingControllerOutput(double present_sensor_output)
Eigen::Matrix3d getRotationX(double angle)
void setDesiredPose(const Eigen::MatrixXd &robot_to_cob, const Eigen::MatrixXd &robot_to_right_foot, const Eigen::MatrixXd &robot_to_left_foot)
~BalanceControlUsingPDController()
Eigen::Matrix3d getRotationY(double angle)
void initialize(const int control_cycle_msec)
void setCOBManualAdjustment(double cob_x_adjustment_m, double cob_y_adjustment_m, double cob_z_adjustment_m)
void setCurrentFootForceTorqueSensorOutput(double r_force_x_N, double r_force_y_N, double r_force_z_N, double r_torque_roll_Nm, double r_torque_pitch_Nm, double r_torque_yaw_Nm, double l_force_x_N, double l_force_y_N, double l_force_z_N, double l_torque_roll_Nm, double l_torque_pitch_Nm, double l_torque_yaw_Nm)
void setGyroBalanceGainRatio(double gyro_balance_gain_ratio)
void setMaximumAdjustment(double cob_x_max_adjustment_m, double cob_y_max_adjustment_m, double cob_z_max_adjustment_m, double cob_roll_max_adjustment_rad, double cob_pitch_max_adjustment_rad, double cob_yaw_max_adjustment_rad, double foot_x_max_adjustment_m, double foot_y_max_adjustment_m, double foot_z_max_adjustment_m, double foot_roll_max_adjustment_rad, double foot_pitch_max_adjustment_rad, double foot_yaw_max_adjustment_rad)
void setCurrentGyroSensorOutput(double gyro_roll, double gyro_pitch)
double getCOBManualAdjustmentY()
double getFeedBack(double present_sensor_output)
void setCutOffFrequency(double cut_off_frequency)
double control_cycle_sec_
void process(int *balance_error, Eigen::MatrixXd *robot_to_cob_modified, Eigen::MatrixXd *robot_to_right_foot_modified, Eigen::MatrixXd *robot_to_left_foot_modified)
void setDesiredCOBGyro(double gyro_roll, double gyro_pitch)
void setDesiredCOBGyro(double gyro_roll, double gyro_pitch)
double getCOBManualAdjustmentY()
void setMaximumAdjustment(double cob_x_max_adjustment_m, double cob_y_max_adjustment_m, double cob_z_max_adjustment_m, double cob_roll_max_adjustment_rad, double cob_pitch_max_adjustment_rad, double cob_yaw_max_adjustment_rad, double foot_x_max_adjustment_m, double foot_y_max_adjustment_m, double foot_z_max_adjustment_m, double foot_roll_max_adjustment_rad, double foot_pitch_max_adjustment_rad, double foot_yaw_max_adjustment_rad)
static const int BalanceLimit
double getGyroBalanceGainRatio(void)
~BalanceControlUsingDampingConroller()
Eigen::Matrix4d getRotation4d(double roll, double pitch, double yaw)
double getCutOffFrequency(void)
void setDesiredPose(const Eigen::MatrixXd &robot_to_cob, const Eigen::MatrixXd &robot_to_right_foot, const Eigen::MatrixXd &robot_to_left_foot)
double time_constant_sec_
void setDesiredFootForceTorque(double r_force_x_N, double r_force_y_N, double r_force_z_N, double r_torque_roll_Nm, double r_torque_pitch_Nm, double r_torque_yaw_Nm, double l_force_x_N, double l_force_y_N, double l_force_z_N, double l_torque_roll_Nm, double l_torque_pitch_Nm, double l_torque_yaw_Nm)
void setCurrentFootForceTorqueSensorOutput(double r_force_x_N, double r_force_y_N, double r_force_z_N, double r_torque_roll_Nm, double r_torque_pitch_Nm, double r_torque_yaw_Nm, double l_force_x_N, double l_force_y_N, double l_force_z_N, double l_torque_roll_Nm, double l_torque_pitch_Nm, double l_torque_yaw_Nm)
void setDesiredFootForceTorque(double r_force_x_N, double r_force_y_N, double r_force_z_N, double r_torque_roll_Nm, double r_torque_pitch_Nm, double r_torque_yaw_Nm, double l_force_x_N, double l_force_y_N, double l_force_z_N, double l_torque_roll_Nm, double l_torque_pitch_Nm, double l_torque_yaw_Nm)
BalanceControlUsingDampingConroller()
BalanceControlUsingPDController()
double getCOBManualAdjustmentX()
void setCurrentOrientationSensorOutput(double cob_orientation_roll, double cob_orientation_pitch)
void initialize(double control_cycle_sec_, double cut_off_frequency)
void setOrientationBalanceEnable(bool enable)
void setOrientationBalanceEnable(bool enable)
void setCOBManualAdjustment(double cob_x_adjustment_m, double cob_y_adjustment_m, double cob_z_adjustment_m)