def testInterface(show=False):
p2 = irispy.Polyhedron()
p2.setA(np.eye(2))
p2.setB(np.array([3.0, 4.0]))
print(p2.contains(np.array([2.5, 5.5]), 0.0))
p3 = irispy.Polyhedron.fromBounds([-1, -1], [2, 2])
problem = irispy.IRISProblem(2)
problem.setBounds(irispy.Polyhedron.fromBounds([-1, -1], [2, 2]))
problem.setSeedPoint(np.array([0.0, 0.0]))
problem.addObstacle(np.array([[1.5, 2], [1.5, 2]]))
region = irispy.iris_wrapper.inflate_region(problem, irispy.IRISOptions())
print(region)
print(region.getPolyhedron().generatorPoints())
print(region.getEllipsoid().getC())
print(region.getEllipsoid().getD())
import matplotlib.pyplot as plt
region.polyhedron.draw2d()
region.ellipsoid.draw2d()
plt.gca().set_xlim([-1.5, 2.5])
plt.gca().set_ylim([-1.5, 2.5])
if show:
plt.show()
def test_constructor(self):
p = irispy.Polyhedron()
A = np.zeros((2, 2))
# print A
p.setA(A)
A2 = p.getA()
A2[0, 0] = 1
self.assertAlmostEqual(p.getA()[0, 0], 0.0)
def test_inner_ellipsoid(self):
# polyhedron from [-1.3, -1.4] to [1.1, 1.2]
# center is at [-0.1, -0.1]
A = np.vstack((np.eye(2), -np.eye(2)))
b = np.array([1.1, 1.2, 1.3, 1.4])
p = irispy.Polyhedron(A, b)
e = irispy.inner_ellipsoid(p)
self.assertTrue(np.allclose(e.getD(), [-0.1, -0.1]))
self.assertTrue(np.allclose(e.getC(), np.array([[1.2, 0], [0, 1.3]])))
def test_plotting_3d(self):
fig = plt.figure()
ax = a3.Axes3D(fig)
p = irispy.Polyhedron()
A = np.vstack((np.eye(3), -np.eye(3)))
b = np.array([1.1, 1.2, 1.3, 1.4, 1.5, 1.6])
p.setA(A)
p.setB(b)
p.draw(ax)
ax.relim()
ax.autoscale_view()
def test_plotting(self):
fig = plt.figure()
ax = fig.add_subplot(1, 1, 1)
p = irispy.Polyhedron()
A = np.vstack((np.eye(2), -np.eye(2)))
b = np.array([1.1, 1.2, 1.3, 1.4])
p.setA(A)
p.setB(b)
p.draw(ax, alpha=0.5)
ax.relim()
ax.autoscale_view()
def sample_convex_polytope(A, b, nsamples):
poly = irispy.Polyhedron(A.shape[1])
poly.setA(A)
poly.setB(b)
generators = np.vstack(poly.generatorPoints())
lb = np.min(generators, axis=0)
ub = np.max(generators, axis=0)
n = 0
samples = np.zeros((len(lb), nsamples))
while n < nsamples:
z = np.random.uniform(lb, ub)
if np.all(poly.A.dot(z) <= poly.b):
samples[:, n] = z
n += 1
return samples
def test_debug_data():
import matplotlib.pyplot as plt
obstacles = [np.array([[0.3, 0.5, 1.0, 1.0], [0.1, 1.0, 1.0, 0.0]])]
bounds = irispy.Polyhedron()
bounds.setA(np.vstack((np.eye(2), -np.eye(2))))
bounds.setB(np.array([2.0, 2, 2, 2]))
start = np.array([0.1, -0.05])
# print "running with debug"
region, debug = irispy.inflate_region(obstacles,
start,
bounds=bounds,
return_debug_data=True)
# print "done"
debug.animate(pause=0.5, show=False)
def test_generators(self):
p = irispy.Polyhedron()
A = np.vstack((np.eye(2), -np.eye(2)))
b = np.array([1.1, 1.2, 1.3, 1.4])
p.setA(A)
p.setB(b)
points = p.generatorPoints()
expected = [
np.array([1.1, 1.2]),
np.array([-1.3, 1.2]),
np.array([-1.3, -1.4]),
np.array([1.1, -1.4])
]
found_expected = [False for i in expected]
for point in points:
for i, ex in enumerate(expected):
if np.all(np.abs(point.T - ex) < 1e-3):
found_expected[i] = True
self.assertTrue(all(found_expected))
def lcon_to_vert(A, b):
poly = irispy.Polyhedron(A.shape[1])
poly.setA(A)
poly.setB(b)
V = np.vstack(poly.generatorPoints()).T