Bugs fixed to make robot have angular velocity. The reflexxes package
will not generate the trajectory point when t=0 (the current position).