OMPL

〜 Open Motion Planning Library 〜

概要 Abstract
Open Motion Planning Library (OMPL) は,サンプリングベースの行動計画ライブラリである.C++で実装されており,Python などのインターフェイスも提供する.OMPL は非常に軽量で,コアライブラリ (OMPL) は衝突判定やGUIインターフェイスなどのタスク依存の実装は提供せず,完全に切り離された設計となっている.衝突判定を組み込んだGUIフロントエンドとして,OMPL.app を提供しているが,コアライブラリだけでも利用できる.
Open Motion Planning Library (OMPL) is a sampling-based motion planning library that is implemented in C++, and provides a python interface. OMPL is lightweight, that is, the core library (OMPL) does not provide task-specific implementations, such as a collision checker and a GUI interface; these are completely separated. OMPL.app is also provided where a collision checker and a GUI interface are integrated, but it is possible to work with the core library only.
目次 Table of contents

Introduction

モーションプラニングの基礎から学ぶには,Exercise/OMPL プログラミングを演習するとよい.

サンプリングベースのプラニング手法,OMPLの概説には,以下の "Open Motion Planning Library: A Primer" がわかりやすい.

In order to learn the motion planning theory, it is recommended to do Exercise/OMPL Programming.

The article "Open Motion Planning Library: A Primer" which is listed below gives a good overview of the sampling based motion planning methods and OMPL.

Reference

  1. OMPL website: http://ompl.kavrakilab.org/
  2. Open Motion Planning Library: A Primer, Kavraki Lab, Rice University, 2014.
  3. Sucan, Moll, Kavraki: The Open Motion Planning Library, IEEE Robotics & Automation Magazine, Vol.19, No.4, pp.72-82, 2012.

Installation

Download

Installation

基本的に,Installationに従うこと.以下の指示は完全ではない.

Basically, follow the Installation guide. The following procedure is not perfect.

  1. 依存ライブラリをインストール. Install the dependencies:
    • Boost, CMake, Python, PyQt4, PyOpenGL, and pkg-config.
      • Note: Boostのバージョンには注意が必要. Be careful about the Boost's version.
    • OpenDE (Open Dynamics Engine; ODE):
  2. アーカイブを解凍したディレクトリで,以下を実行: Execute the following commands in the directory into which you extracted the archive:
    mkdir build
    cd build
  3. CMake-CUI (ccmake) または CMake-GUI (cmake-gui) を実行: Launch CMake-CUI (ccmake) or CMake-GUI (cmake-gui):
    ccmake ..
  4. 設定を変更: Change the configurations:
    • CMAKE_INSTALL_PREFIX : (e.g.) /home/USER/local
    • OPENDE_INCLUDE_DIR : (e.g.) /home/USER/LIBRARY/ode-0.12/include
    • OPENDE_LIBRARY : (e.g.) /home/USER/LIBRARY/ode-0.12/ode/src/.libs/libode.a # Note: toggle "Advanced mode" to ON
    • _OPENDE_CONFIG : (path to ode-config; e.g.) /home/USER/LIBRARY/ode-0.12/ode-config # Note: toggle "Advanced mode" to ON
  5. Download and install Py++
    make installpyplusplus && cmake .
    make update_bindings
  6. Make:
    make
  7. インストール (make install) は必須ではない. You do not need to install (make install)

インストールの検証 Validation of the installation

  • 以下のデモ・テストプログラムを実行してみる. Execute the following demo/test programs:
    • build/bin/test_*, build/bin/demo_*
  • GUIフロントエンドを実行: Execute the GUI frontend:
    gui/ompl_app.py

OMPL.app

OMPL.app は,OMPL のGUIフロントエンドである.OMPL.app 上で,さまざまな問題に対して,OMPL が提供するアルゴリズムをテストできる.OMPL.app のインストール・利用は必須ではないが,サンプリングベースのプラニング・OMPLの初心者には,よい導入の手助けとなる.

The OMPL.app is a GUI front-end of OMPL. On OMPL.app, we can apply the algorithms provided by OMPL to various problems. Though installing and using OMPL.app are optional, using OMPL.app is a good introduction for beginners of sampling-based planning methods and/or OMPL.

Getting started

Execute OMPL.app located in the gui directory:

./gui/ompl_app.py

This will load the GUI:

OMPL.app GUI

Load the workspace

Use the File menu to load an environment (or Ctrl+E) and a robot (or Ctrl-R). Some examples are available in the resources directory.

For example, loading the environment resources/3D/cubicles_env.dae and the robot resources/3D/cubicles_robot.dae would look like this:

Loading environment and robot

(The L-shaped object is the robot).

Set Start and Goal poses

Set the x, y, z coordinates and the roll, pitch and yaw rotations.

Start and Goal

Select the planner

OMPL has implemented several planner. Some planners will be more suitable to solve a specific problem while others can give better results for other kind of problems.

Let's select an RRT planner and solve the problem by clicking on the Solve button. If you uncheck the Animate box, you will see a sequence of the states that form the solution. Additionally, it's possible to show the states and edges by selecting the corresponding option in the Show dropdown control.

Selecting the planner and solving the problem

Limit the environment

The Bounding Box is the space where the robot can move and it is represented by a white box. For example, if you increase the height of the bounding box beyond the walls, the robot can "fly" over in order to reach the goal.

Bounding box

Using configuration files

Once you set up all the necessary to solve your problem, you can save these settings with Save Problem Configuration. Afterwards, you can load it with Load Problem Configuration to continue working.

The file looks like this:

 [problem]
 robot = resources/3D/cubicles_robot.dae
 world = resources/3D/cubicles_env.dae
 start.x = -5.14
 start.y = -43.08
 start.z = 71.2
 start.theta = 0
 start.axis.x = 1
 start.axis.y = 0
 start.axis.z = 0
 goal.x = 171.86
 ...

Solution paths

Once you have solved a problem, you can save the solution using Save Path. Similarly, you can load a path with the Load Path option.

The output file will contain all the states along the path, one per row. For example:

 5.14 -43.08 71.2 0.0 0.0 0.0 1.0
 9.54211307321 -42.9335169134 77.1604052344 0.0316713074111 -0.0162103016154 0.0133720999859 0.999277409607
 13.9442261464 -42.7870338269 83.1208104688 0.0632968440572 -0.0323971764143 0.0267248748698 0.997110682701
 18.3463392196 -42.6405507403 89.0812157032 0.0948309053204 -0.0485372314363 0.0400390274781 0.993502950594
 ...


OMPLプログラミング入門 Introduction to OMPL programming

OMPLを使ったプログラミングについて解説する.ここでは OMPL のコアライブラリのみ必要で,OMPL.app は不要である.

