def spawnBoardAffordance(self, randomize=False):
self.boardLength = 1.5
if randomize:
position = [random.uniform(0.5, 0.8), random.uniform(-0.2, 0.2), random.uniform(0.5, 0.8)]
rpy = [random.choice((random.uniform(-35, 35), random.uniform(70, 110))), random.uniform(-10, 10), random.uniform(-5, 5)]
zwidth = random.uniform(0.5, 1.0)
else:
if self.b.val:
position = [0.4, 0.0, 1.]
else:
position = [0.6, 0.0, 1.]
rpy = [90, 1, 0]
zwidth = self.boardLength
xwidth = 3.75 * .0254
ywidth = 1.75 * .0254
t = transformUtils.frameFromPositionAndRPY(position, rpy)
t.Concatenate(self.b.computeGroundFrame(self.b.robotModel))
xaxis = [1,0,0]
yaxis = [0,1,0]
zaxis = [0,0,1]
for axis in (xaxis, yaxis, zaxis):
t.TransformVector(axis, axis)
self.affordance = segmentation.createBlockAffordance(t.GetPosition(), xaxis, yaxis, zaxis, xwidth, ywidth, zwidth, 'board', parent='affordances')
self.affordance.setProperty('Color', QtGui.QColor(200, 150, 100))
t = self.affordance.actor.GetUserTransform()
self.frame = vis.showFrame(t, 'board frame', parent=self.affordance, visible=False, scale=0.2)
def spawnBoardAffordance(self, randomize=False):
self.boardLength = 1.5
if randomize:
position = [
random.uniform(0.5, 0.8),
random.uniform(-0.2, 0.2),
random.uniform(0.5, 0.8)
]
rpy = [
random.choice((random.uniform(-35,
35), random.uniform(70, 110))),
random.uniform(-10, 10),
random.uniform(-5, 5)
]
zwidth = random.uniform(0.5, 1.0)
else:
if self.b.val:
position = [0.4, 0.0, 1.]
else:
position = [0.6, 0.0, 1.]
rpy = [90, 1, 0]
zwidth = self.boardLength
xwidth = 3.75 * .0254
ywidth = 1.75 * .0254
t = transformUtils.frameFromPositionAndRPY(position, rpy)
t.Concatenate(self.b.computeGroundFrame(self.b.robotModel))
xaxis = [1, 0, 0]
yaxis = [0, 1, 0]
zaxis = [0, 0, 1]
for axis in (xaxis, yaxis, zaxis):
t.TransformVector(axis, axis)
self.affordance = segmentation.createBlockAffordance(
t.GetPosition(),
xaxis,
yaxis,
zaxis,
xwidth,
ywidth,
zwidth,
'board',
parent='affordances')
self.affordance.setProperty('Color', QtGui.QColor(200, 150, 100))
t = self.affordance.actor.GetUserTransform()
self.frame = vis.showFrame(t,
'board frame',
parent=self.affordance,
visible=False,
scale=0.2)
def addCollisionObject(self, obj):
if om.getOrCreateContainer('affordances').findChild(obj.getProperty('Name') + ' affordance'):
return # Affordance has been created previously
frame = obj.findChild(obj.getProperty('Name') + ' frame')
(origin, quat) = transformUtils.poseFromTransform(frame.transform)
(xaxis, yaxis, zaxis) = transformUtils.getAxesFromTransform(frame.transform)
# TODO: move this into transformUtils as getAxisDimensions or so
box = obj.findChild(obj.getProperty('Name') + ' box')
box_np = vtkNumpy.getNumpyFromVtk(box.polyData, 'Points')
box_min = np.amin(box_np, 0)
box_max = np.amax(box_np, 0)
xwidth = np.linalg.norm(box_max[0]-box_min[0])
ywidth = np.linalg.norm(box_max[1]-box_min[1])
zwidth = np.linalg.norm(box_max[2]-box_min[2])
name = obj.getProperty('Name') + ' affordance'
boxAffordance = segmentation.createBlockAffordance(origin, xaxis, yaxis, zaxis, xwidth, ywidth, zwidth, name, parent='affordances')
boxAffordance.setSolidColor(obj.getProperty('Color'))
boxAffordance.setProperty('Alpha', 0.3)
