Ejemplo n.º 1
0
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
Ejemplo n.º 2
0
    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
Ejemplo n.º 3
0
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
Ejemplo n.º 4
0
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)