OMPL programming

概要 Abstract
Open Motion Planning Library (OMPL) を使った演習を行う.
Exercises of Open Motion Planning Library (OMPL) are provided.
参考 Reference
目次 Table of contents

基礎編 Basic level

行動計画とは What is the motion planning?


Motion planning is a problem like that: when a state transition model (dynamics model) of a robot, a start and a goal state (condition), and a cost function (or a reward function) are given, find a path from the start to the goal that minimizes the amount of the cost.


There are wider range of its applications, such as a path planning of mobile robots, a trajectory planning of manipulators, molecular simulations, and so on.


In order to understand motion planning, it is easy to start with a following maze problem:


Fig: Discrete maze problem. S: start, G: goal, green #: state number, red square: obstacle, light-gray square: free cell.


Let's summarize the important elements:

  • 状態空間: ある瞬間におけるロボットの状態を表す空間.上の問題だと,ロボットの位置. State space: A space that indicates a state of the robot at a certain moment. In the above case, the position of the robot is the state.
  • 行動空間: 制御指令値と呼ぶことも.ロボットに入力し,状態を変化させる.上の問題だと,上・下・左・右. Action (control) space: An action changes the state. In the above case, up, down, left, right.
  • パス: 状態・行動の系列. Path: A sequence of (state, action) pairs.
  • 状態遷移モデル: ある状態である行動を実行したとき,どの状態に遷移するか,をモデル化したもの.上の問題だと,障害物の配置が既知なら,このモデルが作られる. State transition model: A model that estimates the succeeding state by taking an action at a certain state. In the above case, we can make this model if we know the location of obstacles.
  • コスト(報酬)関数: ある状態である行動を選択するときに掛かるコスト.例えば行動に必要なエネルギーなど.ロボットに,人の近くなど,あまり通ってほしくない状態がある場合は,それもコストに組み込める.報酬は,コストの反対. Cost (reward) function: Cost given to taking an action at a certain state. For example, energy of the action. Reward is negative of cost.
  • 評価関数: スタート状態からゴール状態に遷移するパスを評価する関数.通常,コスト(または報酬)の総和として定義される.この評価関数を最小にするように(報酬を使う場合は,最大にするように),パスが最適化される. Evaluation function: A function that evaluates a path from the start to the goal. Usually, it is defined as an amount of the cost (or the reward). A path is optimized to minimize this evaluation function (when using reward, the evaluation function is maximized).


Case of continuous state/action space:


Fig: Continuous maze problem. S: start, G: goal, red area: obstacle, white area: free space.


The state space becomes a continuous space. There is a problem setup where the state space is continuous and the action space is discrete.

ロボットがある状態からいかなる状態(ただし障害物が存在しない)に遷移できる,という問題設定の場合,幾何学的なプラニング手法 (Geometric planning) が適用できる.この場合,行動空間を定義する必要はない.パスは状態の系列である.状態遷移モデルとしては,ある状態が取りうるか否かを表すモデル(障害物があるかないか)のみ定義すればよい.

If we can assume that a robot can transit from a state to any state where no obstacle is, geometric planning is applicable. In this case, we need not to define the action space. A path is a sequence of states. The definition of he state transition model needs only a model to represent whether a state is valid or invalid (a state is on an obstacle or not).

そうでない場合は,行動(制御指令)含むをプラニング手法 (Planning with controls) を適用する必要がある.

Otherwise, we need to apply planning with controls.

高次元になった場合: 2次元以上の場合も,同様に考えることができる.ただし,次元数の増加に対して,アルゴリズムの実行時間が指数関数的に増加することに注意する必要がある.

Higher dimensionality case: We can consider more than 2-dimensional cases in the same manner. We need to be careful about the execution time that increases exponentially for increasing dimensionality.

議論 Discussion 1
以下の応用事例における,「重要な要素」を議論しよう. Let's discuss the "important elements" of the following applications. ~ · 移動ロボット(オムニホイール)の経路計画 Path planning of a omni-wheel mobile robot
· 移動ロボット(車いす)の経路計画 Path planning of a wheel chair
· マニピュレータの軌道計画 Trajectory planning of a manipulator
· ヒューマノイドロボットの行動計画 Behavior planning a humanoid robot

解法 Solutions

状態空間と行動空間が離散の場合: ダイクストラ法,A*アルゴリズムなどがよく使われる.

Discrete state and action space: Standard way is to use Dijkstra's algorithm, A* algorithm, etc.

状態空間・行動空間が連続の場合: 空間を離散化して上記手法を適用するか,サンプリングベースのプラニング手法が使われる.

Continuous state and/or action space: One way is to discretize the space and apply the above methods. Another way is to use sampling-based planning methods.


Read the following OMPL document, and learn the basic of sampling-based planning methods.


Important elements of OMPL:

  • SpaceInformation: 状態空間 (StateSpace)・行動(制御入力)空間 (ControlSpace) を定義する. Defines the state space (StateSpace) and the action (control) space (ControlSpace).
  • 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.
