limit_strideの処理

[Naoki-Hiraoka]

現状ほとんどgg.stride_limitation_type = OpenHRP.AutoBalancerService.CIRCLEで運用されていますが、
https://github.com/YutaKojio/rtmros_tutorials/blob/ba5f3ec4cc8e5d8f4067a93ecf9ba29f3ca02c8c/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py#L350

以下の2点について、重要な理由がある意図的なものなのか、そうではないものなのか、教えていただけますでしょうか。

1. goPosやgoVelocityのfootstep生成時に使用されるgait_generator::limit_strideでは円を用いた着地位置制限を行っているが、
   

   hrpsys-base/rtc/AutoBalancer/GaitGenerator.cpp

   Lines 1452 to 1459 in bacb4a0

   	double stride_r = std::pow(cur_fs.worldcoords.pos(0), 2.0) + std::pow(cur_fs.worldcoords.pos(1) + footstep_param.leg_default_translate_pos[cur_leg == LLEG ? RLEG : LLEG](1) - footstep_param.leg_default_translate_pos[cur_leg](1), 2.0);
   	// front, rear, outside limitation
   	double stride_r_limit = std::pow(std::max(limit[cur_fs.worldcoords.pos(0) >= 0 ? 0 : 3], limit[1] - limit[4]), 2.0);
   	if (stride_r > stride_r_limit) {
   	cur_fs.worldcoords.pos(0) *= sqrt(stride_r_limit / stride_r);
   	cur_fs.worldcoords.pos(1) = footstep_param.leg_default_translate_pos[cur_leg](1) - footstep_param.leg_default_translate_pos[cur_leg == LLEG ? RLEG : LLEG](1) +
   	sqrt(stride_r_limit / stride_r) * (cur_fs.worldcoords.pos(1) + footstep_param.leg_default_translate_pos[cur_leg == LLEG ? RLEG : LLEG](1) - footstep_param.leg_default_translate_pos[cur_leg](1));
   	}

   
   着地位置修正時(vision無し)に使用されるgait_generator::limit_stride_rectangle では行っていない点

