Introduction

A motion-planning algorithm evaluates trajectories based on two criteria: 1. Feasible 2. Optimal These approaches can be roughly divided into two categories: 1. Samping-based algorithms 2. Trajectory Optimization algorithms We can formalize it as such: # Definition Given some trajectory $\boldsymbol\theta(t):t\to \mathbb{R}^{D}$ for some $D\in\mathbb{N}$, the objective is to minimize some cost function $\mathcal{F}$ w.r.t. $\boldsymbol\theta$ i.e. $$\begin{align*} \text{minimize}\quad&\mathcal{F}[\boldsymbol\theta(t)]\\ \text{subject to}\quad&\mathcal{G}_{i}[\boldsymbol\theta(t)]\le 0,&i=1,\dots,m_{ineq}\\ &\mathcal{H}_{i}[\boldsymbol\theta(t)]=0,&i=1,\dots,m_{eq} \end{align*}$$where $\mathcal{G}_{i}[\boldsymbol\theta(t)]$ are inequality constraint Functionals such as: ^30a9ac - joint angle limits and $\mathcal{H}_{i}[\boldsymbol\theta(t)]$ are task-dependent equality constraints such as: - desired start and end configurations & velocities - desired end-effector orientation (e.g. holding a cup with water upright)