議論 Discussion 2
「OMPLの重要な要素」が,モーションプラニングにおける重要な要素とどのように関連するのか,議論しよう. Discuss the relations between the "important elements of OMPL" and the those of motion planning described previously.

Robotics/OMPL#OMPLProgramming を読み,OMPLプログラミングの基礎を理解しよう.

Read Robotics/OMPL#OMPLProgramming and learn the basic of OMPL programming.

A* Algorithm


A traditional path planning algorithm is the A* search algorithm. This is an extension of the Dijkstra's algorithm, published in 1968, but sometimes it is still employed as a motion planning algorithm of a robot.


A* search algorithm can be written into a following pseudo-code:

(→ A* search algorithm - Pseudocode)

問題 Exercise 1-01
A*アルゴリズムを実装し,Fig: Discrete maze problemを解け. Implement the A* algorithm and solve Fig: Discrete maze problem.



  1. 多くの問題に適用できるように,汎用性を持たせて実装しよう. Implement it with a generality so that it is applicable to wider problems.
    • 汎用性を持たせるには,状態,状態遷移モデル,結果のパス,コスト関数,ヒューリスティック関数,などを抽象化し,ユーザが定義できるようにすればよい. To make it maintain the generality, we need to abstract the state, the state transition model, the resulting path, the cost function, the heuristic cost function, etc, so that a user can define them.
  2. 状態(ノード)の内部表現,openset/closeset の内部表現をよく考えて決めよう. Choose carefully the internal representation of state (node) and openset/closeset.
    • openset/closeset にはSTLのコンテナを使うとよい. Use STL containers for openset/closeset.

解答のイメージ: Answer is like this: fileout-p1-01-A_star.png

ヒント: 以下のインターフェイスをベースにするとよい.

