This paper provides a comprehensive overview of motion planning for robotic manipulation, encompassing grasp planning, motion planning, MoveIt in ROS, OMPL, RRT, forward and inverse kinematics, singularity of robotic manipulators, and manipulability.

1. OK OK
https://www.slideshare.net/SatoshiMakita/
槇田 諭 / Satoshi Makita
福岡工業大学 / Fukuoka Institute of Technology
ロボットマニピュレーションの作業・動作計画
ROS Japan UG #55 Planner特集！ 2024/05/21

2. OK OK
Self Introduction
槇田 諭（MAKITA, Satoshi） makita@fit.ac.jp
https://www.fit.ac.jp/~makita/
2010年3月 横浜国立大学大学院工学府システム統合工学専攻
博士課程後期修了，博士（工学）
2010年4月～2020年3月 佐世保工業高等専門学校 電子制御工学科
2020年4月～ 福岡工業大学工学部知能機械工学科
Research interests:
ロボットによる物体操作（マニピュレーション）の理論、
作業と動作の計画、手指のバイオメカニクス
Twitter@twmaks
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
2

3. OK OK
Research topics
手を使った物体操作
ロボットハンドによる操作
力学的拘束
（力のつり合い）
グラスプレス・マニピュレーション
幾何学的拘束
（囲い込み）
ケージング (caging)
人の手による操作
バレーボールの
オーバーハンド
パスの動作解析
筋腱複合体の
弾性特性の
発現メカニズム
外力
• RGB-D画像を用いた物体認識，識別
• ロボットアームによる組立作業の自動化
• XRを用いたロボットの動作教示
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
3

4. OK OK
Around motion planning - Grasp planning
ケージング / Caging
把持対象物を幾何学的に
閉じ込めて拘束
多少の位置ずれ，
制御誤差を許容
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
4

5. OK OK
Caging a ring-like object
S. Makita, K. Okita and Y. Maeda: ``3D Two-Fingered Caging for Two Types of Objects: Sufficient
Conditions and Planning,'' Int. J. of Mechatronics and Automation, vol. 3, no. 4, pp. 263--277, Dec 2013.
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
5

6. OK OK
Manipulation planning
K. Makihara, T. Otsubo, and S. Makita: ``Through-hole Detection and Finger Insertion Planning as Preceding
Motion for Hooking and Caging a Ring,'' J. of Robotics and Mechatronics, Vol .35, No. 3, pp.734--742, Jun. 2023.
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
6
Perception and planning

7. OK OK
Task Planning
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
7
藤木，鉢峰，槇田： ``サッシ様平板の溝はめ込み作業における挿入枠の位置・姿勢推定と挿入動作計画''，
日本ロボット学会誌，Vol. 42，No. 2，pp. 159--167，Apr. 2024.

8. OK OK
Sash insertion planning
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
8
Width and
depth of groove
(given)
B E
Thickness
(given)
A
Depth image Frame detection Pose estimation
Insertion planning
by A-star algorithm

9. OK OK
Toward secure manipulation
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
9
本田，槇田：単腕マニピュレータによる複数物体の
同時組み立ての基礎的考察，ROBOMECH2024
濵田，槇田：ロボットマニピュレーションにおける非接触
な幾何学的拘束の効果をポテンシャルエネルギーに基づい
て評価する手法，日本ロボット学会誌，2024．

10. OK OK
Table of Contents
• Self introduction
• Task planning and motion planning
• Motion planning methods in ROS (MoveIt!)
• Motion planning in robotic manipulation
• Configuration space
• Manipulability
• Summary
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
10

11. OK OK
Task planning and motion planning
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
11

12. OK OK
ICRA 2024
Sessions including “planning”: 23/455 (34*3+26*3+32+32+31 session!)
 Planning under Uncertainty
 Motion and Path Planning
 Aerial Systems: Motion Control and Planning
 Manipulation Planning
 Path Planning for Multiple Mobile Robots or Agents
 Task Planning
 Task and Motion Planning
 Reactive and Sensor-Based Planning
 Integrated Planning and Control
 Integrated Planning and Learning
 Human-Aware Motion Planning
 Whole-Body Motion Planning and Control
 Planning, Scheduling and Coordination
 Motion Analysis and Planning
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
12

