def test_dxform(self):
vars = loadmat("data/map1.mat")
map = vars['map']
dx = DXform(map)
dx.plan([50, 35])
path = dx.query([20, 10])
# valid path
self.assertTrue(path is not None)
# valid Nx2 array
self.assertIsInstance(path, np.ndarray)
self.assertEqual(path.shape[1], 2)
# includes start and goal
self.assertTrue(all(path[0,:] == [20,10]))
self.assertTrue(all(path[-1,:] == [50,35]))
# path doesn't include obstacles
for p in path:
self.assertFalse(dx.is_occupied(p))
# there are no gaps
for k in range(len(path)-1):
d = np.linalg.norm(path[k] - path[k+1])
self.assertTrue(d < 1.5)
dx.plot()
dx.plot(path=path)
# ind[ind >= array_shape[0]*array_shape[1]] = -1
# return ind
# def ind2sub(array_shape, ind):
# ind[ind < 0] = -1
# ind[ind >= array_shape[0]*array_shape[1]] = -1
# rows = (ind.astype('int') / array_shape[1])
# cols = ind % array_shape[1]
# return rows, cols
# def col_norm(x):
# y = np.array([])
# if x.ndim > 1:
# x = np.column_stack(x)
# for vector in x:
# y = np.append(y, np.linalg.norm(vector))
# else:
# y = np.linalg.norm(x)
# return y
if __name__ == "__main__":
from roboticstoolbox import loadmat
vars = loadmat("../data/map1.mat")
map = vars['map']
bug = Bug2(map)
# bug.plan()
path = bug.query([20, 10], [50, 35])