This section explains programming with OMPL. Here, we need the core library of OMPL; OMPL.app is not necessary.

まずは,次のOMPLの資料を読み,サンプリングベースのプラニング手法の基礎,OMPLの概要を理解しよう.

First of all, read the following OMPL documents to understand the basic of sampling-based planning methods and the OMPL overview.

オーバービュー Overview

図: Figure: API Overview

OMPLの重要な要素:

Important elements of OMPL:

  • SpaceInformation: 状態空間 (StateSpace)・行動(制御入力)空間 (ControlSpace) を定義する.ControlSpace は,制御指令値を含むプラニング (Planning with controls) のみで必要で,幾何学的プラニング (Geometric planning) では必要でない. Defines the state space (StateSpace) and the action (control) space (ControlSpace). ControlSpace is used only in the planning with controls setup; it is not used in the geometric planning setup.
  • StateSampler: 状態をサンプリングする. Samples a state.
  • ControlSampler: 行動(制御入力)をサンプリングする. Samples an action (control).
  • StateValidityChecker: 状態が取りうるか否か(障害物がないか)判定する. Judge if a state is valid or invalid; i.e. if the state is on an obstacle or not.
  • StatePropagator: 状態,行動,行動の実行時間に対して,結果の状態を計算する. Computes a resulting state for given state, action (control), and duration of the action.
  • ProblemDefinition: 問題の定義.通常はスタート状態とゴール状態の指定. Defines a problem. Usually, it specifies a start state and a goal state.
  • Planner: プラニングアルゴリズムを実装する. Implements a planning algorithm.

SimpleSetup クラスを使うと,OMPL が自動的にデフォルトのセットアップを提供してくれる.ユーザは,問題固有の要素のみを設定すればよいが,必要に応じて各設定を変更できる.

The SimpleSetup class provides a default setup of OMPL. Users need to configure only problem-specific elements, but they can change each setup to satisfy their needs.

OMPL を用いたプラニングの流れは,以下の通り:

The flow of programming with OMPL is like the following way:

  1. StateValidityChecker, サンプラ,状態遷移モデルなどをクラスまたは関数として定義. Define the StateValidityChecker, sampler, state transition model, and so on, as a class or a function.
  2. SimpleSetup, StateSpace, Planner などのインスタンスを生成し,SimpleSetup に登録する.この際,1で定義したクラスや関数も登録する. Instantiate SimpleSetup, StateSpace, Planner, etc. and register them to SimpleSetup. Here, the classes/functions defined in the step 1 are also registered.
  3. プラニングアルゴリズムの実行.SimpleSetup の solve 関数を使う. Execute the planning algorithm. Here, we use a solve function of SimpleSetup.
  4. 結果の出力,可視化. Output and visualize the result.

Reference

幾何学的プラニング Geometric planning

2次元の経路計画問題を例に考える.状態は SE(2): 2次元の位置と姿勢とする.X, Y ともに -1 から 1 の範囲で,原点を中心とする 1x1 の四角形が障害物として置かれている.スタート (-0.9,-0.9,0) からゴール (0.9,0.9,0) に到達するパスを探索する.

Here we think an example of a path planning problem on a 2-dimensional plane. The state is SE(2), i.e. position and rotation on a 2-D plane. Both of X and Y take between -1 and 1, and a 1x1 square is put as an obstacle centered at the origin. We search a path from a start (-0.9,-0.9,0) to a goal (0.9,0.9,0).

PlanningSE2.png

Fig: Path planning problem on a 2-D plane. S: start, G: goal, red area: obstacle, white area: free space.

以下に沿って,幾何学的プラニングを実装・実行してみよう.

Implement and execute the geometric planning by the following way.

  1. ヘッダ: geometric/SimpleSetup.h と,SE(2)の状態空間 base/spaces/SE2StateSpace.h をインクルードする: Header: geometric/SimpleSetup.h, and for a SE(2) state space, base/spaces/SE2StateSpace.h:
    #include <ompl/geometric/SimpleSetup.h>
    #include <ompl/base/spaces/SE2StateSpace.h>
    加えて,数学ライブラリ,出力ライブラリも使う: In addition, include a math and I/O libraries:
    #include <cmath>
    #include <iostream>
    #include <fstream>
    よく使う名前空間のエイリアスを定義する: Define aliases of frequently used namespaces:
    namespace ob = ompl::base;
    namespace og = ompl::geometric;
  2. StateValidityChecker を定義するために,状態が取りうるか否かを判定する関数を定義する: In order to define StateValidityChecker, define a function that judges a state is valid or invalid:
    bool isStateValid(const ob::State *state)
    {
      const ob::SE2StateSpace::StateType *state_2d= state->as<ob::SE2StateSpace::StateType>();
      const double &x(state_2d->getX()), &y(state_2d->getY());
      // State is invalid when it is inside a 1x1 box
      // centered at the origin:
      if(std::fabs(x)<0.5 && std::fabs(y)<0.5)
        return false;
      // Otherwise, the state is valid:
      return true;
    }
    状態は抽象化されたインターフェイスクラス ob::State のポインタとして与えられるので,SE2StateSpace::StateType として使う場合は,上記のようにキャストする必要がある. Here, a state is given as a pointer to an abstracted interface class ob::State; thus, in order to use it as SE2StateSpace::StateType, we need cast it like the above way.
  3. SimpleSetup のインスタンスを生成し,セットアップし,プラニングを実行し,結果を出力する.以下は main 関数に書いてもよいが,ここでは別の関数を用意する: Instantiate the SimpleSetup class, setup it, execute planning, and output the results. Though we can write the following procedure in a main function, we define another function:
    void planWithSimpleSetup(void)
    {
    この関数の中で,以下を行う: In this function, implement the following things:
    1. StateSpace の定義.この問題では,SE2StateSpace を使う: Define StateSpace. For this problem, we use SE2StateSpace:
      ob::StateSpacePtr space(new ob::SE2StateSpace());
      状態の境界(取りうる値の範囲)を定義し,StateSpace に登録する: Define the boundary of StateSpace (possible range of state), then register it to the StateSpace:
      ob::RealVectorBounds bounds(2);
      bounds.setLow(-1);
      bounds.setHigh(1);
      space->as<ob::SE2StateSpace>()->setBounds(bounds);
    2. SimpleSetup をインスタンス化する: Instantiate SimpleSetup:
      og::SimpleSetup ss(space);
    3. StateValidityChecker をセットアップする.具体的には,上記で定義した isStateValid 関数を登録する: Setup the StateValidityChecker. Concretely, register the isStateValid function defined above to it:
      ss.setStateValidityChecker(boost::bind(&isStateValid, _1));
    4. スタート状態とゴール状態を設定する.まずは,スタート状態の設定: Configure the start state and the goal state. First, setup the start state:
      ob::ScopedState<ob::SE2StateSpace> start(space);
      start->setXY(-0.9,-0.9);
      std::cout << "start: "; start.print(std::cout);
      次に,ゴール状態の設定: Next, setup the goal state:
      ob::ScopedState<ob::SE2StateSpace> goal(space);
      goal->setXY(0.9,0.9);
      std::cout << "goal: "; goal.print(std::cout);
      最後に,これらの情報を登録する: Finally, register these information:
      ss.setStartAndGoalStates(start, goal);
    5. 以上でセットアップは終わりで,次にプラニングアルゴリズムを実行する: Now the setup has been completed. Next, execute a planning algorithm:
      ob::PlannerStatus solved = ss.solve(1.0);
      ここで,solve メンバ関数の引数 1.0 はアルゴリズムの実行を待つ時間である.つまり,1秒過ぎると,アルゴリズムは停止する.解が見つかったかどうかは, if(solved) で判定できる.解が見つかった場合には,例えば以下のような処理をする: where the parameter 1.0 of the solve member function denotes a maximum computation time of executing the algorithm. That is to say, the algorithm is terminated after passing 1.0 second. We can judge whether a solution is found by if(solved). Usual processes when a solution is found are as follows:
      if (solved)
      {
        // Simplify the solution
        ss.simplifySolution();
        std::cout << "----------------" << std::endl;
        std::cout << "Found solution:" << std::endl;
        // Print the solution path to screen
        ss.getSolutionPath().print(std::cout);
      
        // Print the solution path to a file
        std::ofstream ofs("path.dat");
        ss.getSolutionPath().printAsMatrix(ofs);
      }
      else
        std::cout << "No solution found" << std::endl;
      simplifySolution は,解のパスを簡単化する処理である.getSolutionPath メンバ関数で,解のパスを取得できる.printAsMatrix は,パスを行列として出力する関数である.上記の場合,ファイル path.dat に保存する.解が見つからなかった場合は,その旨,表示している. The function simplifySolution is a process to simplify a solution path. We can obtain the solution path by the getSolutionPath member function. The function printAsMatrix is to output the path as a matrix. In above case, the path is saved into a file path.dat. When a path is not found, display that message.
  4. 以上で,planWithSimpleSetup 関数の定義は終わり. We have completely defined the planWithSimpleSetup function.
    }
  5. main 関数で planWithSimpleSetup を呼び出せば,プログラムは完成する. We can complete the program by calling planWithSimpleSetup in the main function.
    int main()
    {
      planWithSimpleSetup();
      return 0;
    }

