def _add_obstacle(self, geom):
    obstacles = np.matrix('-0.576036866359447, 0.918128654970760, 1;                        -0.806451612903226,-1.07017543859649, 1;                        1.01843317972350,-0.988304093567252, 1;                        0.640552995391705,0.906432748538011, 1;                        -0.576036866359447, 0.918128654970760, -1;                        -0.806451612903226,-1.07017543859649, -1;                        1.01843317972350,-0.988304093567252, -1;                        0.640552995391705,0.906432748538011, -1')
    body = RaveCreateKinBody(self._env, '')
    vertices = np.array(obstacles)
    indices = np.array([[0, 1, 2], [2, 3, 0], [4, 5, 6], [6, 7, 4], [0, 4, 5], [0, 1, 5], [1, 2, 5], [5, 6, 2], [2, 3, 6], [6, 7, 3], [0, 3, 7], [0, 4, 7]])
    body.InitFromTrimesh(trimesh=TriMesh(vertices, indices), draw=True)
    body.SetName(self.name)
    for link in body.GetLinks():
        for geom in link.GetGeometries():
            geom.SetDiffuseColor((0.9, 0.9, 0.9))
    self.env_body = body
    self._env.AddKinBody(body)
def _add_obstacle(self, geom):
    obstacles = np.matrix('-0.576036866359447, 0.918128654970760, 1;                        -0.806451612903226,-1.07017543859649, 1;                        1.01843317972350,-0.988304093567252, 1;                        0.640552995391705,0.906432748538011, 1;                        -0.576036866359447, 0.918128654970760, -1;                        -0.806451612903226,-1.07017543859649, -1;                        1.01843317972350,-0.988304093567252, -1;                        0.640552995391705,0.906432748538011, -1')
    body = RaveCreateKinBody(self._env, )
    vertices = np.array(obstacles)
    indices = np.array([[0, 1, 2], [2, 3, 0], [4, 5, 6], [6, 7, 4], [0, 4, 5], [0, 1, 5], [1, 2, 5], [5, 6, 2], [2, 3, 6], [6, 7, 3], [0, 3, 7], [0, 4, 7]])
    body.InitFromTrimesh(trimesh=TriMesh(vertices, indices), draw=True)
    body.SetName(self.name)
    for link in body.GetLinks():
        for geom in link.GetGeometries():
            geom.SetDiffuseColor((0.9, 0.9, 0.9))
    self.env_body = body
    self._env.AddKinBody(body)