2. goPosやgoVelocityのfootstep生成時に使用されるgait_generator::limit_strideや、着地位置修正時(vision無し)に使用されるgait_generator::limit_stride_rectangle では、遊脚のyaw旋回に応じてinsideの制約が変化するが、
   

   hrpsys-base/rtc/AutoBalancer/GaitGenerator.cpp

   Lines 1466 to 1474 in bacb4a0

   	cur_leg_vertices_y.push_back((cur_fs.worldcoords.pos + prev_fs_rot.transpose() * cur_fs.worldcoords.rot * hrp::Vector3(leg_margin[0], (cur_leg == LLEG ? 1 : -1) * leg_margin[2], 0.0))(1));
   	cur_leg_vertices_y.push_back((cur_fs.worldcoords.pos + prev_fs_rot.transpose() * cur_fs.worldcoords.rot * hrp::Vector3(leg_margin[0], (cur_leg == LLEG ? -1 : 1) * leg_margin[3], 0.0))(1));
   	cur_leg_vertices_y.push_back((cur_fs.worldcoords.pos + prev_fs_rot.transpose() * cur_fs.worldcoords.rot * hrp::Vector3(-1 * leg_margin[1], (cur_leg == LLEG ? 1 : -1) * leg_margin[2], 0.0))(1));
   	cur_leg_vertices_y.push_back((cur_fs.worldcoords.pos + prev_fs_rot.transpose() * cur_fs.worldcoords.rot * hrp::Vector3(-1 * leg_margin[1], (cur_leg == LLEG ? -1 : 1) * leg_margin[3], 0.0))(1));
   	if (cur_leg == LLEG) {
   	if (*std::min_element(cur_leg_vertices_y.begin(), cur_leg_vertices_y.end()) < limit[4]) cur_fs.worldcoords.pos(1) += limit[4] - *std::min_element(cur_leg_vertices_y.begin(), cur_leg_vertices_y.end());
   	} else {
   	if (*std::max_element(cur_leg_vertices_y.begin(), cur_leg_vertices_y.end()) > -1 * limit[4]) cur_fs.worldcoords.pos(1) += -1 * limit[4] - *std::max_element(cur_leg_vertices_y.begin(), cur_leg_vertices_y.end());
   	}

   
   

   hrpsys-base/rtc/AutoBalancer/GaitGenerator.cpp

   Lines 1494 to 1502 in bacb4a0

   	cur_leg_vertices_y.push_back((cur_fs.worldcoords.pos + prev_fs.worldcoords.rot.transpose() * cur_fs.worldcoords.rot * hrp::Vector3(leg_margin[0], (cur_leg == LLEG ? 1 : -1) * leg_margin[2], 0.0))(1));
   	cur_leg_vertices_y.push_back((cur_fs.worldcoords.pos + prev_fs.worldcoords.rot.transpose() * cur_fs.worldcoords.rot * hrp::Vector3(leg_margin[0], (cur_leg == LLEG ? -1 : 1) * leg_margin[3], 0.0))(1));
   	cur_leg_vertices_y.push_back((cur_fs.worldcoords.pos + prev_fs.worldcoords.rot.transpose() * cur_fs.worldcoords.rot * hrp::Vector3(-1 * leg_margin[1], (cur_leg == LLEG ? 1 : -1) * leg_margin[2], 0.0))(1));
   	cur_leg_vertices_y.push_back((cur_fs.worldcoords.pos + prev_fs.worldcoords.rot.transpose() * cur_fs.worldcoords.rot * hrp::Vector3(-1 * leg_margin[1], (cur_leg == LLEG ? -1 : 1) * leg_margin[3], 0.0))(1));
   	if (cur_leg == LLEG) {
   	if (*std::min_element(cur_leg_vertices_y.begin(), cur_leg_vertices_y.end()) < limit[4]) cur_fs.worldcoords.pos(1) += limit[4] - *std::min_element(cur_leg_vertices_y.begin(), cur_leg_vertices_y.end());
   	} else {
   	if (*std::max_element(cur_leg_vertices_y.begin(), cur_leg_vertices_y.end()) > -1 * limit[4]) cur_fs.worldcoords.pos(1) += -1 * limit[4] - *std::max_element(cur_leg_vertices_y.begin(), cur_leg_vertices_y.end());
   	}

   
   着地位置修正時(vision有り)に使用されるgait_generator::limit_stride_visionでは遊脚のyaw旋回を考慮しない点
   

   hrpsys-base/rtc/AutoBalancer/GaitGenerator.cpp

   Lines 831 to 841 in bacb4a0

   	if (cur_leg == RLEG) {
   	stride_limitation_polygon[0] = (preprev_fs_rot.transpose() * ((prev_fs_pos + prev_fs_rot * hrp::Vector3(-overwritable_stride_limitation[3], -overwritable_stride_limitation[4]-leg_margin[3], 0.0)) - preprev_fs_pos)).head(2);
   	stride_limitation_polygon[1] = (preprev_fs_rot.transpose() * ((prev_fs_pos + prev_fs_rot * hrp::Vector3(-overwritable_stride_limitation[3], -overwritable_stride_limitation[1], 0.0)) - preprev_fs_pos)).head(2);
   	stride_limitation_polygon[2] = (preprev_fs_rot.transpose() * ((prev_fs_pos + prev_fs_rot * hrp::Vector3(overwritable_stride_limitation[0], -overwritable_stride_limitation[1], 0.0)) - preprev_fs_pos)).head(2);
   	stride_limitation_polygon[3] = (preprev_fs_rot.transpose() * ((prev_fs_pos + prev_fs_rot * hrp::Vector3(overwritable_stride_limitation[0], -overwritable_stride_limitation[4]-leg_margin[3], 0.0)) - preprev_fs_pos)).head(2);
   	} else {
   	stride_limitation_polygon[0] = (preprev_fs_rot.transpose() * ((prev_fs_pos + prev_fs_rot * hrp::Vector3(-overwritable_stride_limitation[3], overwritable_stride_limitation[1], 0.0)) - preprev_fs_pos)).head(2);
   	stride_limitation_polygon[1] = (preprev_fs_rot.transpose() * ((prev_fs_pos + prev_fs_rot * hrp::Vector3(-overwritable_stride_limitation[3], overwritable_stride_limitation[4]+leg_margin[3], 0.0)) - preprev_fs_pos)).head(2);
   	stride_limitation_polygon[2] = (preprev_fs_rot.transpose() * ((prev_fs_pos + prev_fs_rot * hrp::Vector3(overwritable_stride_limitation[0], overwritable_stride_limitation[4]+leg_margin[3], 0.0)) - preprev_fs_pos)).head(2);
   	stride_limitation_polygon[3] = (preprev_fs_rot.transpose() * ((prev_fs_pos + prev_fs_rot * hrp::Vector3(overwritable_stride_limitation[0], overwritable_stride_limitation[1], 0.0)) - preprev_fs_pos)).head(2);
   	}

[YutaKojio]

どちらの指摘もあっています．
意図的というか，ちゃんとやっていないことはわかっていてやっていない状態です．

1. visionなしのIROS2019の段階では，着地位置・時間修正でx成分とy成分が分離できるようにしたくて長方形にしました．
   しかし，visionありのIROS2020では，着地位置制限は凸包であればなんでもよくなったので，CIRCLE制限に近い凸包で近似したものを使ってもいい気はします．

2. visionありの，足場制限と着地位置制限の積集合への変換は一歩の最初に一回のみ行っていて，それ以降に変わるyawにちゃんと対応できないので，この時点でもyawを考慮しないことにしました．
   ただ，yawの目標値が遊脚途中で変わることはあまりないので，この時点でも考慮するようにしてもいいと思います．
   また，そもそもこの積集合への変換がどの程度の計算量になっているのか真面目に考えていないので，軽いようであれば毎制御周期行うようにして，最新の足場情報へ更新できるようにしてもいいし，重いようなら実装をもっと賢いものに変えるのもありだと思います．