プログラム全文 Whole program (fileGeomPlanningSE2.cpp):

 1 /*
 2 Compile: g++ -g -Wall -O2 -march=i686 GeomPlanningSE2.cpp -I ~/prg/ompl/app/ompl/src -I ~/prg/ompl/app/src -L ~/prg/ompl/app/build/lib -lompl -Wl,-rpath ~/prg/ompl/app/build/lib -lboost_thread
 3 Execution: ./a.out
 4 Visualize: plot "path.dat" (sequence of x,y,yaw)
 5 */
 6
 7 #include <ompl/geometric/SimpleSetup.h>
 8 #include <ompl/base/spaces/SE2StateSpace.h>
 9 #include <cmath>
10 #include <iostream>
11 #include <fstream>
12
13 namespace ob = ompl::base;
14 namespace og = ompl::geometric;
15
16 // Return true if the state is valid, false if the state is invalid
17 bool isStateValid(const ob::State *state)
18 {
19   const ob::SE2StateSpace::StateType *state_2d= state->as<ob::SE2StateSpace::StateType>();
20   const double &x(state_2d->getX()), &y(state_2d->getY());
21   // State is invalid when it is inside a 1x1 box
22   // centered at the origin:
23   if(std::fabs(x)<0.5 && std::fabs(y)<0.5)
24     return false;
25   // Otherwise, the state is valid:
26   return true;
27 }
28
29 void planWithSimpleSetup(void)
30 {
31   // Construct the state space where we are planning
32   ob::StateSpacePtr space(new ob::SE2StateSpace());
33
34   ob::RealVectorBounds bounds(2);
35   bounds.setLow(-1);
36   bounds.setHigh(1);
37   space->as<ob::SE2StateSpace>()->setBounds(bounds);
38
39   // Instantiate SimpleSetup
40   og::SimpleSetup ss(space);
41
42   // Setup the StateValidityChecker
43   ss.setStateValidityChecker(boost::bind(&isStateValid, _1));
44
45   // Setup Start and Goal
46   ob::ScopedState<ob::SE2StateSpace> start(space);
47   start->setXY(-0.9,-0.9);
48   std::cout << "start: "; start.print(std::cout);
49
50   ob::ScopedState<ob::SE2StateSpace> goal(space);
51   goal->setXY(0.9,0.9);
52   std::cout << "goal: "; goal.print(std::cout);
53
54   ss.setStartAndGoalStates(start, goal);
55
56   std::cout << "----------------" << std::endl;
57
58   // Execute the planning algorithm
59   ob::PlannerStatus solved = ss.solve(1.0);
60
61   // If we have a solution,
62   if (solved)
63   {
64     // Simplify the solution
65     ss.simplifySolution();
66     std::cout << "----------------" << std::endl;
67     std::cout << "Found solution:" << std::endl;
68     // Print the solution path to screen
69     ss.getSolutionPath().print(std::cout);
70
71     // Print the solution path to a file
72     std::ofstream ofs("path.dat");
73     ss.getSolutionPath().printAsMatrix(ofs);
74   }
75   else
76     std::cout << "No solution found" << std::endl;
77 }
78
79 int main()
80 {
81   planWithSimpleSetup();
82   return 0;
83 }

コンパイル方法: How to compile:

g++ -g -Wall -O2 -march=i686 GeomPlanningSE2.cpp -I ~/prg/ompl/app/ompl/src -I ~/prg/ompl/app/src -L ~/prg/ompl/app/build/lib -lompl -Wl,-rpath ~/prg/ompl/app/build/lib -lboost_thread

ここでは,~/prg/ompl/app がOMPLのソースを展開したディレクトリ,~/prg/ompl/app/build がビルドに使ったディレクトリを表す. Here, ~/prg/ompl/app is a directory into which the OMPL source code was extracted, ~/prg/ompl/app/build is a directory where the build was dine.

実行: How to execute:

./a.out

実行の出力を見てみよう.また,path.dat というファイルが生成されるので,Gnuplot などでプロットして確認しよう.

