def drawForce(self, name, linkName, forceLocation, force, color, key=None): forceDirection = force / np.linalg.norm(force) # om.removeFromObjectModel(om.findObjectByName(name)) linkToWorld = self.robotStateModel.getLinkFrame(linkName) forceLocationInWorld = np.array( linkToWorld.TransformPoint(forceLocation)) forceDirectionInWorld = np.array( linkToWorld.TransformDoubleVector(forceDirection)) # point = forceLocationInWorld - 0.1*forceDirectionInWorld # 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) transformForVis = transformUtils.getTransformFromOriginAndNormal( forceLocationInWorld, forceDirectionInWorld) obj = vis.updatePolyData(self.plungerPolyData, name, color=color) obj.actor.SetUserTransform(transformForVis) if key is not None and om.findObjectByName(name) is not None: obj.addProperty('magnitude', self.externalForces[key]['forceMagnitude']) obj.addProperty('linkName', linkName) obj.addProperty('key', key) obj.connectRemovedFromObjectModel(self.removeForceFromFrameObject) obj.properties.connectPropertyChanged( functools.partial(self.onPropertyChanged, obj)) return obj
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 removePlaneAndBeyond(polyData, expectedNormal=[1,0,0], filterRange=[-np.inf, -0.03], whichAxis=1, whichAxisLetter='y', percentile = 95): yvalues = vnp.getNumpyFromVtk(polyData, 'Points')[:, whichAxis] backY = np.percentile(yvalues, percentile) if ( percentile > 50): searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter, [backY - 0.1, np.inf]) else: searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter, [-np.inf, backY + 0.1]) vis.updatePolyData(searchRegion, 'search region', parent="segmentation", colorByName=whichAxisLetter, visible=False) # find the plane of the back wall, remove it and the points behind it: _, origin, normal = segmentation.applyPlaneFit(searchRegion, distanceThreshold=0.02, expectedNormal=expectedNormal, perpendicularAxis=expectedNormal, returnOrigin=True) points = vnp.getNumpyFromVtk(polyData, 'Points') dist = np.dot(points - origin, normal) vnp.addNumpyToVtk(polyData, dist, 'dist_to_plane') backFrame = transformUtils.getTransformFromOriginAndNormal(origin, normal, normalAxis=2) vis.updateFrame(backFrame, 'back frame', parent='segmentation', scale=0.15 , visible=False) vis.updatePolyData(polyData, 'dist to back', parent='segmentation', visible=False) polyData = segmentation.thresholdPoints(polyData, 'dist_to_plane', filterRange) vis.updatePolyData(polyData, 'back off and all', parent='segmentation', visible=False) return polyData
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 removePlaneAndBeyond(polyData, expectedNormal=[1,0,0], filterRange=[-np.inf, -0.03], whichAxis=1, whichAxisLetter='y', percentile = 95): yvalues = vnp.getNumpyFromVtk(polyData, 'Points')[:, whichAxis] backY = np.percentile(yvalues, percentile) if ( percentile > 50): searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter, [backY - 0.1, np.inf]) else: searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter, [-np.inf, backY + 0.1]) vis.updatePolyData(searchRegion, 'search region', parent="segmentation", colorByName=whichAxisLetter, visible=False) # find the plane of the back wall, remove it and the points behind it: _, origin, normal = segmentation.applyPlaneFit(searchRegion, distanceThreshold=0.02, expectedNormal=expectedNormal, perpendicularAxis=expectedNormal, returnOrigin=True) points = vnp.getNumpyFromVtk(polyData, 'Points') dist = np.dot(points - origin, normal) vnp.addNumpyToVtk(polyData, dist, 'dist_to_plane') backFrame = transformUtils.getTransformFromOriginAndNormal(origin, normal, normalAxis=2) vis.updateFrame(backFrame, 'back frame', parent='segmentation', scale=0.15 , visible=False) vis.updatePolyData(polyData, 'dist to back', parent='segmentation', visible=False) polyData = segmentation.thresholdPoints(polyData, 'dist_to_plane', filterRange) vis.updatePolyData(polyData, 'back off and all', parent='segmentation', visible=False) return polyData
def computeGraspPlan(self, targetFrame, graspToHandFrame, inLine=False, ikParameters=None): startPose = self.getPlanningStartPose() endPose, constraintSet = self.computeGraspPose(startPose, targetFrame) if ikParameters: constraintSet.ikParameters = ikParameters constraintSet.ikParameters.usePointwise = False if inLine: handLinkName = self.ikPlanner.getHandLink(self.graspingHand) graspToHand = graspToHandFrame handToWorld1 = self.ikPlanner.getLinkFrameAtPose( handLinkName, startPose) handToWorld2 = self.ikPlanner.getLinkFrameAtPose( handLinkName, endPose) handToWorld1 = transformUtils.concatenateTransforms( [graspToHand, handToWorld1]) handToWorld2 = transformUtils.concatenateTransforms( [graspToHand, handToWorld2]) motionVector = np.array(handToWorld2.GetPosition()) - np.array( handToWorld1.GetPosition()) motionTargetFrame = transformUtils.getTransformFromOriginAndNormal( np.array(handToWorld2.GetPosition()), motionVector) #vis.updateFrame(motionTargetFrame, 'motion target frame', scale=0.1) #d = DebugData() #d.addLine(np.array(handToWorld2.GetPosition()), np.array(handToWorld2.GetPosition()) - motionVector) #vis.updatePolyData(d.getPolyData(), 'motion vector', visible=False) p = self.ikPlanner.createLinePositionConstraint( handLinkName, graspToHand, motionTargetFrame, lineAxis=2, bounds=[-np.linalg.norm(motionVector), 0.001], positionTolerance=0.001) p.tspan = np.linspace(0, 1, 5) constraintSet.constraints.append(p) newPlan = constraintSet.runIkTraj() else: newPlan = self.ikPlanner.computePostureGoal(startPose, endPose) return newPlan
def rotateReconstructionToStandardOrientation(self, pointCloud=None, savePoseToFile=True, filename=None): """ Rotates the reconstruction to right side up and saves the transform to a file :param pointcloud: :return: """ if pointCloud is None: pointCloud = om.findObjectByName('reconstruction').polyData returnData = self.segmentTable(scenePolyData=pointCloud, visualize=False) normal = returnData['normal'] polyData = returnData['polyData'] # get transform that rotates normal --> [0,0,1] origin = returnData['pointOnTable'] - 0.5 * normal pointCloudToWorldTransform = transformUtils.getTransformFromOriginAndNormal( origin, normal).GetLinearInverse() rotatedPolyData = filterUtils.transformPolyData( polyData, pointCloudToWorldTransform) parent = om.getOrCreateContainer('segmentation') vis.updatePolyData(rotatedPolyData, 'reconstruction', colorByName='RGB', parent=parent) if savePoseToFile: if filename is None: assert self.pathDict is not None filename = self.pathDict['transforms'] (pos, quat ) = transformUtils.poseFromTransform(pointCloudToWorldTransform) d = dict() d['firstFrameToWorld'] = [pos.tolist(), quat.tolist()] utils.saveDictToYaml(d, filename) return pointCloudToWorldTransform
def computeGraspPlan(self, targetFrame, graspToHandFrame, inLine=False, ikParameters=None): startPose = self.getPlanningStartPose() endPose, constraintSet = self.computeGraspPose(startPose, targetFrame) if ikParameters: constraintSet.ikParameters = ikParameters constraintSet.ikParameters.usePointwise = False if inLine: handLinkName = self.ikPlanner.getHandLink(self.graspingHand) graspToHand = graspToHandFrame handToWorld1 = self.ikPlanner.getLinkFrameAtPose(handLinkName, startPose) handToWorld2 = self.ikPlanner.getLinkFrameAtPose(handLinkName, endPose) handToWorld1 = transformUtils.concatenateTransforms([graspToHand, handToWorld1]) handToWorld2 = transformUtils.concatenateTransforms([graspToHand, handToWorld2]) motionVector = np.array(handToWorld2.GetPosition()) - np.array(handToWorld1.GetPosition()) motionTargetFrame = transformUtils.getTransformFromOriginAndNormal(np.array(handToWorld2.GetPosition()), motionVector) #vis.updateFrame(motionTargetFrame, 'motion target frame', scale=0.1) #d = DebugData() #d.addLine(np.array(handToWorld2.GetPosition()), np.array(handToWorld2.GetPosition()) - motionVector) #vis.updatePolyData(d.getPolyData(), 'motion vector', visible=False) p = self.ikPlanner.createLinePositionConstraint(handLinkName, graspToHand, motionTargetFrame, lineAxis=2, bounds=[-np.linalg.norm(motionVector), 0.001], positionTolerance=0.001) p.tspan = np.linspace(0, 1, 5) constraintSet.constraints.append(p) newPlan = constraintSet.runIkTraj() else: newPlan = self.ikPlanner.computePostureGoal(startPose, endPose) return newPlan