Given:	Θinitial, t0, Θgoal, Θfree
Cost function:	$$J\left(\Theta \right(t\left)\right)=tr\left(J{J}^T\right)$$
Constraints:	$$\begin{array}{c}\Theta \left(t_0\right)={\Theta }_{\mathit{initial}}\hfill \\ \Theta \left(t_f\right)={\Theta }_{\mathit{goal}}\hfill \\ t_0 < t_f\hfill \\ g\left(\Theta \right(t), \tau (t),t) \le 0\hfill \\ h\left(\Theta \right(t), \tau (t),t) = 0\hfill \end{array}$$