def run(self):
radius = self.properties.getProperty('Radius')
thickness = 0.03
folder = om.getOrCreateContainer('affordances')
frame = self.computeValveFrame()
d = DebugData()
d.addLine(np.array([0, 0, -thickness / 2.0]),
np.array([0, 0, thickness / 2.0]),
radius=radius)
mesh = d.getPolyData()
params = dict(radius=radius,
length=thickness,
xwidth=radius,
ywidth=radius,
zwidth=thickness,
otdf_type='steering_cyl',
friendly_name='valve')
affordance = vis.showPolyData(mesh,
'valve',
color=[0.0, 1.0, 0.0],
cls=affordanceitems.FrameAffordanceItem,
parent=folder,
alpha=1.0)
frame = vis.showFrame(frame,
'valve frame',
parent=affordance,
visible=False,
scale=radius)
affordance.actor.SetUserTransform(frame.transform)
affordance.setAffordanceParams(params)
affordance.updateParamsFromActorTransform()
def newMesh():
d = DebugData()
d.addArrow((0,0,0), (0,0,0.3))
pd = d.getPolyData()
meshId = affordanceitems.MeshAffordanceItem.getMeshManager().add(pd)
desc = dict(classname='MeshAffordanceItem', Name='test mesh', Filename=meshId, pose=((0.5,0.0,1.0), (1,0,0,0)))
return affordanceManager.newAffordanceFromDescription(desc)
def updateGeometryFromProperties(self):
d = DebugData()
d.addCube(
self.getProperty("Dimensions"),
(0, 0, 0),
subdivisions=self.getProperty("Subdivisions"),
)
self.setPolyData(d.getPolyData())
def rayDebug(position, ray):
d = DebugData()
d.addLine(position, position + ray * 5.0)
drcView = app.getViewManager().findView("DRC View")
obj = vis.updatePolyData(
d.getPolyData(), "camera ray", view=drcView, color=[0, 1, 0]
)
obj.actor.GetProperty().SetLineWidth(2)
def updateGeometryFromProperties(self):
d = DebugData()
length = self.getProperty("Length")
d.addCapsule(
center=(0, 0, 0),
axis=(0, 0, 1),
length=self.getProperty("Length"),
radius=self.getProperty("Radius"),
)
self.setPolyData(d.getPolyData())
def draw(self):
d = DebugData()
points = [p if p is not None else self.hoverPos for p in self.points]
# draw points
for p in points:
if p is not None:
d.addSphere(p, radius=0.008)
if self.drawLines:
# draw lines
for a, b in zip(points, points[1:]):
if b is not None:
d.addLine(a, b)
# connect end points
if points[-1] is not None and self.drawClosedLoop:
d.addLine(points[0], points[-1])
self.annotationObj = vis.updatePolyData(
d.getPolyData(),
self.annotationName,
parent=self.annotationFolder,
view=self.view,
)
self.annotationObj.setProperty("Color", [1, 0, 0])
self.annotationObj.actor.SetPickable(False)
def drawCenterOfMass(model):
stanceFrame = robotSystem.footstepsDriver.getFeetMidPoint(model)
com = list(model.model.getCenterOfMass())
com[2] = stanceFrame.GetPosition()[2]
d = DebugData()
d.addSphere(com, radius=0.015)
obj = vis.updatePolyData(
d.getPolyData(),
"COM %s" % model.getProperty("Name"),
color=[1, 0, 0],
visible=False,
parent=model,
)
def draw(self):
d = DebugData()
points = list(self.points)
if self.hoverPos is not None:
points.append(self.hoverPos)
# draw points
radius = 5
scale = (2 * self.view.camera().GetParallelScale()) / (
self.view.renderer().GetSize()[1])
for p in points:
d.addSphere(p, radius=radius * scale)
if self.drawLines and len(points) > 1:
for a, b in zip(points, points[1:]):
d.addLine(a, b)
# connect end points
# d.addLine(points[0], points[-1])
if self.annotationObj:
self.annotationObj.setPolyData(d.getPolyData())
else:
self.annotationObj = vis.updatePolyData(
d.getPolyData(),
"annotation",
parent="segmentation",
color=[1, 0, 0],
view=self.view,
)
self.annotationObj.addToView(self.view)
self.annotationObj.actor.SetPickable(False)
self.annotationObj.actor.GetProperty().SetLineWidth(2)
def updateGeometryFromProperties(self):
radius = self.getProperty("Radius")
circlePoints = np.linspace(0, 2 * np.pi,
self.getProperty("Segments") + 1)
spokes = [(0.0, np.sin(x), np.cos(x)) for x in circlePoints]
spokes = [radius * np.array(x) / np.linalg.norm(x) for x in spokes]
d = DebugData()
for a, b in zip(spokes, spokes[1:]):
d.addCapsule(
center=(a + b) / 2.0,
axis=(b - a),
length=np.linalg.norm(b - a),
radius=self.getProperty("Tube Radius"),
)
self.setPolyData(d.getPolyData())
def onSpawnMesh(self):
d = DebugData()
d.addArrow((0, 0, 0), (0, 0, 0.3))
pd = d.getPolyData()
meshId = affordanceitems.MeshAffordanceItem.getMeshManager().add(pd)
pose = transformUtils.poseFromTransform(self.getSpawnFrame())
desc = dict(
classname="MeshAffordanceItem",
Name="mesh",
Filename=meshId,
uuid=newUUID(),
pose=pose,
)
return self.affordanceManager.newAffordanceFromDescription(desc)
def updateCursor(self, displayPoint):
center = self.displayPointToImagePoint(displayPoint,
restrictToImageDimensions=False)
center = np.array(center)
d = DebugData()
d.addLine(center + [0, -3000, 0], center + [0, 3000, 0])
d.addLine(center + [-3000, 0, 0], center + [3000, 0, 0])
self.cursorObj = vis.updatePolyData(d.getPolyData(),
"cursor",
alpha=0.5,
view=self.view)
self.cursorObj.addToView(self.view)
self.cursorObj.actor.SetUseBounds(False)
self.cursorObj.actor.SetPickable(False)
self.view.render()
def updateGeometryFromProperties(self):
filename = self.getProperty("Filename")
if not filename:
polyData = vtk.vtkPolyData()
else:
polyData = self.getMeshManager().get(filename)
if not polyData:
if not os.path.isabs(filename):
filename = os.path.join(director.getDRCBaseDir(), filename)
if os.path.isfile(filename):
polyData = ioutils.readPolyData(filename)
else:
# use axes as a placeholder mesh
d = DebugData()
d.addFrame(vtk.vtkTransform(), scale=0.1, tubeRadius=0.005)
polyData = d.getPolyData()
self.setPolyData(polyData)
def update(self):
t = self.frameProvider.getFrame("MULTISENSE_SCAN")
p1 = [0.0, 0.0, 0.0]
p2 = [2.0, 0.0, 0.0]
p1 = t.TransformPoint(p1)
p2 = t.TransformPoint(p2)
d = DebugData()
d.addSphere(p1, radius=0.01, color=[0, 1, 0])
d.addLine(p1, p2, color=[0, 1, 0])
self.setPolyData(d.getPolyData())
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()
planePoints = segmentation.thresholdPoints(polyData, 'plane_segmentation_labels', [i, i])
if reorientNormals:
normals = vnp.getNumpyFromVtk(planePoints, 'original_normals')
expectedNormal = normals[0]
else:
expectedNormal = None
chulls.append(computePlanarConvexHull(planePoints, expectedNormal=expectedNormal))
if removeGround:
chulls.append(computePlanarConvexHull(groundPoints, expectedNormal=[0,0,1]))
d = DebugData()
for i, result in enumerate(chulls):
planePoints, chull, plane = result.points, result.convexHull, result.plane
c = segmentation.getRandomColor()
vis.showPolyData(planePoints, 'plane %d' % i, color=c)
chull = vis.showPolyData(chull, 'convex hull %d' % i, color=c)
chull.setProperty('Surface Mode', 'Surface with edges')
chull.actor.GetProperty().SetLineWidth(3)
center = segmentation.computeCentroid(chull.polyData)
chullPoints = vnp.getNumpyFromVtk(chull.polyData, 'Points')
d.addLine(plane.GetOrigin(), np.array(plane.GetOrigin()) + 0.005 * np.array(plane.GetNormal()), radius=0.0001, color=[0,0,0])
#d.addArrow(plane.GetOrigin(), np.array(plane.GetOrigin()) + 0.01 * np.array(plane.GetNormal()), headRadius=0.001, tubeRadius=0.0002)
def newWalkingGoal(self, displayPoint, view):
# put walking goal at robot's base
mainLink = drcargs.getRobotConfig(self.robotName)["pelvisLink"]
footFrame = self.robotModel.getLinkFrame(mainLink)
if not footFrame:
print(
"ERROR: The link '{}' provided for the key 'pelvisLink' in the configuration file does not exist in "
"the robot's URDF. Cannot place walking goal.".format(mainLink)
)
return
worldPt1, worldPt2 = vis.getRayFromDisplayPoint(view, displayPoint)
groundOrigin = footFrame.GetPosition()
groundNormal = [0.0, 0.0, 1.0]
selectedGroundPoint = [0.0, 0.0, 0.0]
t = vtk.mutable(0.0)
vtk.vtkPlane.IntersectWithLine(
worldPt1, worldPt2, groundNormal, groundOrigin, t, selectedGroundPoint
)
walkingTarget = transformUtils.frameFromPositionAndRPY(
selectedGroundPoint, np.array(footFrame.GetOrientation())
)
frameObj = vis.updateFrame(
walkingTarget,
self.robotName + " walking goal",
parent="planning",
scale=0.25,
)
frameObj.setProperty("Edit", True)
rep = frameObj.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
frameObj.widget.HandleRotationEnabledOff()
terrain = om.findObjectByName("HEIGHT_MAP_SCENE")
if terrain:
pos = np.array(frameObj.transform.GetPosition())
polyData = filterUtils.removeNonFinitePoints(terrain.polyData)
if polyData.GetNumberOfPoints():
polyData = segmentation.labelDistanceToLine(
polyData, pos, pos + [0, 0, 1]
)
polyData = segmentation.thresholdPoints(
polyData, "distance_to_line", [0.0, 0.1]
)
if polyData.GetNumberOfPoints():
pos[2] = np.nanmax(vnp.getNumpyFromVtk(polyData, "Points")[:, 2])
frameObj.transform.Translate(
pos - np.array(frameObj.transform.GetPosition())
)
d = DebugData()
d.addSphere((0, 0, 0), radius=0.03)
handle = vis.showPolyData(
d.getPolyData(),
"walking goal terrain handle " + self.robotName,
parent=frameObj,
visible=True,
color=[1, 1, 0],
)
handle.actor.SetUserTransform(frameObj.transform)
placer = PlacerWidget(app.getCurrentRenderView(), handle, terrain)
def onFramePropertyModified(propertySet, propertyName):
if propertyName == "Edit":
if propertySet.getProperty(propertyName):
placer.start()
else:
placer.stop()
frameObj.properties.connectPropertyChanged(onFramePropertyModified)
onFramePropertyModified(frameObj, "Edit")
frameObj.connectFrameModified(self.onWalkingGoalModified)
def makeSphere(self, position, radius=0.0075):
d = DebugData()
d.addSphere(position, radius=radius)
return d.getPolyData()
def makeRobotPolyData():
d = DebugData()
d.addCone(origin=(0,0,0), normal=(1,0,0), radius=0.2, height=0.3)
return d.getPolyData()
def updateGeometryFromProperties(self):
d = DebugData()
d.addSphere((0, 0, 0), self.getProperty("Radius"))
self.setPolyData(d.getPolyData())
def getMergedConvexHullsMesh(chulls):
d = DebugData()
for i, chull in enumerate(chulls):
d.addPolyData(chull.convexHull, color=segmentation.getRandomColor())
return d.getPolyData()
