def makeGraspFrames(obj, graspOffset, pregraspOffset=(-0.08, 0, 0), suffix=''): pos, rpy = graspOffset objectToWorld = obj.getChildFrame().transform graspToObject = transformUtils.frameFromPositionAndRPY(pos, rpy) preGraspToGrasp = transformUtils.frameFromPositionAndRPY( pregraspOffset, [0, 0, 0]) graspToWorld = transformUtils.concatenateTransforms( [graspToObject, objectToWorld]) preGraspToWorld = transformUtils.concatenateTransforms( [preGraspToGrasp, graspToWorld]) graspFrameName = 'grasp to world%s' % suffix pregraspFrameName = 'pregrasp to world%s' % suffix om.removeFromObjectModel(om.findObjectByName(graspFrameName)) om.removeFromObjectModel(om.findObjectByName(pregraspFrameName)) graspFrame = vis.showFrame(graspToWorld, graspFrameName, scale=0.1, parent=obj, visible=False) preGraspFrame = vis.showFrame(preGraspToWorld, pregraspFrameName, scale=0.1, parent=obj, visible=False) obj.getChildFrame().getFrameSync().addFrame(graspFrame, ignoreIncoming=True) graspFrame.getFrameSync().addFrame(preGraspFrame, ignoreIncoming=True)
def fitObjectsOnShelf(polyData, maxHeight = 0.25): # find the shelf plane: polyDataWithoutFront, _ = segmentation.removeMajorPlane(polyData, distanceThreshold=0.02) polyDataPlaneFit, origin, normal = segmentation.applyPlaneFit(polyDataWithoutFront, expectedNormal=np.array([0.0,0.0,1.0]), perpendicularAxis=np.array([0.0,0.0,1.0]), returnOrigin=True) vis.updatePolyData(polyDataPlaneFit, 'polyDataPlaneFit', parent='segmentation', visible=False) shelfSurfacePoints = segmentation.thresholdPoints(polyDataPlaneFit, 'dist_to_plane', [-0.01, 0.01]) shelfCenter = segmentation.computeCentroid(shelfSurfacePoints) shelfFrame = transformUtils.getTransformFromOriginAndNormal(shelfCenter, normal, normalAxis=2) vis.showFrame(shelfFrame, 'shelfFrame', parent='segmentation', scale=0.15 , visible=False) # find the points near to the shelf plane and find objects on it: points = vnp.getNumpyFromVtk(polyData, 'Points') dist = np.dot(points - origin, normal) vnp.addNumpyToVtk(polyData, dist, 'dist_to_plane') shelfPoints = segmentation.thresholdPoints(polyData, 'dist_to_plane', [-0.01, maxHeight]) vis.updatePolyData(shelfPoints, 'shelf', parent='segmentation', visible=False) data = segmentation.segmentTableScene(shelfPoints, shelfCenter, filterClustering = False ) vis.showClusterObjects(data.clusters + [data.table], parent='segmentation') # remove the points that we considered from the orginal cloud dists = vnp.getNumpyFromVtk(polyData, 'dist_to_plane') diffShelf = ( ((dists > maxHeight) + (dists < -0.01))) + 0.1 -0.1 vnp.addNumpyToVtk(polyData, diffShelf, 'diff_shelf') polyData = segmentation.thresholdPoints(polyData, 'diff_shelf', [1, 1]) vis.updatePolyData(polyData, 'rest', parent='segmentation', visible=False) return polyData
def showFusedMaps(self): om.removeFromObjectModel(om.findObjectByName('stereo')) om.getOrCreateContainer('stereo') q = imageManager.queue cameraToLocalNow = vtk.vtkTransform() utime = q.getCurrentImageTime('CAMERA_TSDF') q.getTransform('CAMERA_LEFT', 'local', utime, cameraToLocalNow) cameraToLocalFusedNow = vtk.vtkTransform() q.getTransform('CAMERA_LEFT_ALT', 'local', utime, cameraToLocalFusedNow) for i in range(len(self.pointClouds)): fusedNowToLocalNow = vtk.vtkTransform() fusedNowToLocalNow.PreMultiply() fusedNowToLocalNow.Concatenate(cameraToLocalNow) fusedNowToLocalNow.Concatenate( cameraToLocalFusedNow.GetLinearInverse()) fusedTransform = vtk.vtkTransform() fusedTransform.PreMultiply() fusedTransform.Concatenate(fusedNowToLocalNow) fusedTransform.Concatenate(self.cameraToLocalFusedTransforms[i]) pd = filterUtils.transformPolyData(self.pointClouds[i], fusedTransform) vis.showFrame(fusedTransform, ('cloud frame ' + str(i)), visible=True, scale=0.2, parent='stereo') vis.showPolyData(pd, ('stereo ' + str(i)), parent='stereo', colorByName='rgb_colors')
def run(self): polyData = self.getPointCloud() annotation = self.getAnnotationInput() annotationPoint = annotation.annotationPoints[0] planePoints, normal = segmentation.applyLocalPlaneFit(polyData, annotationPoint, searchRadius=0.1, searchRadiusEnd=0.2) viewDirection = segmentation.SegmentationContext.getGlobalInstance().getViewDirection() if np.dot(normal, viewDirection) < 0: normal = -normal xaxis = normal zaxis = [0, 0, 1] yaxis = np.cross(zaxis, xaxis) xaxis = np.cross(yaxis, zaxis) xaxis /= np.linalg.norm(xaxis) yaxis /= np.linalg.norm(yaxis) t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis) t.PostMultiply() t.Translate(annotationPoint) polyData = annotation.polyData polyData = segmentation.transformPolyData(polyData, t.GetLinearInverse()) annotation.setProperty('Visible', False) om.removeFromObjectModel(om.findObjectByName('wall')) obj = vis.showPolyData(polyData, 'wall') obj.actor.SetUserTransform(t) vis.showFrame(t, 'wall frame', scale=0.2, parent=obj)
def showFusedMaps(self): om.removeFromObjectModel(om.findObjectByName("stereo")) om.getOrCreateContainer("stereo") q = imageManager.queue cameraToLocalNow = vtk.vtkTransform() utime = q.getCurrentImageTime("CAMERA_TSDF") q.getTransform("CAMERA_LEFT", "local", utime, cameraToLocalNow) cameraToLocalFusedNow = vtk.vtkTransform() q.getTransform("CAMERA_LEFT_ALT", "local", utime, cameraToLocalFusedNow) for i in range(len(self.pointClouds)): fusedNowToLocalNow = vtk.vtkTransform() fusedNowToLocalNow.PreMultiply() fusedNowToLocalNow.Concatenate(cameraToLocalNow) fusedNowToLocalNow.Concatenate(cameraToLocalFusedNow.GetLinearInverse()) fusedTransform = vtk.vtkTransform() fusedTransform.PreMultiply() fusedTransform.Concatenate(fusedNowToLocalNow) fusedTransform.Concatenate(self.cameraToLocalFusedTransforms[i]) pd = filterUtils.transformPolyData(self.pointClouds[i], fusedTransform) vis.showFrame(fusedTransform, ("cloud frame " + str(i)), visible=True, scale=0.2, parent="stereo") vis.showPolyData(pd, ("stereo " + str(i)), parent="stereo", colorByName="rgb_colors")
def makeSplineGraspConstraints(self, ikPlanner, positionTolerance=0.03, angleToleranceInDegrees=15): params = self.computeHandleParameterization() segments = zip(params, params[1:]) #times = [np.linspace(segment[0], segment[1], 6) for segment in segments] #times = [[0.0, 0.3, 0.5], np.linspace(params[-2], params[-1], 6)] times = np.linspace(params[-2], params[-1], 6) times = np.hstack(times) times = np.unique(times) frames = [] for t in times: frames.append(self.splineInterp(t)) folder = om.getOrCreateContainer( 'constraint spline samples', parentObj=om.getOrCreateContainer('debug')) for f in frames: vis.showFrame(f, 'frame', scale=0.1) side = self.side graspToPalm = vtk.vtkTransform() graspToHand = ikPlanner.newGraspToHandFrame(side, graspToPalm) constraints = [] for f, t in zip(frames[:-1], times[:-1]): graspToWorld = f p, q = ikPlanner.createPositionOrientationGraspConstraints( side, graspToWorld, graspToHand) p.lowerBound = np.tile(-positionTolerance, 3) p.upperBound = np.tile(positionTolerance, 3) q.angleToleranceInDegrees = angleToleranceInDegrees if t >= params[-2]: q.angleToleranceInDegrees = 0 p.lowerBound = np.tile(-0.0, 3) p.upperBound = np.tile(0.0, 3) constraints.append(q) p.tspan = [t, t] q.tspan = [t, t] constraints.append(p) return constraints
def _handle_message(self, msg): # Removes the folder completely. This is the clearest and easiest way # of doing update. Otherwise we need to store the FrameItem returned # by vis.showFrame, and update its transform. Also need to handle # enable / disable. om.removeFromObjectModel(om.findObjectByName(self._folder_name)) # Recreates folder. folder = om.getOrCreateContainer(self._folder_name) for i in range(0, msg.num_links): name = msg.link_name[i] transform = transformUtils.transformFromPose( msg.position[i], msg.quaternion[i]) vis.showFrame(transform, name, parent=folder, scale=0.1);
def _handle_message(self, msg): # Removes the folder completely. This is the clearest and easiest way # of doing update. Otherwise we need to store the FrameItem returned # by vis.showFrame, and update its transform. Also need to handle # enable / disable. om.removeFromObjectModel(om.findObjectByName(self._folder_name)) # Recreates folder. folder = om.getOrCreateContainer(self._folder_name) for i in range(0, msg.num_links): name = msg.link_name[i] transform = transformUtils.transformFromPose( msg.position[i], msg.quaternion[i]) vis.showFrame(transform, name, parent=folder, scale=0.1)
def newPolyData(self, name, view, parent=None): polyData = self.getNewHandPolyData() if isinstance(parent, str): parent = om.getOrCreateContainer(parent) color = [1.0, 1.0, 0.0] if self.side == 'right': color = [0.33, 1.0, 0.0] obj = vis.showPolyData(polyData, name, view=view, color=color, visible=False, parent=parent) obj.side = self.side frame = vtk.vtkTransform() frame.PostMultiply() obj.actor.SetUserTransform(frame) frameObj = vis.showFrame(frame, '%s frame' % name, view=view, scale=0.2, visible=False, parent=obj) return obj
def __init__(self, jointController): self.jointController = jointController pos, rpy = jointController.q[:3], jointController.q[3:6] t = transformUtils.frameFromPositionAndRPY(pos, np.degrees(rpy)) self.frame = vis.showFrame(t, 'mover widget', scale=0.3) self.frame.setProperty('Edit', True) self.frame.connectFrameModified(self.onFrameModified)
def createHandGoalFrame(self): sides = [] if self.getReachHand() == 'Left': sides.append('left') elif self.getReachHand() == 'Right': sides.append('right') elif self.getReachHand() == 'Both': sides.append('left') sides.append('right') self.removeHandFrames() folder = self.getConstraintFrameFolder() startPose = np.array(self.robotStateJointController.q) for side in sides: linkName = self.ikPlanner.getHandLink(side) frameName = '%s constraint frame' % linkName graspToHand = self.ikPlanner.newPalmOffsetGraspToHandFrame( side, self.palmOffsetDistance) graspToWorld = self.ikPlanner.newGraspToWorldFrame( startPose, side, graspToHand) frame = vis.showFrame(graspToWorld, frameName, parent=folder, scale=0.2) frame.connectFrameModified(self.onGoalFrameModified)
def makeGoalFrames(self): for linkName, positionGoal, _ in self.positionCosts: orientationGoal = [0.0, 0.0, 0.0] t = transformUtils.frameFromPositionAndRPY(positionGoal, orientationGoal) f = vis.showFrame(t, "%s position goal" % linkName) f.connectFrameModified(self.onGoalFrameModified)
def toggleFootstepWidget(displayPoint, view, useHorizontalWidget=False): obj, _ = vis.findPickedObject(displayPoint, view) if not obj: return False name = obj.getProperty('Name') if name in ('footstep widget', 'footstep widget frame'): om.removeFromObjectModel(om.findObjectByName('footstep widget')) return True match = re.match('^step (\d+)$', name) if not match: return False stepIndex = int(match.group(1)) existingWidget = om.findObjectByName('footstep widget') if existingWidget: previousStep = existingWidget.stepIndex print 'have existing widget for step:', stepIndex om.removeFromObjectModel(existingWidget) if previousStep == stepIndex: print 'returning because widget was for selected step' return True footMesh = shallowCopy(obj.polyData) footFrame = transformUtils.copyFrame(obj.getChildFrame().transform) if useHorizontalWidget: rpy = [0.0, 0.0, transformUtils.rollPitchYawFromTransform(footFrame)[2]] footFrame = transformUtils.frameFromPositionAndRPY(footFrame.GetPosition(), np.degrees(rpy)) footObj = vis.showPolyData(footMesh, 'footstep widget', parent='planning', alpha=0.2) footObj.stepIndex = stepIndex frameObj = vis.showFrame(footFrame, 'footstep widget frame', parent=footObj, scale=0.2) footObj.actor.SetUserTransform(frameObj.transform) footObj.setProperty('Color', obj.getProperty('Color')) frameObj.setProperty('Edit', True) rep = frameObj.widget.GetRepresentation() rep.SetTranslateAxisEnabled(2, False) rep.SetRotateAxisEnabled(0, False) rep.SetRotateAxisEnabled(1, False) frameObj.widget.HandleRotationEnabledOff() walkGoal = om.findObjectByName('walking goal') if walkGoal: walkGoal.setProperty('Edit', False) def onFootWidgetChanged(frame): footstepsDriver.onStepModified(stepIndex - 1, frame) frameObj.connectFrameModified(onFootWidgetChanged) return True
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 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 makeSplineGraspConstraints(self, ikPlanner, positionTolerance=0.03, angleToleranceInDegrees=15): params = self.computeHandleParameterization() segments = zip(params, params[1:]) #times = [np.linspace(segment[0], segment[1], 6) for segment in segments] #times = [[0.0, 0.3, 0.5], np.linspace(params[-2], params[-1], 6)] times = np.linspace(params[-2], params[-1], 6) times = np.hstack(times) times = np.unique(times) frames = [] for t in times: frames.append(self.splineInterp(t)) folder = om.getOrCreateContainer('constraint spline samples', parentObj=om.getOrCreateContainer('debug')) for f in frames: vis.showFrame(f, 'frame', scale=0.1) side = self.side graspToPalm = vtk.vtkTransform() graspToHand = ikPlanner.newGraspToHandFrame(side, graspToPalm) constraints = [] for f, t in zip(frames[:-1], times[:-1]): graspToWorld = f p, q = ikPlanner.createPositionOrientationGraspConstraints(side, graspToWorld, graspToHand) p.lowerBound = np.tile(-positionTolerance, 3) p.upperBound = np.tile(positionTolerance, 3) q.angleToleranceInDegrees = angleToleranceInDegrees if t >= params[-2]: q.angleToleranceInDegrees = 0 p.lowerBound = np.tile(-0.0, 3) p.upperBound = np.tile(0.0, 3) constraints.append(q) p.tspan = [t, t] q.tspan = [t, t] constraints.append(p) return constraints
def getRoomSweepFrames(self, rotateHandFrame=False): topFrame = transformUtils.frameFromPositionAndRPY([0.65, 0.0, 0.8], [160, 0, 90]) yawFrame = transformUtils.frameFromPositionAndRPY( [0, 0.0, 0], [0, 0, self.currentYawDegrees]) if rotateHandFrame: fixHandFrame = transformUtils.frameFromPositionAndRPY([0, 0.0, 0], [0, -90, 0]) topFrame.PreMultiply() topFrame.Concatenate(fixHandFrame) topFrame.PostMultiply() topFrame.Concatenate(yawFrame) bottomFrame = transformUtils.frameFromPositionAndRPY([0.6, 0.0, 0.4], [210, 0, 90]) yawFrame = transformUtils.frameFromPositionAndRPY( [0, 0.0, 0], [0, 0, self.currentYawDegrees]) if rotateHandFrame: bottomFrame.PreMultiply() bottomFrame.Concatenate(fixHandFrame) bottomFrame.PostMultiply() bottomFrame.Concatenate(yawFrame) if (self.fromTop): self.startFrame = vis.showFrame(topFrame, 'frame start', visible=False, scale=0.1, parent=self.mapFolder) self.endFrame = vis.showFrame(bottomFrame, 'frame end', visible=False, scale=0.1, parent=self.mapFolder) else: self.startFrame = vis.showFrame(bottomFrame, 'frame start', visible=False, scale=0.1, parent=self.mapFolder) self.endFrame = vis.showFrame(topFrame, 'frame end', visible=False, scale=0.1, parent=self.mapFolder)
def toggleFootstepWidget(displayPoint, view, useHorizontalWidget=False): obj, _ = vis.findPickedObject(displayPoint, view) if not obj: return False name = obj.getProperty("Name") if name in ("footstep widget", "footstep widget frame"): om.removeFromObjectModel(om.findObjectByName("footstep widget")) return True match = re.match("^step (\d+)$", name) if not match: return False stepIndex = int(match.group(1)) existingWidget = om.findObjectByName("footstep widget") if existingWidget: previousStep = existingWidget.stepIndex om.removeFromObjectModel(existingWidget) if previousStep == stepIndex: return True footMesh = shallowCopy(obj.polyData) footFrame = transformUtils.copyFrame(obj.getChildFrame().transform) if useHorizontalWidget: rpy = [0.0, 0.0, transformUtils.rollPitchYawFromTransform(footFrame)[2]] footFrame = transformUtils.frameFromPositionAndRPY( footFrame.GetPosition(), np.degrees(rpy) ) footObj = vis.showPolyData( footMesh, "footstep widget", parent="planning", alpha=0.2 ) footObj.stepIndex = stepIndex frameObj = vis.showFrame( footFrame, "footstep widget frame", parent=footObj, scale=0.2 ) footObj.actor.SetUserTransform(frameObj.transform) footObj.setProperty("Color", obj.getProperty("Color")) frameObj.setProperty("Edit", True) rep = frameObj.widget.GetRepresentation() rep.SetTranslateAxisEnabled(2, False) rep.SetRotateAxisEnabled(0, False) rep.SetRotateAxisEnabled(1, False) frameObj.widget.HandleRotationEnabledOff() walkGoal = om.findObjectByName("walking goal") if walkGoal: walkGoal.setProperty("Edit", False) return True
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 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 computeNextRoomFrame(self): assert self.targetAffordance t = transformUtils.frameFromPositionAndRPY(self.nextPosition, [0, 0, 0]) self.faceTransformLocal = transformUtils.copyFrame(t) # copy required t.Concatenate(self.targetFrame) self.faceFrameDesired = vis.showFrame(t, 'target frame desired', parent=self.targetAffordance, visible=False, scale=0.2)
def run(self): folder = om.getOrCreateContainer('affordances') frame = self.computeAffordanceFrame() mesh = segmentation.getDrillMesh() params = segmentation.getDrillAffordanceParams(np.array(frame.GetPosition()), [1,0,0], [0,1,0], [0,0,1]) affordance = vis.showPolyData(mesh, 'drill', color=[0.0, 1.0, 0.0], cls=affordanceitems.FrameAffordanceItem, parent=folder) frame = vis.showFrame(frame, 'drill frame', parent=affordance, visible=False, scale=0.2) affordance.actor.SetUserTransform(frame.transform) affordance.setAffordanceParams(params) affordance.updateParamsFromActorTransform()
def addNewFrame(): t = transformUtils.copyFrame(affordanceObj.getChildFrame().transform) t.PostMultiply() t.Translate(np.array(pickedPoint) - np.array(t.GetPosition())) newFrame = vis.showFrame( t, '%s frame %d' % (affordanceObj.getProperty('Name'), len(affordanceObj.children())), scale=0.2, parent=affordanceObj) affordanceObj.getChildFrame().getFrameSync().addFrame( newFrame, ignoreIncoming=True)
def drawFrameInCamera(t, frameName='new frame',visible=True): v = imageView.view q = cameraview.imageManager.queue localToCameraT = vtk.vtkTransform() q.getTransform('local', 'CAMERA_LEFT', localToCameraT) res = vis.showFrame( vtk.vtkTransform() , 'temp',view=v, visible=True, scale = 0.2) om.removeFromObjectModel(res) pd = res.polyData pd = filterUtils.transformPolyData(pd, t) pd = filterUtils.transformPolyData(pd, localToCameraT) q.projectPoints('CAMERA_LEFT', pd ) vis.showPolyData(pd, ('overlay ' + frameName), view=v, colorByName='Axes',parent='camera overlay',visible=visible)
def getRoomSweepFrames(self, rotateHandFrame=False): topFrame = transformUtils.frameFromPositionAndRPY([0.65,0.0,0.8],[160,0,90]) yawFrame = transformUtils.frameFromPositionAndRPY([0,0.0,0],[0,0,self.currentYawDegrees]) if rotateHandFrame: fixHandFrame = transformUtils.frameFromPositionAndRPY([0,0.0,0],[0,-90,0]) topFrame.PreMultiply() topFrame.Concatenate( fixHandFrame ) topFrame.PostMultiply() topFrame.Concatenate( yawFrame ) bottomFrame = transformUtils.frameFromPositionAndRPY([0.6,0.0,0.4],[210,0,90]) yawFrame = transformUtils.frameFromPositionAndRPY([0,0.0,0],[0,0,self.currentYawDegrees]) if rotateHandFrame: bottomFrame.PreMultiply() bottomFrame.Concatenate( fixHandFrame ) bottomFrame.PostMultiply() bottomFrame.Concatenate( yawFrame ) if (self.fromTop): self.startFrame = vis.showFrame(topFrame, 'frame start', visible=False, scale=0.1,parent=self.mapFolder) self.endFrame = vis.showFrame(bottomFrame, 'frame end', visible=False, scale=0.1,parent=self.mapFolder) else: self.startFrame = vis.showFrame(bottomFrame, 'frame start', visible=False, scale=0.1,parent=self.mapFolder) self.endFrame = vis.showFrame(topFrame, 'frame end', visible=False, scale=0.1,parent=self.mapFolder)
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 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 planRoomSweep(self): self.initConstraintSet() faceFrameDesired = transformUtils.frameInterpolate( self.startFrame.transform, self.endFrame.transform, 0) vis.showFrame(faceFrameDesired, 'frame 0', visible=True, scale=0.1, parent=self.mapFolder) self.addConstraintForTargetFrame(faceFrameDesired, 0) faceFrameDesired = transformUtils.frameInterpolate( self.startFrame.transform, self.endFrame.transform, 1.0 / 3.0) vis.showFrame(faceFrameDesired, 'frame 1', visible=True, scale=0.1, parent=self.mapFolder) self.addConstraintForTargetFrame(faceFrameDesired, 1) faceFrameDesired = transformUtils.frameInterpolate( self.startFrame.transform, self.endFrame.transform, 2.0 / 3.0) vis.showFrame(faceFrameDesired, 'frame 2', visible=True, scale=0.1, parent=self.mapFolder) self.addConstraintForTargetFrame(faceFrameDesired, 2) faceFrameDesired = transformUtils.frameInterpolate( self.startFrame.transform, self.endFrame.transform, 3.0 / 3.0) vis.showFrame(faceFrameDesired, 'frame 3', visible=True, scale=0.1, parent=self.mapFolder) self.addConstraintForTargetFrame(faceFrameDesired, 3) #self.ikPlanner.ikServer.maxDegreesPerSecond = self.speedLow self.planTrajectory()
def makePostGraspFrame(obj, graspFrameName): graspFrame = om.findObjectByName(graspFrameName).transform goalTransform = vtk.vtkTransform() goalTransform.Translate(0, 0, 0.10) # Copy the resulting matrix. graspFrame can move around if the # robot's frame moves when the gripper occludes the mocap. goalTransform.SetMatrix( transformUtils.concatenateTransforms([graspFrame, goalTransform]).GetMatrix()) goalFrameName = "postgrasp to world" om.removeFromObjectModel(om.findObjectByName(goalFrameName)) goalFrame = vis.showFrame(goalTransform, goalFrameName, scale=0.1, parent=obj, visible=False) obj.getChildFrame().getFrameSync().addFrame(goalFrame, ignoreIncoming=True)
def planRoomSweep(self): self.initConstraintSet() faceFrameDesired = transformUtils.frameInterpolate(self.startFrame.transform , self.endFrame.transform, 0) vis.showFrame(faceFrameDesired, 'frame 0', visible=True, scale=0.1,parent=self.mapFolder) self.addConstraintForTargetFrame(faceFrameDesired, 0) faceFrameDesired = transformUtils.frameInterpolate(self.startFrame.transform , self.endFrame.transform, 1.0/3.0) vis.showFrame(faceFrameDesired, 'frame 1', visible=True, scale=0.1,parent=self.mapFolder) self.addConstraintForTargetFrame(faceFrameDesired, 1) faceFrameDesired = transformUtils.frameInterpolate(self.startFrame.transform , self.endFrame.transform, 2.0/3.0) vis.showFrame(faceFrameDesired, 'frame 2', visible=True, scale=0.1,parent=self.mapFolder) self.addConstraintForTargetFrame(faceFrameDesired, 2) faceFrameDesired = transformUtils.frameInterpolate(self.startFrame.transform , self.endFrame.transform, 3.0/3.0) vis.showFrame(faceFrameDesired, 'frame 3', visible=True, scale=0.1,parent=self.mapFolder) self.addConstraintForTargetFrame(faceFrameDesired, 3) #self.ikPlanner.ikServer.maxDegreesPerSecond = self.speedLow self.planTrajectory()
def createHandGoalFrame(self): sides = [] if self.getReachHand() == 'Left': sides.append('left') elif self.getReachHand() == 'Right': sides.append('right') elif self.getReachHand() == 'Both': sides.append('left') sides.append('right') self.removeHandFrames() folder = self.getConstraintFrameFolder() startPose = self.planningUtils.getPlanningStartPose() for side in sides: linkName = self.ikPlanner.getHandLink(side) frameName = '%s constraint frame' % linkName graspToHand = self.ikPlanner.newPalmOffsetGraspToHandFrame(side, self.palmOffsetDistance) graspToWorld = self.ikPlanner.newGraspToWorldFrame(startPose, side, graspToHand) frame = vis.showFrame(graspToWorld, frameName, parent=folder, scale=0.2) frame.connectFrameModified(self.onGoalFrameModified)
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 toggleFootstepWidget(displayPoint, view, useHorizontalWidget=False): obj, _ = vis.findPickedObject(displayPoint, view) if not obj: return False name = obj.getProperty('Name') if name in ('footstep widget', 'footstep widget frame'): om.removeFromObjectModel(om.findObjectByName('footstep widget')) return True match = re.match('^step (\d+)$', name) if not match: return False stepIndex = int(match.group(1)) existingWidget = om.findObjectByName('footstep widget') if existingWidget: previousStep = existingWidget.stepIndex om.removeFromObjectModel(existingWidget) if previousStep == stepIndex: return True footMesh = shallowCopy(obj.polyData) footFrame = transformUtils.copyFrame(obj.getChildFrame().transform) if useHorizontalWidget: rpy = [ 0.0, 0.0, transformUtils.rollPitchYawFromTransform(footFrame)[2] ] footFrame = transformUtils.frameFromPositionAndRPY( footFrame.GetPosition(), np.degrees(rpy)) footObj = vis.showPolyData(footMesh, 'footstep widget', parent='planning', alpha=0.2) footObj.stepIndex = stepIndex frameObj = vis.showFrame(footFrame, 'footstep widget frame', parent=footObj, scale=0.2) footObj.actor.SetUserTransform(frameObj.transform) footObj.setProperty('Color', obj.getProperty('Color')) frameObj.setProperty('Edit', True) rep = frameObj.widget.GetRepresentation() rep.SetTranslateAxisEnabled(2, False) rep.SetRotateAxisEnabled(0, False) rep.SetRotateAxisEnabled(1, False) frameObj.widget.HandleRotationEnabledOff() walkGoal = om.findObjectByName('walking goal') if walkGoal: walkGoal.setProperty('Edit', False) def onFootWidgetChanged(frame): footstepsDriver.onStepModified(stepIndex - 1, frame) frameObj.connectFrameModified(onFootWidgetChanged) return True
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 addNewFrame(): t = transformUtils.copyFrame(affordanceObj.getChildFrame().transform) t.PostMultiply() t.Translate(np.array(pickedPoint) - np.array(t.GetPosition())) newFrame = vis.showFrame(t, '%s frame %d' % (affordanceObj.getProperty('Name'), len(affordanceObj.children())), scale=0.2, parent=affordanceObj) affordanceObj.getChildFrame().getFrameSync().addFrame(newFrame, ignoreIncoming=True)
def drawFootstepPlan(self, msg, folder, left_color=None, right_color=None, alpha=1.0): for step in folder.children(): om.removeFromObjectModel(step) allTransforms = [] volFolder = getWalkingVolumesFolder() map(om.removeFromObjectModel, volFolder.children()) slicesFolder = getTerrainSlicesFolder() map(om.removeFromObjectModel, slicesFolder.children()) for i, footstep in enumerate(msg.footsteps): trans = footstep.pos.translation trans = [trans.x, trans.y, trans.z] quat = footstep.pos.rotation quat = [quat.w, quat.x, quat.y, quat.z] footstepTransform = transformUtils.transformFromPose(trans, quat) allTransforms.append(footstepTransform) if i < 2: continue if footstep.is_right_foot: mesh = getRightFootMesh() if (right_color is None): color = getRightFootColor() else: color = right_color else: mesh = getLeftFootMesh() if (left_color is None): color = getLeftFootColor() else: color = left_color # add gradual shading to steps to indicate destination frac = float(i)/ float(msg.num_steps-1) this_color = [0,0,0] this_color[0] = 0.25*color[0] + 0.75*frac*color[0] this_color[1] = 0.25*color[1] + 0.75*frac*color[1] this_color[2] = 0.25*color[2] + 0.75*frac*color[2] if self.show_contact_slices: self.drawContactVolumes(footstepTransform, color) contact_pts_left, contact_pts_right = FootstepsDriver.getContactPts() if footstep.is_right_foot: sole_offset = np.mean(contact_pts_right, axis=0) else: sole_offset = np.mean(contact_pts_left, axis=0) t_sole_prev = frameFromPositionMessage(msg.footsteps[i-2].pos) t_sole_prev.PreMultiply() t_sole_prev.Translate(sole_offset) t_sole = transformUtils.copyFrame(footstepTransform) t_sole.Translate(sole_offset) yaw = np.arctan2(t_sole.GetPosition()[1] - t_sole_prev.GetPosition()[1], t_sole.GetPosition()[0] - t_sole_prev.GetPosition()[0]) T_terrain_to_world = transformUtils.frameFromPositionAndRPY([t_sole_prev.GetPosition()[0], t_sole_prev.GetPosition()[1], 0], [0, 0, math.degrees(yaw)]) path_dist = np.array(footstep.terrain_path_dist) height = np.array(footstep.terrain_height) # if np.any(height >= trans[2]): terrain_pts_in_local = np.vstack((path_dist, np.zeros(len(footstep.terrain_path_dist)), height)) d = DebugData() for j in range(terrain_pts_in_local.shape[1]-1): d.addLine(terrain_pts_in_local[:,j], terrain_pts_in_local[:,j+1], radius=0.01) obj = vis.showPolyData(d.getPolyData(), 'terrain slice', parent=slicesFolder, visible=slicesFolder.getProperty('Visible'), color=[.8,.8,.3]) obj.actor.SetUserTransform(T_terrain_to_world) renderInfeasibility = False if renderInfeasibility and footstep.infeasibility > 1e-6: d = DebugData() start = allTransforms[i-1].GetPosition() end = footstepTransform.GetPosition() d.addArrow(start, end, 0.02, 0.005, startHead=True, endHead=True) vis.showPolyData(d.getPolyData(), 'infeasibility %d -> %d' % (i-2, i-1), parent=folder, color=[1, 0.2, 0.2]) stepName = 'step %d' % (i-1) obj = vis.showPolyData(mesh, stepName, color=this_color, alpha=alpha, parent=folder) obj.setIcon(om.Icons.Feet) frameObj = vis.showFrame(footstepTransform, stepName + ' frame', parent=obj, scale=0.3, visible=False) obj.actor.SetUserTransform(footstepTransform) obj.addProperty('Support Contact Groups', footstep.params.support_contact_groups, attributes=om.PropertyAttributes(enumNames=['Whole Foot', 'Front 2/3', 'Back 2/3'])) obj.properties.setPropertyIndex('Support Contact Groups', 0) obj.footstep_index = i obj.footstep_property_callback = obj.properties.connectPropertyChanged(functools.partial(self.onFootstepPropertyChanged, obj)) self.drawContactPts(obj, footstep, color=this_color)
from director import visualization as vis from director import consoleapp from director import vtkAll as vtk import numpy as np app = consoleapp.ConsoleApp() view = app.createView() view.show() t = vtk.vtkTransform() obj = vis.showFrame(t, 'frame') obj.setProperty('Trace', True) # move the frame along a spiral to create a path trace for theta in np.linspace(0, 30, 1000): p1 = np.array(t.GetPosition()) p2 = np.array([theta * 0.03, np.sin(theta) * 0.1, np.cos(theta) * 0.1]) t.Translate(p2 - p1) t.Modified() view.resetCamera() app.start()
def addFrame(self, frameName): t = self.getFrameTransform(frameName) folder = self.getFramesFolder() frame = vis.showFrame(t, frameName, parent=folder, scale=0.2) frame.setProperty("Trace", True)
def addFrame(self, frameName): t = self.getFrameTransform(frameName) folder = self.getFramesFolder() frame = vis.showFrame(t, frameName, parent=folder, scale=0.2) frame.setProperty('Trace', True)