Let's see the output of the execution. The file path.dat is generated, plot it by Gnuplot or other plotter.

GeomPlanningSE2.png

Fig: Planning result on the 2-D plane.


制御指令値を含むプラニング Planning with controls

制御指令値を含むプラニングの場合,大きく違うのは,行動(制御指令)空間の定義と,状態遷移モデルの定義である.SimpleSetup クラスも,制御指令値を含むプラニング用のものを用いる.逆に,これら以外の要素については,幾何学的なプラニングの場合とほぼ同じである.

The case of planning wit controls, the major difference is defining an action (control) spate and a state transition (propagation) model.

ここでも,2次元の経路計画問題を解く.状態は SE(2): 2次元の位置と姿勢とする.X, Y ともに -1 から 1 の範囲で,原点を中心とする 1x1 の四角形が障害物として置かれている.スタート (-0.9,-0.9,0) からゴール (0.9,0.9,0) に到達するパスを探索する.制御指令空間は,前進速度と,旋回速度の2次元連続ベクトル空間である.

Here we also think an example of a path planning problem on a 2-dimensional plane. Both of X and Y take between -1 and 1, and a 1x1 square is put as an obstacle centered at the origin. We search a path from a start (-0.9,-0.9,0) to a goal (0.9,0.9,0). The control space is a 2-dimensional vector space consisting of a forward velocity and a rotation velocity.

PlanningSE2.png

Fig: Path planning problem on a 2-D plane with controls. S: start, G: goal, red area: obstacle, white area: free space.

以下に沿って,制御指令値を含むプラニングを実装・実行してみよう. 幾何学的なプラニングと異なる部分を赤色で強調してある.

Implement and execute the planning with controls by the following way. The different processes from the geometric planning are emphasized by the red color.

  1. ヘッダ: control/SimpleSetup.h と,SE(2)の状態空間 base/spaces/SE2StateSpace.h, さらに,制御指令空間用の control/spaces/RealVectorControlSpace.h をインクルードする: Header: control/SimpleSetup.h, and base/spaces/SE2StateSpace.h for a SE(2) state space, additionally, control/spaces/RealVectorControlSpace.h for the control space:
    #include <ompl/control/SimpleSetup.h>
    #include <ompl/base/spaces/SE2StateSpace.h>
    #include <ompl/control/spaces/RealVectorControlSpace.h>
    加えて,数学ライブラリ,出力ライブラリも使う: In addition, include a math and I/O libraries:
    #include <cmath>
    #include <iostream>
    #include <fstream>
    よく使う名前空間のエイリアスを定義する: Define aliases of frequently used namespaces:
    namespace ob = ompl::base;
    namespace oc = ompl::control;
  2. StateValidityChecker を定義するために,状態が取りうるか否かを判定する関数を定義する: In order to define StateValidityChecker, define a function that judges a state is valid or invalid:
    bool isStateValid(const ob::State *state)
    {
      const ob::SE2StateSpace::StateType *state_2d= state->as<ob::SE2StateSpace::StateType>();
      const double &x(state_2d->getX()), &y(state_2d->getY());
      // State is invalid when it is inside a 1x1 box
      // centered at the origin:
      if(std::fabs(x)<0.5 && std::fabs(y)<0.5)
        return false;
      // Otherwise, the state is valid:
      return true;
    }
    状態は抽象化されたインターフェイスクラス ob::State のポインタとして与えられるので,SE2StateSpace::StateType として使う場合は,上記のようにキャストする必要がある. Here, a state is given as a pointer to an abstracted interface class ob::State; thus, in order to use it as SE2StateSpace::StateType, we need cast it like the above way.
  3. 状態遷移モデル StatePropagator を定義するために,以下の propagate 関数を定義する.まず,姿勢が -M_PI から M_PI を超えないように制約する補助関数を定義する: In order to define the state transition model StatePropagator, we defined the following propagate function. First, we define a supplementary function to constrain the orientation into [-M_PI,M_PI]:
    template<typename T>
    inline T FMod(const T &x, const T &y)
    {
      if(y==0)  return x;
      return x-y*std::floor(x/y);
    }
    // Convert radian to [-pi,pi)
    double RadToNPiPPi(const double &x)
    {
      return FMod(x+M_PI,M_PI*2.0)-M_PI;
    }
    次に,propagate 関数を定義する.(現在の)状態 start,制御指令 control,制御指令の実行時間 duration に対して,結果の状態 result を計算する関数である: Then we define the propagate function. It computes the resulting state "result" from the current state "start", the control "control", and the duration of the control "duration":
    void propagate(const ob::State *start, const oc::Control *control, const double duration, ob::State *result)
    {
      const ob::SE2StateSpace::StateType *start_2d= start->as<ob::SE2StateSpace::StateType>();
      const double &x(start_2d->getX()), &y(start_2d->getY()), &rot(start_2d->getYaw());
      const oc::RealVectorControlSpace::ControlType *control_2= control->as<oc::RealVectorControlSpace::ControlType>();
      const double &c0((*control_2)[0]), &c1((*control_2)[1]);
      ob::SE2StateSpace::StateType *result_2d= result->as<ob::SE2StateSpace::StateType>();
    
      double nx= x+c0*duration*std::cos(rot);
      double ny= y+c0*duration*std::sin(rot);
      if(nx<-1.0) nx= -1.0;  else if(nx>1.0) nx= 1.0;
      if(ny<-1.0) ny= -1.0;  else if(ny>1.0) ny= 1.0;
      result_2d->setXY(nx, ny);
      result_2d->setYaw(RadToNPiPPi(rot+c1*duration));
    }
  4. SimpleSetup のインスタンスを生成し,セットアップし,プラニングを実行し,結果を出力する.以下は main 関数に書いてもよいが,ここでは別の関数を用意する: Instantiate the SimpleSetup class, setup it, execute planning, and output the results. Though we can write the following procedure in a main function, we define another function:
    void planWithSimpleSetup(void)
    {
    この関数の中で,以下を行う: In this function, implement the following things:
    1. StateSpace の定義.この問題では,SE2StateSpace を使う: Define StateSpace. For this problem, we use SE2StateSpace:
      // Construct the state space where we are planning
      ob::StateSpacePtr space(new ob::SE2StateSpace());
      状態の境界(取りうる値の範囲)を定義し,StateSpace に登録する: Define the boundary of StateSpace (possible range of state), then register it to the StateSpace:
      ob::RealVectorBounds bounds(2);
      bounds.setLow(-1);
      bounds.setHigh(1);
      space->as<ob::SE2StateSpace>()->setBounds(bounds);
    2. ControlSpace の定義.この問題では,RealVectorControlSpace を使う: Define ControlSpace. For this problem, we use RealVectorControlSpace:
      // Construct the control space
      oc::ControlSpacePtr cspace(new oc::RealVectorControlSpace(space, 2));
      境界(取りうる値の範囲)を定義し,ControlSpace に登録する: Define the boundary of ControlSpace (possible range of controls), then register it to the ControlSpace:
      ob::RealVectorBounds cbounds(2);
      cbounds.setLow(0,0.0);
      cbounds.setHigh(0,0.5);
      cbounds.setLow(1,-2.0);
      cbounds.setHigh(1,2.0);
      cspace->as<oc::RealVectorControlSpace>()->setBounds(cbounds);
    3. SimpleSetup をインスタンス化する.制御指令値を含むプラニング用であることに注意. Instantiate SimpleSetup. Be careful for that SimpleSetup is for planning with controls.
      oc::SimpleSetup ss(cspace);
    4. StateValidityChecker をセットアップする.具体的には,上記で定義した isStateValid 関数を登録する: Setup the StateValidityChecker. Concretely, register the isStateValid function defined above to it:
      ss.setStateValidityChecker(boost::bind(&isStateValid, _1));
    5. StatePropagator をセットアップする.具体的には,上記で定義した propagate 関数を登録する: Setup the StatePropagator. Concretely, register the propagate function defined above to it:
      ss.setStatePropagator(boost::bind(&propagate, _1, _2, _3, _4));
    6. スタート状態とゴール状態を設定する.まずは,スタート状態の設定: Configure the start state and the goal state. First, setup the start state:
      ob::ScopedState<ob::SE2StateSpace> start(space);
      start->setXY(-0.9,-0.9);
      start->setYaw(0.25*M_PI);
      std::cout << "start: "; start.print(std::cout);
      次に,ゴール状態の設定: Next, setup the goal state:
      ob::ScopedState<ob::SE2StateSpace> goal(space);
      goal->setXY(0.9,0.9);
      goal->setYaw(0.25*M_PI);
      std::cout << "goal: "; goal.print(std::cout);
      最後に,これらの情報を登録する: Finally, register these information:
      ss.setStartAndGoalStates(start, goal, 0.1);
      ここで,最後のパラメータは解の精度を表す. where the last parameter denotes the accuracy.
    7. 制御指令値のステップサイズ(1ステップあたりの実行時間),最小・最大のステップ数(1回に実行されるステップ数)を設定する: Configure the step-size of each control (duration per each step), and minimum/maximum number of steps (number of steps per control):
      ss.getSpaceInformation()->setMinMaxControlDuration(1,50);
      ss.getSpaceInformation()->setPropagationStepSize(0.1);
    8. 以上でセットアップは終わりで,次にプラニングアルゴリズムを実行する: Now the setup has been completed. Next, execute a planning algorithm:
      ob::PlannerStatus solved = ss.solve(10.0);
      ここで,solve メンバ関数の引数 10.0 はアルゴリズムの実行を待つ時間である.つまり,10秒過ぎると,アルゴリズムは停止する(制御指令値を含むプラニングは時間がかかるため,時間を増やしている).解が見つかったかどうかは, if(solved) で判定できる.解が見つかった場合には,例えば以下のような処理をする: where the parameter 10.0 of the solve member function denotes a maximum computation time of executing the algorithm. That is to say, the algorithm is terminated after passing 10.0 second (the value is increased since the planning with controls takes longer time). We can judge whether a solution is found by if(solved). Usual processes when a solution is found are as follows:
      if (solved)
      {
        std::cout << "----------------" << std::endl;
        std::cout << "Found solution:" << std::endl;
        // Print the solution path to screen
        ss.getSolutionPath().print(std::cout);
      
        // Print the solution path to a file
        std::ofstream ofs("path.dat");
        ss.getSolutionPath().printAsMatrix(ofs);
      }
      else
        std::cout << "No solution found" << std::endl;
      simplifySolution は,制御指令値を含むプラニングでは使えない. simplifySolution is not available in planning with controls. getSolutionPath メンバ関数で,解のパスを取得できる.printAsMatrix は,パスを行列として出力する関数である.上記の場合,ファイル path.dat に保存する.解が見つからなかった場合は,その旨,表示している. We can obtain the solution path by the getSolutionPath member function. The function printAsMatrix is to output the path as a matrix. In above case, the path is saved into a file path.dat. When a path is not found, display that message.
  5. 以上で,planWithSimpleSetup 関数の定義は終わり. We have completely defined the planWithSimpleSetup function.
    }
  6. main 関数で planWithSimpleSetup を呼び出せば,プログラムは完成する. We can complete the program by calling planWithSimpleSetup in the main function.
    int main()
    {
      planWithSimpleSetup();
      return 0;
    }

