Fix buffer order of acceleration and velocity

This was read in the reverse order from the script. Since the URScript
functions use a first and then v, we should keep the buffer in the same
order instead of switching the order we read from the stream.

Author: urfeex

src/control/trajectory_point_interface.cpp

@@ -78,20 +78,20 @@ bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions,
     }
     for (size_t i = 0; i < positions->size(); ++i)
     {
-      int32_t val = static_cast<int32_t>(round(velocity * MULT_JOINTSTATE));
+      int32_t val = static_cast<int32_t>(round(acceleration * MULT_JOINTSTATE));
       val = htobe32(val);
       b_pos += append(b_pos, val);
     }
     for (size_t i = 0; i < positions->size(); ++i)
     {
-      int32_t val = static_cast<int32_t>(round(acceleration * MULT_JOINTSTATE));
+      int32_t val = static_cast<int32_t>(round(velocity * MULT_JOINTSTATE));
       val = htobe32(val);
       b_pos += append(b_pos, val);
     }
   }
   else
   {
-    b_pos += 6 * sizeof(int32_t);
+    b_pos += 3 * 6 * sizeof(int32_t);
   }
 
   int32_t val = static_cast<int32_t>(round(goal_time * MULT_TIME));