Hint: Implement the program based on the following interface.


  1 // A* implementation sample written by Akihiko Yamaguchi
  3 #include <list>
  4 #include <vector>
  5 #include <algorithm> // std::find
  6 #include <cassert>
  8 class TAStarSearch
  9 {
 10 public:
 11   typedef int TState;  // Type of node
 12   typedef double TValue;  // Type of cost
 13   static const TState InvalidState;
 15   TAStarSearch()
 16     : state_max_(InvalidState),
 17       start_(InvalidState),
 18       goal_(InvalidState),
 19       cost_function_(NULL),
 20       heuristic_cost_function_(NULL),
 21       successors_function_(NULL),
 22       callback_on_updated_(NULL)  {}
 24   // Solve the problem; return true if solved, false if failed
 25   bool Solve();
 27   /* Return a solved shortest path to goal.
 28       If goal is not speficied, the path to the stored goal is returned. */
 29   std::list<TState> GetSolvedPath(const TState &goal=InvalidState);
 31   // State space is {0,..,state_max_}
 32   void SetStateMax(const TState &state_max)  {state_max_= state_max;}
 33   void SetProblem(const TState &start, const TState &goal)  {start_= start; goal_= goal;}
 34   const TState& StateMax() const {return state_max_;}
 35   const TState& Start() const {return start_;}
 36   const TState& Goal() const {return goal_;}
 38   // Get a vector: each element(state) indicates a state where that state comes from
 39   const std::vector<TState>& InversePolicy() const {return inverse_policy_;}
 41   // User-defined cost funcion:
 42   void SetCostFunction(TValue (*f)(const TState &state1, const TState &state2))  {cost_function_= f;}
 43   // User-defined heuristic funcion:
 44   void SetHeuristicCostFunction(TValue (*f)(const TState &state1, const TState &state2))  {heuristic_cost_function_= f;}
 45   /* User-defined state-transition function that returns
 46       a set of possible successors from the current state */
 47   void SetSuccessorsFunction(std::list<TState> (*f)(const TState &state))  {successors_function_= f;}
 49   /* User-defined callback function executed when InversePolicy is updated.
 50       Edge state1 --> state2 shows a new one. */
 51   void SetCallbackOnUpdated(void (*f)(const TAStarSearch &a, const TState &state1, const TState &state2))  {callback_on_updated_= f;}
 53 private:
 54   TState state_max_;
 56   TState start_, goal_;
 58   // Each element(state) indicates a state where that state comes from
 59   std::vector<TState>  inverse_policy_;
 61   // Function pointers
 62   TValue (*cost_function_)(const TState &state1, const TState &state2);
 63   TValue (*heuristic_cost_function_)(const TState &state1, const TState &state2);
 64   std::list<TState> (*successors_function_)(const TState &state);
 66   void (*callback_on_updated_)(const TAStarSearch &a, const TState &state1, const TState &state2);
 68   int get_state_num()  {return state_max_+1;}
 70   // Find a state ``s'' in ``set' where ``value[s]' is the lowest in ``value'
 71   TState find_lowest(const std::list<TState> &set, const std::vector<TValue> &value)
 72     {
 73       if(set.empty())  return InvalidState;
 74       std::list<TState>::const_iterator lowest= set.begin();
 75       for(std::list<TState>::const_iterator itr(lowest),last(set.end()); itr!=last; ++itr)
 76         if(value[*itr]<value[*lowest])  lowest= itr;
 77       return *lowest;
 78     }
 80   // Return true if ``item'' is included in ``set''
 81   bool is_in(const TState &item, const std::list<TState> &set)
 82     {
 83       return std::find(set.begin(),set.end(),item)!=set.end();
 84     }
 85 };
 86 const TAStarSearch::TState TAStarSearch::InvalidState(-1);
 89 // Solve the problem; return true if solved, false if failed
 90 bool TAStarSearch::Solve()
 91 {
 92   // Check the configuration validity:
 93   assert(state_max_!=InvalidState);
 94   assert(start_!=InvalidState);
 95   assert(goal_!=InvalidState);
 96   assert(cost_function_);
 97   assert(heuristic_cost_function_);
 98   assert(successors_function_);
100   std::list<TState> closedset; // The set of nodes already evaluated
101   std::list<TState> openset;   // The set of tentative nodes to be evaluated,
102   openset.push_back(start_);   // initially containing the start node
104   // The map of navigated nodes:
105   inverse_policy_.resize(get_state_num());
106   std::fill(inverse_policy_.begin(), inverse_policy_.end(), InvalidState);
108   std::vector<TValue>  g_score(get_state_num(), 0.0l);
109   std::vector<TValue>  f_score(get_state_num(), 0.0l);
110   g_score[start_]= 0.0l;   // Cost from start along best known path
111   // Estimated total cost from start to goal
112   f_score[start_]= g_score[start_] + heuristic_cost_function_(start_, goal_);
115   /*
116   *   ===========================================================
118   **_search_algorithm#Pseudocode
119   *   ===========================================================
120   */
122   // Return false if the solution is not found
123   return false;
124 }
126 /* Return a solved shortest path to goal.
127     If goal is not speficied, the path to the stored goal is returned. */
128 std::list<TAStarSearch::TState> TAStarSearch::GetSolvedPath(const TState &goal)
129 {
130   std::list<TState> path;
131   TState current= ((goal!=InvalidState)?goal:goal_);
132   path.push_front(current);
133   while((current=inverse_policy_[current])!=InvalidState)
134     path.push_front(current);
135   return path;
136 }



  • TAStarSearch::Solve を実装するだけでよいようになっている.問題依存の関数(例えばコスト関数)は,ユーザが定義できるように,関数ポインタで実装されている.TAStarSearch::Solve の中で使う場合は,以下のように,通常の関数として扱える. Remaining task is only implementing TAStarSearch::Solve. Problem-specific functions (e.g. a cost function) are implemented as function pointers so that a user can define it. To use such a function in TAStarSearch::Solve, write like a usual function.
    TValue cost= cost_function_(state1, state2);
  • inverse_policy_ は擬似コード中の came_from の実装. inverse_policy_ corresponds with came_from in the pseudo code.
  • このアルゴリズムを使うには: To use this algorithm:
    1. コスト関数,ヒューリスティック関数,状態遷移モデルを関数として定義. Define a cost function, a heuristic cost function, a state transition model as functions.
    2. TAStarSearch のインスタンスを生成. Generate an instance of TAStarSearch.
    3. SetCostFunction, SetHeuristicCostFunction, SetSuccessorsFunction を使って,自分で定義した関数を設定. Register the problem specific functions by using SetCostFunction, SetHeuristicCostFunction, SetSuccessorsFunction.
    4. SetStateMax で状態空間を定義. Define the state space by SetStateMax.
    5. SetProblem でスタートとゴールを指定. Specify the start and the goal by SetProblem.
    6. Solve で実行. Run by Solve.
    7. GetSolvedPath で得られたパスを取得. Get a solution path by GetSolvedPath.
  • 探索されたすべてのエッジを参照するには,InversePolicy を使えば良い. In order to refer to every edge, use InversePolicy.

問題 Exercise 1-02
A*アルゴリズムを,Fig: Continuous maze problemに適用する.このとき,以下の図のように空間を離散化し,A*アルゴリズムを適用せよ.離散化の間隔は,後で変更できるように変数化しておくこと. Apply the A* algorithm to Fig: Continuous maze problem. Here, discretize the space like the following way to apply the A* algorithm. Define a variable to denote the interval of discretization so that we can change them later.


Fig: Discretization for the continuous maze problem. Horizontal: x-axis, vertical: y-axis. S: start, G: goal, red area: obstacle, white area: free space, gray grid: each grid point (intersection) is a robot state, aqua grid: each cell is used to check the occupancy of the corresponding state.


Definition of the obstacles:

  • 以下の条件がすべて満たされていれば障害物: A state is an obstacle if all following conditions are satisfied:
    • y<4.5*x-2.25
    • y<-4.5*x+11.25
  • 以下の条件がすべて満たされていれば障害物: A state is an obstacle if all following conditions are satisfied:
    • y<4.4*x-12.9
    • y<-4.4*x+22.3
    • y>-4.0*x+16.5
    • y>4.0*x-15.5