プログラム全文 Whole program (fileCtrlPlanningSE2.cpp):

  1 /*
  2 Compile: g++ -g -Wall -O2 -march=i686 CtrlPlanningSE2.cpp -I ~/prg/ompl/app/ompl/src -I ~/prg/ompl/app/src -L ~/prg/ompl/app/build/lib -lompl -Wl,-rpath ~/prg/ompl/app/build/lib -lboost_thread
  3 Execution: ./a.out
  4 Visualize: plot "path.dat" (sequence of x,y,yaw)
  5 */
  6
  7 ///>>>CHANGE
  8 #include <ompl/control/SimpleSetup.h>
  9 ///<<<CHANGE
 10 #include <ompl/base/spaces/SE2StateSpace.h>
 11 ///>>>+++
 12 #include <ompl/control/spaces/RealVectorControlSpace.h>
 13 ///<<<+++
 14 #include <cmath>
 15 #include <iostream>
 16 #include <fstream>
 17
 18 namespace ob = ompl::base;
 19 ///>>>CHANGE
 20 namespace oc = ompl::control;
 21 ///<<<CHANGE
 22
 23 // Return true if the state is valid, false if the state is invalid
 24 bool isStateValid(const ob::State *state)
 25 {
 26   const ob::SE2StateSpace::StateType *state_2d= state->as<ob::SE2StateSpace::StateType>();
 27   const double &x(state_2d->getX()), &y(state_2d->getY());
 28   // State is invalid when it is inside a 1x1 box
 29   // centered at the origin:
 30   if(std::fabs(x)<0.5 && std::fabs(y)<0.5)
 31     return false;
 32   // Otherwise, the state is valid:
 33   return true;
 34 }
 35
 36 ///>>>+++
 37 template<typename T>
 38 inline T FMod(const T &x, const T &y)
 39 {
 40   if(y==0)  return x;
 41   return x-y*std::floor(x/y);
 42 }
 43 // Convert radian to [-pi,pi)
 44 double RadToNPiPPi(const double &x)
 45 {
 46   return FMod(x+M_PI,M_PI*2.0)-M_PI;
 47 }
 48
 49 void propagate(const ob::State *start, const oc::Control *control, const double duration, ob::State *result)
 50 {
 51   const ob::SE2StateSpace::StateType *start_2d= start->as<ob::SE2StateSpace::StateType>();
 52   const double &x(start_2d->getX()), &y(start_2d->getY()), &rot(start_2d->getYaw());
 53   const oc::RealVectorControlSpace::ControlType *control_2= control->as<oc::RealVectorControlSpace::ControlType>();
 54   const double &c0((*control_2)[0]), &c1((*control_2)[1]);
 55   ob::SE2StateSpace::StateType *result_2d= result->as<ob::SE2StateSpace::StateType>();
 56
 57   double nx= x+c0*duration*std::cos(rot);
 58   double ny= y+c0*duration*std::sin(rot);
 59   if(nx<-1.0) nx= -1.0;  else if(nx>1.0) nx= 1.0;
 60   if(ny<-1.0) ny= -1.0;  else if(ny>1.0) ny= 1.0;
 61   result_2d->setXY(nx, ny);
 62   result_2d->setYaw(RadToNPiPPi(rot+c1*duration));
 63 }
 64 ///<<<+++
 65
 66 void planWithSimpleSetup(void)
 67 {
 68   // Construct the state space where we are planning
 69   ob::StateSpacePtr space(new ob::SE2StateSpace());
 70
 71   ob::RealVectorBounds bounds(2);
 72   bounds.setLow(-1);
 73   bounds.setHigh(1);
 74   space->as<ob::SE2StateSpace>()->setBounds(bounds);
 75
 76 ///>>>+++
 77   // Construct the control space
 78   oc::ControlSpacePtr cspace(new oc::RealVectorControlSpace(space, 2));
 79
 80   ob::RealVectorBounds cbounds(2);
 81   cbounds.setLow(0,0.0);
 82   cbounds.setHigh(0,0.5);
 83   cbounds.setLow(1,-2.0);
 84   cbounds.setHigh(1,2.0);
 85   cspace->as<oc::RealVectorControlSpace>()->setBounds(cbounds);
 86 ///<<<+++
 87
 88 ///>>>CHANGE
 89   // Instantiate SimpleSetup
 90   oc::SimpleSetup ss(cspace);
 91 ///<<<CHANGE
 92
 93   // Setup the StateValidityChecker
 94   ss.setStateValidityChecker(boost::bind(&isStateValid, _1));
 95
 96 ///>>>+++
 97   // Setup the StatePropagator
 98   ss.setStatePropagator(boost::bind(&propagate, _1, _2, _3, _4));
 99 ///<<<+++
100
101   // Setup Start and Goal
102   ob::ScopedState<ob::SE2StateSpace> start(space);
103   start->setXY(-0.9,-0.9);
104   start->setYaw(0.25*M_PI);
105   std::cout << "start: "; start.print(std::cout);
106
107   ob::ScopedState<ob::SE2StateSpace> goal(space);
108   goal->setXY(0.9,0.9);
109   goal->setYaw(0.25*M_PI);
110   std::cout << "goal: "; goal.print(std::cout);
111
112 ///>>>CHANGE
113   ss.setStartAndGoalStates(start, goal, 0.1);
114 ///<<<CHANGE
115
116 ///>>>+++
117   ss.getSpaceInformation()->setMinMaxControlDuration(1,50);
118   ss.getSpaceInformation()->setPropagationStepSize(0.1);
119 ///<<<+++
120
121   std::cout << "----------------" << std::endl;
122
123   // Execute the planning algorithm
124   ob::PlannerStatus solved = ss.solve(10.0);
125
126   // If we have a solution,
127   if (solved)
128   {
129 ///>>>---
130     // Simplify the solution
131     // ss.simplifySolution();
132 ///<<<---
133     std::cout << "----------------" << std::endl;
134     std::cout << "Found solution:" << std::endl;
135     // Print the solution path to screen
136     ss.getSolutionPath().print(std::cout);
137
138     // Print the solution path to a file
139     std::ofstream ofs("path.dat");
140     ss.getSolutionPath().printAsMatrix(ofs);
141   }
142   else
143     std::cout << "No solution found" << std::endl;
144 }
145
146 int main()
147 {
148   planWithSimpleSetup();
149   return 0;
150 }

