def shrink_polygon(vertices, shrink_ratio, res=10):
"""
Shrink polygon from its Chebyshev center.
Parameters
----------
vertices : list of arrays
Vertices of the initial polygon.
shrink_ratio : scalar
Scalar factor between 0 and 1.
res : int
Number of vertices for the shrunk polygon.
Returns
-------
new_vertices : list of arrays
Vertices of the new polygon.
"""
assert 0. <= shrink_ratio <= 1. and type(res) is int and res > 0
A, b = compute_polytope_halfspaces(vertices)
c = compute_chebyshev_center(A, b)
v = b - dot(A, c)
n = len(v)
new_vertices = []
for ray_index in xrange(res):
theta = ray_index * (2. * pi / res)
r = array([cos(theta), sin(theta)])
u = dot(A, r)
lambda_ = min([v[i] / u[i] for i in xrange(n) if u[i] > 1e-10])
new_vertices.append(c + shrink_ratio * lambda_ * r)
return new_vertices
def sample_points_from_polygon(vertices, nb_points):
A, b = compute_polytope_halfspaces(vertices)
vertices = array(vertices)
p_min = vertices.min(axis=0)
p_max = vertices.max(axis=0)
points = []
while len(points) < nb_points:
p = random.random(2) * (p_max - p_min) + p_min
if (dot(A, p) <= b).all():
points.append(p)
return points
def sample_grid_from_polygon(vertices, res=None, xres=None, yres=None):
xres = res if xres is None else xres
yres = res if yres is None else yres
A, b = compute_polytope_halfspaces(vertices)
vertices = array(vertices)
p_min = vertices.min(axis=0)
p_max = vertices.max(axis=0)
points = []
for x in linspace(p_min[0], p_max[0], xres):
for y in linspace(p_min[1], p_max[1], yres):
p = array([x, y])
if (dot(A, p) <= b).all():
points.append(p)
return points
def compute(self, draw_height=None):
assert len(self.working_set) > 0 and len(self.working_set[0]) == 2
self.all_vertices = []
for i_cur, p_cur in enumerate(self.working_set):
p_cur = array(p_cur)
A_voronoi, b_voronoi = [], []
for i_other, p_other in enumerate(self.working_set):
if i_other == i_cur:
continue
p_other = array(p_other)
p_mid = 0.5 * (p_other + p_cur)
a = p_other - p_cur
A_voronoi.append(a)
b_voronoi.append(dot(a, p_mid))
A_voronoi = vstack(A_voronoi)
b_voronoi = hstack(b_voronoi)
self.stance.com.set_x(p_cur[0])
self.stance.com.set_y(p_cur[1])
self.robot.ik.solve(warm_start=True)
proj_vertices = compute_local_actuation_dependent_polygon(
self.robot, self.stance, method="bretl")
A_proj, b_proj = compute_polytope_halfspaces(proj_vertices)
A = vstack([A_proj, A_voronoi])
b = hstack([b_proj, b_voronoi])
if draw_height is not None and (dot(A, p_cur) > b).any():
self.sample_handles.append(
draw_point([p_cur[0], p_cur[1], draw_height],
color='r',
pointsize=5e-3))
continue
elif draw_height is not None:
self.sample_handles.append(
draw_point([p_cur[0], p_cur[1], draw_height],
color='g',
pointsize=5e-3))
vertices = pypoman.compute_polytope_vertices(A, b)
if draw_height is not None:
self.polygons.append(
draw_horizontal_polygon(vertices,
draw_height,
combined='b-#'))
self.all_vertices.extend(vertices)
return self.all_vertices
import numpy as np
from numpy import array
from pypoman import compute_polytope_halfspaces
import polytope as pc
vertices = map(array, [1, 3, 2])
A, b = compute_polytope_halfspaces(vertices)
print(A, b)
import numpy as np
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d.art3d import Poly3DCollection
P = [[12.31052917, 45.18707857, 0.549995],
[12.31052917, 45.18707857, 0.550005], [12.31052917, 45.192353, 0.549995],
[12.31052917, 45.192353, 0.550005], [12.313735, 45.18707857, 0.549995],
[12.313735, 45.18707857, 0.550005], [12.313735, 45.192353, 0.549995],
[12.313735, 45.192353, 0.550005]]
print(x)
print(y)
print(z)
from mpl_toolkits.mplot3d import Axes3D
from mpl_toolkits.mplot3d.art3d import Poly3DCollection
import matplotlib.pyplot as plt
from matplotlib import cm
# version.
#
# pypoman is distributed in the hope that it will be useful, but WITHOUT ANY
# WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR
# A PARTICULAR PURPOSE. See the GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License along with
# pypoman. If not, see <http://www.gnu.org/licenses/>.
import pylab
from numpy import arange, array, cos, pi, sin
import pypoman
vertices = [(cos(theta), sin(theta)) for theta in arange(0, 2 * pi, pi / 6)]
A, b = pypoman.compute_polytope_halfspaces(vertices)
point = array([2.1, 1.9])
proj = pypoman.project_point_to_polytope(point, (A, b))
if __name__ == "__main__": # plot projected polytope
pylab.ion()
pylab.figure()
pylab.gca().set_aspect("equal")
pypoman.plot_polygon(vertices)
pylab.plot([point[0]], [point[1]], marker='o', markersize=3, color='r')
pylab.plot([proj[0]], [proj[1]], marker='o', markersize=3, color='b')
pylab.plot([point[0], proj[0]], [point[1], proj[1]], 'b--')
pylab.show(block=True)