Update Project 3: Control.

Add more MPC tests to identify rollout issues.

Fix minor bug in path_sender when resetting initial car state.

control/scripts/path_sender

@@ -21,10 +21,9 @@ path_generators = {
     "saw": saw,
 }
 
-
 if __name__ == "__main__":
     # anonymous so multiple can run at the same time
-    rospy.init_node("controller_runner", [], anonymous=True)
+    rospy.init_node("path_sender", [], anonymous=True)
 
     parser = argparse.ArgumentParser()
     parser.add_argument(
@@ -83,11 +82,8 @@ if __name__ == "__main__":
     )
 
     # roslaunch passes additional arguments to <node> executables, so we'll
-    # gracefully ignore those
-    args, unknown = parser.parse_known_args()
-    if args.reset:
-        # reset car pose to (0, 0, 0)
-        rospy.ServiceProxy("/mushr_sim/reposition", CarPose)("car", 0, 0, 0)
+    # gracefully discard those
+    args = parser.parse_args(rospy.myargv()[1:])
 
     # Determine the args to pass through
     extra_args = vars(args).copy()
@@ -95,8 +91,16 @@ if __name__ == "__main__":
     del extra_args["reset"]
     del extra_args["path_name"]
     extra_args = {key: value for key, value in extra_args.items() if value is not None}
+    start_config = (0, 0, 0)
     waypoints = path_generators[args.path_name](**extra_args)
 
+    if args.reset:
+        # reset car pose
+        rospy.ServiceProxy("/mushr_sim/reposition", CarPose)("car", *start_config)
+        # HACK(nickswalker5-12-21): Sim doesn't wait for reposition to propagate
+        # before returning from service
+        rospy.sleep(1)
+
     h = Header()
     # Our generated paths start at (0, 0, 0), so let's make the frame correspond
     # to the current base_footprint, aka the frame where the current position is

control/test/mpc.py

 #!/usr/bin/env python
-
+from __future__ import division
 import numpy as np
 import rosunit
 import unittest
@@ -70,10 +70,13 @@ class TestMPCController(unittest.TestCase):
         params["K"] = 10
         params["T"] = 4
         controller = ModelPredictiveController(**params)
+        dt = 0.1
 
-        pose = np.array([0, 0, np.pi / 4])
         controls = controller.sample_controls()
-        rollouts = controller.get_rollout(pose, controls)
+        controls[:, :, 0] = 1.0  # set velocity
+
+        pose = np.array([0, 0, np.pi / 4])
+        rollouts = controller.get_rollout(pose, controls, dt)
         self.assertEqual(
             rollouts.shape,
             (controller.K, controller.T, 3),
@@ -87,6 +90,64 @@ class TestMPCController(unittest.TestCase):
                 err_msg="Rollouts start from the current state",
             )
 
+        translated_pose = np.array([47, 8, np.pi / 4])
+        translated_rollouts = controller.get_rollout(translated_pose, controls, dt)
+        self.assertEqual(
+            translated_rollouts.shape,
+            (controller.K, controller.T, 3),
+            msg="get_rollout should return K T-length state sequences",
+        )
+
+        for k in range(controller.K):
+            np.testing.assert_allclose(
+                translated_rollouts[k, 0, :],
+                translated_pose,
+                err_msg="Rollouts start from the current state",
+            )
+
+        # Compare original rollout to translated rollout
+        np.testing.assert_allclose(
+            translated_rollouts - translated_pose.reshape((1, 1, 3)),
+            rollouts - pose.reshape((1, 1, 3)),
+            err_msg="Rollouts are relative to the current state",
+        )
+
+        diffs = rollouts[:, 1 : controller.T, :] - rollouts[:, 0 : controller.T - 1, :]
+        translated_diffs = (
+            translated_rollouts[:, 1 : controller.T, :]
+            - translated_rollouts[:, 0 : controller.T - 1, :]
+        )
+        for t in range(diffs.shape[1]):
+            np.testing.assert_allclose(
+                diffs[:, t, :],
+                controller.motion_model.compute_changes(
+                    rollouts[:, t, :], controls[:, t, :], dt
+                ),
+                err_msg="Difference between rollout states should match compute_changes",
+            )
+            np.testing.assert_allclose(
+                translated_diffs[:, t, :],
+                controller.motion_model.compute_changes(
+                    translated_rollouts[:, t, :], controls[:, t, :], dt
+                ),
+                err_msg="Difference between rollout states should match compute_changes",
+            )
+            self.assertGreater(
+                np.linalg.norm(diffs[:, t, :]),
+                0,
+                msg="compute_changes should be different at each time step",
+            )
+            self.assertGreater(
+                np.linalg.norm(translated_diffs[:, t, :]),
+                0,
+                msg="compute_changes should be different at each time step",
+            )
+        np.testing.assert_allclose(
+            translated_diffs,
+            diffs,
+            err_msg="compute_changes is relative to the rollout state",
+        )
+
     def test_distance_cost(self):
         params = self.defaults.copy()
         params["K"] = 1