なお,幾何学的プラニングの場合との変更箇所が,//>>> ~ //<<< で示されている(+++ は追加,--- は削除,CHANGE は変更).

Here, the different codes from the geometric planning are specified by //>>> ... //<<< (+++ denotes added code, --- denotes removed code, CHANGE denotes changed code).

コンパイル方法: How to compile:

g++ -g -Wall -O2 -march=i686 CtrlPlanningSE2.cpp -I ~/prg/ompl/app/ompl/src -I ~/prg/ompl/app/src -L ~/prg/ompl/app/build/lib -lompl -Wl,-rpath ~/prg/ompl/app/build/lib -lboost_thread

ここでは,~/prg/ompl/app がOMPLのソースを展開したディレクトリ,~/prg/ompl/app/build がビルドに使ったディレクトリを表す. Here, ~/prg/ompl/app is a directory into which the OMPL source code was extracted, ~/prg/ompl/app/build is a directory where the build was dine.

実行: How to execute:

./a.out

実行の出力を見てみよう.また,path.dat というファイルが生成されるので,Gnuplot などでプロットして確認しよう.

Let's see the output of the execution. The file path.dat is generated, plot it by Gnuplot or other plotter.

CtrlPlanningSE2.png

Fig: Planning with controls result on the 2-D plane.


より高度なプラニングの設定 Advanced setup for planning

問題に合わせて OMPL の設定をカスタマイズする方法について述べる.

This section describes how to customize OMPL for each problem.

OMPL はオープンソースソフトウェアなので,基本的に,オリジナルのソースを読めばすべて理解できる.以下で不足している情報は,各自でコードを読んで補おう.

OMPL is an open source software; so, basically you can understand everything by reading the original source code. Make up for the information that is not written below by reading the code.

Reference

クラスリファレンス:

Class reference:

利用できる状態空間 Available state space

Reference:

OMPLでは,以下の状態空間が利用できる:

