def draw(self):
d = DebugData()
points = list(self.points)
if self.hoverPos is not None:
points.append(self.hoverPos)
# draw points
for p in points:
d.addSphere(p, radius=5)
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 onMouseMove(self, displayPoint, modifiers=None):
om.removeFromObjectModel(om.findObjectByName('link selection'))
self.linkName = None
self.pickedPoint = None
selection = self.getSelection(displayPoint)
if selection is None:
return
pickedPoint, linkName, normal, pickedCellId = selection
d = DebugData()
d.addSphere(pickedPoint, radius=0.01)
d.addLine(pickedPoint, np.array(pickedPoint) + 0.1 * np.array(normal), radius=0.005)
obj = vis.updatePolyData(d.getPolyData(), 'link selection', color=[0,1,0])
obj.actor.SetPickable(False)
self.linkName = linkName
self.pickedPoint = pickedPoint
self.normal = normal
self.pickedCellId = pickedCellId
modifiers = QtGui.QApplication.keyboardModifiers()
if modifiers == QtCore.Qt.ControlModifier and self.cellCaptureMode:
self.provisionallyCaptureCell(linkName, pickedCellId)
def updateDrawIntersection(self, frame):
origin = np.array(frame.transform.GetPosition())
#print "origin is now at", origin
d = DebugData()
sliderIdx = self.slider.value
controllerType = self.getControllerTypeFromCounter(sliderIdx)
colorMaxRange = self.colorMap[controllerType]
# if the QValue was empty then color it green
if self.emptyQValue[sliderIdx]:
colorMaxRange = [1, 1, 0] # this is yellow
for i in xrange(self.Sensor.numRays):
ray = self.Sensor.rays[:, i]
rayTransformed = np.array(frame.transform.TransformNormal(ray))
#print "rayTransformed is", rayTransformed
intersection = self.Sensor.raycast(
self.locator, origin,
origin + rayTransformed * self.Sensor.rayLength)
if intersection is not None:
d.addLine(origin, intersection, color=[1, 0, 0])
else:
d.addLine(origin,
origin + rayTransformed * self.Sensor.rayLength,
color=colorMaxRange)
vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255')
def updateDrawIntersection(self, frame, locator="None"):
if locator == "None":
locator = self.locator
origin = np.array(frame.transform.GetPosition())
# print "origin is now at", origin
d = DebugData()
sliderIdx = self.slider.value
controllerType = self.getControllerTypeFromCounter(sliderIdx)
colorMaxRange = self.colorMap[controllerType]
for i in xrange(self.Sensor.numRays):
ray = self.Sensor.rays[:, i]
rayTransformed = np.array(frame.transform.TransformNormal(ray))
# print "rayTransformed is", rayTransformed
intersection = self.Sensor.raycast(locator, origin, origin + rayTransformed * self.Sensor.rayLength)
if intersection is not None:
d.addLine(origin, intersection, color=[1, 0, 0])
else:
d.addLine(origin, origin + rayTransformed * self.Sensor.rayLength, color=colorMaxRange)
vis.updatePolyData(d.getPolyData(), "rays", colorByName="RGB255")
def drawActionSetFull(self, go_nowhere=False):
# print "I am drawing the action set"
d = DebugData()
for index, value in enumerate(self.pos_trajectories):
for time_step_index in xrange(2 * self.numPointsToDraw - 1):
firstX = value[0, time_step_index]
firstY = value[1, time_step_index]
firstZ = value[2, time_step_index]
secondX = value[0, time_step_index + 1]
secondY = value[1, time_step_index + 1]
secondZ = value[2, time_step_index + 1]
firstEndpt = (firstX, firstY, firstZ)
secondEndpt = (secondX, secondY, secondZ)
if time_step_index >= 10:
color = [0.8, 0, 0.8]
else:
color = [0.1, 0.1, 1.0]
if go_nowhere:
firstEndpt = [0, 0, 0]
secondEndpt = [0, 0, 0.001]
d.addLine(firstEndpt, secondEndpt, radius=0.02, color=color)
obj = vis.updatePolyData(d.getPolyData(), "action_set", colorByName="RGB255")
def buildWorld():
d = DebugData()
d.addLine((2,-1,0), (2,1,0), radius=0.1)
d.addLine((2,-1,0), (1,-2,0), radius=0.1)
obj = vis.showPolyData(d.getPolyData(), 'world')
return obj
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 buildWorld():
d = DebugData()
d.addLine((2, -1, 0), (2, 1, 0), radius=0.1)
d.addLine((2, -1, 0), (1, -2, 0), radius=0.1)
obj = vis.showPolyData(d.getPolyData(), 'world')
return obj
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 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 handle_message(self, msg):
# Limits the rate of message handling, since redrawing is done in the
# message handler.
self._sub.setSpeedLimit(30)
# Removes the folder completely.
om.removeFromObjectModel(om.findObjectByName(self._folder_name))
# Recreates folder.
folder = om.getOrCreateContainer(self._folder_name)
# Though strangely named, DebugData() is the object through which
# drawing is done in DrakeVisualizer.
d = DebugData()
# Iterate over all triangles, drawing the contact surface as a triangle
# outlined in blue.
for surface in msg.hydroelastic_contacts:
for tri in surface.triangles:
va = np.array([tri.p_WA[0], tri.p_WA[1], tri.p_WA[2]])
vb = np.array([tri.p_WB[0], tri.p_WB[1], tri.p_WB[2]])
vc = np.array([tri.p_WC[0], tri.p_WC[1], tri.p_WC[2]])
d.addLine(p1=va, p2=vb, radius=0, color=[0, 0, 1])
d.addLine(p1=vb, p2=vc, radius=0, color=[0, 0, 1])
d.addLine(p1=va, p2=vc, radius=0, color=[0, 0, 1])
key = (str(surface.body1_name), str(surface.body2_name))
vis.showPolyData(
d.getPolyData(), str(key), parent=folder, color=[0, 0, 1])
def buildGlobalGoal():
#print "building circle world"
d = DebugData()
worldXmin = 25
worldXmax = 50
worldYmin = -20
worldYmax = 20
firstX = worldXmin + np.random.rand()*(worldXmax-worldXmin)
firstY = worldYmin + np.random.rand()*(worldYmax-worldYmin)
firstEndpt = (firstX,firstY,0.2)
secondEndpt = (firstX,firstY,-0.2)
#d.addLine(firstEndpt, secondEndpt, radius=2*np.random.randn())
d.addLine(firstEndpt, secondEndpt, radius=0.3, color=[0.5,1,0])
obj = vis.updatePolyData(d.getPolyData(), 'global_goal', colorByName='RGB255')
world = World()
world.visObj = obj
world.global_goal_x = firstX
world.global_goal_y = firstY
return world
def draw(self):
d = DebugData()
points = list(self.points)
if self.hoverPos is not None:
points.append(self.hoverPos)
# draw points
for p in points:
d.addSphere(p, radius=5)
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 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 updateDrawPolyApprox(self, frame):
distances = self.Sensor.raycastAll(frame)
polyCoefficients = self.SensorApproximator.polyFitConstrainedLP(distances)
if polyCoefficients == None:
polyCoefficients = [0,0]
d = DebugData()
x = self.SensorApproximator.approxThetaVector
y = x * 0.0
for index,val in enumerate(y):
y[index] = self.horner(x[index],polyCoefficients)
origin = np.array(frame.transform.GetPosition())
origin[2] = -0.001
for i in xrange(self.SensorApproximator.numApproxPoints):
if y[i] > 0:
ray = self.SensorApproximator.approxRays[:,i]
rayTransformed = np.array(frame.transform.TransformNormal(ray))
intersection = origin + rayTransformed * y[i]
intersection[2] = -0.001
d.addLine(origin, intersection, color=[0,0,1])
vis.updatePolyData(d.getPolyData(), 'polyApprox', colorByName='RGB255')
def to_polydata(self, bottle_detected=False):
"""Converts the sensor to polydata."""
d = DebugData()
origin = np.array([self._state[0], self._state[1], self.z_distance])
for hit, intersection, ray in zip(self._hit,
self._intersections,
self._rays):
if hit:
color = [1., 0.45882353, 0.51372549]
endpoint = intersection
else:
color = [0., 0.6, 0.58823529]
endpoint = origin + ray
d.addLine(origin, endpoint, color=color, radius=0.05)
# add vision sensor
center = [self._state[0], self._state[1], 0]
axis = [0, 0, 1]
if bottle_detected:
color = [0., 0.8, 0.]
else:
color = [0., 0.6, 0.58823529]
d.addCircle(center, axis, 5, color=color)
return d.getPolyData()
def updateDrawNormals(self, frame):
leftDistances = self.raycastLeftOrRight(frame, self.leftRays)
rightDistances = self.raycastLeftOrRight(frame, self.rightRays)
d = DebugData()
x = self.SensorApproximator.approxThetaVector
y = x * 0.0
for index,val in enumerate(y):
y[index] = self.horner(x[index],polyCoefficients)
origin = np.array(frame.transform.GetPosition())
origin[2] = -0.001
for i in xrange(self.numRays):
#ray = self.SensorApproximator.approxRays[:,i]
#rayTransformed = np.array(frame.transform.TransformNormal(ray))
#intersection = origin + rayTransformed * y[i]
#intersection[2] = -0.001
p1 = leftDistances[i]
p2 = rightDistances[i]
d.addLine(p1, p2, color=[0,0.1,1])
vis.updatePolyData(d.getPolyData(), 'polyApprox', colorByName='RGB255')
def buildGlobalGoal(scale):
#print "building circle world"
d = DebugData()
worldXmin, worldXmax, worldYmin, worldYmax = World.buildBoundaries(d, scale=scale, boundaryType="Square", withData=False)
#print "boundaries done"
firstX = worldXmin + np.random.rand()*(worldXmax-worldXmin)
firstY = worldYmin + np.random.rand()*(worldYmax-worldYmin)
firstEndpt = (firstX,firstY,0.2)
secondEndpt = (firstX,firstY,-0.2)
#d.addLine(firstEndpt, secondEndpt, radius=2*np.random.randn())
d.addLine(firstEndpt, secondEndpt, radius=1.0, color=[0.5,1,0])
obj = vis.updatePolyData(d.getPolyData(), 'global_goal', colorByName='RGB255')
world = World()
world.visObj = obj
world.global_goal_x = firstX
world.global_goal_y = firstY
print "I should have actually built a goal"
return world
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 updateIntersection(frame):
origin = np.array(frame.transform.GetPosition())
rayLength = 5
for i in range(0, numRays):
ray = rays[:, i]
rayTransformed = np.array(frame.transform.TransformNormal(ray))
intersection = computeIntersection(locator, origin,
origin + rayTransformed * rayLength)
name = 'ray intersection ' + str(i)
if intersection is not None:
om.removeFromObjectModel(om.findObjectByName(name))
d = DebugData()
d.addLine(origin, intersection)
color = [1, 0, 0]
# d.addSphere(intersection, radius=0.04)
vis.updatePolyData(d.getPolyData(), name, color=color)
else:
om.removeFromObjectModel(om.findObjectByName(name))
d = DebugData()
d.addLine(origin, origin + rayTransformed * rayLength)
color = [0, 1, 0]
# d.addSphere(intersection, radius=0.04)
vis.updatePolyData(d.getPolyData(), name, color=color)
def _get_line(self, x, y, num, color):
data = DebugData()
data.addLine(x, y, radius=0.7, color=[1, 1, 1])
polydata = data.getPolyData()
self._add_polydata(polydata, "line" + str(num), color)
return polydata
def buildCircleWorld(percentObsDensity,
nonRandom=False,
circleRadius=3,
scale=None,
randomSeed=5,
obstaclesInnerFraction=1.0):
#print "building circle world"
if nonRandom:
np.random.seed(randomSeed)
d = DebugData()
worldXmin, worldXmax, worldYmin, worldYmax = World.buildBoundaries(
d, scale=scale)
#print "boundaries done"
worldArea = (worldXmax - worldXmin) * (worldYmax - worldYmin)
#print worldArea
obsScalingFactor = 1.0 / 12.0
maxNumObstacles = obsScalingFactor * worldArea
numObstacles = int(obstaclesInnerFraction**2 * percentObsDensity /
100.0 * maxNumObstacles)
#print numObstacles
# draw random stick obstacles
obsLength = 2.0
obsXmin = worldXmin + (1 - obstaclesInnerFraction) / 2.0 * (worldXmax -
worldXmin)
obsXmax = worldXmax - (1 - obstaclesInnerFraction) / 2.0 * (worldXmax -
worldXmin)
obsYmin = worldYmin + (1 - obstaclesInnerFraction) / 2.0 * (worldYmax -
worldYmin)
obsYmax = worldYmax - (1 - obstaclesInnerFraction) / 2.0 * (worldYmax -
worldYmin)
for i in xrange(numObstacles):
firstX = obsXmin + np.random.rand() * (obsXmax - obsXmin)
firstY = obsYmin + np.random.rand() * (obsYmax - obsYmin)
firstEndpt = (firstX, firstY, +0.2)
secondEndpt = (firstX, firstY, -0.2)
#d.addLine(firstEndpt, secondEndpt, radius=2*np.random.randn())
d.addLine(firstEndpt, secondEndpt, radius=circleRadius)
obj = vis.showPolyData(d.getPolyData(), 'world')
world = World()
world.visObj = obj
world.Xmax = worldXmax
world.Xmin = worldXmin
world.Ymax = worldYmax
world.Ymin = worldYmin
world.numObstacles = numObstacles
world.percentObsDensity = percentObsDensity
return world
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 buildLineSegmentTestWorld(percentObsDensity, nonRandom=False, circleRadius=3, scale=1.0, randomSeed=5,
obstaclesInnerFraction=1.0):
#print "building circle world"
if nonRandom:
np.random.seed(randomSeed)
d = DebugData()
worldXmin = 3
worldXmax = 15
worldYmin = -10
worldYmax = 10
#print "boundaries done"
worldArea = (worldXmax-worldXmin)*(worldYmax-worldYmin)
#print worldArea
obsScalingFactor = 5.0/12.0
maxNumObstacles = obsScalingFactor * worldArea
numObstacles = int(obstaclesInnerFraction**2 * percentObsDensity/100.0 * maxNumObstacles)
print numObstacles, " is num obstacles"
# draw random stick obstacles
obsLength = 2.0
obsXmin = worldXmin + (1-obstaclesInnerFraction)/2.0*(worldXmax - worldXmin)
obsXmax = worldXmax - (1-obstaclesInnerFraction)/2.0*(worldXmax - worldXmin)
obsYmin = worldYmin + (1-obstaclesInnerFraction)/2.0*(worldYmax - worldYmin)
obsYmax = worldYmax - (1-obstaclesInnerFraction)/2.0*(worldYmax - worldYmin)
for i in xrange(numObstacles):
firstX = obsXmin + np.random.rand()*(obsXmax-obsXmin)
firstY = obsYmin + np.random.rand()*(obsYmax-obsYmin)
secondX = obsXmin + np.random.rand()*(obsXmax-obsXmin)
secondY = obsYmin + np.random.rand()*(obsYmax-obsYmin)
firstEndpt = (firstX,firstY,0.2)
secondEndpt = (firstX,firstY,-0.2)
#d.addLine(firstEndpt, secondEndpt, radius=2*np.random.randn())
d.addLine(firstEndpt, secondEndpt, radius=0.5)
obj = vis.updatePolyData(d.getPolyData(), 'world')
world = World()
world.visObj = obj
world.Xmax = worldXmax
world.Xmin = worldXmin
world.Ymax = worldYmax
world.Ymin = worldYmin
world.numObstacles = numObstacles
world.percentObsDensity = percentObsDensity
return world
def drawTargetPath(self):
path = DebugData()
for i in range(1,len(self.targetPath)):
p0 = self.targetPath[i-1].GetPosition()
p1 = self.targetPath[i].GetPosition()
path.addLine ( np.array( p0 ) , np.array( p1 ), radius= 0.005)
pathMesh = path.getPolyData()
self.targetPathMesh = vis.showPolyData(pathMesh, 'target frame desired path', color=[0.0, 0.3, 1.0], parent=self.targetAffordance, alpha=0.6)
self.targetPathMesh.actor.SetUserTransform(self.targetFrame)
def drawActionSetEmpty(self):
# print "I am drawing the action set"
d = DebugData()
for index, value in enumerate(self.pos_trajectories):
for time_step_index in xrange(2 * self.numPointsToDraw - 1):
d.addLine([0, 0, 0], [0, 0, 0], radius=0.01, color=[1, 1, 1])
obj = vis.updatePolyData(d.getPolyData(), "action_set", colorByName="RGB255")
def buildCircleWorld(percentObsDensity, nonRandom=False, circleRadius=3, scale=None, randomSeed=5,
obstaclesInnerFraction=1.0, alpha=0.0):
#print "building circle world"
list_of_circles = []
if nonRandom:
np.random.seed(randomSeed)
d = DebugData()
worldXmin, worldXmax, worldYmin, worldYmax = World.buildBoundaries(d, scale=scale, boundaryType="Square", alpha=alpha)
#print "boundaries done"
worldArea = (worldXmax-worldXmin)*(worldYmax-worldYmin)
#print worldArea
obsScalingFactor = 1.0/12.0
maxNumObstacles = obsScalingFactor * worldArea
numObstacles = int(obstaclesInnerFraction**2 * percentObsDensity/100.0 * maxNumObstacles)
#print numObstacles
# draw random stick obstacles
obsLength = 2.0
obsXmin = worldXmin + (1-obstaclesInnerFraction)/2.0*(worldXmax - worldXmin)
obsXmax = worldXmax - (1-obstaclesInnerFraction)/2.0*(worldXmax - worldXmin)
obsYmin = worldYmin + (1-obstaclesInnerFraction)/2.0*(worldYmax - worldYmin)
obsYmax = worldYmax - (1-obstaclesInnerFraction)/2.0*(worldYmax - worldYmin)
for i in xrange(numObstacles):
firstX = obsXmin + np.random.rand()*(obsXmax-obsXmin)
firstY = obsYmin + np.random.rand()*(obsYmax-obsYmin)
list_of_circles.append( (firstX, firstY) )
firstEndpt = (firstX,firstY,+0.2)
secondEndpt = (firstX,firstY,-0.2)
#d.addLine(firstEndpt, secondEndpt, radius=2*np.random.randn())
d.addLine(firstEndpt, secondEndpt, radius=circleRadius)
obj = vis.showPolyData(d.getPolyData(), 'world', alpha=alpha)
world = World()
world.visObj = obj
world.Xmax = worldXmax
world.Xmin = worldXmin
world.Ymax = worldYmax
world.Ymin = worldYmin
world.numObstacles = numObstacles
world.percentObsDensity = percentObsDensity
world.list_of_circles = list_of_circles
return world
def drawModel(self, model):
modelFolder = om.getOrCreateContainer(model.name,
parentObj=self.getRootFolder())
markerFolder = om.getOrCreateContainer('markers',
parentObj=modelFolder)
modelName = model.name
markerObjects = markerFolder.children()
if len(markerObjects) != model.nummarkers:
for obj in markerObjects:
om.removeFromObjectModel(obj)
modelColor = vis.getRandomColor()
markerObjects = self.createMarkerObjects(model.nummarkers,
markerFolder, modelName,
modelColor)
self.models[modelName] = markerObjects
if len(markerObjects):
modelColor = markerObjects[0].getProperty('Color')
for i, marker in enumerate(model.markers):
xyz = np.array(marker.xyz) * self.unitConversion
markerFrame = vtk.vtkTransform()
markerFrame.Translate(xyz)
markerObjects[i].getChildFrame().copyFrame(markerFrame)
if self.drawEdges:
d = DebugData()
for m1 in model.markers:
xyz = np.array(m1.xyz) * self.unitConversion
for m2 in model.markers:
xyz2 = np.array(m2.xyz) * self.unitConversion
d.addLine(xyz, xyz2)
edges = shallowCopy(d.getPolyData())
vis.updatePolyData(edges,
modelName + ' edges',
color=modelColor,
parent=modelFolder)
else:
edgesObj = om.findObjectByName(modelName + ' edges')
om.removeFromObjectModel(edgesObj)
computeModelFrame = True
if computeModelFrame:
pos = np.array(model.segments[0].T) * self.unitConversion
rpy = np.array(model.segments[0].A)
modelFrame = transformUtils.frameFromPositionAndRPY(
pos, np.degrees(rpy))
vis.updateFrame(modelFrame,
modelName + ' frame',
parent=modelFolder,
scale=0.1)
def createPlunger(self):
forceLocationInWorld = np.array([0, 0, 0])
forceDirectionInWorld = np.array([0, 0, 1])
point = forceLocationInWorld - 0.1 * forceDirectionInWorld
color = [1, 0, 0]
d = DebugData()
# d.addArrow(point, forceLocationInWorld, headRadius=0.025, tubeRadius=0.005, color=color)
d.addSphere(forceLocationInWorld, radius=0.01)
d.addLine(point, forceLocationInWorld, radius=0.005)
self.plungerPolyData = d.getPolyData()
def buildWarehouseWorld(
percentObsDensity, nonRandom=False, circleRadius=0.1, scale=None, randomSeed=5, obstaclesInnerFraction=1.0
):
if nonRandom:
np.random.seed(randomSeed)
d = DebugData()
worldXmin, worldXmax, worldYmin, worldYmax = World.buildBoundaries(d, scale=scale, boundaryType="Warehouse")
numObstacles = 8
obsLength = 2.0
worldLength = worldXmax - worldXmin
print worldXmin
print worldXmax
obstacleZone = [worldXmin + 0.2 * worldLength, worldXmax - 0.2 * worldLength]
print obstacleZone
obstacleLength = obstacleZone[1] - obstacleZone[0]
incrementSize = obstacleLength * 1.0 / numObstacles
print incrementSize
leftOrRight = -1.0
for i in xrange(numObstacles):
firstX = obstacleZone[0] + incrementSize * i
leftOrRight = leftOrRight * -1.0
firstEndpt = (firstX, leftOrRight * worldYmax, 0.0)
secondEndpt = (firstX, 0.0, 0.0)
# d.addLine(firstEndpt, secondEndpt, radius=2*np.random.randn())
d.addLine(firstEndpt, secondEndpt, radius=circleRadius)
obj = vis.showPolyData(d.getPolyData(), "world")
world = World()
world.visObj = obj
world.Xmax = worldXmax
world.Xmin = worldXmin
world.Ymax = worldYmax
world.Ymin = worldYmin
world.numObstacles = numObstacles
world.percentObsDensity = percentObsDensity
return world
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 drawActionSetFinal(self):
# print "I am drawing the action set"
d = DebugData()
for x_index, x_value in enumerate(self.p_x_final):
for y_index, y_value in enumerate(self.p_y_final):
firstEndpt = (0.0, 0.0, 0.0)
secondEndpt = (x_value, y_value, 0.0)
d.addLine(firstEndpt, secondEndpt, radius=0.02, color=[0.8, 0, 0.8])
obj = vis.updatePolyData(d.getPolyData(), "action_set", colorByName="RGB255")
def getCameraFrustumMesh(view, rayLength=1.0):
origin = np.array(view.camera().GetPosition())
def getCameraRay(displayPoint):
_, ray = vis.getRayFromDisplayPoint(view, displayPoint)
ray = ray-origin
ray /= np.linalg.norm(ray)
return ray
viewWidth, viewHeight = view.renderWindow().GetSize()
rays = [getCameraRay(x) for x in [[0.0, 0.0], [viewWidth, 0.0], [viewWidth, viewHeight], [0.0, viewHeight]]]
rays = [rayLength*r for r in rays]
camPos = origin
lineRadius = 0.0
color = [1.0, 1.0, 1.0]
d = DebugData()
d.addLine(camPos, camPos+rays[0], radius=lineRadius, color=color)
d.addLine(camPos, camPos+rays[1], radius=lineRadius, color=color)
d.addLine(camPos, camPos+rays[2], radius=lineRadius, color=color)
d.addLine(camPos, camPos+rays[3], radius=lineRadius, color=color)
d.addLine(camPos+rays[0], camPos+rays[1], radius=lineRadius, color=color)
d.addLine(camPos+rays[1], camPos+rays[2], radius=lineRadius, color=color)
d.addLine(camPos+rays[2], camPos+rays[3], radius=lineRadius, color=color)
d.addLine(camPos+rays[3], camPos+rays[0], radius=lineRadius, color=color)
pd = d.getPolyData()
return pd
def update(self):
t = self.frameProvider.getFrame('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 drawTargetPath(self):
path = DebugData()
for i in range(1, len(self.targetPath)):
p0 = self.targetPath[i - 1].GetPosition()
p1 = self.targetPath[i].GetPosition()
path.addLine(np.array(p0), np.array(p1), radius=0.005)
pathMesh = path.getPolyData()
self.targetPathMesh = vis.showPolyData(pathMesh,
'target frame desired path',
color=[0.0, 0.3, 1.0],
parent=self.targetAffordance,
alpha=0.6)
self.targetPathMesh.actor.SetUserTransform(self.targetFrame)
def update(self):
t = self.frameProvider.getFrame('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 updateDrawIntersectionManual(self, frame):
print "I am getting called too"
d = DebugData()
originHigher = np.array(frame.transform.GetPosition())
originHigher[2] = 1.0
sliderIdx = self.slider.value
for i in xrange(self.Sensor.numRays):
ray = self.Sensor.rays[:,i]
rayTransformed = np.array(frame.transform.TransformNormal(ray))
distance = self.raycastDataManual[sliderIdx,i]
d.addLine(originHigher, originHigher+rayTransformed*distance, color=[0,1,1])
vis.updatePolyData(d.getPolyData(), 'raysManual', colorByName='RGB255')
def debugFill(self):
d = DebugData()
print "got here"
for i in xrange(self.numX):
print "and here"
for j in xrange(self.numY):
print "and even here"
for k in xrange(self.numZ):
firstEndpt = (i, j, k - 0.1)
secondEndpt = (i, j, k + 0.1)
d.addLine(firstEndpt, secondEndpt, radius=0.2)
obj = vis.showPolyData(d.getPolyData(), "world")
def doFTDraw(self, unusedrobotstate):
frames = []
fts = []
vis_names = []
if (hasattr(self.robotStateJointController, 'lastRobotStateMessage') and
self.robotStateJointController.lastRobotStateMessage):
if self.draw_right:
rft = np.array(self.robotStateJointController.lastRobotStateMessage.force_torque.r_hand_force +
self.robotStateJointController.lastRobotStateMessage.force_torque.r_hand_torque)
rft[0] = -1.*rft[0] # right FT looks to be rotated 180deg around the z axis
rft[1] = -1.*rft[1]
rft[3] = -1.*rft[3]
rft[4] = -1.*rft[4]
rft -= self.ft_right_bias
fts.append(rft)
frames.append(self.robotStateModel.getLinkFrame('r_hand_force_torque'))
vis_names.append('ft_right')
if self.draw_left:
lft = np.array(self.robotStateJointController.lastRobotStateMessage.force_torque.l_hand_force +
self.robotStateJointController.lastRobotStateMessage.force_torque.l_hand_torque)
lft -= self.ft_left_bias
fts.append(lft)
frames.append(self.robotStateModel.getLinkFrame('l_hand_force_torque'))
vis_names.append('ft_left')
for i in range(0, len(frames)):
frame = frames[i]
ft = fts[i]
offset = [0., 0., 0.]
p1 = frame.TransformPoint(offset)
scale = 1.0/25.0 # todo add slider for this?
scalet = 1.0 / 2.5
p2f = frame.TransformPoint(offset + ft[0:3]*scale)
p2t = frame.TransformPoint(offset + ft[3:6])
normalt = (np.array(p2t) - np.array(p1))
normalt = normalt / np.linalg.norm(normalt)
d = DebugData()
# force
if np.linalg.norm(np.array(p2f) - np.array(p1)) < 0.1:
d.addLine(p1, p2f, color=[1.0, 0.0, 0.0])
else:
d.addArrow(p1, p2f, color=[1.,0.,0.])
# torque
if self.draw_torque:
d.addCircle(p1, normalt, scalet*np.linalg.norm(ft[3:6]))
# frame (largely for debug)
vis.updateFrame(frame, vis_names[i]+'frame', view=self.view, parent='wristft', visible=False, scale=0.2)
vis.updatePolyData(d.getPolyData(), vis_names[i], view=self.view, parent='wristft')
def to_polydata(self):
"""Converts the sensor to polydata."""
d = DebugData()
origin = np.array([self._state[0], self._state[1], 0])
for hit, intersection, ray in zip(self._hit, self._intersections,
self._rays):
if hit:
color = [1., 0.45882353, 0.51372549]
endpoint = intersection
else:
color = [0., 0.6, 0.58823529]
endpoint = origin + ray
d.addLine(origin, endpoint, color=color, radius=0.05)
return d.getPolyData()
def updateCameraMesh():
scale = cameraObj.getChildFrame().getProperty('Scale') * 10.0
rayLength = scale
d = DebugData()
d.addCube(dimensions=[0.04, 0.08, 0.06], center=[-0.02, 0.0, 0.0], color=[1,0.5,0])
d.addLine([0.0, 0.0, 0.0], [0.01, 0.0, 0.0], radius=0.023, color=[1,0.5,0])
cameraModelMesh = d.getPolyData()
t = vtk.vtkTransform()
t.Scale(scale, scale, scale)
cameraModelMesh = filterUtils.transformPolyData(cameraModelMesh, t)
cameraMesh = getCameraFrustumMesh(depthScanner.view, rayLength)
cameraMesh = filterUtils.transformPolyData(cameraMesh, getCameraTransform(depthScanner.view.camera()).GetLinearInverse())
cameraMesh = filterUtils.appendPolyData([cameraMesh, cameraModelMesh])
cameraObj.setPolyData(cameraMesh)
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 to_polydata(self):
"""Converts the sensor to polydata."""
d = DebugData()
origin = np.array([self._state[0], self._state[1], 0])
for hit, intersection, ray in zip(self._hit,
self._intersections,
self._rays):
if hit:
color = [1., 0.45882353, 0.51372549]
endpoint = intersection
else:
color = [0., 0.6, 0.58823529]
endpoint = origin + ray
d.addLine(origin, endpoint, color=color, radius=0.05)
return d.getPolyData()
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 drawModel(self, model):
modelFolder = om.getOrCreateContainer(model.name, parentObj=self.getRootFolder())
markerFolder = om.getOrCreateContainer("markers", parentObj=modelFolder)
modelName = model.name
markerObjects = markerFolder.children()
if len(markerObjects) != model.nummarkers:
for obj in markerObjects:
om.removeFromObjectModel(obj)
modelColor = vis.getRandomColor()
markerObjects = self.createMarkerObjects(model.nummarkers, markerFolder, modelName, modelColor)
self.models[modelName] = markerObjects
if len(markerObjects):
modelColor = markerObjects[0].getProperty("Color")
for i, marker in enumerate(model.markers):
xyz = np.array(marker.xyz) * self.unitConversion
markerFrame = vtk.vtkTransform()
markerFrame.Translate(xyz)
markerObjects[i].getChildFrame().copyFrame(markerFrame)
if self.drawEdges:
d = DebugData()
for m1 in model.markers:
xyz = np.array(m1.xyz) * self.unitConversion
for m2 in model.markers:
xyz2 = np.array(m2.xyz) * self.unitConversion
d.addLine(xyz, xyz2)
edges = shallowCopy(d.getPolyData())
vis.updatePolyData(edges, modelName + " edges", color=modelColor, parent=modelFolder)
else:
edgesObj = om.findObjectByName(modelName + " edges")
om.removeFromObjectModel(edgesObj)
computeModelFrame = True
if computeModelFrame:
pos = np.array(model.segments[0].T) * self.unitConversion
rpy = np.array(model.segments[0].A)
modelFrame = transformUtils.frameFromPositionAndRPY(pos, np.degrees(rpy))
vis.updateFrame(modelFrame, modelName + " frame", parent=modelFolder, scale=0.1)
def updateDrawIntersection(frame):
origin = np.array(frame.transform.GetPosition())
#print "origin is now at", origin
d = DebugData()
for i in xrange(numRays):
ray = rays[:,i]
rayTransformed = np.array(frame.transform.TransformNormal(ray))
#print "rayTransformed is", rayTransformed
intersection = computeIntersection(locator, origin, origin + rayTransformed*rayLength)
if intersection is not None:
d.addLine(origin, intersection, color=[1,0,0])
else:
d.addLine(origin, origin+rayTransformed*rayLength, color=[0,1,0])
vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255')
def drawFirstIntersections(self, frame, firstRaycast):
origin = np.array(frame.transform.GetPosition())
d = DebugData()
firstRaycastLocations = self.Sensor.invertRaycastsToLocations(self.frame, firstRaycast)
for i in xrange(self.Sensor.numRays):
endpoint = firstRaycastLocations[i,:]
if firstRaycast[i] == 20.0:
d.addLine(origin, endpoint, color=[0,1,0])
else:
d.addLine(origin, endpoint, color=[1,0,0])
vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255')
self.LineSegmentWorld = World.buildLineSegmentWorld(firstRaycastLocations)
self.LineSegmentLocator = World.buildCellLocator(self.LineSegmentWorld.visObj.polyData)
self.Sensor.setLocator(self.LineSegmentLocator)
def buildFixedTriangleWorld(percentObsDensity):
print "building fixed triangle world"
d = DebugData()
worldXmin, worldXmax, worldYmin, worldYmax = World.buildBoundaries(d)
print "boundaries done"
worldArea = (worldXmax-worldXmin)*(worldYmax-worldYmin)
print worldArea
obsScalingFactor = 1.0/12.0
maxNumObstacles = obsScalingFactor * worldArea
numObstacles = int(percentObsDensity/100.0 * maxNumObstacles)
print numObstacles
# draw random stick obstacles
obsLength = 2.0
for i in xrange(numObstacles):
firstX = worldXmin + np.random.rand()*(worldXmax-worldXmin)
firstY = worldYmin + np.random.rand()*(worldYmax-worldYmin)
firstEndpt = (firstX,firstY,0)
randTheta = np.random.rand() * 2.0*np.pi
secondEndpt = (firstX+obsLength*np.cos(randTheta), firstY+obsLength*np.sin(randTheta), 0)
d.addLine(firstEndpt, secondEndpt, radius=0.1)
obj = vis.showPolyData(d.getPolyData(), 'world')
world = World()
world.visObj = obj
world.Xmax = worldXmax
world.Xmin = worldXmin
world.Ymax = worldYmax
world.Ymin = worldYmin
world.numObstacles = numObstacles
world.percentObsDensity = percentObsDensity
return world
def drawEndEffectorTrajFromPlan(plan=None):
if plan is None:
plan = robotSystem.ikPlanner.lastManipPlan
linkName = getEndEffectorLinkName()
frames = getLinkFrameSamplesFromPlan(plan, linkName)
pointInFrame = np.array(getGraspToHandLink().GetPosition())
pts = []
for f in frames:
p = np.array(f.TransformPoint(pointInFrame))
pts.append(p)
d = DebugData()
for p1, p2 in zip(pts, pts[1:]):
d.addLine(p1, p2)
vis.updatePolyData(d.getPolyData(), 'end effector traj', parent='debug')
def buildStickWorld(percentObsDensity):
print "building stick world"
d = DebugData()
worldXmin, worldXmax, worldYmin, worldYmax = World.buildBoundaries(d)
print "boundaries done"
worldArea = (worldXmax - worldXmin) * (worldYmax - worldYmin)
print worldArea
obsScalingFactor = 1.0 / 12.0
maxNumObstacles = obsScalingFactor * worldArea
numObstacles = int(percentObsDensity / 100.0 * maxNumObstacles)
print numObstacles
# draw random stick obstacles
obsLength = 2.0
for i in xrange(numObstacles):
firstX = worldXmin + np.random.rand() * (worldXmax - worldXmin)
firstY = worldYmin + np.random.rand() * (worldYmax - worldYmin)
firstEndpt = (firstX, firstY, 0)
randTheta = np.random.rand() * 2.0 * np.pi
secondEndpt = (firstX + obsLength * np.cos(randTheta),
firstY + obsLength * np.sin(randTheta), 0)
d.addLine(firstEndpt, secondEndpt, radius=0.2)
obj = vis.showPolyData(d.getPolyData(), 'world')
world = World()
world.visObj = obj
world.Xmax = worldXmax
world.Xmin = worldXmin
world.Ymax = worldYmax
world.Ymin = worldYmin
world.numObstacles = numObstacles
world.percentObsDensity = percentObsDensity
return world
def onMouseMove(self, displayPoint, modifiers=None):
om.removeFromObjectModel(om.findObjectByName('link selection'))
self.linkName = None
self.pickedPoint = None
selection = self.getSelection(displayPoint)
if selection is None:
return
pickedPoint, linkName, normal = selection
d = DebugData()
d.addSphere(pickedPoint, radius=0.01)
d.addLine(pickedPoint, np.array(pickedPoint) + 0.1 * np.array(normal), radius=0.005)
obj = vis.updatePolyData(d.getPolyData(), 'link selection', color=[0,1,0])
obj.actor.SetPickable(False)
self.linkName = linkName
self.pickedPoint = pickedPoint
def onMouseMove(self, displayPoint, modifiers=None):
om.removeFromObjectModel(om.findObjectByName('link selection'))
self.linkName = None
self.pickedPoint = None
selection = self.getSelection(displayPoint)
if selection is None:
return
pickedPoint, linkName, normal = selection
d = DebugData()
d.addSphere(pickedPoint, radius=0.01)
d.addLine(pickedPoint, np.array(pickedPoint) + 0.1 * np.array(normal), radius=0.005)
obj = vis.updatePolyData(d.getPolyData(), 'link selection', color=[0,1,0])
obj.actor.SetPickable(False)
self.linkName = linkName
self.pickedPoint = pickedPoint
def updateDrawIntersection(frame):
origin = np.array(frame.transform.GetPosition())
#print "origin is now at", origin
d = DebugData()
for i in xrange(numRays):
ray = rays[:, i]
rayTransformed = np.array(frame.transform.TransformNormal(ray))
#print "rayTransformed is", rayTransformed
intersection = computeIntersection(locator, origin,
origin + rayTransformed * rayLength)
if intersection is not None:
d.addLine(origin, intersection, color=[1, 0, 0])
else:
d.addLine(origin,
origin + rayTransformed * rayLength,
color=[0, 1, 0])
vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255')
def visualizeTwoStepEstimateSingleContact(self, data):
# draw the contact ray if that option is set
if self.options['twoStepEstimator']['showContactRay']:
d = DebugData()
d.addLine(data['contactRay']['rayOriginInWorld'],
data['contactRay']['rayEndInWorld'],
radius=0.005)
color = data['contactRay']['color']
visName = data['contactRay']['visObjectName']
obj = vis.updatePolyData(d.getPolyData(), visName, color=color)
self.visObjectDrawTime[visName] = time.time()
# draw the actual contact point if it exists
if data['pt'] is not None:
visName = data['linkName'] + " active link estimated external force"
self.drawForce(visName,
data['linkName'],
data['pt'],
data['force'],
color=[1, 0, 0])
self.visObjectDrawTime[visName] = time.time()
def updateCameraMesh():
scale = cameraObj.getChildFrame().getProperty('Scale') * 10.0
rayLength = scale
d = DebugData()
d.addCube(dimensions=[0.04, 0.08, 0.06],
center=[-0.02, 0.0, 0.0],
color=[1, 0.5, 0])
d.addLine([0.0, 0.0, 0.0], [0.01, 0.0, 0.0],
radius=0.023,
color=[1, 0.5, 0])
cameraModelMesh = d.getPolyData()
t = vtk.vtkTransform()
t.Scale(scale, scale, scale)
cameraModelMesh = filterUtils.transformPolyData(cameraModelMesh, t)
cameraMesh = getCameraFrustumMesh(depthScanner.view, rayLength)
cameraMesh = filterUtils.transformPolyData(
cameraMesh,
getCameraTransform(depthScanner.view.camera()).GetLinearInverse())
cameraMesh = filterUtils.appendPolyData([cameraMesh, cameraModelMesh])
cameraObj.setPolyData(cameraMesh)
def getCameraFrustumGeometry(self, rayLength):
camPos, rays = self.getCameraFrustumRays()
rays = [rayLength * r for r in rays]
d = DebugData()
d.addLine(camPos, camPos + rays[0])
d.addLine(camPos, camPos + rays[1])
d.addLine(camPos, camPos + rays[2])
d.addLine(camPos, camPos + rays[3])
d.addLine(camPos + rays[0], camPos + rays[1])
d.addLine(camPos + rays[1], camPos + rays[2])
d.addLine(camPos + rays[2], camPos + rays[3])
d.addLine(camPos + rays[3], camPos + rays[0])
return d.getPolyData()
def spawnTargetAffordance(self):
for obj in om.getObjects():
if obj.getProperty('Name') == 'target':
om.removeFromObjectModel(obj)
targetFrame = transformUtils.frameFromPositionAndRPY([0.6, 0.2, 0.6],
[180, 0, 90])
folder = om.getOrCreateContainer('affordances')
z = DebugData()
z.addLine(np.array([0, 0, 0]),
np.array([-self.boxLength, 0, 0]),
radius=0.02) # main bar
z.addLine(np.array([-self.boxLength, 0, 0]),
np.array([-self.boxLength, 0, self.boxLength]),
radius=0.02) # main bar
z.addLine(np.array([-self.boxLength, 0, self.boxLength]),
np.array([0, 0, self.boxLength]),
radius=0.02) # main bar
z.addLine(np.array([0, 0, self.boxLength]),
np.array([0, 0, 0]),
radius=0.02) # main bar
targetMesh = z.getPolyData()
self.targetAffordance = vis.showPolyData(
targetMesh,
'target',
color=[0.0, 1.0, 0.0],
cls=affordanceitems.FrameAffordanceItem,
parent=folder,
alpha=0.3)
self.targetAffordance.actor.SetUserTransform(targetFrame)
self.targetFrame = vis.showFrame(targetFrame,
'target frame',
parent=self.targetAffordance,
visible=False,
scale=0.2)
self.targetFrame = self.targetFrame.transform
params = dict(length=self.boxLength,
otdf_type='target',
friendly_name='target')
self.targetAffordance.setAffordanceParams(params)
self.targetAffordance.updateParamsFromActorTransform()
def fitRunningBoardAtFeet(self):
# get stance frame
startPose = self.getPlanningStartPose()
stanceFrame = self.robotSystem.footstepsDriver.getFeetMidPoint(
self.robotSystem.robotStateModel, useWorldZ=False)
stanceFrameAxes = transformUtils.getAxesFromTransform(stanceFrame)
# get pointcloud and extract search region covering the running board
polyData = segmentation.getCurrentRevolutionData()
polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01)
_, polyData = segmentation.removeGround(polyData)
polyData = segmentation.cropToBox(polyData, stanceFrame,
[1.0, 1.0, 0.1])
if not polyData.GetNumberOfPoints():
print('empty search region point cloud')
return
vis.updatePolyData(polyData,
'running board search points',
parent=segmentation.getDebugFolder(),
color=[0, 1, 0],
visible=False)
# extract maximal points along the stance x axis
perpAxis = stanceFrameAxes[0]
edgeAxis = stanceFrameAxes[1]
edgePoints = segmentation.computeEdge(polyData, edgeAxis, perpAxis)
edgePoints = vnp.getVtkPolyDataFromNumpyPoints(edgePoints)
vis.updatePolyData(edgePoints,
'edge points',
parent=segmentation.getDebugFolder(),
visible=True)
# ransac fit a line to the edge points
linePoint, lineDirection, fitPoints = segmentation.applyLineFit(
edgePoints)
if np.dot(lineDirection, stanceFrameAxes[1]) < 0:
lineDirection = -lineDirection
linePoints = segmentation.thresholdPoints(fitPoints, 'ransac_labels',
[1.0, 1.0])
dists = np.dot(
vnp.getNumpyFromVtk(linePoints, 'Points') - linePoint,
lineDirection)
p1 = linePoint + lineDirection * np.min(dists)
p2 = linePoint + lineDirection * np.max(dists)
vis.updatePolyData(fitPoints,
'line fit points',
parent=segmentation.getDebugFolder(),
colorByName='ransac_labels',
visible=False)
# compute a new frame that is in plane with the stance frame
# and matches the orientation and position of the detected edge
origin = np.array(stanceFrame.GetPosition())
normal = np.array(stanceFrameAxes[2])
# project stance origin to edge, then back to foot frame
originProjectedToEdge = linePoint + lineDirection * np.dot(
origin - linePoint, lineDirection)
originProjectedToPlane = segmentation.projectPointToPlane(
originProjectedToEdge, origin, normal)
zaxis = np.array(stanceFrameAxes[2])
yaxis = np.array(lineDirection)
xaxis = np.cross(yaxis, zaxis)
xaxis /= np.linalg.norm(xaxis)
yaxis = np.cross(zaxis, xaxis)
yaxis /= np.linalg.norm(yaxis)
d = DebugData()
d.addSphere(p1, radius=0.005)
d.addSphere(p2, radius=0.005)
d.addLine(p1, p2)
d.addSphere(originProjectedToEdge, radius=0.001, color=[1, 0, 0])
d.addSphere(originProjectedToPlane, radius=0.001, color=[0, 1, 0])
d.addLine(originProjectedToPlane, origin, color=[0, 1, 0])
d.addLine(originProjectedToEdge, origin, color=[1, 0, 0])
vis.updatePolyData(d.getPolyData(),
'running board edge',
parent=segmentation.getDebugFolder(),
colorByName='RGB255',
visible=False)
# update the running board box affordance position and orientation to
# fit the detected edge
box = self.spawnRunningBoardAffordance()
boxDimensions = box.getProperty('Dimensions')
t = transformUtils.getTransformFromAxesAndOrigin(
xaxis, yaxis, zaxis, originProjectedToPlane)
t.PreMultiply()
t.Translate(-boxDimensions[0] / 2.0, 0.0, -boxDimensions[2] / 2.0)
box.getChildFrame().copyFrame(t)
self.initialize()
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)
#d.addSphere(chullPoints[0], radius=0.001, color=[1,0,0])
#d.addSphere(chullPoints[1], radius=0.001, color=[0,1,0])
vis.showPolyData(d.getPolyData(), 'plane normals', colorByName='RGB255')
#saveConvexHulls(chulls, name)
#vis.showPolyData(ioUtils.readPolyData(os.path.join(name, 'merged_planes.ply')), 'merged_planes')
applogic.resetCamera([1, 1, 0])
applogic.setBackgroundColor([1, 1, 1])
view.orientationMarkerWidget().Off()
app.gridObj.setProperty('Color', [0, 0, 0])
x = theta
z = np.sin(theta)
y = np.cos(theta)
pts = np.vstack((x, y, z)).T.copy()
pts /= np.max(pts)
return pts
app = consoleapp.ConsoleApp()
view = app.createView()
view.show()
d = DebugData()
d.addLine((0,0,0), (1,0,0), radius=0.03)
show(d, (0, 0, 0))
d = DebugData()
d.addPolygon([[0,0,0], [0.8, 0, 0], [1, 0.6, 0], [0.4, 1, 0], [-0.2, 0.6, 0]])
show(d, (2, 0, 0))
d = DebugData()
d.addPolyLine(getHelixPoints(), radius=0.01)
show(d, (4, 0, 0))
d = DebugData()
d.addSphere([0, 0, 0], radius=0.3)
