Motion Planning for Robot Manipulators

Transcription

1 Motion Planning for Robot Manipulators Manuel Bonilla PhD Student 26/05/15 1

2 1. What is the interesting part of Motion planning in Robotics Ok, lets first talk about problem faced in robotics doing motion planning. Robotics includes: Perception: Get information about the environment using sensors (Cameras, Laser ) Planning: Decide the steps to follow in order to perform a task. Execution: Control the robot to perform this action (Prof Bicchi talked about this: Computed torque, Back steeping, Arimoto, Feedforward control, Sliding modes OMG ) 26/05/15 2

3 The Motion Planning Problem Consider a Configura4on Space (CS) as a compact set of q i R d elements called configura4ons. Defining the obstacle region as CS obs CS, CS free CS\ CS obs is the free region. Thus, we need to find a con4nuous path σ:[0,1] CS free {σ(0)= q ini, σ(1)= q final } 26/05/15 3

4 just recalling The three main philosophies for addressing the mo4on planning problem are: Combinatorial planning (exact Planning) Sampling- based planning Ar4ficial poten4al fields methods 26/05/15 4

5 State of the art in combinatorial planning (cell decompositions) Fast Marching Method (FMM) It is based on the solu4on of Eikonal equa4ons over a grid 26/05/15 5

6 FMM: Algorithm Ini4aliza4on U N U N F N U N U 26/05/15 6

7 FMM: Algorithm - II Itera4ons 26/05/15 7

8 State of the art in combinatorial planning (cell decompositions) z 400 z y x Advantages: Op4mality is guaranteed Can be applied over manifolds Disadvantages Resolu4on complete 26/05/15 8 Slow in high dimensional spaces

9 just recalling The three main philosophies for addressing the mo4on planning problem are: Combinatorial planning (exact Planning) Sampling- based planning Ar4ficial poten4al fields methods 26/05/15 9

10 The Configuration Space x 2 Possible configura4ons lies on q=( x 1, x 2 ) R 2 x x 2 2 =L x 2 In this case the topological space S 1 x 1 x 1 26/05/15 10

11 The Configuration Space x 2 x 1 26/05/15 11

12 The Configuration Space Topological Space CS = S 1 S 1 = T 2 26/05/15 12

13 The Configuration Space Typical Examples S 1 S 1 S 1 (n 4mes) = T n S n S 1 S 1 S 1 SO(3) SE(2) R 3 SE(3) R 6 R 1 and SO(2) are 1- dimension manifolds R 2, S 2 and T 2 are 2- dimension manifolds R 3 and SE(2) SO(3) and are 3- dimension manifolds R 6, SE(3) and T 6 are 6- dimension manifolds R 26/05/15 13

14 The Configuration Space Typical Examples Robot Type Robot movil with planar translation Robot movil with planar translation and rotation Rigid object with traslations in 3D Free Rigid Object in 3D Robot arm with n articulations CS Representation R 2 SE(2) or R 2 S 1 R 3 SE(3) or R 3 SO(3) T n Planar mobile robot with an Robot arm with n articulations mounted on it SE(2) T n 26/05/15 14

15 Random Sampling-based Methods There exist two main approaches RRT (Rapidly- Exploring Random Trees) PRM (Probabilis4c Roadmap Method) 26/05/15 15

