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)."
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)."
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 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')