In OMPL, we can use the following state spaces:

  • SO(2): 2次元平面における回転. Rotation in a 2D plane. (Yaw). ompl::base::SO2StateSpace
  • SO(3): 3次元空間における回転.内部表現はクォータニオン. Rotation in a 3D space. The internal representation is a quaternion. (X,Y,Z,W). ompl::base::SO3StateSpace
  • SE(2): 2次元平面における位置と回転. Position and rotation in a 2D plane. (X, Y, Yaw). ompl::base::SE2StateSpace
  • SE(3): 3次元空間における位置と回転. Psition and rotation in a 3D space. (X, Y, Z) + SO(3). ompl::base::SE3StateSpace
  • RealVector: N次元の実数ベクトル. N-dimensional real value vector. (X1,X2,...,XN). ompl::base::RealVectorStateSpace
  • ...
  • Compound: 複合状態空間.上記の空間を自由に組み合わせられる. Compound state space. We can combine the above spaces. ompl::base::CompoundStateSpace

プランナの指定 Specifying a planner

Reference:

プランナを指定しないと,SimpleSetup が指定するデフォルトのプランナが使われる.自分で指定する場合は,以下のようにする.

If we do not specify a planner, a default planner defined by SimpleSetup is used. If you specify by yourself, write as follows:

  • 幾何学的プラニングの場合: Geometric planning case:
    1. 用いるプランナのヘッダを追加する.以下は PRM の例: Add a header for the planner. The following is an example of PRM:
      #include <ompl/geometric/planners/prm/PRM.h>
    2. SimpleSetup の solve メンバ関数を使う前あたりで,プランナクラスのインスタンス化と登録を行う: Instantiate the planner class and register it right before executing SimpleSetup::solve:
      ob::PlannerPtr planner(new og::PRM(ss.getSpaceInformation()));
      ss.setPlanner(planner);
  • 制御指令値を含むプラニングの場合: Planning with controls case:
    1. 用いるプランナのヘッダを追加する.以下は PRM の例: Add a header for the planner. The following is an example of PRM:
      #include <ompl/control/planners/rrt/RRT.h>
    2. SimpleSetup の solve メンバ関数を使う前あたりで,プランナクラスのインスタンス化と登録を行う: Instantiate the planner class and register it right before executing SimpleSetup::solve:
      ob::PlannerPtr planner(new oc::RRT(ss.getSpaceInformation()));
      ss.setPlanner(planner);

プログラム全文: Whole program: fileGeomPlanningSE2_PRM.cpp

プログラム全文: Whole program: fileCtrlPlanningSE2_PRM.cpp

プラニングデータの可視化 Visualizing the planning data

プラニング結果の可視化や解析のために,サンプル点およびエッジ情報を取得し,可視化する方法について述べる.

Here we describe how to get the sampled data and the edges and how to visualize them. You want to do this for the visualization, the analysis of the planning result, and so on.

参考:

Reference:

  1. 必要なヘッダ: Required header:
    #include <ompl/base/PlannerData.h>
  2. 頂点(サンプル点)を出力するための補助関数を定義: Define a supplementary function to output a vertex (a sampled point):
    void printEdge(std::ostream &os, const ob::StateSpacePtr &space, const ob::PlannerDataVertex &vertex)
    {
      std::vector<double> reals;
      if(vertex!=ob::PlannerData::NO_VERTEX)
      {
        space->copyToReals(reals, vertex.getState());
        for(size_t j(0); j<reals.size(); ++j)  os<<" "<<reals[j];
      }
    }
  3. プランナのデータを取得: Get the planner's data:
    // Get the planner data to visualize the vertices and the edges
    ob::PlannerData pdat(ss.getSpaceInformation());
    ss.getPlannerData(pdat);
  4. 頂点(サンプル点)をファイルに書き出し: Output the vertices (sampled points) to a file:
    std::ofstream ofs_v("vertices.dat");
    for(unsigned int i(0); i<pdat.numVertices(); ++i)
    {
      printEdge(ofs_v, ss.getStateSpace(), pdat.getVertex(i));
      ofs_v<<std::endl;
    }
  5. エッジをファイルに書き出し: Output the edges to a file:
    std::ofstream ofs_e("edges.dat");
    std::vector<unsigned int> edge_list;
    for(unsigned int i(0); i<pdat.numVertices(); ++i)
    {
      unsigned int n_edge= pdat.getEdges(i,edge_list);
      for(unsigned int i2(0); i2<n_edge; ++i2)
      {
        printEdge(ofs_e, ss.getStateSpace(), pdat.getVertex(i));
        ofs_e<<std::endl;
        printEdge(ofs_e, ss.getStateSpace(), pdat.getVertex(edge_list[i2]));
        ofs_e<<std::endl;
        ofs_e<<std::endl<<std::endl;
      }
    }

プログラム全文: Whole program: fileGeomPlanningSE2_Info.cpp

以下のようにGnuplotなどでプロットすれば可視化できる:

You can visualize the data with Gnuplot (or other plotter) as follows:

set size square
plot  "edges.dat" w l lt 3 , "vertices.dat" w p lt 2 pt 7 ps 3 , "path0.dat" w l lt 4 lw 2 , "path.dat" w lp lt 1 lw 1

GeomPlanningSE2_Info.png

Fig: Planning result on the 2-D plane.


StateSampler の定義 Defining a StateSampler

サンプリングベースのプラニング手法では,サンプルの生成方法 (StateSampler) はプラニングの性能を左右する重要な要素である.問題によって適切なサンプラは変わってくる.

In sampling-based planning algorithms, the selection of sampling method (StateSampler) is an important element since it affects the planning performance. A proper sampler changes with a problem.

参考: Reference: Available State Samplers

OMPLが提供しているほかの(デフォルト以外の)サンプラを使う方法と,自分で新しいサンプラを定義する方法がある.以下では,自分で定義する方法と,その登録方法を述べる.