16 What do I need to apply this algorithms? Local mo4on planning policy (gradient- based, interpola4on, op4mal control) Collision detector (PQP, SOLID, V- Clip, Rapid, V- Collide). Example Cost func4on (Metric in topological spaces: Euclidean norm, Manha`an norm, Lp- norm) Sampling Method (Uniform, normal/biased sampling, manipulability) Geometric informa4on about robot and environment (Mesh of links and obstacles) Smoothing func4on 26/05/15 16

17 What is the Idea? The method: Instead of Exploring exhaus4vely all possibili4es, why not exploring randomly a sub- set of these possibili4es but maintaining progress in explora4on. The Cost: Relax Completeness and op4mality of the solu4on The aim: Trade- off among quality of the solu6on and computa6onal 6me 26/05/15 17

18 PRM The method The objects are already augmented using Minkowsky sum, thus the robot can be considered as a point. Cost Func4on: Euclidean Norm Local planning: Interpola4on Kavraki, L. E.; Svestka, P.; Latombe, J.- C.; Overmars, M. H. (1996), "Probabilis4c roadmaps for path planning in high- dimensional configura4on spaces", IEEE Transac2ons on Robo2cs and Automa2on 12 (4): /05/15 18

19 PRM The method Explore C S free using a finite number of randomly sampled configura4on 26/05/15 19

20 PRM The method Reject all configura4ons in C S free 26/05/15 20

21 PRM The method Find all feasible connec4ons in all remaining points in C S free Also called learning phase Roadmap is done!!! Let s make a query 26/05/15 21

22 PRM The method Connect Ini4al and final configura4on to the roadmap Find a path!!! Dijktra s, A* 26/05/15 22

23 PRM Application examples Topology: CS= T 16 Multiple PRM is used to connect the complete space going through singular configurations 26/05/15 23

24 PRM Application examples Finding better grasp configurations Using continuation Methods PRM is performed in the object PRM VIDEOS 26/05/15 24

25 RRT The method Topology of the space : CS= R 2 Lavalle, S.M. (1998). "Rapidly- exploring random trees: A new tool for path planning". Computer Science Dept, Iowa State University, Tech. Rep. TR: /05/15 25

26 RRT The method RRT algorithm using 200 samples RRT algorithm using 600 samples Is the same path!!! 26/05/15 26

27 RRT and PRM Differences RRT Single query : you have to build a tree for each query Fast ini4al solu4on Be`er for dynamic environments Applicable in real time PRM Mul4ple query : If the environment does not change the roadmap is reusable. Slow but the quality of the path is be`er Be`er for sta4c environments This methods deliver a global solution, there are local minima (using uniform distribution) What if we combine them? It depends on your application! 26/05/15 27

28 RRT* Can we Rewire?? RRT* 200 samples Basically you have to apply the A* algorithm in each itera4on Is it be`er?? Sertac Karaman and Emilio Frazzoli. Incremental Sampling- based Algorithms for Op2mal Mo2on Planning, /05/15 28

29 RRT vs. RRT* RRT 200 samples RRT* 200 samples RRT 2000 samples RRT* 2000 samples 26/05/15 29

30 RRT vs. RRT* in T 7 RRT RRT* Time RRT = ~.97sec RRT*=~1.8sec Cost Function Samples /05/15 30

31 5. RRT vs. RRT* 26/05/15 31

32 Summarizing + Posi6ve Issues Probabilis4c Completeness but not determinis4c It is not necessary to build the Configura4on Spaces. Easy applica4on in High- Dimensional Spaces Fast queries - Nega6ve Issues Behavior is not good when narrow passages. Connec4on among nodes is difficult when there exist addi4onal constraints It is difficult to guarantee completeness and op4mality. Remember to use an smoother to apply paths in real 26/05/15 robots 32

33 State of the Art Constrained Mo4on Planning (Projec4on, rejec4on, direct sampling) Kavraki 20001, Cortes 2005, S4lman 2010, S4lman 2013, Berenson 2013 Accelera4on level constraints (Kinodynamic Planning) Lavalle 2001, Masoud 2010 Introduce Compliant (Can we relax constraints through compliance?) Relaxing Constraints Reac4ve Planning 26/05/15 33

34 Considering non-holonomic constraints The difference resides in the local planning policy 26/05/15 34

35 The Motion Planning Problem with constraints Consider a Configura4on Space (CS) R d as a compact set of q i elements called configura4ons. Defining the obstacle region as CS obs CS, CS free CS\ CS obs is the free region. Then a constrained subspace is defined by C S con { q i F( q i )=0}. Thus, we need to find a con4nuous path σ:[0,1] CS valid {σ(0)= q ini, σ(1)= q final } Where C S valid =C S free C S con 26/05/15 35

36 The Motion Planning Problem with constraints 26/05/15 36

37 What is the main problem First C S con is defined by in an implicit func4on F( q i ) describing, thus impossible use direct sampling C S con has lower dimension than CS The probability of sampling a point in C S con is defined by ρ=vol(c S con )/vol(cs), this is zero!!! 26/05/15 37

38 State of The art Random sampling approaches for constrained systems can deal with Nonholonomic Constraints (Lavalle 2000) Closed kinema4c Chains (Cortes 2005) Task Constraints (S4llman 2007) Dynamic Constraints (Masoud 2010) The main methods are: Rejec4on Projec4on Direct Sampling (It requires a splicit parameteriza4on of C S valid 26/05/15 38

39 State fo the Art What are the problems with this approaches? Most (prac4cally all) samples are rejected in method one Projec4ons take long computa4onal 4me PRM and RRT are far from op4mality (unnecessary complex mo4ons typically appear) There is no provision for planning or controlling interac4on forces Good News!!! Most of the 4mes, mo4ons need not to exactly match the plan Indeed, execu4on will not coincide with plans, and environments are not as modeled, so some built- in robustness is mandatory Systems are not rigid indeed, modern robots are rather sor, or even have variable s4ffness 26/05/15 39

40 So what can we do? Relax!!! Instead of planning on F( q i )= 0 we can plan in F( q i ) ε Now there is a narrow but fully dimensional boundary layer. Thus, there is probability of picking a point on the C S valid 26/05/15 40

41 So what can we do? Recently Recenlty Frazzoli in 2013 propose a method to bias new samples to free space using an weighted kd- tree. We can use it to bias new samples to the manifold. 26/05/15 41

42 Adaptive kd-tree 26/05/15 42

43 Example 26/05/15 43

44 What does relaxation means in real robots? 26/05/15 44

45 So we need a Controller Controller task Follow the planned path Project back the configura4ons to the constraint Maintain desired interac4on forces We can do this with Force/Posi4on Controllers There are a lot of approaches to do that (linear, nonlinear, robust, adap4ve, Force- posi4on subspaces, etc.) 26/05/15 45

46 Linear Controller Linearized dynamic equa4ons of the system 26/05/15 46

47 Linear Controller Linearized dynamic equa4ons of the system Rigid Body mo4ons Internal forces 26/05/15 47

48 Linear Controller Linearized dynamic equa4ons of the system Internal forces Rigid Body mo4ons 26/05/15 48

49 Linear Controller Linearized dynamic equa4ons of the system 26/05/15 49

50 The method 26/05/15 50

51 Results 26/05/15 51

52 4 dof Example 26/05/15 52

53 Things to add to randomized planning Exploit Randomized planers (Not to expect just a yes/no answer) Compliance (Bonilla) Planning for contact points (Houser) Minimal Constraint Removal (MCR) problem (Hauser) Uncertain4ty (Zito) Bias for High Dimensional Spaces (S4lman) 26/05/15 53

54 Libraries for Motion Planning OMPL h`p://ompl.kavrakilab.org/ Moveit (ROS Interface for OMPL) h`p://moveit.ros.org/ Klampt h`p://mo4on.pra`.duke.edu/klampt/ Openrave h`p://openrave.org/ There is a repository on github with the kuka nimanual platorm of centro piaggio. Ask for an account to use it. h`p://github.com/centroepiaggio Manue.bonilla@centropiaggio.unipi.it 26/05/15 54