# Exam ## Motion Planning, Navigation - motion planning - global map, off-line, strategic resoning - motion optimality - convergence to the goal - no reactivity - reactive & end-to-end navigation - local sensor map, on-line, tactical reasoning - motion optimality? - convergence to the goal? - reactivity ### Workspace, Configuration Space - motion planning - feasible plan = takes the capabilities of the robot into account - motion strategies can have different forms – it can be a path, a set of procedural instructions - for a walking humanoid robot, the plan would be quite complex - motion planning vs. navigation (obstacle avoidance etc.) - motion planning – global map, strategic level - obstacle avoidance – local map, tactical level - workspace - physical space where the robot lives, usually $\mathbb R^2$ or $\mathbb R^3$ - obstacles, free space - types of models - continuous metric model - polygons – their vertices - memory complexity proportional to the number of obstacles - discrete metric model - pixels: free, fully occupied, partially occupied - memory complexity proportional to the size of workspace and the resolution - topological model - nodes and edges - we don't care about the geometry – we only capture places and ways to move from one to another - the robot needs to be able to localize itself (what is the current “place”) and to get from one place to another according to the edges in the graph - hybrid - selection criteria: sensors available, precision required, complexity (memory requirements) - two classes of kinematic constraints - holonomic – restrict the set of possible position (like obstacles) - examples - limitations of the robotic arm due to its joints etc. - robotic hand with glass of water has to be upright – we limit the hand to some angles - to account for the constraints, we only add some virtual obstacles - nonholonomic – restrict the set of possible differential motions - example: a wheel rolling without slipping on the plane (you should move in a direction perpendicular to the axis of the wheel) - **configuration** - fundamental tool to address motion planning - *set of independent parameters uniquely specifying the position and orientation of every component of a robotic system relative to a fixed coordinate system* - usually expressed as a vector of positions/orientations - configuration space = space of all the configuration - example: polygonal robot which can only translate (no rotation) - we pick a reference point $R$ - $q=(q_x,q_y)$ … configuration (coordinates of $R$) - $C=\mathbb R^2$ … configuration space - $W=\mathbb R^2$ … workspace - example: robot which can also rotate - $q=(q_x,q_y,\theta)$ - $C=\mathbb R^2\times S^1$ - $S^1$ … $[0,2\pi[$ (to some extent) - we could also use $[-\pi,\pi[$ or something similar - example: free-flying robot in 3D - $q=(q_x,q_y,q_z,\theta_\alpha,\theta_\beta,\theta_\gamma)$ - Euler angles (roll, pitch, yaw) - example: robotic arm with two joints - $q=(\theta_1,\theta_2)$ - $C=S^2$ - example: mobile robot (car) with an arm with two joints - $q=(x,y,\theta,\theta_1,\theta_2)$ - configuration space vs. workspace - example: robotic arm with two joints - base of the robot can be put anywhere → workspace … $\mathbb R^2$ - $C=S^2$ is bounded - $S^2$ topology = sphere - but we cannot uniquely convert the top of the sphere to two angles → we need to use a torus - $C=S^1\times S^1$ - what is the shortest path between two configurations in $S^1\times S^1$? - in Cartesian space, it would be easy - we need a metric - configuration space path - path $\pi:[0,1]\to C$ … continuous sequence of configurations - trajectory … time-parametrized path - configuration types - free, collision, contact - contact configurations are interesting if you want to pick up something with a robotic arm - image of an obstacle in a $C$ space becomes much more complicated - we need to capture the structure of the workspace, compute forbidden regions - in the workspace, the robot is a body × in the configuration space, the robot is a point ### Path Planning - path planning - input - robot model (configuration space) - start, goal (configurations) - workspace model (obstacles, …) - output – path - completeness issue - complete algorithm: finds a solution if one exists, reports failure if not - complexity of complete path planning: strong evidence that it takes time exponential in $d$, the dimension of the configuration space $C$ - specific complete path planning algorithms have been implemented for $d=$ 2, 3, or 4 - two complete general purpose path planning algorithms have been proposed, none has been implemented - algorithms - complete … finds a solution - heuristic … finds a solution in most cases - probabilistic completeness … probability of finding a solution approaches 1 if given enough time - resolution completeness … complete for a given resolution level - usual methods - exploring a search graph - incrementally building a search tree #### Graph-Based Methods - visibility graph - topology of (semifree) C space is described using a network of 1D curves - we connect start and goal to the network, then use graph search - finds shortest path in 2D space (but not in 3D space) - Voronoï diagram - paths maximizing the clearance to the obstacles - mostly in 2D spaces - cellular decomposition - trapezoidal decomposition - how to move from one cell to another? we could use the centroids of the shared sides - cannot be used if we don't have polygonal obstacles - typical problem if the C space is torus - approximate cellular decomposition - discretize the C space - remove the occupied and partially occupied cells - problems: narrow passage - hierarchical cellular decomposition - highly depends on the dimensionality of the C space – the memory requirements grow exponentially - probabilistic roadmap - it's difficult to compute C space obstacles - it's easy to check if a configuration collides or not - also, we can easily check if a line segment collides or not - we randomly sample points, check if they collide and if the lines connecting them collide - this way, we form a network - the algorithm is probabilistically complete - very useful for C space of high dimensionality (like 97 dimensions) #### Tree-Based Methods - grid-based methods - configuration-space lattices - A* - bi-directional search - is somehow better - rapidly-exploring random tree - algorithm - we have a tree - sample a configuration in the whole space - find the closest node of the tree - extend the tree by $\varepsilon$ distance in the direction of the new configuration (if such path is collision-free) - Brownian motion explores circle/sphere - RRT works better - RRT is naturally biased towards large unexplored regions - probabilistic completeness - we need a metric #### Other Methods - navigation function - “feedback motion planning” - navigation function should be smooth, should have global minimum at the goal and no other local minima - gradient descent - potential field - obstacle … repulsive field - goal … attractive field - robot … particle, subject to forces - there can be multiple local minima :( - how to avoid getting stuck in a local minimum - we can use “basin of attraction” - or we can try to move randomly out of the basin - path deformation - start from the original path and deform it until it becomes collision-free - ill-suited for dynamic environments - space deformation × time deformation #### Nonholonomic Constraints - non integrable - example in $R^2\times S^1$ - $\dot x\sin\theta-\dot y\cos\theta=\theta$ - “the wheel has to always move in this direction” - nonholonomic system: the dimension of the control space is less than the dimension of the configuration space - controllability - locally controllable system - small-time locally controllable system - steering - no general method - we can use circles for a car - untractable problem, we need to decouple geometric and kinematic constraints - topological property - the space we need to maneuver - we want to get from the middle of the blue circle to every point in the blue circle - to get there, we may need to exist the blue circle but we always stay in the pink circle - using topological property - we have a geometrically feasible path - we find the largest pink circle that does not collide with any obstacles on the path - from the size of the pink circle, we compute the blue circle and “tile” the path using blue circles - Reeds & Shepp optimal paths - steering method – verifying the topological property - holonomic path approximation - we apply postprocessing to remove weird parts of the path - graph-based nonholonomic planner - we use PRM, but connect the points using the steering method - can we change RRT to use it to plan paths for cars? - yes, but the epsilon advancement has to be feasible – so we can use the steering method or choose from all the possible epsilon advancements the closest one to the sampled point ### Obstacle Avoidance - reactive navigation (curve in the C space connecting A and B) - robot model - local map (sensor data) - nominal motion/goal - bug 1 (Lumelski 86) - algorithm - robot moves to goal until obstacle - fully circumnavigate obstacle - circumnavigate again to boundary closest to the goal - repeat until goal is reached - low memory requirements - path can be very long - converges to the goal - bug 2 - we use a line towards the goal - convergency does not directly follow from the algorithm - potential field - handling local minima – random motion - vector field histogram - occupancy probability (in a grid) - detect candidate openings & select the best one (depending on the target direction and the robot heading) - dynamic window - translational and rotational velocities - takes into account the acceleration capabilities of the robot - velocity obstacles - takes into account moving obstacles - end-to-end navigation - we don't separate perception, localization, planning, and control - instead, the robot learns a mapping between sensor data and actions - supervised learning – ALVINN - reinforcement learning - policy learned through trial-and-error - training in simulation - dynamic programming, monte carlo, Q-learning, SARSA, DDPG, … - LLM-based navigation - GPT-driver - how to combine end-to-end techniques with the more robust ones? - safety filters? ### Safety - we'll focus on misreasoning in dynamic environments (one of the causes of collisions) - can motion safety be guaranteed? - in dynamic environments - we reason about the future - we are limited by the decision time - appropriate time horizon – how far do we look in the future? - inevitable collision states (ICS) - “I will be in collision in the future no matter what I do” - we need to stay away from ICS - obstacles are not independent! - in general, we need an infinite time horizon - the time horizon and the decision time are determined by the environment - if the time horizon is infinite, we cannot guarantee absolute motion safety - modeling the future (forbidden regions) - deterministic – we know where the obstacles end up - conservative – are not sure, so we let the forbidden regions grow - problem: if they grow infinitely → no collision-free path in the future (with an infinite time horizon) - probabilistic - no obstacles in the far future - passive motion safety - should a collision take place, the robot will be at rest - now, we don't need an infinite time horizon - everybody enforces it → no collision at all