ompl+αでリアルタイム経路生成

この記事は学ロボアドベントカレンダー11日目の記事です。
adventar.org

この記事はompl+αでロボットアームの経路をリアルタイムで生成する手続きをまとめた記事です。

FMT*で生成したglobal pathをスプラインで二重に補間し、台形加減速まで経路側で処理します。
moveitは使いません。

動作環境にはUbuntu 22.04 + ROS2 humbleを想定しています。

==============目次==============

1. 一般的な経路生成の流れ
2. ompl+αを用いた経路生成
3. global pathの生成
4. local pathの生成
- 4.1. スプライン補完
- 4.2. 台形加減速の実装
5. 実際の動作

===============================

1. 一般的な経路生成の流れ

実装した手法について説明する前に、一般的な経路生成の流れを説明します。

一般的に、ロボットの経路生成を行う場合、先に大雑把なglobal pathを引いてから
問題設定に合わせて細かいlocal pathを引くという手続きを取る事が多いです。

この様に問題を分割する事で、経路全体を生成するには実行時間が足りない場合でも、
1周期内に必要なパスの生成が間に合うようにlocal pathの計算を調整する事で対応出来るようになります。

特にアームの軌道生成では、global pathの生成サンプリングベースの手法を、
local pathの生成には最適化ベースの手法を用いる事が多いです。

というのも、サンプリングベースの手法は、複雑な系に対してでも実行可能な経路を生成する事が出来ますが、
計算コストが大きく、滑らかな経路が引かれるとも限らない為、後処理が必要となります。(ex. RRT、PRM)

対して、最適化ベースの手法は、高速に滑らかな実行可能解が得られるものの、
目的関数が非凸で局所解に嵌りやすく現実には不適切な経路が引かれる事も多いです。(ex. CHOMP、STOMP)

これらの特徴から、サンプリングベースの手法で予めおおまかな経路を引いた後に、
その経路を元に最適化ベースの手法で滑らかな経路を生成するといった手続きは妥当だと考えられます。
他にも学習ベースで入力からEnd2Endで経路を生成する手法も存在しますが、ここでは扱いません。

moveit!を例に取ると、omplに実装されたサンプリングベースの手法でglobal pathを生成し、内部に
実装されたCHOMP/STOMPでlocal pathを生成するという手続きでロボットの経路/動作を生成しています。
global pathは系の性質に合わせて特化したアルゴリズムも多く、問題に合わせて選べる様になっています。

ここまで長々と説明しましたが、この記事ではlocal pathには最適化ベースの手法を用いずに、
global pathを直接補間する事でlocal pathを補間する方法を紹介します。

この記事のコードはキャチロボ2023年度大会に向けて実装、投入されていたコードです。

2. ompl+αを用いた経路生成

以降では手先座標の始点座標と終点座標を与え、状態制約下でそれらを結ぶ経路を生成する問題を考えます。
複数のwaypointを通過する経路を引く問題も、経由点での連続性を無視すれば、上記の問題に帰着出来ます。

また、アームの座標系が2種類ある事に注意して下さい。(手先座標/パラメータ座標)

今回の実装では、FMT*でstart/goalの2点を繋ぐglobal pathを生成し、
パラメータ空間上で粗めにスプライン補間してから、手先座標上でもう一度補間する事で経路を生成します。

台形加減速も経路側で実装します。

経路生成のコードはこちらにまとまっています。
catch23_robot_controller/src/trajectory/r_theta_optimal_planning.cpp at main · Emile-Aquila/catch23_robot_controller · GitHub

突貫工事で作ったリファクタ無しのコードなのでかなり汚いです!!(ごめんなさい)

プログラム全体の流れとしては、
・本体のNode (Client)からServiceにwaypointsを投げる
・Serviceでglobal / local path含めて全て生成し、Nodeに送信　(経路生成)
・Nodeが受け取ったpathを目標位置として、F7 (micro-ROS)に一定周期で順次送信　(経路追従)
といった流れです。

キャチロボで使ったプログラムはmicro-ROS側もROS2側も両方公開しています。

3. global pathの生成

FMT*を用いてパラメータ空間上でglobal pathを生成します。

3.1 FMT*について

FMT*(Fast Marching Tree*)は、漸近最適なサンプリングベースの経路計画アルゴリズムで、
複雑な制約を伴う高次元空間での動作計画をターゲットとして考案された手法です。

2015年にこちらの論文で提案されました。
arxiv.org

FMT*では、予め生成したサンプル点から、DPを用いて再帰的にグラフの構築と探索を行います。
経路点同士の再接続計算時に、衝突チェックを遅延評価にする事でチェック回数を減らしているのが特徴です。

やたらめったら早くて、それなりに良い経路が得られます。

3.2. omplの導入

ompl公式のInstall手順に従えば良いです。
ompl.kavrakilab.org

omplはaptでinstallできます。

sudo apt install libompl-dev ompl-demos

あとはCMake等で適当に呼び出せばomplを使う事が出来ます。

CMakeであれば、

find_package(Boost REQUIRED)
find_package(Boost COMPONENTS program_options REQUIRED)
find_package(ompl REQUIRED)
include_directories(
        ${EIGEN_INCLUDE_DIR}
        ${OMPL_INCLUDE_DIRS}
        ${Boost_LIBRARIES}
)