There are two ways: using a sampler (other than a default one) provided by OMPL, and defining a new sampler by ourselves. In the following, we will see how to define a new sampler, and its registration.

  1. まず,OMPL の StateSampler クラスを派生させて,自分のサンプラクラスを定義する.このサンプラクラスでは,サンプルを実行するメンバ関数をオーバーライドして定義する.以下は一例: First, we define our own sampler class by inheriting OMPL's StateSampler. In this sampler class, we define functions for sampling by overriding the member functions. For example:
    class TMyStateSampler : public ob::StateSampler
    {
    public:
      TMyStateSampler(const ob::StateSpace *space) : StateSampler(space)
      {
      }
    
      // Used in PRM, etc.
      virtual void sampleUniform (ob::State *state)
      {
        ob::SE3StateSpace::StateType *state_3d= state->as<ob::SE3StateSpace::StateType>();
        state_3d->setX(rng_.uniformReal(0.0,2.0));
        state_3d->setY(rng_.uniformReal(0.0,2.0));
        state_3d->setZ(rng_.uniformReal(0.0,2.0));
        state_3d->rotation().setIdentity();
      }
      // Used in LBKPIECE1, etc.
      virtual void sampleUniformNear (ob::State *state, const ob::State *near, const double distance)
      {
        ob::SE3StateSpace::StateType *state_3d= state->as<ob::SE3StateSpace::StateType>();
        const ob::SE3StateSpace::StateType *near_3d= near->as<ob::SE3StateSpace::StateType>();
        state_3d->setX(near_3d->getX()+distance*rng_.uniformReal(-1.0,1.0));
        state_3d->setY(near_3d->getY()+distance*rng_.uniformReal(-1.0,1.0));
        state_3d->setZ(near_3d->getZ()+distance*rng_.uniformReal(-1.0,1.0));
        state_3d->rotation().setIdentity();
      }
      virtual void sampleGaussian (ob::State *state, const ob::State *mean, const double stdDev)
      {
        throw ompl::Exception("TMyStateSampler::sampleGaussian", "not implemented");
      }
    protected:
      ompl::RNG rng_;
    };
    rng_ は乱数生成用のオブジェクト. rng_ is an object to generate random numbers. プランナごとに利用するサンプラの種類が異なるため,PRM など用に sampleUniform, LBKPIECE1 など用に sampleUniformNear を定義している.上の例では sampleGaussian は実装していない(つまり,PRM を使わないなら,sampleUniform を同様に実装しておけば良い).sampleUniform は,状態空間を一様にサンプルし,引数で指定された state に代入する.sampleUniformNear は near から距離 distance 以内にある点をサンプルし,引数で指定された state に代入する.前者はロードマップ法,後者はツリータイプの探索で使われる. Since a required sampling method is different in each sampler, we need to define sampleUniform for PRM, etc., and sampleUniformNear for LBKPIECE1, etc. In the above example, sampleGaussian is not implemented; namely, if you do not use PRM, you can implement sampleUniform in the same manner. The sampleUniform function generates a sample uniformly and assign it into "state" given as an argument. The sampleUniformNear function generates a sample that is inside "distance" from the state "near", and assign it into "state" given as an argument. The former one is used in road map methods, and the latter one is used in tree search methods.
  2. サンプラクラスのインスタンスを生成するための補助関数を定義する: Define a supplementary function to instantiate the sampler class:
    ob::StateSamplerPtr AllocTMyStateSampler(const ob::StateSpace *space)
    {
      return ob::StateSamplerPtr(new TMyStateSampler(space));
    }
  3. StateSpace オブジェクトに登録する(境界 bounds を定義した後あたりで): Register it to a StateSpace object (around right after defining the bounds):
    space->setStateSamplerAllocator(AllocTMyStateSampler);

ValidStateSampler の定義 Defining a ValidStateSampler

StateSampler が取りうるか否か不明な状態をサンプルするのに対し,ValidStateSampler とは,取りうる状態のみをサンプルするクラスである.もし,どの状態をサンプルすればよいかわかっているなら,ValidStateSampler を定義すれば,サンプルにおける無駄を軽減できる.

StateSampler samples a state whose validity is unknown. On the other hand, ValidStateSampler samples a state that is valid. If we know a valid state to be sampled, we can reduce the cost of sampling by defining a ValidStateSampler.

参考: Reference: Available State Samplers

方法は,StateSampler とほとんど同じ.ValidStateSampler から派生させる.

The way is almost the same as one for StateSampler. We inherit a new one from the ValidStateSampler class.

  1. サンプラクラスの定義と,インスタンス生成用の補助関数を定義: We define a sampler class and the supplementary function to instantiate the class:
    class TMyValidStateSampler : public ob::ValidStateSampler
    {
    public:
      TMyValidStateSampler(const ob::SpaceInformation *si) : ValidStateSampler(si)
      {
        name_ = "my sampler";
      }
      virtual bool sample(ob::State *state)
      {
        ob::SE3StateSpace::StateType *state_3d= state->as<ob::SE3StateSpace::StateType>();
        const ob::RealVectorBounds &bounds= si_->getStateSpace()->as<ob::SE3StateSpace>()->getBounds();
        state_3d->setX(rng_.uniformReal(0.0,2.0));
        state_3d->setY(rng_.uniformReal(0.0,2.0));
        state_3d->setZ(rng_.uniformReal(0.0,2.0));
        state_3d->rotation().setIdentity();
        return true;
      }
      virtual bool sampleNear(ob::State*, const ob::State*, const double)
      {
        throw ompl::Exception("TMyValidStateSampler::sampleNear", "not implemented");
        return false;
      }
    protected:
      ompl::RNG rng_;
    };
    ob::ValidStateSamplerPtr AllocTMyValidStateSampler(const ob::SpaceInformation *si)
    {
      return ob::ValidStateSamplerPtr(new TMyValidStateSampler(si));
    }
  2. StateSpace オブジェクトに登録する(境界 bounds を定義した後あたりで): Register it to a StateSpace object (around right after defining the bounds):
    ss.getSpaceInformation()->setValidStateSamplerAllocator(AllocTMyValidStateSampler);

行列・ベクトル計算 Matrix and vector computation

OMPL の内部では,boost::numeric::ublasを利用しており,このライブラリを使うのがもっとも手軽だと言える.当然,Eigen などと組み合わせてもよい.

Inside the OMPL, it seems to be using boost::numeric::ublas, so we can say that using this library is the easiest way. Of course, you can use Eigen, etc.

参考: Reference:

ヘッダ:

Header:

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>

ベクトル・行列の型:

Types of vector and matrix:

boost::numeric::ublas::vector<double>
boost::numeric::ublas::matrix<double>

代入:

Assignment:

boost::numeric::ublas::vector<double> v(3);
v(0)= 1.0;
v(1)= 2.0;
boost::numeric::ublas::matrix<double> M(3,3);
M(0,0)= 1.0;
M(0,1)= 2.0;
M(1,2)= 3.0;

足し算:

Addition:

boost::numeric::ublas::vector<double> v1(3), v2(3), v3(3);
v3= v1+v2;

行列同士・行列とベクトルの積:

Multiplication between matrices, matrix and vector:

boost::numeric::ublas::vector<double> v1(3), v2(3);
boost::numeric::ublas::matrix<double> M1(3,3), M2(3,3), M3(3,3);
M3= prod(M1,M2);
v2= prod(M1,v1);

ノルム:

Norm:

boost::numeric::ublas::vector<double> v(3);
double d= norm_2(v);

トップ   編集 凍結 差分 バックアップ 添付 複製 名前変更 リロード   新規 一覧 単語検索 最終更新   ヘルプ   最終更新のRSS
Last-modified: 2014-02-23 (日) 18:51:21 (1272d)