13. OK OK
そもそもなぜプランニングするのか？
教示（ティーチング）がとても大変なとき
工程が複雑
環境が動的に変化する
ロボットが思い通りに動かない
（関節の構造的に）
実機を動かさないといけない
（オンラインティーチング）
シミュレーションは実環境と
合わない
文字数
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
13

14. OK OK
Task planning?
Task and symbolic planning
Motion planning
Force and torque
control
State
classification and
detection
Motion planning
Force and torque
control
State
classification and
detection
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
14
Wan, W., Harada, K., & Nagata, K. (2018). Assembly sequence planning for motion planning.
Assembly Automation, 38(2), 195–206. https://doi.org/10.1108/AA-01-2017-009
High
Level
Middle
Level
Low
Level

15. OK OK
Task planning?
Task and symbolic planning in
the high level
Motion
planning
Force and
torque
control
State
classification
and
detection
Motion
planning
Force and
torque
control
State
classification
and
detection
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
15
What
to do?
How to
move?

16. OK OK
From Task planning to motion control
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
16
Motion
Planning
ロボットはどのような行動をすべきか？
行動を実現できるロボットの動きはどのようなものか？
ロボットの動きを実現できる制御則，制約条件は何か？

17. OK OK
Manipulation planning
Perception and Recognition
Decision making
↓
Grasp planning
Manipulation planning
↓
Motion control strategy
Manipulator motion
Fingers motion
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
17
High
Level
Middle
Level
Low
Level

18. OK OK
Motion planning in ROS (MoveIt)
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
18

19. OK OK
https://moveit.ros.org
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
19
David Coleman, Ioan A. Șucan, Sachin Chitta, Nikolaus Correll, Reducing the Barrier to Entry of Complex
Robotic Software: a MoveIt! Case Study, Journal of Software Engineering for Robotics, 5(1):3–16, May 2014.
doi: 10.6092/JOSER_2014_05_01_p3.

20. OK OK
Planners in MoveIt
• Open Motion Planning Library (OMPL)
• Pilz Industrial Motion Planner
• Stochastic Trajectory Optimization for Motion Planning
(STOMP)
• Search-Based Planning Library (SBPL)
• Covariant Hamiltonian Optimization for Motion Planning
(CHOMP)
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
20

21. OK OK
https://ompl.kavrakilab.org
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
21

22. OK OK
Planners in OMPL
• Geometric planners
幾何学的制約のある動作計画
PRM and its expansions
RRT and its expansion
and more
• Control-based planners
時間制約のある動的な動作計画
• Multilevel-based planners
より高次元空間での動作計画
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
22
Random sampling
Initial
configuration
Goal
configuration

23. OK OK
Randomly-exploring Random Trees (RRT)
ランダムサンプリングに基づくグラフ構造生成
高次元に強い
さまざまな空間の問題に適用可能
シンプルなアルゴリズム
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
23
Lavalle, S. M.: Rapidly-Exploring Random Trees: A New Tool for Path Planning,
The annual research report, Dept. of Computer Science, Iowa State University, 1998.

24. OK OK 24/6
4
RRTによる経路探索
Random sampling
Collision check
Initial
configuration
Satisfying
Sufficient conditions
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
• 枝を伸ばす量
• ノルム（距離）の計算法
• ゴールのわかっている場合

25. OK OK
RRT on Unity (3D space)
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
25
https://github.com/smakita/RRT_on_Unity

26. OK OK
Searching configuration space
直交空間で探索するか？関節空間で探索するか？
2次元平面 → 3次元C空間（位置2自由度，姿勢1自由度）
3次元空間 → 6次元C空間（位置3自由度，姿勢3自由度）
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
26
obstacle
: OK
: OK
: NG

27. OK OK
直交空間での探索
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
27
Collision
check
Initial
configuration
obstacle
• モーションが直観的
• 逆運動学解法が課題
↓
経由点（waypoint）間を
接続できない場合がある