How to discretize:

  • 四方の壁に囲まれた領域を,(CellX,CellY)=(0.5,0.5) 間隔のセルで区切る.図の水色の点線で囲まれた四角は,各セルを表す. Partition the area surrounded by the four-walls into (CellX,CellY)=(0.5,0.5)-size cells. In the above figure, each square bounded by dotted aqua lines denotes a cell.
  • 離散化された状態は,各セルの中心に対応するとする.つまり,図の灰色の格子の交点がすべての状態を表す.例えば図の S は (0.25,0.25), G は (5.75,4.75) である. Discretized state corresponds to the center of each cell. That is, all grid points (intersections) on the gray grid denote the whole state space. For example, S is (0.25,0.25), G is (5.75,4.75).
  • 各セルを,セルのサイズの SampleRatio=0.05 倍の間隔でサンプリングし,1点でも障害物上にあれば,そのセルは通れないセル(障害物セル)として扱う. Sample each cell with the interval of SampleRatio=0.05 -times of the cell size, and if there is a sample point that is on an obstacle, that cell is treated as an obstacle cell that the robot cannot be on.

解答のイメージ: Answer is like this: fileout-p1-02-A_star.png

OMPLによる解法 Solving by OMPL

問題 Exercise 2-01
「問題1-02」をOMPLで解け.何回か実行してみて,結果の挙動を調べよ. Solve the exercise 1-02 with OMPL. Execute several times, and check the solution paths.

解答のイメージ: Answer is like this: fileout-p2-01-ompl-for-p1-02.png

応用編 Advanced level

単純化されたUAVの経路プラニング Path planning of simplified UAV


Here we solve for a path of a box assumed as a UAV. The UAV avoids to collide with obstacles consisting of multiple spheres and reaches the target point. We modify gradually the simplification of the problem and exercise how to apply the planning algorithms.

問題設定 Problem setup

状態空間は SE(3)とする.状態空間の範囲は,原点と (SizeX,SizeY,SizeZ) で定義されるボックスとする.これらのサイズは以下で定義される.

The state space is assumed to be SE(3). The range of the state space is inside a box defined by the origin and (SizeX,SizeY,SizeZ). These sizes are defined as:

static const double SizeX(6),SizeY(5),SizeZ(5);


The radius of each obstacle (sphere) is defined as:

static const double ObstacleRadius(0.5);


The size of UAV is defined as:

static const double RobotX(0.5),RobotY(0.4),RobotZ(0.3);


In order to check the collision between the UAV and an obstacle (sphere), we can treat the UAV as a sphere. The radius of the sphere containing the UAV is given by:

static const double RobotRadius(std::sqrt(0.25*Sq(RobotX)+0.25*Sq(RobotY)+0.25*Sq(RobotZ)));


Obstacles (spheres) are randomly allocated in a certain number (e.g. 50). The following figure is an example of the environment.


Fig: Path planning of UAV. S: start, G: goal, sphere: obstacle. x: [0,6], y: [0,5], z: [0,5].

補助ツール Supplementary tools


