76 cut_off_freq_ = cut_off_frequency;
80 if(cut_off_frequency > 0)
91 cut_off_freq_ = cut_off_frequency;
95 if(cut_off_frequency > 0)
103 cut_off_freq_ = cut_off_frequency;
105 if(cut_off_frequency > 0)
113 return cut_off_freq_;
118 prev_output_ = alpha_*present_raw_value + (1.0 - alpha_)*prev_output_;
137 prev_err_ = curr_err_;
138 curr_err_ =
desired_ - present_sensor_output;
140 return (p_gain_*curr_err_ + d_gain_*(curr_err_ - prev_err_));
151 orientation_enable_ = 1.0;
154 desired_robot_to_cob_ = Eigen::MatrixXd::Identity(4,4);
155 desired_robot_to_right_foot_ = Eigen::MatrixXd::Identity(4,4);
156 desired_robot_to_left_foot_ = Eigen::MatrixXd::Identity(4,4);
158 gyro_cut_off_freq_ = 10.0;
160 gyro_roll_filtered_ = gyro_pitch_filtered_ = 0;
161 desired_gyro_roll_ = desired_gyro_pitch_ = 0;
164 current_gyro_roll_rad_per_sec_ = current_gyro_pitch_rad_per_sec_ = 0;
166 current_orientation_roll_rad_ = current_orientation_pitch_rad_ = 0;
168 current_right_fx_N_ = current_right_fy_N_ = current_right_fz_N_ = 0;
169 current_right_tx_Nm_ = current_right_ty_Nm_ = current_right_tz_Nm_ = 0;
170 current_left_fx_N_ = current_left_fy_N_ = current_left_fz_N_ = 0;
171 current_left_tx_Nm_ = current_left_ty_Nm_ = current_left_tz_Nm_ = 0;
174 foot_roll_adjustment_by_gyro_roll_ = 0;
175 foot_pitch_adjustment_by_gyro_pitch_ = 0;
177 foot_roll_adjustment_by_orientation_roll_ = 0;
178 foot_pitch_adjustment_by_orientation_pitch_ = 0;
180 foot_z_adjustment_by_force_z_difference_ = 0;
181 r_foot_z_adjustment_by_force_z_ = 0;
182 l_foot_z_adjustment_by_force_z_ = 0;
184 r_foot_x_adjustment_by_force_x_ = 0;
185 r_foot_y_adjustment_by_force_y_ = 0;
186 r_foot_roll_adjustment_by_torque_roll_ = 0;
187 r_foot_pitch_adjustment_by_torque_pitch_ = 0;
189 l_foot_x_adjustment_by_force_x_ = 0;
190 l_foot_y_adjustment_by_force_y_ = 0;
191 l_foot_roll_adjustment_by_torque_roll_ = 0;
192 l_foot_pitch_adjustment_by_torque_pitch_ = 0;
195 cob_x_manual_adjustment_m_ = 0;
196 cob_y_manual_adjustment_m_ = 0;
197 cob_z_manual_adjustment_m_ = 0;
200 gyro_balance_gain_ratio_ = 0.0;
201 gyro_balance_roll_gain_ = -0.10*0.75*gyro_balance_gain_ratio_;
202 gyro_balance_pitch_gain_ = -0.10*0.5 *gyro_balance_gain_ratio_;
206 cob_x_adjustment_abs_max_m_ = 0.05;
207 cob_y_adjustment_abs_max_m_ = 0.05;
208 cob_z_adjustment_abs_max_m_ = 0.05;
212 foot_x_adjustment_abs_max_m_ = 0.05;
213 foot_y_adjustment_abs_max_m_ = 0.05;
214 foot_z_adjustment_abs_max_m_ = 0.05;
219 mat_robot_to_cob_modified_ = Eigen::MatrixXd::Identity(4,4);
220 mat_robot_to_right_foot_modified_ = Eigen::MatrixXd::Identity(4,4);
221 mat_robot_to_left_foot_modified_ = Eigen::MatrixXd::Identity(4,4);
222 pose_cob_adjustment_ = Eigen::VectorXd::Zero(6);
223 pose_right_foot_adjustment_ = Eigen::VectorXd::Zero(6);;
224 pose_left_foot_adjustment_ = Eigen::VectorXd::Zero(6);;
236 gyro_cut_off_freq_ = 10.0;
239 pose_cob_adjustment_.fill(0);
240 pose_right_foot_adjustment_.fill(0);
241 pose_left_foot_adjustment_.fill(0);
272 orientation_enable_ = 1.0;
274 orientation_enable_ = 0.0;
289 pose_cob_adjustment_.fill(0);
290 pose_right_foot_adjustment_.fill(0);
291 pose_left_foot_adjustment_.fill(0);
294 gyro_roll_filtered_ = current_gyro_roll_rad_per_sec_*gyro_lpf_alpha_ + (1.0 - gyro_lpf_alpha_)*gyro_roll_filtered_;
295 gyro_pitch_filtered_ = current_gyro_pitch_rad_per_sec_*gyro_lpf_alpha_ + (1.0 - gyro_lpf_alpha_)*gyro_pitch_filtered_;
297 foot_roll_adjustment_by_gyro_roll_ = gyro_enable_ * (desired_gyro_roll_ - gyro_roll_filtered_) * gyro_balance_roll_gain_;
298 foot_pitch_adjustment_by_gyro_pitch_ = gyro_enable_ * (desired_gyro_pitch_ - gyro_pitch_filtered_) * gyro_balance_pitch_gain_;
301 foot_roll_adjustment_by_orientation_roll_ = orientation_enable_ * foot_roll_angle_ctrl_.getDampingControllerOutput(current_orientation_roll_rad_);
302 foot_pitch_adjustment_by_orientation_pitch_ = orientation_enable_ * foot_pitch_angle_ctrl_.getDampingControllerOutput(current_orientation_pitch_rad_);
304 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);
305 Eigen::MatrixXd mat_r_xy, mat_l_xy;
306 mat_r_xy.resize(4,1); mat_l_xy.resize(4,1);
307 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));
308 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));
309 mat_r_xy.coeffRef(2,0) = 0.0;
310 mat_r_xy.coeffRef(3,0) = 1;
312 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));
313 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));
314 mat_l_xy.coeffRef(2,0) = 0.0;
315 mat_l_xy.coeffRef(3,0) = 1;
317 mat_r_xy = mat_orientation_adjustment_by_imu * mat_r_xy;
318 mat_l_xy = mat_orientation_adjustment_by_imu * mat_l_xy;
321 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_);
322 r_foot_z_adjustment_by_force_z_ = ft_enable_*0.001*right_foot_force_z_ctrl_.getDampingControllerOutput(current_right_fz_N_);
323 l_foot_z_adjustment_by_force_z_ = ft_enable_*0.001*left_foot_force_z_ctrl_.getDampingControllerOutput(current_left_fz_N_);
325 r_foot_x_adjustment_by_force_x_ = ft_enable_*0.001*right_foot_force_x_ctrl_.getDampingControllerOutput(current_right_fx_N_);
326 r_foot_y_adjustment_by_force_y_ = ft_enable_*0.001*right_foot_force_y_ctrl_.getDampingControllerOutput(current_right_fy_N_);
327 r_foot_roll_adjustment_by_torque_roll_ = ft_enable_*right_foot_torque_roll_ctrl_.getDampingControllerOutput(current_right_tx_Nm_);
328 r_foot_pitch_adjustment_by_torque_pitch_ = ft_enable_*right_foot_torque_pitch_ctrl_.getDampingControllerOutput(current_right_ty_Nm_);
330 l_foot_x_adjustment_by_force_x_ = ft_enable_*0.001*left_foot_force_x_ctrl_.getDampingControllerOutput(current_left_fx_N_);
331 l_foot_y_adjustment_by_force_y_ = ft_enable_*0.001*left_foot_force_y_ctrl_.getDampingControllerOutput(current_left_fy_N_);
332 l_foot_roll_adjustment_by_torque_roll_ = ft_enable_*left_foot_torque_roll_ctrl_.getDampingControllerOutput(current_left_tx_Nm_);
333 l_foot_pitch_adjustment_by_torque_pitch_ = ft_enable_*left_foot_torque_pitch_ctrl_.getDampingControllerOutput(current_left_ty_Nm_);
335 r_foot_z_adjustment_by_force_z_ += ft_enable_*0.001*0.0*(right_foot_force_z_ctrl_.desired_ - current_right_fz_N_);
336 l_foot_z_adjustment_by_force_z_ += ft_enable_*0.001*0.0*(left_foot_force_z_ctrl_.desired_ - current_left_fz_N_);
338 pose_cob_adjustment_.coeffRef(0) = cob_x_manual_adjustment_m_;
339 pose_cob_adjustment_.coeffRef(1) = cob_y_manual_adjustment_m_;
340 pose_cob_adjustment_.coeffRef(2) = cob_z_manual_adjustment_m_;
342 pose_right_foot_adjustment_.coeffRef(0) = r_foot_x_adjustment_by_force_x_;
343 pose_right_foot_adjustment_.coeffRef(1) = r_foot_y_adjustment_by_force_y_;
344 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;
345 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_);
346 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_);
348 pose_left_foot_adjustment_.coeffRef(0) = l_foot_x_adjustment_by_force_x_;
349 pose_left_foot_adjustment_.coeffRef(1) = l_foot_y_adjustment_by_force_y_;
350 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;
351 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_);
352 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_);
355 if((fabs(pose_cob_adjustment_.coeff(0)) == cob_x_adjustment_abs_max_m_ ) ||
356 (fabs(pose_cob_adjustment_.coeff(1)) == cob_y_adjustment_abs_max_m_ ) ||
357 (fabs(pose_cob_adjustment_.coeff(2)) == cob_z_adjustment_abs_max_m_ ) ||
358 (fabs(pose_cob_adjustment_.coeff(3)) == cob_roll_adjustment_abs_max_rad_ ) ||
359 (fabs(pose_cob_adjustment_.coeff(4)) == cob_pitch_adjustment_abs_max_rad_) ||
360 (fabs(pose_right_foot_adjustment_.coeff(0)) == foot_x_adjustment_abs_max_m_ ) ||
361 (fabs(pose_right_foot_adjustment_.coeff(1)) == foot_y_adjustment_abs_max_m_ ) ||
362 (fabs(pose_right_foot_adjustment_.coeff(2)) == foot_z_adjustment_abs_max_m_ ) ||
363 (fabs(pose_right_foot_adjustment_.coeff(3)) == foot_roll_adjustment_abs_max_rad_ ) ||
364 (fabs(pose_right_foot_adjustment_.coeff(4)) == foot_pitch_adjustment_abs_max_rad_) ||
365 (fabs(pose_left_foot_adjustment_.coeff(0)) == foot_x_adjustment_abs_max_m_ ) ||
366 (fabs(pose_left_foot_adjustment_.coeff(1)) == foot_y_adjustment_abs_max_m_ ) ||
367 (fabs(pose_left_foot_adjustment_.coeff(2)) == foot_z_adjustment_abs_max_m_ ) ||
368 (fabs(pose_left_foot_adjustment_.coeff(3)) == foot_roll_adjustment_abs_max_rad_ ) ||
369 (fabs(pose_left_foot_adjustment_.coeff(4)) == foot_pitch_adjustment_abs_max_rad_))
372 pose_cob_adjustment_.coeffRef(0) = copysign(fmin(fabs(pose_cob_adjustment_.coeff(0)), cob_x_adjustment_abs_max_m_ ), pose_cob_adjustment_.coeff(0));
373 pose_cob_adjustment_.coeffRef(1) = copysign(fmin(fabs(pose_cob_adjustment_.coeff(1)), cob_x_adjustment_abs_max_m_ ), pose_cob_adjustment_.coeff(1));
374 pose_cob_adjustment_.coeffRef(2) = copysign(fmin(fabs(pose_cob_adjustment_.coeff(2)), cob_x_adjustment_abs_max_m_ ), pose_cob_adjustment_.coeff(2));
375 pose_cob_adjustment_.coeffRef(3) = copysign(fmin(fabs(pose_cob_adjustment_.coeff(3)), cob_roll_adjustment_abs_max_rad_ ), pose_cob_adjustment_.coeff(3));
376 pose_cob_adjustment_.coeffRef(4) = copysign(fmin(fabs(pose_cob_adjustment_.coeff(4)), cob_pitch_adjustment_abs_max_rad_), pose_cob_adjustment_.coeff(4));
377 pose_cob_adjustment_.coeffRef(5) = 0;
379 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));
380 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));
381 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));
382 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));
383 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));
384 pose_right_foot_adjustment_.coeffRef(5) = 0;
386 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));
387 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));
388 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));
389 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));
390 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));
391 pose_left_foot_adjustment_.coeffRef(5) = 0;
396 mat_robot_to_cob_modified_.block<3,3>(0,0) = cob_rotation_adj * desired_robot_to_cob_.block<3,3>(0,0);
397 mat_robot_to_right_foot_modified_.block<3,3>(0,0) = rf_rotation_adj * desired_robot_to_right_foot_.block<3,3>(0,0);;
398 mat_robot_to_left_foot_modified_.block<3,3>(0,0) = lf_rotation_adj * desired_robot_to_left_foot_.block<3,3>(0,0);;
400 mat_robot_to_cob_modified_.coeffRef(0,3) = desired_robot_to_cob_.coeff(0,3) + pose_cob_adjustment_.coeff(0);
401 mat_robot_to_cob_modified_.coeffRef(1,3) = desired_robot_to_cob_.coeff(1,3) + pose_cob_adjustment_.coeff(1);
402 mat_robot_to_cob_modified_.coeffRef(2,3) = desired_robot_to_cob_.coeff(2,3) + pose_cob_adjustment_.coeff(2);
404 mat_robot_to_right_foot_modified_.coeffRef(0,3) = desired_robot_to_right_foot_.coeff(0,3) + pose_right_foot_adjustment_.coeff(0);
405 mat_robot_to_right_foot_modified_.coeffRef(1,3) = desired_robot_to_right_foot_.coeff(1,3) + pose_right_foot_adjustment_.coeff(1);
406 mat_robot_to_right_foot_modified_.coeffRef(2,3) = desired_robot_to_right_foot_.coeff(2,3) + pose_right_foot_adjustment_.coeff(2);
408 mat_robot_to_left_foot_modified_.coeffRef(0,3) = desired_robot_to_left_foot_.coeff(0,3) + pose_left_foot_adjustment_.coeff(0);
409 mat_robot_to_left_foot_modified_.coeffRef(1,3) = desired_robot_to_left_foot_.coeff(1,3) + pose_left_foot_adjustment_.coeff(1);
410 mat_robot_to_left_foot_modified_.coeffRef(2,3) = desired_robot_to_left_foot_.coeff(2,3) + pose_left_foot_adjustment_.coeff(2);
412 if(balance_error != 0)
413 *balance_error = balance_control_error_;
415 *robot_to_cob_modified = mat_robot_to_cob_modified_;
416 *robot_to_right_foot_modified = mat_robot_to_right_foot_modified_;
417 *robot_to_left_foot_modified = mat_robot_to_left_foot_modified_;
422 desired_robot_to_cob_ = robot_to_cob;
423 desired_robot_to_right_foot_ = robot_to_right_foot;
424 desired_robot_to_left_foot_ = robot_to_left_foot;
429 desired_gyro_roll_ = gyro_roll;
430 desired_gyro_pitch_ = gyro_pitch;
435 foot_roll_angle_ctrl_.desired_ = cob_orientation_roll;
436 foot_pitch_angle_ctrl_.desired_ = cob_orientation_pitch;
440 double r_torque_roll_Nm,
double r_torque_pitch_Nm,
double r_torque_yaw_Nm,
441 double l_force_x_N,
double l_force_y_N,
double l_force_z_N,
442 double l_torque_roll_Nm,
double l_torque_pitch_Nm,
double l_torque_yaw_Nm)
444 foot_force_z_diff_ctrl_.desired_ = l_force_z_N - r_force_z_N;
445 right_foot_force_z_ctrl_.desired_ = r_force_z_N;
446 left_foot_force_z_ctrl_.desired_ = l_force_z_N;
448 right_foot_force_x_ctrl_.desired_ = r_force_x_N;
449 right_foot_force_y_ctrl_.desired_ = r_force_y_N;
450 right_foot_torque_roll_ctrl_.desired_ = r_torque_roll_Nm;
451 right_foot_torque_pitch_ctrl_.desired_ = r_torque_pitch_Nm;
453 left_foot_force_x_ctrl_.desired_ = l_force_x_N;
454 left_foot_force_y_ctrl_.desired_ = l_force_y_N;
455 left_foot_torque_roll_ctrl_.desired_ = l_torque_roll_Nm;
456 left_foot_torque_pitch_ctrl_.desired_ = l_torque_pitch_Nm;
462 current_gyro_roll_rad_per_sec_ = gyro_roll;
463 current_gyro_pitch_rad_per_sec_ = gyro_pitch;
468 current_orientation_roll_rad_ = cob_orientation_roll;
469 current_orientation_pitch_rad_ = cob_orientation_pitch;
473 double r_torque_roll_Nm,
double r_torque_pitch_Nm,
double r_torque_yaw_Nm,
474 double l_force_x_N,
double l_force_y_N,
double l_force_z_N,
475 double l_torque_roll_Nm,
double l_torque_pitch_Nm,
double l_torque_yaw_Nm)
477 current_right_fx_N_ = r_force_x_N;
478 current_right_fy_N_ = r_force_y_N;
479 current_right_fz_N_ = r_force_z_N;
480 current_right_tx_Nm_ = r_torque_roll_Nm;
481 current_right_ty_Nm_ = r_torque_pitch_Nm;
482 current_right_tz_Nm_ = r_torque_yaw_Nm;
484 current_left_fx_N_ = l_force_x_N;
485 current_left_fy_N_ = l_force_y_N;
486 current_left_fz_N_ = l_force_z_N;
487 current_left_tx_Nm_ = l_torque_roll_Nm;
488 current_left_ty_Nm_ = l_torque_pitch_Nm;
489 current_left_tz_Nm_ = l_torque_yaw_Nm;
494 double cob_roll_max_adjustment_rad,
double cob_pitch_max_adjustment_rad,
double cob_yaw_max_adjustment_rad,
495 double foot_x_max_adjustment_m,
double foot_y_max_adjustment_m,
double foot_z_max_adjustment_m,
496 double foot_roll_max_adjustment_rad,
double foot_pitch_max_adjustment_rad,
double foot_yaw_max_adjustment_rad)
498 cob_x_adjustment_abs_max_m_ = cob_x_max_adjustment_m;
499 cob_y_adjustment_abs_max_m_ = cob_y_max_adjustment_m;
500 cob_z_adjustment_abs_max_m_ = cob_z_max_adjustment_m;
501 cob_roll_adjustment_abs_max_rad_ = cob_roll_max_adjustment_rad;
502 cob_pitch_adjustment_abs_max_rad_ = cob_pitch_max_adjustment_rad;
503 cob_yaw_adjustment_abs_max_rad_ = cob_yaw_max_adjustment_rad;
504 foot_x_adjustment_abs_max_m_ = foot_x_max_adjustment_m;
505 foot_y_adjustment_abs_max_m_ = foot_y_max_adjustment_m;
506 foot_z_adjustment_abs_max_m_ = foot_z_max_adjustment_m;
507 foot_roll_adjustment_abs_max_rad_ = foot_roll_max_adjustment_rad;
508 foot_pitch_adjustment_abs_max_rad_ = foot_pitch_max_adjustment_rad;
509 foot_yaw_adjustment_abs_max_rad_ = foot_yaw_max_adjustment_rad;
515 cob_x_manual_adjustment_m_ = cob_x_adjustment_m;
516 cob_y_manual_adjustment_m_ = cob_y_adjustment_m;
517 cob_z_manual_adjustment_m_ = cob_z_adjustment_m;
522 return cob_x_manual_adjustment_m_;
527 return cob_y_manual_adjustment_m_;
532 return cob_z_manual_adjustment_m_;
537 gyro_balance_gain_ratio_ = gyro_balance_gain_ratio;
538 gyro_balance_roll_gain_ = -0.10*0.75*gyro_balance_gain_ratio_;
539 gyro_balance_pitch_gain_ = -0.10*0.5 *gyro_balance_gain_ratio_;
544 return gyro_balance_gain_ratio_;
555 orientation_enable_ = 1.0;
558 desired_robot_to_cob_ = Eigen::MatrixXd::Identity(4,4);
559 desired_robot_to_right_foot_ = Eigen::MatrixXd::Identity(4,4);
560 desired_robot_to_left_foot_ = Eigen::MatrixXd::Identity(4,4);
563 current_gyro_roll_rad_per_sec_ = current_gyro_pitch_rad_per_sec_ = 0;
565 current_orientation_roll_rad_ = current_orientation_pitch_rad_ = 0;
567 current_right_fx_N_ = current_right_fy_N_ = current_right_fz_N_ = 0;
568 current_right_tx_Nm_ = current_right_ty_Nm_ = current_right_tz_Nm_ = 0;
569 current_left_fx_N_ = current_left_fy_N_ = current_left_fz_N_ = 0;
570 current_left_tx_Nm_ = current_left_ty_Nm_ = current_left_tz_Nm_ = 0;
573 foot_roll_adjustment_by_gyro_roll_ = 0;
574 foot_pitch_adjustment_by_gyro_pitch_ = 0;
576 foot_roll_adjustment_by_orientation_roll_ = 0;
577 foot_pitch_adjustment_by_orientation_pitch_ = 0;
579 r_foot_z_adjustment_by_force_z_ = 0;
580 l_foot_z_adjustment_by_force_z_ = 0;
582 r_foot_x_adjustment_by_force_x_ = 0;
583 r_foot_y_adjustment_by_force_y_ = 0;
584 r_foot_roll_adjustment_by_torque_roll_ = 0;
585 r_foot_pitch_adjustment_by_torque_pitch_ = 0;
587 l_foot_x_adjustment_by_force_x_ = 0;
588 l_foot_y_adjustment_by_force_y_ = 0;
589 l_foot_roll_adjustment_by_torque_roll_ = 0;
590 l_foot_pitch_adjustment_by_torque_pitch_ = 0;
593 cob_x_manual_adjustment_m_ = 0;
594 cob_y_manual_adjustment_m_ = 0;
595 cob_z_manual_adjustment_m_ = 0;
598 cob_x_adjustment_abs_max_m_ = 0.05;
599 cob_y_adjustment_abs_max_m_ = 0.05;
600 cob_z_adjustment_abs_max_m_ = 0.05;
604 foot_x_adjustment_abs_max_m_ = 0.1;
605 foot_y_adjustment_abs_max_m_ = 0.1;
606 foot_z_adjustment_abs_max_m_ = 0.1;
611 mat_robot_to_cob_modified_ = Eigen::MatrixXd::Identity(4,4);
612 mat_robot_to_right_foot_modified_ = Eigen::MatrixXd::Identity(4,4);
613 mat_robot_to_left_foot_modified_ = Eigen::MatrixXd::Identity(4,4);
614 pose_cob_adjustment_ = Eigen::VectorXd::Zero(6);
615 pose_right_foot_adjustment_ = Eigen::VectorXd::Zero(6);;
616 pose_left_foot_adjustment_ = Eigen::VectorXd::Zero(6);;
628 pose_cob_adjustment_.fill(0);
629 pose_right_foot_adjustment_.fill(0);
630 pose_left_foot_adjustment_.fill(0);
662 orientation_enable_ = 1.0;
664 orientation_enable_ = 0.0;
679 pose_cob_adjustment_.fill(0);
680 pose_right_foot_adjustment_.fill(0);
681 pose_left_foot_adjustment_.fill(0);
684 double roll_gyro_filtered = roll_gyro_lpf_.getFilteredOutput(current_gyro_roll_rad_per_sec_);
685 double pitch_gyro_filtered = pitch_gyro_lpf_.getFilteredOutput(current_gyro_pitch_rad_per_sec_);
687 double roll_angle_filtered = roll_angle_lpf_.getFilteredOutput(current_orientation_roll_rad_);
688 double pitch_angle_filtered = pitch_angle_lpf_.getFilteredOutput(current_orientation_pitch_rad_);
690 double right_foot_force_x_filtered = right_foot_force_x_lpf_.getFilteredOutput(current_right_fx_N_);
691 double right_foot_force_y_filtered = right_foot_force_y_lpf_.getFilteredOutput(current_right_fy_N_);
692 double right_foot_force_z_filtered = right_foot_force_z_lpf_.getFilteredOutput(current_right_fz_N_);
693 double right_foot_torque_roll_filtered = right_foot_torque_roll_lpf_.getFilteredOutput(current_right_tx_Nm_);
694 double right_foot_torque_pitch_filtered = right_foot_torque_pitch_lpf_.getFilteredOutput(current_right_ty_Nm_);;
696 double left_foot_force_x_filtered = left_foot_force_x_lpf_.getFilteredOutput(current_left_fx_N_);
697 double left_foot_force_y_filtered = left_foot_force_y_lpf_.getFilteredOutput(current_left_fy_N_);
698 double left_foot_force_z_filtered = left_foot_force_z_lpf_.getFilteredOutput(current_left_fz_N_);
699 double left_foot_torque_roll_filtered = left_foot_torque_roll_lpf_.getFilteredOutput(current_left_tx_Nm_);
700 double left_foot_torque_pitch_filtered = left_foot_torque_pitch_lpf_.getFilteredOutput(current_left_ty_Nm_);
704 foot_roll_adjustment_by_gyro_roll_ = -0.1*gyro_enable_*foot_roll_gyro_ctrl_.getFeedBack(roll_gyro_filtered);
705 foot_pitch_adjustment_by_gyro_pitch_ = -0.1*gyro_enable_*foot_pitch_gyro_ctrl_.getFeedBack(pitch_gyro_filtered);
708 foot_roll_adjustment_by_orientation_roll_ = -1.0*orientation_enable_ * foot_roll_angle_ctrl_.getFeedBack(roll_angle_filtered);
709 foot_pitch_adjustment_by_orientation_pitch_ = -1.0*orientation_enable_ * foot_pitch_angle_ctrl_.getFeedBack(pitch_angle_filtered);
711 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);
712 Eigen::MatrixXd mat_r_xy, mat_l_xy;
713 mat_r_xy.resize(4,1); mat_l_xy.resize(4,1);
714 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));
715 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));
716 mat_r_xy.coeffRef(2,0) = 0.0;
717 mat_r_xy.coeffRef(3,0) = 1;
719 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));
720 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));
721 mat_l_xy.coeffRef(2,0) = 0.0;
722 mat_l_xy.coeffRef(3,0) = 1;
724 mat_r_xy = mat_orientation_adjustment_by_imu * mat_r_xy;
725 mat_l_xy = mat_orientation_adjustment_by_imu * mat_l_xy;
728 r_foot_x_adjustment_by_force_x_ = ft_enable_*0.001*right_foot_force_x_ctrl_.getFeedBack(right_foot_force_x_filtered);
729 r_foot_y_adjustment_by_force_y_ = ft_enable_*0.001*right_foot_force_y_ctrl_.getFeedBack(right_foot_force_y_filtered);
730 r_foot_z_adjustment_by_force_z_ = ft_enable_*0.001*right_foot_force_z_ctrl_.getFeedBack(right_foot_force_z_filtered);
731 r_foot_roll_adjustment_by_torque_roll_ = ft_enable_*right_foot_torque_roll_ctrl_.getFeedBack(right_foot_torque_roll_filtered);
732 r_foot_pitch_adjustment_by_torque_pitch_ = ft_enable_*right_foot_torque_pitch_ctrl_.getFeedBack(right_foot_torque_pitch_filtered);
734 l_foot_x_adjustment_by_force_x_ = ft_enable_*0.001*left_foot_force_x_ctrl_.getFeedBack(left_foot_force_x_filtered);
735 l_foot_y_adjustment_by_force_y_ = ft_enable_*0.001*left_foot_force_y_ctrl_.getFeedBack(left_foot_force_y_filtered);
736 l_foot_z_adjustment_by_force_z_ = ft_enable_*0.001*left_foot_force_z_ctrl_.getFeedBack(left_foot_force_z_filtered);
737 l_foot_roll_adjustment_by_torque_roll_ = ft_enable_*left_foot_torque_roll_ctrl_.getFeedBack(left_foot_torque_roll_filtered);
738 l_foot_pitch_adjustment_by_torque_pitch_ = ft_enable_*left_foot_torque_pitch_ctrl_.getFeedBack(left_foot_torque_pitch_filtered);
741 pose_cob_adjustment_.coeffRef(0) = cob_x_manual_adjustment_m_;
742 pose_cob_adjustment_.coeffRef(1) = cob_y_manual_adjustment_m_;
743 pose_cob_adjustment_.coeffRef(2) = cob_z_manual_adjustment_m_;
745 pose_right_foot_adjustment_.coeffRef(0) = r_foot_x_adjustment_by_force_x_;
746 pose_right_foot_adjustment_.coeffRef(1) = r_foot_y_adjustment_by_force_y_;
747 pose_right_foot_adjustment_.coeffRef(2) = mat_r_xy.coeff(2, 0) + r_foot_z_adjustment_by_force_z_*1.0;
748 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_);
749 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_);
751 pose_left_foot_adjustment_.coeffRef(0) = l_foot_x_adjustment_by_force_x_;
752 pose_left_foot_adjustment_.coeffRef(1) = l_foot_y_adjustment_by_force_y_;
753 pose_left_foot_adjustment_.coeffRef(2) = mat_l_xy.coeff(2, 0) + l_foot_z_adjustment_by_force_z_*1.0;
754 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_);
755 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_);
758 if((fabs(pose_cob_adjustment_.coeff(0)) == cob_x_adjustment_abs_max_m_ ) ||
759 (fabs(pose_cob_adjustment_.coeff(1)) == cob_y_adjustment_abs_max_m_ ) ||
760 (fabs(pose_cob_adjustment_.coeff(2)) == cob_z_adjustment_abs_max_m_ ) ||
761 (fabs(pose_cob_adjustment_.coeff(3)) == cob_roll_adjustment_abs_max_rad_ ) ||
762 (fabs(pose_cob_adjustment_.coeff(4)) == cob_pitch_adjustment_abs_max_rad_) ||
763 (fabs(pose_right_foot_adjustment_.coeff(0)) == foot_x_adjustment_abs_max_m_ ) ||
764 (fabs(pose_right_foot_adjustment_.coeff(1)) == foot_y_adjustment_abs_max_m_ ) ||
765 (fabs(pose_right_foot_adjustment_.coeff(2)) == foot_z_adjustment_abs_max_m_ ) ||
766 (fabs(pose_right_foot_adjustment_.coeff(3)) == foot_roll_adjustment_abs_max_rad_ ) ||
767 (fabs(pose_right_foot_adjustment_.coeff(4)) == foot_pitch_adjustment_abs_max_rad_) ||
768 (fabs(pose_left_foot_adjustment_.coeff(0)) == foot_x_adjustment_abs_max_m_ ) ||
769 (fabs(pose_left_foot_adjustment_.coeff(1)) == foot_y_adjustment_abs_max_m_ ) ||
770 (fabs(pose_left_foot_adjustment_.coeff(2)) == foot_z_adjustment_abs_max_m_ ) ||
771 (fabs(pose_left_foot_adjustment_.coeff(3)) == foot_roll_adjustment_abs_max_rad_ ) ||
772 (fabs(pose_left_foot_adjustment_.coeff(4)) == foot_pitch_adjustment_abs_max_rad_))
775 pose_cob_adjustment_.coeffRef(0) = copysign(fmin(fabs(pose_cob_adjustment_.coeff(0)), cob_x_adjustment_abs_max_m_ ), pose_cob_adjustment_.coeff(0));
776 pose_cob_adjustment_.coeffRef(1) = copysign(fmin(fabs(pose_cob_adjustment_.coeff(1)), cob_x_adjustment_abs_max_m_ ), pose_cob_adjustment_.coeff(1));
777 pose_cob_adjustment_.coeffRef(2) = copysign(fmin(fabs(pose_cob_adjustment_.coeff(2)), cob_x_adjustment_abs_max_m_ ), pose_cob_adjustment_.coeff(2));
778 pose_cob_adjustment_.coeffRef(3) = copysign(fmin(fabs(pose_cob_adjustment_.coeff(3)), cob_roll_adjustment_abs_max_rad_ ), pose_cob_adjustment_.coeff(3));
779 pose_cob_adjustment_.coeffRef(4) = copysign(fmin(fabs(pose_cob_adjustment_.coeff(4)), cob_pitch_adjustment_abs_max_rad_), pose_cob_adjustment_.coeff(4));
780 pose_cob_adjustment_.coeffRef(5) = 0;
782 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));
783 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));
784 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));
785 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));
786 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));
787 pose_right_foot_adjustment_.coeffRef(5) = 0;
789 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));
790 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));
791 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));
792 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));
793 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));
794 pose_left_foot_adjustment_.coeffRef(5) = 0;
799 mat_robot_to_cob_modified_.block<3,3>(0,0) = cob_rotation_adj * desired_robot_to_cob_.block<3,3>(0,0);
800 mat_robot_to_right_foot_modified_.block<3,3>(0,0) = rf_rotation_adj * desired_robot_to_right_foot_.block<3,3>(0,0);;
801 mat_robot_to_left_foot_modified_.block<3,3>(0,0) = lf_rotation_adj * desired_robot_to_left_foot_.block<3,3>(0,0);;
803 mat_robot_to_cob_modified_.coeffRef(0,3) = desired_robot_to_cob_.coeff(0,3) + pose_cob_adjustment_.coeff(0);
804 mat_robot_to_cob_modified_.coeffRef(1,3) = desired_robot_to_cob_.coeff(1,3) + pose_cob_adjustment_.coeff(1);
805 mat_robot_to_cob_modified_.coeffRef(2,3) = desired_robot_to_cob_.coeff(2,3) + pose_cob_adjustment_.coeff(2);
807 mat_robot_to_right_foot_modified_.coeffRef(0,3) = desired_robot_to_right_foot_.coeff(0,3) + pose_right_foot_adjustment_.coeff(0);
808 mat_robot_to_right_foot_modified_.coeffRef(1,3) = desired_robot_to_right_foot_.coeff(1,3) + pose_right_foot_adjustment_.coeff(1);
809 mat_robot_to_right_foot_modified_.coeffRef(2,3) = desired_robot_to_right_foot_.coeff(2,3) + pose_right_foot_adjustment_.coeff(2);
811 mat_robot_to_left_foot_modified_.coeffRef(0,3) = desired_robot_to_left_foot_.coeff(0,3) + pose_left_foot_adjustment_.coeff(0);
812 mat_robot_to_left_foot_modified_.coeffRef(1,3) = desired_robot_to_left_foot_.coeff(1,3) + pose_left_foot_adjustment_.coeff(1);
813 mat_robot_to_left_foot_modified_.coeffRef(2,3) = desired_robot_to_left_foot_.coeff(2,3) + pose_left_foot_adjustment_.coeff(2);
815 if(balance_error != 0)
816 *balance_error = balance_control_error_;
818 *robot_to_cob_modified = mat_robot_to_cob_modified_;
819 *robot_to_right_foot_modified = mat_robot_to_right_foot_modified_;
820 *robot_to_left_foot_modified = mat_robot_to_left_foot_modified_;
825 desired_robot_to_cob_ = robot_to_cob;
826 desired_robot_to_right_foot_ = robot_to_right_foot;
827 desired_robot_to_left_foot_ = robot_to_left_foot;
832 foot_roll_gyro_ctrl_.desired_ = gyro_roll;
833 foot_pitch_gyro_ctrl_.desired_ = gyro_pitch;
838 foot_roll_angle_ctrl_.desired_ = cob_orientation_roll;
839 foot_pitch_angle_ctrl_.desired_ = cob_orientation_pitch;
843 double r_torque_roll_Nm,
double r_torque_pitch_Nm,
double r_torque_yaw_Nm,
844 double l_force_x_N,
double l_force_y_N,
double l_force_z_N,
845 double l_torque_roll_Nm,
double l_torque_pitch_Nm,
double l_torque_yaw_Nm)
847 right_foot_force_x_ctrl_.desired_ = r_force_x_N;
848 right_foot_force_y_ctrl_.desired_ = r_force_y_N;
849 right_foot_force_z_ctrl_.desired_ = r_force_z_N;
850 right_foot_torque_roll_ctrl_.desired_ = r_torque_roll_Nm;
851 right_foot_torque_pitch_ctrl_.desired_ = r_torque_pitch_Nm;
853 left_foot_force_x_ctrl_.desired_ = l_force_x_N;
854 left_foot_force_y_ctrl_.desired_ = l_force_y_N;
855 left_foot_force_z_ctrl_.desired_ = l_force_z_N;
856 left_foot_torque_roll_ctrl_.desired_ = l_torque_roll_Nm;
857 left_foot_torque_pitch_ctrl_.desired_ = l_torque_pitch_Nm;
863 current_gyro_roll_rad_per_sec_ = gyro_roll;
864 current_gyro_pitch_rad_per_sec_ = gyro_pitch;
869 current_orientation_roll_rad_ = cob_orientation_roll;
870 current_orientation_pitch_rad_ = cob_orientation_pitch;
874 double r_torque_roll_Nm,
double r_torque_pitch_Nm,
double r_torque_yaw_Nm,
875 double l_force_x_N,
double l_force_y_N,
double l_force_z_N,
876 double l_torque_roll_Nm,
double l_torque_pitch_Nm,
double l_torque_yaw_Nm)
878 current_right_fx_N_ = r_force_x_N;
879 current_right_fy_N_ = r_force_y_N;
880 current_right_fz_N_ = r_force_z_N;
881 current_right_tx_Nm_ = r_torque_roll_Nm;
882 current_right_ty_Nm_ = r_torque_pitch_Nm;
883 current_right_tz_Nm_ = r_torque_yaw_Nm;
885 current_left_fx_N_ = l_force_x_N;
886 current_left_fy_N_ = l_force_y_N;
887 current_left_fz_N_ = l_force_z_N;
888 current_left_tx_Nm_ = l_torque_roll_Nm;
889 current_left_ty_Nm_ = l_torque_pitch_Nm;
890 current_left_tz_Nm_ = l_torque_yaw_Nm;
895 double cob_roll_max_adjustment_rad,
double cob_pitch_max_adjustment_rad,
double cob_yaw_max_adjustment_rad,
896 double foot_x_max_adjustment_m,
double foot_y_max_adjustment_m,
double foot_z_max_adjustment_m,
897 double foot_roll_max_adjustment_rad,
double foot_pitch_max_adjustment_rad,
double foot_yaw_max_adjustment_rad)
899 cob_x_adjustment_abs_max_m_ = cob_x_max_adjustment_m;
900 cob_y_adjustment_abs_max_m_ = cob_y_max_adjustment_m;
901 cob_z_adjustment_abs_max_m_ = cob_z_max_adjustment_m;
902 cob_roll_adjustment_abs_max_rad_ = cob_roll_max_adjustment_rad;
903 cob_pitch_adjustment_abs_max_rad_ = cob_pitch_max_adjustment_rad;
904 cob_yaw_adjustment_abs_max_rad_ = cob_yaw_max_adjustment_rad;
905 foot_x_adjustment_abs_max_m_ = foot_x_max_adjustment_m;
906 foot_y_adjustment_abs_max_m_ = foot_y_max_adjustment_m;
907 foot_z_adjustment_abs_max_m_ = foot_z_max_adjustment_m;
908 foot_roll_adjustment_abs_max_rad_ = foot_roll_max_adjustment_rad;
909 foot_pitch_adjustment_abs_max_rad_ = foot_pitch_max_adjustment_rad;
910 foot_yaw_adjustment_abs_max_rad_ = foot_yaw_max_adjustment_rad;
916 cob_x_manual_adjustment_m_ = cob_x_adjustment_m;
917 cob_y_manual_adjustment_m_ = cob_y_adjustment_m;
918 cob_z_manual_adjustment_m_ = cob_z_adjustment_m;
923 return cob_x_manual_adjustment_m_;
928 return cob_y_manual_adjustment_m_;
933 return cob_z_manual_adjustment_m_;
void setGyroBalanceEnable(bool enable)
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)
Eigen::Matrix3d getRotationZ(double angle)
void setCutOffFrequency(double cut_off_frequency)
void setGyroBalanceGainRatio(double gyro_balance_gain_ratio)
double getGyroBalanceGainRatio(void)
void setCOBManualAdjustment(double cob_x_adjustment_m, double cob_y_adjustment_m, double cob_z_adjustment_m)
double getDampingControllerOutput(double present_sensor_output)
void setCOBManualAdjustment(double cob_x_adjustment_m, double cob_y_adjustment_m, double cob_z_adjustment_m)
void setDesiredPose(const Eigen::MatrixXd &robot_to_cob, const Eigen::MatrixXd &robot_to_right_foot, const Eigen::MatrixXd &robot_to_left_foot)
double control_cycle_sec_
void setForceTorqueBalanceEnable(bool enable)
double getFeedBack(double present_sensor_output)
Eigen::Matrix3d getRotationX(double angle)
Eigen::Matrix3d getRotationY(double angle)
void setOrientationBalanceEnable(bool enable)
void setForceTorqueBalanceEnable(bool enable)
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 process(int *balance_error, Eigen::MatrixXd *robot_to_cob_modified, Eigen::MatrixXd *robot_to_right_foot_modified, Eigen::MatrixXd *robot_to_left_foot_modified)
double getCOBManualAdjustmentX()
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 setOrientationBalanceEnable(bool enable)
double getCOBManualAdjustmentZ()
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 setCurrentGyroSensorOutput(double gyro_roll, double gyro_pitch)
double getCOBManualAdjustmentY()
void setDesiredCOBOrientation(double cob_orientation_roll, double cob_orientation_pitch)
BalanceControlUsingPDController()
void setDesiredCOBGyro(double gyro_roll, double gyro_pitch)
double getCutOffFrequency(void)
double time_constant_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)
double getFilteredOutput(double present_raw_value)
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 setCurrentOrientationSensorOutput(double cob_orientation_roll, double cob_orientation_pitch)
void setDesiredPose(const Eigen::MatrixXd &robot_to_cob, const Eigen::MatrixXd &robot_to_right_foot, const Eigen::MatrixXd &robot_to_left_foot)
void initialize(const int control_cycle_msec)
BalanceControlUsingDampingConroller()
void initialize(double control_cycle_sec_, double cut_off_frequency)
static const int BalanceLimit
void initialize(const int control_cycle_msec)
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)
~BalanceControlUsingPDController()
double getCOBManualAdjustmentX()
void setGyroBalanceEnable(bool enable)
double getCOBManualAdjustmentZ()
Eigen::Matrix4d getRotation4d(double roll, double pitch, double yaw)
~BalanceControlUsingDampingConroller()
void setDesiredCOBGyro(double gyro_roll, double gyro_pitch)
void setCurrentGyroSensorOutput(double gyro_roll, double gyro_pitch)
void setDesiredCOBOrientation(double cob_orientation_roll, double cob_orientation_pitch)
void setCurrentOrientationSensorOutput(double cob_orientation_roll, double cob_orientation_pitch)
double getCOBManualAdjustmentY()