Commit 70829590 authored by Brian Hou's avatar Brian Hou
Browse files

Update Project 3: Control.

Add more MPC tests to identify rollout issues.

Fix minor bug in path_sender when resetting initial car state.
parent e4e882c7
Pipeline #421347 failed with stage
in 9 minutes and 9 seconds
......@@ -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
......
#!/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
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment