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 fitSwitchBox(self, polyData, points): boxPosition = points[0] wallPoint = points[1] # find a frame that is aligned with wall searchRadius = 0.2 planePoints, normal = segmentation.applyLocalPlaneFit(polyData, points[0], searchRadius=np.linalg.norm(points[1] - points[0]), searchRadiusEnd=1.0) obj = vis.updatePolyData(planePoints, 'wall plane points', color=[0,1,0], visible=False) obj.setProperty('Point Size', 7) viewDirection = segmentation.SegmentationContext.getGlobalInstance().getViewDirection() if np.dot(normal, viewDirection) < 0: normal = -normal origin = segmentation.computeCentroid(planePoints) zaxis = [0,0,1] xaxis = normal yaxis = np.cross(zaxis, xaxis) yaxis /= np.linalg.norm(yaxis) zaxis = np.cross(xaxis, yaxis) zaxis /= np.linalg.norm(zaxis) t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis) # translate that frame to the box position t.PostMultiply() t.Translate(boxPosition) boxFrame = transformUtils.copyFrame(t) self.switchPlanner.spawnBoxAffordanceAtFrame(boxFrame)
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 placeHandModel(displayPoint, view, side='left'): obj, _ = vis.findPickedObject(displayPoint, view) if isinstance(obj, vis.FrameItem): _, handFrame = handFactory.placeHandModelWithTransform(obj.transform, view, side=side, parent=obj.parent()) handFrame.frameSync = vis.FrameSync() handFrame.frameSync.addFrame(obj) handFrame.frameSync.addFrame(handFrame, ignoreIncoming=True) return pickedPoint, prop, _, normal = vis.pickPoint(displayPoint, view, pickType='cells', tolerance=0.0, returnNormal=True) obj = vis.getObjectByProp(prop) if not obj: return yaxis = -normal zaxis = [0,0,1] xaxis = np.cross(yaxis, zaxis) xaxis /= np.linalg.norm(xaxis) zaxis = np.cross(xaxis, yaxis) zaxis /= np.linalg.norm(zaxis) t = transformUtils.getTransformFromAxes(-zaxis, yaxis, xaxis) t.PostMultiply() t.Translate(pickedPoint) handObj, handFrame = handFactory.placeHandModelWithTransform(t, view, side=side, parent=obj) syncFrame = getChildFrame(obj) if syncFrame: handFrame.frameSync = vis.FrameSync() handFrame.frameSync.addFrame(handFrame, ignoreIncoming=True) handFrame.frameSync.addFrame(syncFrame)
def fitDrillBarrel ( drillPoints, forwardDirection, plane_origin, plane_normal): ''' Given a point cloud which ONLY contains points from a barrell drill, standing upright and the equations of a table its resting on, and the general direction of the robot Fit a barrell drill ''' if not drillPoints.GetNumberOfPoints(): return vis.updatePolyData(drillPoints, 'drill cluster', parent=getDebugFolder(), visible=False) drillBarrelPoints = thresholdPoints(drillPoints, 'dist_to_plane', [0.177, 0.30]) if not drillBarrelPoints.GetNumberOfPoints(): return # fit line to drill barrel points linePoint, lineDirection, _ = applyLineFit(drillBarrelPoints, distanceThreshold=0.5) if np.dot(lineDirection, forwardDirection) < 0: lineDirection = -lineDirection vis.updatePolyData(drillBarrelPoints, 'drill barrel points', parent=getDebugFolder(), visible=False) pts = vtkNumpy.getNumpyFromVtk(drillBarrelPoints, 'Points') dists = np.dot(pts-linePoint, lineDirection) p1 = linePoint + lineDirection*np.min(dists) p2 = linePoint + lineDirection*np.max(dists) p1 = projectPointToPlane(p1, plane_origin, plane_normal) p2 = projectPointToPlane(p2, plane_origin, plane_normal) d = DebugData() d.addSphere(p1, radius=0.01) d.addSphere(p2, radius=0.01) d.addLine(p1, p2) vis.updatePolyData(d.getPolyData(), 'drill debug points', color=[0,1,0], parent=getDebugFolder(), visible=False) drillToBasePoint = np.array([-0.07, 0.0 , -0.12]) zaxis = plane_normal xaxis = lineDirection xaxis /= np.linalg.norm(xaxis) yaxis = np.cross(zaxis, xaxis) yaxis /= np.linalg.norm(yaxis) xaxis = np.cross(yaxis, zaxis) xaxis /= np.linalg.norm(xaxis) t = getTransformFromAxes(xaxis, yaxis, zaxis) t.PreMultiply() t.Translate(-drillToBasePoint) t.PostMultiply() t.Translate(p1) return t
def computeRobotStanceFrame(self, objectTransform, relativeStanceTransform): ''' Given a robot model, determine the height of the ground using an XY and Yaw standoff, combined to determine the relative 6DOF standoff For a grasp or approach stance ''' groundFrame = self.computeGroundFrame(self.robotModel) groundHeight = groundFrame.GetPosition()[2] graspPosition = np.array(objectTransform.GetPosition()) graspYAxis = [0.0, 1.0, 0.0] graspZAxis = [0.0, 0.0, 1.0] objectTransform.TransformVector(graspYAxis, graspYAxis) objectTransform.TransformVector(graspZAxis, graspZAxis) xaxis = graspYAxis #xaxis = graspZAxis zaxis = [0, 0, 1] yaxis = np.cross(zaxis, xaxis) yaxis /= np.linalg.norm(yaxis) xaxis = np.cross(yaxis, zaxis) graspGroundTransform = transformUtils.getTransformFromAxes( xaxis, yaxis, zaxis) graspGroundTransform.PostMultiply() graspGroundTransform.Translate(graspPosition[0], graspPosition[1], groundHeight) robotStance = transformUtils.copyFrame(relativeStanceTransform) robotStance.Concatenate(graspGroundTransform) return robotStance
def computeGroundFrame(self, robotModel): ''' Given a robol model, returns a vtkTransform at a position between the feet, on the ground, with z-axis up and x-axis aligned with the robot pelvis x-axis. ''' t1 = robotModel.getLinkFrame(self.ikPlanner.leftFootLink) t2 = robotModel.getLinkFrame(self.ikPlanner.rightFootLink) pelvisT = robotModel.getLinkFrame(self.ikPlanner.pelvisLink) xaxis = [1.0, 0.0, 0.0] pelvisT.TransformVector(xaxis, xaxis) xaxis = np.array(xaxis) zaxis = np.array([0.0, 0.0, 1.0]) yaxis = np.cross(zaxis, xaxis) yaxis /= np.linalg.norm(yaxis) xaxis = np.cross(yaxis, zaxis) stancePosition = (np.array(t2.GetPosition()) + np.array(t1.GetPosition())) / 2.0 footHeight = 0.0811 t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis) t.PostMultiply() t.Translate(stancePosition) t.Translate([0.0, 0.0, -footHeight]) return t
def computeRobotStanceFrame(self, objectTransform, relativeStanceTransform): """ Given a robot model, determine the height of the ground using an XY and Yaw standoff, combined to determine the relative 6DOF standoff For a grasp or approach stance """ groundFrame = self.computeGroundFrame(self.robotModel) groundHeight = groundFrame.GetPosition()[2] graspPosition = np.array(objectTransform.GetPosition()) graspYAxis = [0.0, 1.0, 0.0] graspZAxis = [0.0, 0.0, 1.0] objectTransform.TransformVector(graspYAxis, graspYAxis) objectTransform.TransformVector(graspZAxis, graspZAxis) xaxis = graspYAxis # xaxis = graspZAxis zaxis = [0, 0, 1] yaxis = np.cross(zaxis, xaxis) yaxis /= np.linalg.norm(yaxis) xaxis = np.cross(yaxis, zaxis) graspGroundTransform = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis) graspGroundTransform.PostMultiply() graspGroundTransform.Translate(graspPosition[0], graspPosition[1], groundHeight) robotStance = transformUtils.copyFrame(relativeStanceTransform) robotStance.Concatenate(graspGroundTransform) return robotStance
def computeGroundFrame(self, robotModel): """ Given a robol model, returns a vtkTransform at a position between the feet, on the ground, with z-axis up and x-axis aligned with the robot pelvis x-axis. """ t1 = robotModel.getLinkFrame(self.ikPlanner.leftFootLink) t2 = robotModel.getLinkFrame(self.ikPlanner.rightFootLink) pelvisT = robotModel.getLinkFrame(self.ikPlanner.pelvisLink) xaxis = [1.0, 0.0, 0.0] pelvisT.TransformVector(xaxis, xaxis) xaxis = np.array(xaxis) zaxis = np.array([0.0, 0.0, 1.0]) yaxis = np.cross(zaxis, xaxis) yaxis /= np.linalg.norm(yaxis) xaxis = np.cross(yaxis, zaxis) stancePosition = (np.array(t2.GetPosition()) + np.array(t1.GetPosition())) / 2.0 footHeight = 0.0811 t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis) t.PostMultiply() t.Translate(stancePosition) t.Translate([0.0, 0.0, -footHeight]) return t
def onSegmentBin(self, p1, p2): print p1 print p2 self.picker.stop() om.removeFromObjectModel(self.picker.annotationObj) self.picker = None om.removeFromObjectModel(om.findObjectByName('bin frame')) binEdge = p2 - p1 zaxis = [0.0, 0.0, 1.0] xaxis = np.cross(binEdge, zaxis) xaxis /= np.linalg.norm(xaxis) yaxis = np.cross(zaxis, xaxis) t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis) t.PostMultiply() t.Translate(p1) self.binFrame = vis.showFrame(t, 'bin frame', parent=None, scale=0.2)
def createGoalSteps(self, model, pose): distanceForward = 1.0 fr = model.getLinkFrame(_leftFootLink) fl = model.getLinkFrame(_rightFootLink) pelvisT = model.getLinkFrame(_pelvisLink) xaxis = [1.0, 0.0, 0.0] pelvisT.TransformVector(xaxis, xaxis) xaxis = np.array(xaxis) zaxis = np.array([0.0, 0.0, 1.0]) yaxis = np.cross(zaxis, xaxis) xaxis = np.cross(yaxis, zaxis) numGoalSteps = 3 is_right_foot = True self.goalSteps = [] for i in range(numGoalSteps): t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis) t.PostMultiply() if is_right_foot: t.Translate(fr.GetPosition()) else: t.Translate(fl.GetPosition()) t.Translate(xaxis*distanceForward) distanceForward += 0.15 is_right_foot = not is_right_foot step = lcmdrc.footstep_t() step.pos = transformUtils.positionMessageFromFrame(t) step.is_right_foot = is_right_foot step.params = self.getDefaultStepParams() self.goalSteps.append(step) request = self.constructFootstepPlanRequest(pose) request.num_goal_steps = len(self.goalSteps) request.goal_steps = self.goalSteps self.sendFootstepPlanRequest(request)
def fitDrillBarrel(drillPoints, forwardDirection, plane_origin, plane_normal): ''' Given a point cloud which ONLY contains points from a barrell drill, standing upright and the equations of a table its resting on, and the general direction of the robot Fit a barrell drill ''' if not drillPoints.GetNumberOfPoints(): return vis.updatePolyData(drillPoints, 'drill cluster', parent=getDebugFolder(), visible=False) drillBarrelPoints = thresholdPoints(drillPoints, 'dist_to_plane', [0.177, 0.30]) if not drillBarrelPoints.GetNumberOfPoints(): return # fit line to drill barrel points linePoint, lineDirection, _ = applyLineFit(drillBarrelPoints, distanceThreshold=0.5) if np.dot(lineDirection, forwardDirection) < 0: lineDirection = -lineDirection vis.updatePolyData(drillBarrelPoints, 'drill barrel points', parent=getDebugFolder(), visible=False) pts = vtkNumpy.getNumpyFromVtk(drillBarrelPoints, 'Points') dists = np.dot(pts - linePoint, lineDirection) p1 = linePoint + lineDirection * np.min(dists) p2 = linePoint + lineDirection * np.max(dists) p1 = projectPointToPlane(p1, plane_origin, plane_normal) p2 = projectPointToPlane(p2, plane_origin, plane_normal) d = DebugData() d.addSphere(p1, radius=0.01) d.addSphere(p2, radius=0.01) d.addLine(p1, p2) vis.updatePolyData(d.getPolyData(), 'drill debug points', color=[0, 1, 0], parent=getDebugFolder(), visible=False) drillToBasePoint = np.array([-0.07, 0.0, -0.12]) zaxis = plane_normal xaxis = lineDirection xaxis /= np.linalg.norm(xaxis) yaxis = np.cross(zaxis, xaxis) yaxis /= np.linalg.norm(yaxis) xaxis = np.cross(yaxis, zaxis) xaxis /= np.linalg.norm(xaxis) t = getTransformFromAxes(xaxis, yaxis, zaxis) t.PreMultiply() t.Translate(-drillToBasePoint) t.PostMultiply() t.Translate(p1) return t