You can use the supplementary functions by including the following header.


  1 // Utility for the UAV planning problem
  3 #include <ompl/geometric/SimpleSetup.h>
  4 #include <ompl/base/spaces/SE3StateSpace.h>
  5 #include <boost/numeric/ublas/vector.hpp>
  6 #include <boost/numeric/ublas/matrix.hpp>
  7 #include <cassert>
  8 #include <iostream>
  9 #include <cmath>
 10 #include <fstream>
 11 #include <vector>
 13 namespace ob = ompl::base;
 14 namespace og = ompl::geometric;
 16 template<typename T> inline T Sq(const T &x)  {return x*x;}
 18 typedef boost::numeric::ublas::matrix<double> TMatrix;
 19 typedef boost::numeric::ublas::vector<double> TVector;
 21 static const double SizeX(6),SizeY(5),SizeZ(5);
 23 std::vector<TVector>  Obstacles;
 24 static const double ObstacleRadius(0.5);
 26 static const double RobotX(0.5),RobotY(0.4),RobotZ(0.3);
 27 static const double RobotRadius(std::sqrt(0.25*Sq(RobotX)+0.25*Sq(RobotY)+0.25*Sq(RobotZ)));
 29 // Generate a vector with 3-dim
 30 inline TVector V3(const double &x, const double &y, const double &z)
 31 {
 32   TVector v(3);  v(0)= x; v(1)= y; v(2)= z;
 33   return v;
 34 }
 35 // Convert a quaternion to a rotation matrix
 36 inline TMatrix QtoR(const double &qx, const double &qy, const double &qz, const double &qw)
 37 {
 38   TMatrix M(3,3);
 39   M(0,0)= qw*qw+qx*qx-qy*qy-qz*qz; M(0,1)= 2.0*(qx*qy-qw*qz);       M(0,2)= 2.0*(qx*qz+qw*qy);
 40   M(1,0)= 2.0*(qx*qy+qw*qz);       M(1,1)= qw*qw-qx*qx+qy*qy-qz*qz; M(1,2)= 2.0*(qy*qz-qw*qx);
 41   M(2,0)= 2.0*(qx*qz-qw*qy);       M(2,1)= 2.0*(qy*qz+qw*qx);       M(2,2)= qw*qw-qx*qx-qy*qy+qz*qz;
 42   return M;
 43 }
 44 // Convert a OMPL's quaternion to a rotation matrix
 45 inline TMatrix QtoR(const ob::SO3StateSpace::StateType &rot)
 46 {
 47   return QtoR(rot.x, rot.y, rot.z, rot.w);
 48 }
 50 // Standard quaternion multiplication: q= q0 * q1
 51 inline void QuaternionProduct(ob::SO3StateSpace::StateType &q, const ob::SO3StateSpace::StateType &q0, const ob::SO3StateSpace::StateType &q1)
 52 {
 53   q.x = q0.w*q1.x + q0.x*q1.w + q0.y*q1.z - q0.z*q1.y;
 54   q.y = q0.w*q1.y + q0.y*q1.w + q0.z*q1.x - q0.x*q1.z;
 55   q.z = q0.w*q1.z + q0.z*q1.w + q0.x*q1.y - q0.y*q1.x;
 56   q.w = q0.w*q1.w - q0.x*q1.x - q0.y*q1.y - q0.z*q1.z;
 57 }
 59 std::ostream& operator<<(std::ostream &os, const TVector &x)
 60 {
 61   for(std::size_t i(0); i<x.size(); ++i)
 62     os<<" "<<x(i);
 63   return os;
 64 }
 66 /* Save a sequence of box on ``path'' into file that is gnuplot-compatible.
 67     The path should be a sequence of SE(3) state. The box size is ``(sizex,sizey,sizez)''.
 68     The parameter ``skip'' is an interval to sample from ``path'' (1 for every sample). */
 69 void PrintBoxSequence(const char *filename, const og::PathGeometric &path, const double &sizex, const double &sizey, const double &sizez, int skip=1)
 70 {
 71   using namespace std;
 72   using namespace boost::numeric::ublas;
 73   ofstream ofs(filename);
 74   for(size_t i(0); i<path.getStateCount(); i+=skip)
 75   {
 76     const ob::SE3StateSpace::StateType *s= path.getState(i)->as<ob::SE3StateSpace::StateType>();
 77     TVector pos(3), d(3);
 78     TMatrix R= QtoR(s->rotation());
 79     pos(0)= s->getX(); pos(1)= s->getY(); pos(2)= s->getZ();
 80     ofs<<pos+prod(R,V3( sizex, sizey, sizez))<<endl;
 81     ofs<<pos+prod(R,V3( sizex,-sizey, sizez))<<endl;
 82     ofs<<pos+prod(R,V3(-sizex,-sizey, sizez))<<endl;
 83     ofs<<pos+prod(R,V3(-sizex, sizey, sizez))<<endl;
 84     ofs<<pos+prod(R,V3( sizex, sizey, sizez))<<endl;
 85     ofs<<pos+prod(R,V3( sizex, sizey,-sizez))<<endl;
 86     ofs<<pos+prod(R,V3( sizex,-sizey,-sizez))<<endl;
 87       ofs<<pos+prod(R,V3( sizex,-sizey,sizez))<<endl;
 88       ofs<<pos+prod(R,V3( sizex,-sizey,-sizez))<<endl;
 89     ofs<<pos+prod(R,V3(-sizex,-sizey,-sizez))<<endl;
 90       ofs<<pos+prod(R,V3(-sizex,-sizey,sizez))<<endl;
 91       ofs<<pos+prod(R,V3(-sizex,-sizey,-sizez))<<endl;
 92     ofs<<pos+prod(R,V3(-sizex, sizey,-sizez))<<endl;
 93       ofs<<pos+prod(R,V3(-sizex, sizey,sizez))<<endl;
 94       ofs<<pos+prod(R,V3(-sizex, sizey,-sizez))<<endl;
 95     ofs<<pos+prod(R,V3( sizex, sizey,-sizez))<<endl;
 96       ofs<<pos+prod(R,V3( sizex, sizey,sizez))<<endl;
 97     ofs<<endl<<endl;
 98   }
 99 }
