def updatePlanFrames(self):
if self.getViewMode() != 'frames':
return
numberOfSamples = self.getNumberOfSamples()
meshes = self.planPlayback.getPlanPoseMeshes(self.plan, self.playbackJointController, self.playbackRobotModel, numberOfSamples)
d = DebugData()
startColor = [0.8, 0.8, 0.8]
endColor = [85/255.0, 255/255.0, 255/255.0]
colorFunc = scipy.interpolate.interp1d([0, numberOfSamples-1], [startColor, endColor], axis=0, kind='slinear')
for i, mesh in reversed(list(enumerate(meshes))):
d.addPolyData(mesh, color=colorFunc(i))
pd = d.getPolyData()
clean = vtk.vtkCleanPolyData()
clean.SetInput(pd)
clean.Update()
pd = clean.GetOutput()
self.planFramesObj = vis.updatePolyData(d.getPolyData(), 'robot plan', alpha=1.0, visible=False, colorByName='RGB255', parent='planning')
self.showPlanFrames()
def updatePlanFrames(self):
if self.getViewMode() != 'frames':
return
numberOfSamples = self.getNumberOfSamples()
meshes = self.planPlayback.getPlanPoseMeshes(
self.plan, self.playbackJointController, self.playbackRobotModel,
numberOfSamples)
d = DebugData()
startColor = [0.8, 0.8, 0.8]
endColor = [85 / 255.0, 255 / 255.0, 255 / 255.0]
colorFunc = scipy.interpolate.interp1d([0, numberOfSamples - 1],
[startColor, endColor],
axis=0,
kind='slinear')
for i, mesh in reversed(list(enumerate(meshes))):
d.addPolyData(mesh, color=colorFunc(i))
pd = d.getPolyData()
clean = vtk.vtkCleanPolyData()
clean.SetInput(pd)
clean.Update()
pd = clean.GetOutput()
self.planFramesObj = vis.updatePolyData(d.getPolyData(),
'robot plan',
alpha=1.0,
visible=False,
colorByName='RGB255',
parent='planning')
self.showPlanFrames()
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 clear_locator(self):
d = DebugData()
for ship, frame in self._commonships:
d.addPolyData(ship.to_positioned_polydata())
self.locator = vtk.vtkCellLocator()
self.locator.SetDataSet(d.getPolyData())
self.locator.BuildLocator()
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 loadFeet():
meshDir = os.path.join(app.getDRCBase(), 'software/models/atlas_v3/meshes')
meshes = []
for foot in ['l', 'r']:
d = DebugData()
d.addPolyData(io.readPolyData(os.path.join(meshDir, '%s_talus.stl' % foot)))
d.addPolyData(io.readPolyData(os.path.join(meshDir, '%s_foot.stl' % foot)))
meshes.append(d.getPolyData())
return meshes
def loadFeet():
meshDir = os.path.join(app.getDRCBase(), 'software/models/atlas_v3/meshes')
meshes = []
for foot in ['l', 'r']:
d = DebugData()
d.addPolyData(io.readPolyData(os.path.join(meshDir, '%s_talus.stl' % foot)))
d.addPolyData(io.readPolyData(os.path.join(meshDir, '%s_foot.stl' % foot)))
meshes.append(d.getPolyData())
return meshes
def update_locator(self):
"""Updates cell locator."""
d = DebugData()
d.addPolyData(self._world.to_polydata())
for obstacle, frame in self._obstacles:
d.addPolyData(obstacle.to_positioned_polydata())
self.locator = vtk.vtkCellLocator()
self.locator.SetDataSet(d.getPolyData())
self.locator.BuildLocator()
def update_locator(self):
"""Updates cell locator."""
d = DebugData()
d.addPolyData(self._world.to_polydata())
for obstacle, frame in self._obstacles:
d.addPolyData(obstacle.to_positioned_polydata())
self.locator = vtk.vtkCellLocator()
self.locator.SetDataSet(d.getPolyData())
self.locator.BuildLocator()
def loadFootMeshes():
meshes = []
for i in range(0,2):
d = DebugData()
for footMeshFile in _footMeshFiles[i]:
d.addPolyData(ioUtils.readPolyData( footMeshFile , computeNormals=True))
t = vtk.vtkTransform()
t.Scale(0.98, 0.98, 0.98)
pd = filterUtils.transformPolyData(d.getPolyData(), t)
meshes.append(pd)
return meshes
def testCreateForeground(self):
d = DebugData()
dims = np.array([0.2,0.2,0.2])
center = (0,0,dims[2]/2.0)
d.addCube(dims, center, color=[0,1,0])
d.addPolyData(self.backgroundPolyData)
self.foregroundPolyData = d.getPolyData()
if self.vis:
view=self.views['foreground']
vis.updatePolyData(self.foregroundPolyData, 'foreground', view=view, colorByName='RGB255')
view.resetCamera()
def testCreateForeground(self):
d = DebugData()
dims = np.array([0.2,0.2,0.2])
center = (0,0,dims[2]/2.0)
d.addCube(dims, center, color=[0,1,0])
d.addPolyData(self.backgroundPolyData)
self.foregroundPolyData = d.getPolyData()
if self.vis:
view=self.views['foreground']
vis.updatePolyData(self.foregroundPolyData, 'foreground', view=view, colorByName='RGB255')
view.resetCamera()
def update_locator(self):
"""Updates cell locator."""
d = DebugData()
size = self._worldsize
#d.addPolyData(self._world.to_polydata())
for robot, frame in self._robots:
d.addPolyData(
self._get_line(
[robot._initPos[0], robot._initPos[1] + 51, 0],
[robot._target[0] + 30, robot._target[1] + 51, 0],
1,
color=[1, 1, 1]))
d.addPolyData(
self._get_line(
[robot._initPos[0], robot._initPos[1] - 51, 0],
[robot._target[0] + 30, robot._target[1] - 51, 0],
2,
color=[1, 1, 1]))
for ship, frame in self._commonships:
d.addPolyData(ship.to_positioned_polydata())
self.locator = vtk.vtkCellLocator()
self.locator.SetDataSet(d.getPolyData())
self.locator.BuildLocator()
d = DebugData()
d.addCone(origin=(0,0,0), normal=(0,1,0), radius=0.3, height=0.8, color=[1, 1, 0])
show(d, (0, 4, 0))
d = DebugData()
d.addCube(dimensions=[0.8, 0.5, 0.3], center=[0, 0, 0], color=[0, 1, 1])
show(d, (2, 4, 0))
d = DebugData()
d.addPlane(origin=[0, 0, 0], normal=[0, 0, 1], width=0.8, height=0.7, resolution=10, color=[0, 1, 0])
show(d, (4, 4, 0)).setProperty('Surface Mode', 'Surface with edges')
d = DebugData()
d.addCapsule(center=[0, 0, 0], axis=[1, 0, 0], length=1.0, radius=0.1, color=[0.5, 0.5, 1])
show(d, (6, 4, 0))
d = DebugData()
polyData = vnp.numpyToPolyData(np.random.random((1000, 3)))
vnp.addNumpyToVtk(polyData, np.arange(polyData.GetNumberOfPoints()), 'point_ids')
d.addPolyData(polyData)
show(d, (2.5, 5, 0)).setProperty('Color By', 'point_ids')
applogic.resetCamera(viewDirection=[0, 0.1, -1])
app.start()
def getMergedConvexHullsMesh(chulls):
d = DebugData()
for i, chull in enumerate(chulls):
d.addPolyData(chull.convexHull, color=segmentation.getRandomColor())
return d.getPolyData()
def getMergedConvexHullsMesh(chulls):
d = DebugData()
for i, chull in enumerate(chulls):
d.addPolyData(chull.convexHull, color=segmentation.getRandomColor())
return d.getPolyData()
