def __convert_cone2d_to_vertices(vertices, rays, BIG_DIST=1000):
if not rays:
return vertices
r0, r1 = __pick_extreme_rays([r[:2] for r in rays])
r0 = array([r0[0], r0[1], 0.])
r1 = array([r1[0], r1[1], 0.])
r0 = r0 / norm(r0)
r1 = r1 / norm(r1)
conv_vertices = [v for v in vertices]
conv_vertices += [v + r0 * BIG_DIST for v in vertices]
conv_vertices += [v + r1 * BIG_DIST for v in vertices]
return conv_vertices
def compute_static_stability_area(self, debug=True):
t0 = time.time()
W_gi = self.compute_gaw_to_gi_matrix()
# Inequalities: A [GAW_1 GAW_2 ...] <= 0
A = block_diag(*[c.gaw_face_world for c in self.contacts])
b = zeros((A.shape[0], 1))
# Equalities: C [GAW_1 GAW_2 ...] + d == 0
C = W_gi[(0, 1, 2, 5), :]
d = -array([0, 0, -self.mass * 9.81, 0])
# H-representation: b - A x >= 0
# ftp://ftp.ifor.math.ethz.ch/pub/fukuda/cdd/cddlibman/node3.html
# input to cdd.Matrix is [b, -A]
M = cdd.Matrix(hstack([b, -A]), number_type='float')
M.rep_type = cdd.RepType.INEQUALITY
M.extend(hstack([d.reshape((4, 1)), C]), linear=True)
P = cdd.Polyhedron(M)
V = array(P.get_generators())
if V.shape[0] < 1:
return []
if debug:
assert all(dot(A, V[1, 1:]) < 1e-2), dot(A, V[1, 1:])
assert norm(dot(C, V[1, 1:]) + d) < 1e-5
# COM position from GAW: [pGx, pGy] = D * [GAW_1 GAW_2 ...]
D = 1. / (-self.mass * 9.81) * vstack([-W_gi[4, :], +W_gi[3, :]])
vertices, _ = project_cdd_output(V, D)
if debug:
print "Static stability polygon (%d vertices) computed in %d ms" \
% (len(vertices), int(1000 * (time.time() - t0)))
return vertices
def project_cdd_output(V, D):
origin = array([0., 0., 0.])
normal = array([0., 0., 1.])
assert norm(cross(normal, [0., 0., 1.])) < 1e-10
vertices, rays = [], []
for i in xrange(V.shape[0]):
if V[i, 0] == 1: # 1 = vertex, 0 = ray
p = dot(D, V[i, 1:])
vertices.append([p[0], p[1], origin[2]])
else:
r = dot(D, V[i, 1:])
rays.append([r[0], r[1], origin[2]])
return vertices, rays
def draw_polygon(env, points, n=None, color=None, plot_type=3, linewidth=1.,
pointsize=0.02):
"""
Draw a polygon defined as the convex hull of a set of points. The normal
vector n of the plane containing the polygon must also be supplied.
env -- openravepy environment
points -- list of 3D points
n -- plane normal vector
color -- RGBA vector
plot_type -- bitmask with 1 for edges, 2 for surfaces and 4 for summits
linewidth -- openravepy format
pointsize -- openravepy format
"""
assert n is not None, "Please provide the plane normal as well"
t1 = array([n[2] - n[1], n[0] - n[2], n[1] - n[0]], dtype=float)
t1 /= norm(t1)
t2 = cross(n, t1)
points2d = [[dot(t1, x), dot(t2, x)] for x in points]
hull = ConvexHull(points2d)
return draw_polyhedron(env, points, color, plot_type, hull, linewidth,
pointsize)