Added trajectory output and progress feedback.
Added Trajectory on grid plot
Added trajGrid output and differente optTauMatrix initializtion due to problem applying torque of size "Nan"