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)