でOMPLが使えるはずです。(EigenとBoostが必要)

3.3. FKとIK

FK: パラメータ座標 -> 手先座標
IK: 手先座標 -> パラメータ座標　と定義します。

今回は円筒座標系なので簡単に計算できます。
catch23_robot_controller/src/utils/robot_state.cpp at main · Emile-Aquila/catch23_robot_controller · GitHub

冗長自由度がある場合には、特異姿勢の処理や複数解の存在に気を付けて実装して下さい。

3.4. omplのプログラムを書く

自分で実装する場合には公式のDemoを見るのが早いです。
実装をまとめたリポジトリもあります。

ompl.kavrakilab.org

github.com

対応するStateSpaceを作成 (閉区間を設定)
　-> SpaceInfomationを作成 (状態制約はここに対応)
　-> アルゴリズムを指定し、ProblemDefinitionを作成してstart, goalを設定
　-> planning
の流れが基本になります。

簡単なコードなので詳細は省きますが、以下のコードでglobal pathを生成します。
こちらを参考にしてClassにまとめて実装しました。
ompl.kavrakilab.org

・planning全体

auto state_space(std::make_shared<ob::RealVectorStateSpace>(3));
auto space_info = _space_info_our_area;
if(is_common)space_info = _space_info_common;  // 共通エリアの処理

// start point
ob::ScopedState<> start(state_space);  // (theta, r, phi)
ArmState start_tmp = arm_ik(start_tip);
std::cout<<"start: x,y,theta -> " << start_tip.x <<", "<< start_tip.y <<", "<< start_tip.theta <<std::endl;
std::cout<<"start: r,theta,phi -> " << start_tmp.r <<", "<< start_tmp.theta <<", "<< start_tmp.phi <<std::endl;
start->as<ob::RealVectorStateSpace::StateType>()->values[0] = start_tmp.theta;
start->as<ob::RealVectorStateSpace::StateType>()->values[1] = start_tmp.r;
start->as<ob::RealVectorStateSpace::StateType>()->values[2] = start_tmp.phi;

// goal point
ob::ScopedState<> goal(state_space);  // (1, 1, 1)
ArmState goal_tmp = arm_ik(goal_tip);
std::cout<<"goal: x,y,theta -> " << goal_tip.x <<", "<< goal_tip.y <<", "<< goal_tip.theta <<std::endl;
std::cout<<"goal: r,theta,phi -> " << goal_tmp.r <<", "<< goal_tmp.theta <<", "<< goal_tmp.phi <<std::endl;
goal->as<ob::RealVectorStateSpace::StateType>()->values[0] = goal_tmp.theta;
goal->as<ob::RealVectorStateSpace::StateType>()->values[1] = goal_tmp.r;
goal->as<ob::RealVectorStateSpace::StateType>()->values[2] = goal_tmp.phi;

// problem definition
auto prob_def(std::make_shared<ob::ProblemDefinition>(space_info));  // problem instance
prob_def->setStartAndGoalStates(start, goal);
prob_def->setOptimizationObjective(getBalancedObjective1(space_info));  // 目的関数の設定

// planner
auto planner = allocatePlanner(space_info, PLANNER_FMTSTAR);
planner->setProblemDefinition(prob_def);  // problem instanceを代入
planner->setup();  // plannerのsetup
planner->checkValidity();

ob::PlannerStatus solved = planner->ob::Planner::solve(0.4);ob::PlannerStatus solved = planner->ob::Planner::solve(0.4);  // 0.4sで解く

StateSpace, SpaceInformationについては、Classのコンストラクタに実装しています。
分かりにくくて申し訳ないです。

・classのコンストラクタ

auto state_space(std::make_shared<ob::RealVectorStateSpace>(3));
auto state_space_common(std::make_shared<ob::RealVectorStateSpace>(3));
matrix<double> bounds_pre{
    std::vector<double>{-M_PI_2, M_PI*2.0 - deg_to_rad(10.0)}, // theta
    std::vector<double>{325.0, 975.0},  // r
    std::vector<double>{deg_to_rad(-97.0f), deg_to_rad(110.0f)},
};

ob::RealVectorBounds bounds(3), bounds_common(3);
for(int i=0; i<bounds_pre.size(); i++){
    bounds.setLow(i, bounds_pre[i][0]);
    bounds.setHigh(i, bounds_pre[i][1]);
    bounds_common.setLow(i, bounds_pre[i][0]);
    bounds_common.setHigh(i, bounds_pre[i][1]);
}
state_space->setBounds(bounds);  // bounds for param
state_space_common->setBounds(bounds);  // bounds for param

_space_info_our_area = std::make_shared<ob::SpaceInformation>(state_space);
_space_info_our_area->setStateValidityChecker(std::make_shared<ValidityCheckerRobotArea>(_space_info_our_area));
_space_info_our_area->setup();
_space_info_our_area->printSettings(std::cout);

_space_info_common = std::make_shared<ob::SpaceInformation>(state_space_common);
_space_info_common->setStateValidityChecker(std::make_shared<ValidityCheckerRobotAreaCommon>(_space_info_common));
_space_info_common->setup();
_space_info_common->printSettings(std::cout);