Fixed bug on the trajectory generator. There was a bug in the trajectory generation such that the robot was getting stuck in a configuration without reaching the desired final posture (workaround was to increase the velocity).