Fixed some bugs.
Start new trajectory at current real joint position
Fixed wrong number of joint names in moveRobotToStartPos
Fixed all errors in Gazebo
Fixed some bugs
Fixed constraint computation
Fixed bug with old joint positions
Set transition points to 5 to make transition smoother ans slower