Exemple #1
0
 def setRegion(self, safe_region):
     debug = DebugData()
     pos = safe_region.point
     try:
         xy_verts = safe_region.xy_polytope()
         if xy_verts.shape[1] == 0:
             raise QhullError("No points returned")
         xyz_verts = np.vstack((xy_verts, pos[2] + 0.02 + np.zeros((1, xy_verts.shape[1]))))
         xyz_verts = np.hstack((xyz_verts, np.vstack((xy_verts, pos[2] + 0.015 + np.zeros((1, xy_verts.shape[1]))))))
         # print xyz_verts.shape
         polyData = vnp.getVtkPolyDataFromNumpyPoints(xyz_verts.T.copy())
         vol_mesh = filterUtils.computeDelaunay3D(polyData)
         for j in range(xy_verts.shape[1]):
             z = pos[2] + 0.005
             p1 = np.hstack((xy_verts[:,j], z))
             if j < xy_verts.shape[1] - 1:
                 p2 = np.hstack((xy_verts[:,j+1], z))
             else:
                 p2 = np.hstack((xy_verts[:,0], z))
             debug.addLine(p1, p2, color=[.7,.7,.7], radius=0.003)
         debug.addPolyData(vol_mesh)
         # self.setPolyData(vol_mesh)
         self.setPolyData(debug.getPolyData())
         self.safe_region = safe_region
     except QhullError:
         print "Could not generate convex hull (polytope is likely unbounded)."
Exemple #2
0
 def setRegion(self, safe_region):
     debug = DebugData()
     pos = safe_region.point
     try:
         xy_verts = safe_region.xy_polytope()
         if xy_verts.shape[1] == 0:
             raise QhullError("No points returned")
         xyz_verts = np.vstack((xy_verts, pos[2] + 0.02 + np.zeros(
             (1, xy_verts.shape[1]))))
         xyz_verts = np.hstack((xyz_verts,
                                np.vstack(
                                    (xy_verts, pos[2] + 0.015 + np.zeros(
                                        (1, xy_verts.shape[1]))))))
         # print xyz_verts.shape
         polyData = vnp.getVtkPolyDataFromNumpyPoints(xyz_verts.T.copy())
         vol_mesh = filterUtils.computeDelaunay3D(polyData)
         for j in range(xy_verts.shape[1]):
             z = pos[2] + 0.005
             p1 = np.hstack((xy_verts[:, j], z))
             if j < xy_verts.shape[1] - 1:
                 p2 = np.hstack((xy_verts[:, j + 1], z))
             else:
                 p2 = np.hstack((xy_verts[:, 0], z))
             debug.addLine(p1, p2, color=[.7, .7, .7], radius=0.003)
         debug.addPolyData(vol_mesh)
         # self.setPolyData(vol_mesh)
         self.setPolyData(debug.getPolyData())
         self.safe_region = safe_region
     except QhullError:
         print "Could not generate convex hull (polytope is likely unbounded)."
Exemple #3
0
 def drawContactVolumes(self, footstepTransform, color):
     volFolder = getWalkingVolumesFolder()
     for zs, xy in self.contact_slices.iteritems():
         points0 = np.vstack((xy, zs[0] + np.zeros((1,xy.shape[1]))))
         points1 = np.vstack((xy, zs[1] + np.zeros((1,xy.shape[1]))))
         points = np.hstack((points0, points1))
         points = points + np.array([[0.05],[0],[-0.0811]])
         points = points.T
         polyData = vnp.getVtkPolyDataFromNumpyPoints(points.copy())
         vol_mesh = filterUtils.computeDelaunay3D(polyData)
         obj = vis.showPolyData(vol_mesh, 'walking volume', parent=volFolder, alpha=0.5, visible=self.show_contact_slices, color=color)
         obj.actor.SetUserTransform(footstepTransform)
Exemple #4
0
 def drawContactVolumes(self, footstepTransform, color):
     volFolder = getWalkingVolumesFolder()
     for zs, xy in self.contact_slices.iteritems():
         points0 = np.vstack((xy, zs[0] + np.zeros((1,xy.shape[1]))))
         points1 = np.vstack((xy, zs[1] + np.zeros((1,xy.shape[1]))))
         points = np.hstack((points0, points1))
         points = points + np.array([[0.05],[0],[-0.0811]])
         points = points.T
         polyData = vnp.getVtkPolyDataFromNumpyPoints(points.copy())
         vol_mesh = filterUtils.computeDelaunay3D(polyData)
         obj = vis.showPolyData(vol_mesh, 'walking volume', parent=volFolder, alpha=0.5, visible=self.show_contact_slices, color=color)
         obj.actor.SetUserTransform(footstepTransform)
def drawPolytope(msg):
    _id = msg.id
    if msg.remove:
        om.removeFromObjectModel(om.findObjectByName("IRIS polytopes"))
        return
    if msg.highlighted:
        color = [0.8, 0.2, 0.2]
    else:
        color = [0.8, 0.8, 0.8]
    name = "polytope {:d}".format(_id)
    obj = om.findObjectByName(name)
    if obj:
        om.removeFromObjectModel(obj)
    V = np.array(msg.V)
    polyData = vnp.numpyToPolyData(V.T.copy())
    vol_mesh = filterUtils.computeDelaunay3D(polyData)
    debug = DebugData()
    debug.addPolyData(vol_mesh)
    vis.showPolyData(debug.getPolyData(), name, color=color, alpha=0.4, parent="IRIS polytopes")
def drawPolytope(msg):
    _id = msg.id;
    if msg.remove:
        om.removeFromObjectModel(om.findObjectByName('IRIS polytopes'))
        return
    if msg.highlighted:
        color = [.8,.2,.2]
    else:
        color = [.8,.8,.8]
    name = 'polytope {:d}'.format(_id)
    obj = om.findObjectByName(name)
    if obj:
        om.removeFromObjectModel(obj)
    V = np.array(msg.V)
    polyData = vnp.numpyToPolyData(V.T.copy())
    vol_mesh = filterUtils.computeDelaunay3D(polyData)
    debug = DebugData()
    debug.addPolyData(vol_mesh)
    vis.showPolyData(debug.getPolyData(), name, color=color, alpha=0.4, parent='IRIS polytopes')