def setUp(self):
self.n = 3 # length of state
self.center = np.array([1, 2])
self.radius = 2.0
self.obstacle = BufferedSphericalObstacle(self.center, self.radius)
self.Sigma = np.eye(self.n)
BufferedSphericalObstacle.set_buffer_ellipsoid(self.Sigma)
def test_ProjectionBufferEllipsoid(self):
Sigma = np.diag([1, 2, 3])
BufferedSphericalObstacle.set_buffer_ellipsoid(Sigma)
np_testing.assert_almost_equal(
BufferedSphericalObstacle.buffer_ellipsoid[0],
np.array([[0, 1], [1, 0]]))
np_testing.assert_almost_equal(
BufferedSphericalObstacle.buffer_ellipsoid[1], np.array([2, 1]))
def test_distance(self):
BufferedSphericalObstacle.set_buffer_ellipsoid(
np.zeros((self.n, self.n)))
self.assertEqual(self.obstacle.distance(np.array([10, 0, 0]))[0], 0.0)
self.assertEqual(self.obstacle.distance(np.array([3, 2, 3]))[0], 0.0)
self.assertLess(self.obstacle.distance(np.array([1, 1, 3]))[0], 0.0)
BufferedSphericalObstacle.set_buffer_ellipsoid(np.diag([5, 3, 10]))
self.assertEqual(self.obstacle.distance(np.array([1, -3, 0]))[0], 0.0)
self.assertEqual(self.obstacle.distance(np.array([1, -4, 0]))[0], 0.0)
self.assertLess(self.obstacle.distance(np.array([1, -2, 0]))[0], 0.0)
self.assertLess(self.obstacle.distance(np.array([1, 2, 0]))[0], 0.0)
class TestBufferedObstacle(unittest.TestCase):
def setUp(self):
self.n = 3 # length of state
self.center = np.array([1, 2])
self.radius = 2.0
self.obstacle = BufferedSphericalObstacle(self.center, self.radius)
self.Sigma = np.eye(self.n)
BufferedSphericalObstacle.set_buffer_ellipsoid(self.Sigma)
def testBufferEllipsoid(self):
obstacle_new = BufferedSphericalObstacle(np.zeros(2), 5)
self.assertEqual(obstacle_new.buffer_ellipsoid,
self.obstacle.buffer_ellipsoid)
def test_ProjectionBufferEllipsoid(self):
Sigma = np.diag([1, 2, 3])
BufferedSphericalObstacle.set_buffer_ellipsoid(Sigma)
np_testing.assert_almost_equal(
BufferedSphericalObstacle.buffer_ellipsoid[0],
np.array([[0, 1], [1, 0]]))
np_testing.assert_almost_equal(
BufferedSphericalObstacle.buffer_ellipsoid[1], np.array([2, 1]))
def test_mapStateJacobian(self):
np_testing.assert_almost_equal(
self.obstacle.mapStateJacobian(np.array([1, 3, 4])),
np.array([[1, 0, 0], [0, 1, 0]]))
def test_distance(self):
BufferedSphericalObstacle.set_buffer_ellipsoid(
np.zeros((self.n, self.n)))
self.assertEqual(self.obstacle.distance(np.array([10, 0, 0]))[0], 0.0)
self.assertEqual(self.obstacle.distance(np.array([3, 2, 3]))[0], 0.0)
self.assertLess(self.obstacle.distance(np.array([1, 1, 3]))[0], 0.0)
BufferedSphericalObstacle.set_buffer_ellipsoid(np.diag([5, 3, 10]))
self.assertEqual(self.obstacle.distance(np.array([1, -3, 0]))[0], 0.0)
self.assertEqual(self.obstacle.distance(np.array([1, -4, 0]))[0], 0.0)
self.assertLess(self.obstacle.distance(np.array([1, -2, 0]))[0], 0.0)
self.assertLess(self.obstacle.distance(np.array([1, 2, 0]))[0], 0.0)
def test_distance_jacobian(self):
distance_fun = lambda x: self.obstacle.distance(x)[0]
x = np.random.sample(3)
grad = optimize.approx_fprime(x, distance_fun, 1e-6)
dist, jac = self.obstacle.distance(x, True)
np_testing.assert_almost_equal(jac, grad, decimal=4)
dt = 0.02 N = 100 Q = dt * np.zeros(dynamics.n) R = 0.1 * dt * np.eye(dynamics.m) #R[0,0] = 1e-3 # Thrust R[0, 0] = 1 * dt Qf_arr = np.zeros(dynamics.n) Qf_arr[:3] = 100 # Position Qf_arr[3:6] = 100 # Velocity Qf_arr[6:9] = 10 # RPY Qf_arr[9:12] = 1 # 0mega Qf = np.diag(Qf_arr) ts = np.arange(N + 1) * dt # Obstacles ko = 1000 obs1 = BufferedSphericalObstacle(np.array([0.6, 0.6, 0.9]), 0.2) obs2 = BufferedSphericalObstacle(np.array([0.3, 0.3, 0.3]), 0.2) obs_list = [obs1, obs2] # Covariance Sigma0 = 0.001 * np.eye(dynamics.n) Sigma0[:3, :3] = 0.1 * np.eye(3) Sigma_w = 0.1 * np.eye(dynamics.n) # For now # Cost # Add ud with default thrust as gravity maybe xd = np.zeros(dynamics.n) xd[:3] = 1.0 ud = np.zeros(dynamics.m) ud[0] = 10 # Negative of gravity cost = RobustLQRObstacleCost(N, Q, R, Qf, xd, ko=ko, obstacles=obs_list, ud=ud) max_step = 100.0 # Allowed step for control
def createObstacleList(obs_params):
obs_list = []
for i, param in enumerate(obs_params):
radius = max(param[2], 0.1)
obs_list.append(BufferedSphericalObstacle(param[:2], radius))
return obs_list
def testBufferEllipsoid(self):
obstacle_new = BufferedSphericalObstacle(np.zeros(2), 5)
self.assertEqual(obstacle_new.buffer_ellipsoid,
self.obstacle.buffer_ellipsoid)
sns.set(font_scale=1.2)
np.set_printoptions(precision=3, suppress=True)
dynamics = SimpleCarDynamics()
# Trajectory info
dt = 0.1
N = 20
Q = dt*np.zeros(dynamics.n)
R = 0.5*dt*np.eye(dynamics.m)
Qf = 30*np.eye(dynamics.n)
Qf[-1, -1] = 0 # Done care about steering angle
Qf[2, 2] = 0 # Done care about angle
ts = np.arange(N+1)*dt
# Obstacles
ko = 100 # Obstacle gain
obs1 = BufferedSphericalObstacle(np.array([3, 2]), 1)
obs2 = BufferedSphericalObstacle(np.array([6, 5]), 1)
obs_list = [obs1, obs2]
#obs_list = []
# Covariance:
# x, y, theta, v, phi
Sigma0 = np.diag([0.8, 0.8, 0.02, 0.5, 0.1])
Sigma_w = 1*np.diag([0.01, 0.01, 0.001, 0.1, 0.001]) # Multiplied by dt
# Desired terminal condition
xd = np.array([8.0, 8.0, 0.0, 0.0, 0.0])
cost = RobustLQRObstacleCost(N, Q, R, Qf, xd, ko=ko, obstacles=obs_list,
kSigma = 1)
max_step = 0.2 # Allowed step for control
# x,y,theta, v, phi
x0 = np.array([0, 0, 0, 0, 0])