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 add_target(self, target):
data = DebugData()
center = [target[0], target[1], 1]
axis = [0, 0, 1] # Upright cylinder.
data.addCylinder(center, axis, 2, 3)
om.removeFromObjectModel(om.findObjectByName("target"))
self._add_polydata(data.getPolyData(), "target", [0, 0.8, 0])
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 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 __init__(self, robotSystem, view):
self.robotStateModel = robotSystem.robotStateModel
self.robotStateJointController = robotSystem.robotStateJointController
self.robotSystem = robotSystem
self.lFootFtFrameId = self.robotStateModel.model.findLinkID(
self.robotSystem.ikPlanner.leftFootLink)
self.rFootFtFrameId = self.robotStateModel.model.findLinkID(
self.robotSystem.ikPlanner.rightFootLink)
self.leftInContact = 0
self.rightInContact = 0
self.view = view
self.ddDrakeWrapper = PythonQt.dd.ddDrakeWrapper()
self.warningButton = QtGui.QLabel('COP Warning')
self.warningButton.setStyleSheet("background-color:white")
self.dialogVisible = False
d = DebugData()
vis.updatePolyData(d.getPolyData(),
'measured cop',
view=self.view,
parent='robot state model')
om.findObjectByName('measured cop').setProperty('Visible', False)
self.robotStateModel.connectModelChanged(self.update)
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 drawCenterOfMass(model):
stanceFrame = 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 createCylinder(params):
d = DebugData()
d.addCylinder(center=(0, 0, 0),
axis=(0, 0, 1),
radius=params["radius"],
length=params["length"])
return [d.getPolyData()]
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 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 handle_message(self, msg):
# Limits the rate of message handling, since redrawing is done in the
# message handler.
self._sub.setSpeedLimit(0.1)
# Removes the folder completely.
om.removeFromObjectModel(om.findObjectByName(self._folder_name))
# Recreates folder.
folder = om.getOrCreateContainer(self._folder_name)
# A map from pair of body names to a list of contact forces
collision_pair_to_forces = {}
for arrow in msg.arrow_info:
point = np.array([
arrow.arrow_origin[0], arrow.arrow_origin[1],
arrow.arrow_origin[2]
])
vec = np.array([
arrow.arrow_vector[0], arrow.arrow_vector[1],
arrow.arrow_vector[2]
])
d = DebugData()
d.addArrow(start=point,
end=vec + point,
tubeRadius=0.005,
headRadius=0.01)
vis.showPolyData(d.getPolyData(),
str(0),
parent=folder,
color=[arrow.rgb[0], arrow.rgb[1], arrow.rgb[2]])
def updateGeometryFromProperties(self):
filename = self.getProperty('Filename')
scale = self.getProperty('Scale')
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)
if not scale == [1, 1, 1]:
transform = vtk.vtkTransform()
transform.Scale(scale)
transformFilter = vtk.vtkTransformPolyDataFilter()
transformFilter.SetInput(polyData)
transformFilter.SetTransform(transform)
transformFilter.Update()
polyData = transformFilter.GetOutput()
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 __init__(self,
velocity=40.0,
scale=0.5,
exploration=0.5,
size=100,
model="A10.obj"):
"""Constructs a Robot.
Args:
velocity: Velocity of the robot in the forward direction.
scale: Scale of the model.
exploration: Exploration rate.
model: Object model to use.
"""
self._size = size
self._target = (0, 0, 0)
self._initPos = (0, 0)
self._exploration = exploration
self._changetheta = 0
self._inlaser = True
# t = vtk.vtkTransform()
# t.Scale(scale, scale, scale)
# polydata = ioUtils.readPolyData(model)
# polydata = filterUtils.transformPolyData(polydata, t)
data = DebugData()
center = [0, 0, 1.0 / 2 - 0.5]
dimensions = [10, 5, 1.0]
data.addCube(dimensions, center)
polydata = data.getPolyData()
super(Robot, self).__init__(velocity, polydata)
self._ctrl = Controller()
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 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 getMarkerGeometry(self):
if self.markerGeometry is None:
d = DebugData()
d.addSphere(np.zeros(3), radius=0.007, resolution=8)
self.markerGeometry = shallowCopy(d.getPolyData())
return self.markerGeometry
def buildRobot():
d = DebugData()
d.addCone((0, 0, 0), (1, 0, 0), height=0.2, radius=0.1)
obj = vis.showPolyData(d.getPolyData(), 'robot')
frame = vis.addChildFrame(obj)
return obj
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 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 buildRobot(x=0,y=0):
d = DebugData()
d.addCone((x,y,0), (1,0,0), height=0.2, radius=0.1)
obj = vis.showPolyData(d.getPolyData(), 'robot')
frame = vis.addChildFrame(obj)
return obj
def drawCenterOfMass(model):
stanceFrame = 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 updateGeometryFromProperties(self):
filename = self.getProperty('Filename')
scale = self.getProperty('Scale')
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)
if not scale == [1, 1, 1]:
transform = vtk.vtkTransform()
transform.Scale(scale)
polyData = filterUtils.transformPolyData(
polyData, transform)
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 add_target(self, target):
data = DebugData()
center = [target[0], target[1], 1]
axis = [0, 0, 1] # Upright cylinder.
data.addCylinder(center, axis, 2, 3)
om.removeFromObjectModel(om.findObjectByName("target"))
self._add_polydata(data.getPolyData(), "target", [0, 0.8, 0])
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 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 setScale(self, scale):
data = DebugData()
center = [0, 0, 1.0 / 2 - 0.5]
dimensions = [scale, scale / 2, 1.0]
data.addCube(dimensions, center)
polydata = data.getPolyData()
self._polydata = polydata
self._raw_polydata = polydata
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 getMarkerGeometry(self):
if self.markerGeometry is None:
d = DebugData()
d.addSphere(np.zeros(3), radius=0.007, resolution=12)
self.markerGeometry = shallowCopy(d.getPolyData())
return self.markerGeometry
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 onNewWalkingGoal(self, walkingGoal=None):
walkingGoal = walkingGoal or self.newWalkingGoalFrame(self.robotModel)
frameObj = vis.updateFrame(walkingGoal,
'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()
if self.placer:
self.placer.stop()
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',
parent=frameObj,
visible=True,
color=[1, 1, 0])
handle.actor.SetUserTransform(frameObj.transform)
self.placer = PlacerWidget(app.getCurrentRenderView(), handle,
terrain)
def onFramePropertyModified(propertySet, propertyName):
if propertyName == 'Edit':
if propertySet.getProperty(propertyName):
self.placer.start()
else:
self.placer.stop()
frameObj.properties.connectPropertyChanged(onFramePropertyModified)
onFramePropertyModified(frameObj, 'Edit')
frameObj.connectFrameModified(self.onWalkingGoalModified)
self.onWalkingGoalModified(frameObj)
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 createCylinder(params):
d = DebugData()
color = params.get("color", DEFAULT_COLOR)[:3]
d.addCylinder(center=(0, 0, 0),
axis=(0, 0, 1),
radius=params["radius"],
length=params["length"],
color=color)
return [d.getPolyData()]
def testCreateBackground(self):
d = DebugData()
d.addCube((1,1,0.1), (0,0,0), color=[1,1,1])
self.backgroundPolyData = d.getPolyData()
if self.vis:
view = self.views['background']
vis.updatePolyData(self.backgroundPolyData, 'background', view=view, colorByName='RGB255')
view.resetCamera()
def drawContactPts(self, obj, footstep, **kwargs):
leftPoints, rightPoints = FootstepsDriver.getContactPts(footstep.params.support_contact_groups)
contact_pts = rightPoints if footstep.is_right_foot else leftPoints
d = DebugData()
for pt in contact_pts:
d.addSphere(pt, radius=0.01)
d_obj = vis.showPolyData(d.getPolyData(), "contact points", parent=obj, **kwargs)
d_obj.actor.SetUserTransform(obj.actor.GetUserTransform())
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 createCylinder(params):
d = DebugData()
color = params.get("color", DEFAULT_COLOR)[:3]
d.addCylinder(center=(0, 0, 0),
axis=(0, 0, 1),
radius=params["radius"],
length=params["length"],
color=color)
return [d.getPolyData()]
def testCreateBackground(self):
d = DebugData()
d.addCube((1,1,0.1), (0,0,0), color=[1,1,1])
self.backgroundPolyData = d.getPolyData()
if self.vis:
view = self.views['background']
vis.updatePolyData(self.backgroundPolyData, 'background', view=view, colorByName='RGB255')
view.resetCamera()
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 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 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 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 showBoardDebug(affs=None):
referenceFrame = vtk.vtkTransform()
referenceFrame.Translate(0, 0, 5.0)
affs = affs or om.getObjects()
for obj in affs:
if isinstance(obj, BlockAffordanceItem):
d = DebugData()
d.addSphere(computeClosestCorner(obj, referenceFrame), radius=0.015)
showPolyData(d.getPolyData(), 'closest corner', parent='board debug', visible=True)
showFrame(computeGroundFrame(obj, referenceFrame), 'ground frame', parent='board debug', visible=True)
def showBoardDebug(affs=None):
referenceFrame = vtk.vtkTransform()
referenceFrame.Translate(0, 0, 5.0)
affs = affs or om.getObjects()
for obj in affs:
if isinstance(obj, BlockAffordanceItem):
d = DebugData()
d.addSphere(computeClosestCorner(obj, referenceFrame), radius=0.015)
showPolyData(d.getPolyData(), 'closest corner', parent='board debug', visible=True)
showFrame(computeGroundFrame(obj, referenceFrame), 'ground frame', parent='board debug', visible=True)
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 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 createPolyLine(params):
d = DebugData()
points = [np.asarray(p) for p in params["points"]]
color = params.get("color", DEFAULT_COLOR)[:3]
radius = params.get("radius", 0.01)
startHead = params.get("start_head", False)
endHead = params.get("end_head", False)
headRadius = params.get("head_radius", 0.05)
headLength = params.get("head_length", headRadius)
isClosed = params.get("closed", False)
if startHead:
normal = points[0] - points[1]
normal = normal / np.linalg.norm(normal)
points[0] = points[0] - 0.5 * headLength * normal
d.addCone(origin=points[0],
normal=normal,
radius=headRadius,
height=headLength,
color=color,
fill=True)
if endHead:
normal = points[-1] - points[-2]
normal = normal / np.linalg.norm(normal)
points[-1] = points[-1] - 0.5 * headLength * normal
d.addCone(origin=points[-1],
normal=normal,
radius=headRadius,
height=headLength,
color=color,
fill=True)
d.addPolyLine(points, isClosed, radius=radius, color=color)
return [d.getPolyData()]
def __init__(self, uid, view, seed_pose, irisDriver, existing_region=None):
d = DebugData()
self.uid = uid
vis.PolyDataItem.__init__(self, "IRIS region {:d}".format(uid), d.getPolyData(), view)
self.transform = seed_pose
d.addSphere((0,0,0), radius=0.02)
self.seedObj = vis.showPolyData(d.getPolyData(), 'region seed', parent=om.getOrCreateContainer('IRIS region seeds'))
self.seedObj.actor.SetUserTransform(self.transform)
self.frameObj = vis.showFrame(self.transform, 'region seed frame',
scale=0.2,
visible=False,
parent=self.seedObj)
self.frameObj.setProperty('Edit', True)
self.frameObj.widget.HandleRotationEnabledOff()
terrain = om.findObjectByName('HEIGHT_MAP_SCENE')
if terrain:
rep = self.frameObj.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
pos = np.array(self.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])
self.frameObj.transform.Translate(pos - np.array(self.frameObj.transform.GetPosition()))
self.placer = PlacerWidget(view, self.seedObj, terrain)
self.placer.start()
else:
self.frameObj.setProperty('Edit', True)
self.frameObj.setProperty('Visible', True)
self.driver = irisDriver
self.safe_region = None
self.addProperty('Visible', True)
self.addProperty('Enabled for Walking', True)
self.addProperty('Alpha', 1.0)
self.addProperty('Color', QtGui.QColor(200,200,20))
self.frameObj.connectFrameModified(self.onFrameModified)
if existing_region is None:
self.onFrameModified(self.frameObj)
else:
self.setRegion(existing_region)
self.setProperty('Alpha', 0.5)
self.setProperty('Color', QtGui.QColor(220,220,220))
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 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 __init__(self, width, height):
"""Construct an empty world.
Args:
width: Width of the field.
height: Height of the field.
"""
self._data = DebugData()
self._width = width
self._height = height
self._add_boundaries()
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 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 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 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 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 createSpheres(self, sensorValues):
d = DebugData()
for key in list(sensorValues.keys()):
frame, pos, rpy = self.sensorLocations[key]
t = transformUtils.frameFromPositionAndRPY(pos, rpy)
t.PostMultiply()
t.Concatenate(self.frames[frame])
d.addSphere(t.GetPosition(),
radius=0.005,
color=self.getColor(sensorValues[key], key),
resolution=8)
vis.updatePolyData(d.getPolyData(), self.name, colorByName='RGB255')
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 spawnCropBox(self, dims=None):
if dims is None:
dim_x = CONFIG['crop_box']['dimensions']['x']
dim_y = CONFIG['crop_box']['dimensions']['y']
dim_z = CONFIG['crop_box']['dimensions']['z']
dims = [dim_x, dim_y, dim_z]
transform = director_utils.transformFromPose(CONFIG['crop_box']['transform'])
d = DebugData()
d.addCube(dims, [0,0,0], color=[0,1,0])
self.cube_vis = vis.updatePolyData(d.getPolyData(), 'Crop Cube', colorByName='RGB255')
vis.addChildFrame(self.cube_vis)
self.cube_vis.getChildFrame().copyFrame(transform)
self.cube_vis.setProperty('Alpha', 0.3)
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)
