Correct interpolation to use both points

Author: mikelikesrobots

example_7/controller/r6bot_controller.cpp

@@ -125,12 +125,12 @@ void interpolate_point(
 {
   for (size_t i = 0; i < point_1.positions.size(); i++)
   {
-    point_interp.positions[i] = delta * point_2.positions[i] + (1.0 - delta) * point_2.positions[i];
+    point_interp.positions[i] = delta * point_2.positions[i] + (1.0 - delta) * point_1.positions[i];
   }
   for (size_t i = 0; i < point_1.positions.size(); i++)
   {
     point_interp.velocities[i] =
-      delta * point_2.velocities[i] + (1.0 - delta) * point_2.velocities[i];
+      delta * point_2.velocities[i] + (1.0 - delta) * point_1.velocities[i];
   }
 }