101 /* Generate ``num'' shperes and store them into ``Obstacles''
102     where each center is decided randomly. */
103 void CreateMap(int num, long seed)
104 {
105   using namespace std;
106   srand(seed);
107   Obstacles.resize(num);
108   for(int i(0); i<num; ++i)
109   {
110     Obstacles[i].resize(3);
111     Obstacles[i](0)= SizeX*(double)rand()/(double)RAND_MAX;
112     Obstacles[i](1)= SizeY*(double)rand()/(double)RAND_MAX;
113     Obstacles[i](2)= SizeZ*(double)rand()/(double)RAND_MAX;
114   }
115 }
117 /* Print every center shperes into a file "res/map.dat". */
118 void PrintMap()
119 {
120   using namespace std;
121   ofstream ofs("res/map.dat");
122   for(vector<TVector>::const_iterator itr(Obstacles.begin()),last(Obstacles.end()); itr!=last; ++itr)
123     ofs<<(*itr)<<endl;
124 }
126 /* Print the planning result into a file.
127     The resulting file is a gnuplot script that plots the path,
128     the sequence of box on the path, and the obstacles.
129     ``path'': stored into "res/path.dat",
130     sequence of box on ``path'': stored into "res/frame_all.dat",
131     obstacles: stored into the resulting script.
132     Usage:  gnuplot -persistent filename */
133 void PrintSolution(const char *filename, const og::PathGeometric &path, int skip=1)
134 {
135   using namespace std;
136   ofstream ofs(filename);
137   {
138     ofstream ofs("res/path.dat");
139     path.printAsMatrix(ofs);
140   }
141   PrintBoxSequence("res/frame_all.dat", path, RobotX, RobotY, RobotZ, skip);
142   ofs<<"\
143 #set terminal png size 800, 640 transparent                     \n\
144 #set terminal svg size 1200 780 fname 'Trebuchet MS' fsize 24   \n\
145 set xlabel 'x'         \n\
146 set ylabel 'y'         \n\
147 set zlabel 'z'         \n\
148 set hidden3d           \n\
149 set ticslevel 0        \n\
150 set size 0.7,1         \n\
151 set parametric         \n\
152 set urange [0:6.28]    \n\
153 set vrange [0:6.28]    \n\
154 set isosample 8,8      \n\
155 set samples 10         \n\
156 r= "<<ObstacleRadius<<endl;
157   ofs<<"splot \\"<<endl;
158   for(vector<TVector>::const_iterator itr(Obstacles.begin()),last(Obstacles.end()); itr!=last; ++itr)
159   {
160     const double &ox((*itr)(0)), &oy((*itr)(1)), &oz((*itr)(2));
161     ofs<<"  "<<"r*cos(u)*cos(v)+"<<ox
162         <<",r*sin(u)*cos(v)+"<<oy
163         <<",r*sin(v)+"<<oz<<" w l lt 1 lw 0.2 t '',"<<"\\"<<endl;
164   }
165   ofs<<"'res/frame_all.dat' w l lt 3, \\"<<endl;
166   ofs<<"'res/path.dat' w l lt 4"<<endl;
167 }
  • Obstacles: 障害物の中心座標を格納するベクタ. Obstacles: Vector to store the center positions of the obstacles.
  • PrintSolution: プラニング結果を渡すと,UAVの軌跡,障害物などを可視化するための Gnuplot スクリプトを出力する.以下のように使う: PrintSolution: By giving the planning result, it outputs a Gnuplot script to visualize the UAV trajectory and the obstacles. The usage is as follows:
    PrintSolution("res/", ss.getSolutionPath());
    res ディレクトリを,プログラムの実行前に作成する必要がある.上記で,res/ が生成されるので,コマンドラインから以下を実行して描画しよう: We need to make a "res" directory before executing the program. By above, a file named "res/" is generated. Execute the following command to plot:
    gnuplot -persistent filename
  • CreateMap: 障害物を生成する.以下のように使う: CreateMap: Generates obstacles. Usage is as follows:
    CreateMap(50, 0);
    2つめの引数は,乱数のシード.これを変更すれば,障害物の配置が変えられる. The 2nd parameter is a seed of the random numbers. By changing this, the allocation of obstacles are modified.

問題 Exercises

問題 Exercise 3-01
上記で定義された問題を解け. Solve the problem defined above.

解答のイメージ: Answer is like this: fileout-p3-01-uav.png

問題 Exercise 3-02a
UAVの姿勢が,ロールまたはピッチ方向に RobotRot=0.004 ラジアン以上回転できない場合を考える.StateValidityChecker としてこの制約を実装し,解を与えよ. We consider a case where the UAV cannot rotate more than RobotRot=0.004 radian around the roll or the pitch axis. Solve for the path by implementing this constraint as the StateValidityChecker.

解答のイメージ: Answer is like this: fileout-p3-02a-uav.png

議論 Discussion 3
「問題3-02a」の解法の問題点を考察しよう. Discuss the problem of the solution of the exercise 3-02a.

問題 Exercise 3-02b
「問題3-02a」を効率的な方法で解け. Solve the exercise 3-02a with an efficient way.

問題 Exercise 3-03
UAVが以下のようなダイナミクスに従うと仮定する.制御指令値を含むプラニング手法を適用し,解を与えよ. We assume that the UAV is subjected by the following dynamics. Solve for the path by applying a planning method with controls.
ダイナミクス: ロボットの状態 SE(3) を位置 p と姿勢のクォータニオン q と表し,q を姿勢行列に変換したものを R=(e_x,e_y,e_z) と書く.ここで,e_x, e_y, e_z はそれぞれ姿勢行列を構成する 3x1 のベクトルである.一方,制御指令空間は,(u_f,u_h,u_r) の3次元ベクトルであり,それぞれ前進速度,上方向の速度,旋回速度を表す.この制御入力を時間 \Delta t だけ実行したとき,ロボットの状態は以下のように変化する. Dynamics: Let us denote the robot state SE(3) as the position p and the orientation quaternion q. Let us denote the rotation-matrix representation of q as R=(e_x,e_y,e_z) where e_x, e_y, e_z indicate 3x1 vectors consisting of the rotation matrix. On the other hand, the control space is a 3-dimensional real vector space (u_f,u_h,u_r), that indicates a forward velocity, an upward velocity, and a rotation velocity respectively. By executing this control input in a duration \Delta t, the robot state changes as follows:
p'= p + \Delta t u_f e_x + \Delta t u_h e_z
R'= \exp(e_z, \Delta t u_r) R
ただし,\exp(e_z, \Delta t u_r) は,軸 e_z を中心に,\Delta t u_r ラジアン回転させる回転行列を表す.
制御指令値の範囲は,それぞれ,-0.3 から 0.3 とする.制御の時間ステップは 0.05 とする.また,解の精度は 0.2 とする.
where \exp(e_z, \Delta t u_r) denotes a rotation matrix of rotating \Delta t u_r radian around the axis e_z. The range of the control space is from -0.3 to 0.3 respectively. The step-size of the control is 0.05. The required accuracy of the solution is 0.2.

