fixed trajectory generation error

joint velocity limits obeyed