Controls Non Linear

Started implementing the non-linear model following the Pendubot VI
example