解答のイメージ: Answer is like this: fileout-p3-03-uavdyn.png

マニピュレータの軌道計画 Trajectory planning of manipulators


Here we apply OMPL to solve for a trajectory of a manipulator. For the simplicity, we assume that the manipulator consists of line segments, and the obstacles consist of several spheres.

問題設定 Problem setup

マニピュレータの自由度は,自由に変更できるようにすること.障害物は,#Adv_UAV と同じで,複数の球とする.初期姿勢から,ゴール姿勢まで移動するようなパスを探索する.

The degree of freedoms (DoF) of the manipulator should be changed easily. The obstacles are the same definition as #Adv_UAV; i.e. several spheres. We find a path from a start pose to a goal pose.


The following figure shows an image of this problem setup.


Fig: Path planning of a manipulator. S: start, G: goal, sphere: obstacle.

補助ツール Supplementary tools

以下のヘッダをインクルードして用いると,この問題の補助関数が使えるようになる. ただし,#Adv_UAV のヘッダも参照している.

You can use the supplementary functions by including the following header. It refers to the header of #Adv_UAV.


  1 // Utility for the manipulator (arm) planning problem
  3 #include "p3-uav-helper.h"
  4 #include <ompl/base/spaces/RealVectorStateSpace.h>
  6 struct TLink
  7 {
  8   TVector Axis;  // Joint axis
  9   TVector End;   // End-point position of the link defined on the local frame
 10   TLink(const TVector &a, const TVector &e) : Axis(a), End(e) {}
 11 };
 12 std::vector<TLink> Arm;  // Manipulator
 13 TVector ArmBase;  // The base position of Manipulator
 15 // Compute a rotation matrix from axis (x,y,z) and angle
 16 inline TMatrix RfromAxisAngle(const double &x, const double &y, const double &z, const double &angle)
 17 {
 18   ob::SO3StateSpace::StateType rot;
 19   rot.setAxisAngle(x,y,z, angle);
 20   return QtoR(rot);
 21 }
 22 // Compute a rotation matrix from axis (v) and angle
 23 inline TMatrix RfromAxisAngle(const TVector &v, const double &angle)
 24 {
 25   assert(v.size()==3);
 26   return RfromAxisAngle(v(0), v(1), v(2), angle);
 27 }
 29 /* Compute the forward kinematics of a manipulator ``linkes''
 30     whose joint angles are specified by ``angles'',
 31     and the base position is ``base''.
 32     The result is stored into ``result'' that contains
 33     the base position and every position of the end-points.
 34     i.e. result.size()==linkes.size()+1 */
 35 void ForwardKinematics(const std::vector<TLink> &linkes, const std::vector<double> &angles, const TVector &base, std::vector<TVector> &result)
 36 {
 37   assert(linkes.size()==angles.size());
 38   assert(base.size()==3);
 39   result.resize(linkes.size()+1);
 40   TMatrix R(3,3),Rtmp(3,3);
 41   R(0,0)=R(1,1)=R(2,2)= 1.0;
 42   R(0,1)=R(0,2)=R(1,0)=R(1,2)=R(2,0)=R(2,1)= 0.0;
 43   result[0]= base;
 44   for(size_t i(1); i<result.size(); ++i)
 45   {
 46     result[i].resize(3);
 47     Rtmp= prod(R,RfromAxisAngle(linkes[i-1].Axis, angles[i-1]));
 48     R= Rtmp;
 49     result[i]= prod(R,linkes[i-1].End) + result[i-1];
 50   }
 51 }
 53 /* Save a sequence of the arm on ``path'' into file that is gnuplot-compatible.
 54     The path should be a sequence of joint-angles.
 55     The parameter ``skip'' is an interval to sample from ``path'' (1 for every sample). */
 56 void PrintArmSequence(const char *filename, const og::PathGeometric &path, int skip=1)
 57 {
 58   using namespace std;
 59   using namespace boost::numeric::ublas;
 60   ofstream ofs(filename);
 61   std::vector<double> angles(Arm.size());
 62   std::vector<TVector> jpos;
 63   for(size_t i(0); i<path.getStateCount(); i+=skip)
 64   {
 65     const ob::RealVectorStateSpace::StateType *s= path.getState(i)->as<ob::RealVectorStateSpace::StateType>();
 66     for(size_t i(0); i<Arm.size(); ++i)
 67       angles[i]= (*s)[i];
 68     ForwardKinematics(Arm, angles, ArmBase, jpos);
 69     for(size_t i(0); i<jpos.size(); ++i)
 70       ofs<<jpos[i]<<endl;
 71     ofs<<endl<<endl;
 72   }
 73 }
 75 /* Print the planning result into a file.
 76     The resulting file is a gnuplot script that plots the path,
 77     the sequence of the arm on the path, and the obstacles.
 78     Sequence of the arm on ``path'': stored into "res/frame_all.dat",
 79     obstacles: stored into the resulting script.
 80     Usage:  gnuplot -persistent filename */
 81 void PrintArmSolution(const char *filename, const og::PathGeometric &path, int skip=1)
 82 {
 83   using namespace std;
 84   ofstream ofs(filename);
 85   PrintArmSequence("res/frame_all.dat", path, skip);
 86   ofs<<"\
 87 #set terminal png size 800, 640 transparent                     \n\
 88 #set terminal svg size 1200 780 fname 'Trebuchet MS' fsize 24   \n\
 89 set xlabel 'x'         \n\
 90 set ylabel 'y'         \n\
 91 set zlabel 'z'         \n\
 92 set hidden3d           \n\
 93 set ticslevel 0        \n\
 94 set size 0.7,1         \n\
 95 set parametric         \n\
 96 set urange [0:6.28]    \n\
 97 set vrange [0:6.28]    \n\
 98 set isosample 8,8      \n\
 99 set samples 10         \n\
100 r= "<<ObstacleRadius<<endl;
101   ofs<<"splot \\"<<endl;
102   for(vector<TVector>::const_iterator itr(Obstacles.begin()),last(Obstacles.end()); itr!=last; ++itr)
103   {
104     const double &ox((*itr)(0)), &oy((*itr)(1)), &oz((*itr)(2));
105     ofs<<"  "<<"r*cos(u)*cos(v)+"<<ox
106         <<",r*sin(u)*cos(v)+"<<oy
107         <<",r*sin(v)+"<<oz<<" w l lt 1 lw 0.2 t '',"<<"\\"<<endl;
108   }
109   ofs<<"'res/frame_all.dat' w lp lt 3 pt 6 lw 1.5"<<endl;
110 }
  • ArmBase: マニピュレータのベース位置.以下のように設定する: ArmBase: The base position of the manipulator. It is configured as follows:
    ArmBase= V3(0.0,0.0,0.0);
  • Arm: マニピュレータオブジェクト.実質,ローカルフレームで定義された関節の方向ベクトルと,エンドポイント(端点)のベクトルから構成される構造体のベクトルである.以下のように,多リンク系を作る: Arm: Manipulator object. Actually, it is a vector of a structure consisting of a joint axis vector and a end-point vector defined in the local frame. To construct a multi-link structure:
    double total_len(0.99*(SizeZ-ArmBase(2)));
    Arm.push_back(TLink(V3(0,0,1), V3(0,0,0.0)));
    Arm.push_back(TLink(V3(0,1,0), V3(0,0,total_len/(double)3)));
    Arm.push_back(TLink(V3(0,1,0), V3(0,0,total_len/(double)3)));
    Arm.push_back(TLink(V3(0,1,0), V3(0,0,total_len/(double)3)));
  • ForwardKinematics: 順運動学は既に実装してある.以下のように,angles に関節角を代入し,実行すると,jpos にすべてのエンドポイントの位置(ArmBaseも含む)が代入される. ForwardKinematics: The forward kinematics is already implemented. By assigning the joint angles into "angles" and execute this function as follows, the positions of all endpoints including ArmBase are stored into jpos.
    std::vector<double> angles(Arm.size());
    angles[0]= ...
    std::vector<TVector> jpos;
    ForwardKinematics(Arm, angles, ArmBase, jpos);
  • PrintArmSolution: プラニング結果を渡すと,マニピュレータの軌跡,障害物などを可視化するための Gnuplot スクリプトを出力する.以下のように使う: PrintArmSolution: By giving the planning result, it outputs a Gnuplot script to visualize the manipulator trajectory and the obstacles. The usage is as follows:
    PrintArmSolution("res/", ss.getSolutionPath());
    res ディレクトリを,プログラムの実行前に作成する必要がある.上記で,res/ が生成されるので,コマンドラインから以下を実行して描画しよう: We need to make a "res" directory before executing the program. By above, a file named "res/" is generated. Execute the following command to plot:
    gnuplot -persistent filename
  • CreateMap: UAVの問題と同様に,障害物を生成しよう. CreateMap: Generate the obstacles the same as the UAV case.
    CreateMap(50, 0);

問題 Exercises

問題 Exercise 4-01
上記で定義された問題を解け.マニピュレータの自由度を変更したときに,プラニングに掛かる時間がどのように変化するか調べよ. Solve the problem defined above. Observe how the computation time changes with modifying the manipulator's DoF.

解答のイメージ: Answer is like this: fileout-p4-01-arm.png

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