28. OK OK
順運動学と逆運動学
逆運動学は難しい…
（一般解法が見つかっていない）
関節角度：𝜃𝜃1, 𝜃𝜃2
手先の位置： 𝑥𝑥, 𝑦𝑦
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
28
𝜃𝜃1
𝜃𝜃2
順運動学
Forward Kinematics
逆運動学
Inverse Kinematics
𝑥𝑥, 𝑦𝑦
必ず一つに決まる
（同次変換行列）
複数の解が存在することがある…
解が求まらないことがある…
𝑥𝑥0, 𝑦𝑦0

29. OK OK
順運動学と逆運動学
座標系 Σ0 から Σ1 への座標変換
1
0
𝑻𝑻 𝜃𝜃0 = 1
0
𝑹𝑹 𝜃𝜃0
0
𝒑𝒑1 𝜃𝜃0
𝟎𝟎 1
1
0
𝑹𝑹 𝜃𝜃0 =
𝑟𝑟11 𝜃𝜃0 𝑟𝑟12 𝜃𝜃0 𝑟𝑟13 𝜃𝜃0
𝑟𝑟21 𝜃𝜃0 𝑟𝑟22 𝜃𝜃0 𝑟𝑟23 𝜃𝜃0
𝑟𝑟31 𝜃𝜃0 𝑟𝑟32 𝜃𝜃0 𝑟𝑟33 𝜃𝜃0
・・・Σ0 から見た Σ1 の姿勢
0
𝒑𝒑1 𝜃𝜃0 = 0
𝑥𝑥1
0
𝑦𝑦1
0
𝑧𝑧1
𝑇𝑇
・・・Σ0 から見た Σ1 の位置
座標系 Σ0 から Σ𝑛𝑛 への座標変換
1
0
𝑻𝑻 𝜃𝜃0, … , 𝜃𝜃𝑛𝑛−1 = 1
0
𝑻𝑻 𝜃𝜃0 2
1
𝑻𝑻 𝜃𝜃1 … 𝑛𝑛
𝑛𝑛−1𝑻𝑻 𝜃𝜃𝑛𝑛−1 …𝑛𝑛個の未知数，12本の方程式
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
29
Σ0
Σ1
Σ𝑛𝑛

30. OK OK
逆運動学 / Inverse Kinematics (IK)
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
30
Joint 1
Joint 2
Joint 3 Joint 4
Joint 5
Joint 6
• 解析的解法
解ける問題だけ解く
→ 解けるようなリンク構造
• 数値的解法
RRT，モンテカルロ法など
• 逐次的解法
̇
𝜽𝜽 = 𝑱𝑱+ ̇
𝒙𝒙 ⟹ ∆𝜽𝜽 = 𝑱𝑱+
∆𝒙𝒙
𝑱𝑱: ヤコビ行列，𝑱𝑱+
: 擬似逆行列 �
= 𝑱𝑱𝑇𝑇 𝑱𝑱𝑱𝑱𝑇𝑇 −1
（関節数 > 自由度）
= 𝑱𝑱𝑇𝑇
𝑱𝑱
−1
𝑱𝑱𝑇𝑇
（関節数 < 自由度）

31. OK OK 31/6
4
RRTによる逆運動学解法
Random sampling
Forward kinematics
check
previous
joint variables 𝜽𝜽𝒊𝒊
Goal
joint variables 𝜽𝜽𝑖𝑖+1
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」

32. OK OK
課題：特異点，特異姿勢 / Singularity point
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
32
https://www.youtube.com/watch?v=6Wmw4lUHlX8

33. OK OK
課題：姿勢表現の難しさ
オイラー角（各座標軸周りの回転角）は
 回転の順序によって最終的な姿勢が異なる
 姿勢によっては回転自由度が欠落する
種類が多すぎる
• XYZ固定角
• ZYZオイラー角
• ZYXオイラー角，etc.
• 等価回転軸・回転角
（単一回転であらゆる姿勢を表現）
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
33
�
𝒌𝒌
𝜙𝜙

