Jenseits von
RRTConnect
Plannerwahl in MoveIt 2
November, 2023

Sebastian Jahr
Robotics Engineer
sebastian.jahr@picknik.ai

Inhalt●●●●●2MoveIt!?
Anforderungen an Planer und Pfade
Planungsalgorithmen in MoveIt 2
Parallel Planning
MoveIt 2 Update

Konzept
Robot Description

SENSE

Planning Scene
MonitorSensorData

Planning
Scene
MotionPlan
RequestPlanning Pipeline

PLAN

MotionPlan
Response

Trajectory
Execution

MotionCommands

ACT

Mehr Informationen: https://moveit.picknik.ai/main/index.html
4MoveIt 2 ist ein Open-Source Bewegungsplanung Framework!
Planning Algorithms

Collision Checking

OMPL (Open Motion Planning Library), Pilz Industrial
Motion Planner, CHOMP, STOMP

FCL, Bullet

Inverse Kinematics

Trajectory Generation
TOTG, Ruckig

KDL, IKFast, pick_ik, BioIK, TRAC-IK

Perception

Simulation
Gazebo, Isaac Sim

Point Cloud, Depth Image

Benchmarking
Database support, Planner, IK

5Online Planning
MoveIt Servo, Hybrid PlanningAnforderungen an PlanungsalgorithmenAnforderungen an Planungsalgorithmen● Algorithmus○○○

Schnell
Vollständig (Complete)
Deterministisch● Lösung○○

Zwangsbedingungen (Constraints)■ Frei von Kollisionen■ Ausführbar vom Roboter
Optimal■ kurz, glatt, Abstand von Objekten, minimale Bewegung…

→ Es gibt bisher keinen universellen Planungsalgorithmus
7

Welche Planungsalgorithmen gibt es?

OMPL● Software Bibliothek mit Vielzahl von sampling-based
Planungsalgorithmus (RRTConnect, RRT*, PRM, …)● Findet i.d.R. einen Pfad falls möglich○“Rückgrat” von MoveIt● Unterstützt Constrained Planning unabhängig vomkonkreten Planungsalgorithmus zB für○○

9TCP Zwangsbedingungen zB Schweißen, Polieren etc.
Bewegungen in definierten Volumen aus Sicherheitsgründen

Quelle: https://ompl.kavrakilab.org/

STOMP und CHOMP●CHOMP○○○●STOMP○○

10

Covariant Hamiltonian Optimization for Motion Planning
Plant glatte kollisionsfreie Pfade mit Covariant Gradient
Descent Ansatz
Minimiert Kombi aus Glattheit (Smoothness) und Constraint(z.B. Kollisions) Kostenfunktionen
Stochastic Trajectory Optimization for Motion Planning
Plant glatte kollisionsfreie Pfade mit probabilistischem
Optimierungsalgorithmus → Gradient wird stochastischapproximiert → Kostenfunktion muss nicht
differenzierbar sein

Quellen: Kalakrishnan et al. 2011 (STOMP),Ratliff et al., 2009c (CHOMP)

PILZ Industrial Motion Planner● Generiert Trajektorien für den Endeffektor○○○

LIN → Lineare Interpolation im Kartesischen RaumPTP → Lineare Interpolation im Konfigurations Raum (Joint Space)
CIRC (auch ARC genannt) → Kreisförmige Interpolation im Kartesischen Raum

TCP
Startposition
LIN

PTP
TCP
Endposition

CIRC11

Mehr Infos:
https://ros-planning.github.io/moveit_tutorials/doc/pilz_industrial_motion_planner/pilz_industrial_motion_planner.html

Einen Planer auszuwählen ist hart!
OMPL+++PILZ+++

CHOMP/STOMP

12++Große Auswahl an Samlingbased Planern
Probabilistisch vollständig
Findet relativ schnell und sehrzuverlässig eine Lösung

-Generiert KartesischeTrajektorien
Sehr schnell
Bewegungen typisch fürIndustrielle Arme (LIN, PTP &CIRC)-

Generiert glatte undreproduzierbare Pfade
Kann anwendungsspezifische
Zwangsbedingungen undKostenfunktionen einbeziehen---

Pfade sind oft nicht
optimal und intuitiv
Parametrisierung
erfordert
Expertenwissen

Beachtet keineZwangsbedingungen
Keine kollisionsfreieBewegung➔

Geeignet für komplexe Probleme(Viele Kollisionsobjekte +Zwangsbedingungen)zB Pick & Place in unstrukturierter
Umgebung➔

Typische Industrieanwendungenin strukturierten UmgebungenzB Schweißen, Bewegung Freiräumen
Relativ langsam
Nicht vollständig
Sehr hart zu
parametrisieren➔

Nützlich für optimale BewegungenzB Optimiere Taktzeit

Parallel PlanningMotivation● Oft ist es unklar welcher Planer die (beste)
Lösung berechnet● Manchmal braucht man einen Backup Lösung● Robuster Planer für unterschiedliche Probleme○PTP

Für jedes Problem individuell Algorithmus wählen istmühsamSTOMP

RRTConnect→ Nutze Portfolio von Planungsalgorithmen
14Implementierung
Konfigurierbar!
OMPL Pipeline

MotionPlan
Response

CHOMP Pipeline

MotionPlan
Response

PILZ Pipeline

MotionPlan
Response

Planner Threads

15

Abbruchkriterium

Vector of
MotionPlanResponses

Lösungsauswahl

Beste
MotionPlan
Response

Evaluation
●
●
●
●
●

●

16

10 Benchmark Probleme(Queries)
50 Iterationen pro Problem
1,5s max. Planungszeit
Kürzeste (L1 Norm JointSpace) Lösung gilt als
optimalPlanerportfolio:○ RRTConnect(OMPL)○ LIN (PILZ)○ CHOMP (CHOMP)
Start Pose:Allgemeines Ergebnis

Path Length [rad]1110Overall path lengths

987

617

CHOMP

RRTConnect

67%

100%

Algorithmus

LIN (PILZ)

Parallel Planning

40%100%Problem 7

Path Length [rad]1612

Wenn CHOMP
scheitert wird
RRTConnect
benutzt

8418

CHOMP

RRTConnect

80%

100%Planner

LIN (PILZ)

Parallel Planning

0%100%

Problem 9

Path Length [rad]4030

2010CHOMP

0%

RRTConnect

100%Algorithmus

LIN (PILZ)

Parallel Planning

0%100%→ Parallel Planning niemals besser als der beste Planungsalgorithmus!
19MoveIt 2 Update

Moving MoveIt 2Python Bindings!

TRAC-IK Kinematics
Plugin(Peter David Fagan)

C++ MoveIt 2 Funktionalität
nutzbar in Python

21(TRACLabs)

Newton und SQP Verfahrenbasierter IK Algorithmus

IK cost function API

Isaac Integration(Wyatt Rees)(Marq Rasmussen & Jafar Abdi)

Benutzerdefinierte
Kostenfunktionen für IK
Algorithmen

NVIDIA IsaacSimulationsumgebung
nutzbar mit MoveIt 2

Und mehr inEntwicklung

Hier gehts weiter:

Documentation

DevTool and SDK

Danke!
22