---
title: motion planning
tags: cs 593 robotics
---

## Collision Avoidance

subspace of w-space and c-space that represents free space and space used by an obstacle
w-space -> x_free and x_obs
c-space -> q_free and q_obs

$$H(q,w_{space}) = {0 \text{ or } 1}$$
> functional mapping of x_free and x_obs

separation of axis theorem
- two shapes only intersect if they have overlap when "projected" onto every unique normal axis of both shapes
- method to determine if bodies are colliding

fcl library: https://github.com/flexible-collision-library/fcl
- used for collision detection

## Motion Planning
Definition: to coordinate agent's motion sequence to move from desired start to desired goal while respecting all task constraints

- collision avoidance constraint

$$
min_q \int_0^1H(q(u))du + \phi q(1)
$$

$\phi q(1)$ = weight matrix

$q(0) = q_0$ -> start
$q(1)$ = goal

optimal control: control (MPC, PID - good for local search, but not for long horizon)
ai: optimal search (A*)
numerical optimization optimization: trajectory optimization (gradient descent, struggle with hard constraints)
motion planning: global solutions, algorithmic analysis, computational complexity, completeness guarantee

Sampling-based methods (most used in industry, arguable)

Learning-based SMP

History

### sampling-based methods

#### RRT (rapidly exploring random tree)

T = init_tree(q_start)
for i = 0 to u = N
    q_rand= sample_obs_free(i))
    q_nearest = Nearest(T, q_rand)
    if steer(q_nearest,q_rand):
        T = insert_node(T,q_nearest,q_rand)
return T

def sample_obs_free(i):
    # sampling of points in c_space, independent to tree T

def steer(q_nearest, q_rand):
    # use separation of axis theorem to sample points on the line connecting closest point on tree and see if there is a collision

probabilitistic completeness guarantee: probability of finding a path approaches 1 as samples approaches infinity if one exists

### PRM (probabilistic roadmaps)
multi-query planner

initialize G = (V, E)
for i = 0 to u = N
    q_rand= sample(i)
    q_nearest = select k nearest neighbor (q_rand)
    for all q'in q_nearest do:
        if (q_rand, q') not in E and steerA(q_rand, q'):
            E = E U {(q, q')}

### RRT Connect
- bi-directional version of RRT
- one tree generated from q_initial and one tree generated from q_goal

connect
- finds nearest neighbor in the other tree
- finds steerA (True, False)

initialize $T^a_{in}(q_{in})$,$T^b_{in}(q_{goal})$

for i = 1 to i = N:
    q_rand= sample(i)
    q_nearest = nearestNeig(q_rand, T_a)
    q_new = steerB(q_nearest, q_rand, T_a)
    if q_new != None:
        T_a = (q_new, q_nearest, T_a)
        if connect(q_new, T_b):
            return Path(T_a, T_b)
    swap(T_a, T_b)

> We use this in robot arm, its super fast
thats cool ooh okay now i get to find out what it is haha :smiley: haha cute

### RRT*

T = (E, V)
T = initialize_tree(q_init)
for i = 0 to i = N
    q_rand= sample(i))
    q_nearest = Nearest(T, q_rand)
    if steerA(q_nearest,q_rand):
        q_near = nearNodes(T, q_rand, \V|)
        q_best = chooseBestParent(q_near, q_rand, q_nearest)
        T = insert_node(T,q_best,q_rand)
        T = rewire(T, q_near, q_best, q_rand)
return T

def sample_obs_free(i):
    # sampling of points in c_space, independent to tree T

def steer(q_nearest, q_rand):
    # use separation of axis theorem to sample points on the line connecting closest point on tree and see if there is a collision

### assignment 1
- given code of RRT for 2D point mass
- must do near nodes, choose best parent, and rewire
- circular-rigid body
- planning
- collision detection
- rectangular-rigid body
- implement SAT

RRT* is nlogn computational complexity
asymptotic optimality

### RRT* & RRT
- probabilistic completeness if there exists a path solution
P(T $\cap$ X_goal) = 1
asa n approaches infinity

### limitations
- slow processing
- suffer from curse of dimensionality
- adaptive sampling

**Take home question: Think of unconventional examples of planning**

**Take home question 2: What is the problem with the iron man suit?**