34. OK OK
課題：姿勢表現の難しさ
クォータニオン / Quaternion
𝒒𝒒 = 𝜖𝜖1 𝜖𝜖2 𝜖𝜖3 𝜖𝜖4
𝑇𝑇
𝒒𝒒 = 𝜖𝜖1
2
+ 𝜖𝜖2
2
+ 𝜖𝜖3
2
+ 𝜖𝜖4
2
= 1
→ 4次元単位超球上の1点
半球内で同一姿勢を取らない
（点対称な点は同一姿勢を表す）
ランダムサンプリングしやすい
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
34
𝜖𝜖1
𝜖𝜖2
𝜖𝜖3
𝜖𝜖4
𝒒𝒒
Kuffner, J. J. (2004). Effective Sampling and Distance Metrics for 3D Rigid Body
Path Planning. IEEE Int’l Conf. on Robotics and Automation, April, 3993–3998.
�
𝒌𝒌
𝜙𝜙

35. OK OK
Searching in joint space
• 関節可動域内であれば経由点間を到達可能
• 直交空間においては湾曲した経路をとる
（回転関節の場合）
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
35
𝜃𝜃1
𝜃𝜃2

36. OK OK
直交空間と関節空間の探索の組み合わせ
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
36
挿入時は
位置決めの
拘束が厳しい

37. OK OK
Manipulability / 可操作性
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
37

38. OK OK
Manipulability / 可操作性
ロボットマニピュレータの動作しやすさを示す指標
𝑤𝑤 = det 𝑱𝑱 𝜽𝜽 𝑱𝑱𝑇𝑇 𝜽𝜽
𝑱𝑱 ∈ ℝ𝑚𝑚×𝑛𝑛: ヤコビ行列
𝜽𝜽 ∈ ℝ𝑛𝑛: 関節変数
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
38
𝑛𝑛 DOF manipulator
in 𝑚𝑚-dimensional Euclidean space
Yoshikawa, T. (1985). Manipulability of Robotic Mechanisms.
International Journal of Robotics Research, 4(2), 439–446.

39. OK OK
Considering manipulability
位置・姿勢推定精度は悪くないのに！計画・動作に失敗する問題
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
39
D（Rotate CCW
about Y-axis）10/10
C（Rotate CW
about X-axis）2/10
B（Rotate CCW
about X-axis）7/10
A（Standing vertically）
10/10
H（Rotate about
X-axis and Y-axis）9/10
G（Rotate CW
about Z-axis）9/10
F（Rotate CCW
about Z-axis）9/10
E（Rotate CW
about Y-axis）9/10
relatively low
Depth Camera
Target frame
Sash-like plate
藤木，鉢峰，槇田： ``サッシ様平板の溝はめ込み作業における挿入枠の位置・姿勢推定と挿入動作計画''，
日本ロボット学会誌，Vol. 42，No. 2，pp. 159--167，Apr. 2024.

40. OK OK
Considering manipulability
Pattern
B (Short
distance)
B (Long
distance)
C (Short
distance)
C (Long
distance)
Succeed insertion
motion 53% 83% 33% 87%
Suspend
transition to the
next waypoint
37% 3% 0% 0%
Be aborted during
moving between
waypoints
0% 0% 0% 7%
Fail to plan 10% 13% 67% 0%
Manipulability 0.0116 0.0212 0.0165 0.0226
Short distance
-> Low manipulability
Long distance
-> High manipulability
High manipulability leads higher success rates and fewer failures.
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
40

41. OK OK
• Workspace の確保
可操作性
ロボットのリーチング
可搬重量（姿勢依存）
特異点回避
ビジョン・測距センサの
計測レンジ
モーションプランニングする前に
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
41
F. Suárez-Ruiz and Q. -C. Pham: “A framework for fine robotic assembly,”
Proc. of IEEE Int. Conf. on Robotics and Automation, pp. 421–426, 2016.

42. OK OK
Planning for robotic manipulation
• どのレベルのプランニング？
Task and symbolic planning
Motion planning
Force and torque control, state classification and detection
• Kinematics and workspace
探索空間と自由度の決定
Forward and inverse kinematics
Manipulability
Singularity
ROS Japan UG #55 Planner特集！「ロボットマニピュレーションの作業・動作計画」
42
What to do?
How to move?
Enjoy motion planning!