def getNominalPose(self): axes = transformUtils.getAxesFromTransform( self.computeGraspFrame().transform) yaxis = axes[1] yawDesired = np.arctan2(yaxis[1], yaxis[0]) seedDistance = 1 nominalPose = self.ikPlanner.jointController.getPose('q_nom') nominalPose[0] = (self.computeGraspFrame().transform.GetPosition()[0] - seedDistance * yaxis[0]) nominalPose[1] = (self.computeGraspFrame().transform.GetPosition()[1] - seedDistance * yaxis[1]) nominalPose[5] = yawDesired if self.scribeDirection == 1: # Clockwise nominalPose = self.ikPlanner.getMergedPostureFromDatabase( nominalPose, 'valve', 'reach-nominal-cw', side=self.graspingHand) else: # Counter-clockwise nominalPose = self.ikPlanner.getMergedPostureFromDatabase( nominalPose, 'valve', 'reach-nominal-ccw', side=self.graspingHand) return nominalPose
def planInsertTraj(self, speed, lockFeet=True, lockBase=None, resetBase=False, wristAngleCW=0, startPose=None, verticalOffset=0.01, usePoses=False, resetPoses=True, planFromCurrentRobotState=False, retract=False): ikParameters = IkParameters(usePointwise=False, maxDegreesPerSecond=speed, numberOfAddedKnots=1, quasiStaticShrinkFactor=self.quasiStaticShrinkFactor, fixInitialState=planFromCurrentRobotState) _, yaxis, _ = transformUtils.getAxesFromTransform(self.computeGraspFrame().transform) yawDesired = np.arctan2(yaxis[1], yaxis[0]) if startPose is None: startPose = self.getPlanningStartPose() nominalPose = self.getNominalPose() self.ikPlanner.addPose(nominalPose, self.nominalPoseName) self.ikPlanner.addPose(startPose, self.startPoseName) self.ikPlanner.reachingSide = self.graspingHand constraints = [] constraints.extend(self.createBaseConstraints(resetBase, lockBase, lockFeet, yawDesired)) constraints.append(self.createBackPostureConstraint()) constraints.append(self.ikPlanner.createQuasiStaticConstraint()) constraints.extend(self.createFootConstraints(lockFeet)) constraints.append(self.ikPlanner.createLockedArmPostureConstraint(self.startPoseName)) constraints.append(self.ikPlanner.createKneePostureConstraint([0.7, 2.5])) constraints.append(self.createElbowPostureConstraint()) constraints.append(self.createStaticTorqueConstraint()) constraints.append(self.createHandGazeConstraint()) constraints.append(self.createHandFixedOrientConstraint()) constraints.append(self.createWristAngleConstraint(wristAngleCW, planFromCurrentRobotState)) constraints.extend(self.createAllHandPositionConstraints(self.coaxialTol, retract)) if retract: startPoseName = self.getStartPoseName(planFromCurrentRobotState, True, usePoses) endPoseName = self.getEndPoseName(True, usePoses) endPose = self.ikPlanner.jointController.getPose(endPoseName) endPose = self.ikPlanner.mergePostures(endPose, robotstate.matchJoints('lwy'), startPose) endPoseName = 'q_retract' self.ikPlanner.addPose(endPose, endPoseName) else: startPoseName = self.getStartPoseName(planFromCurrentRobotState, retract, usePoses) endPoseName = self.getEndPoseName(retract, usePoses) plan = self.ikPlanner.runIkTraj(constraints, startPoseName, endPoseName, self.nominalPoseName, ikParameters=ikParameters) if resetPoses and not retract and max(plan.plan_info) <= 10: self.setReachAndTouchPoses(plan) return plan
def updateCamera(msg): T = transformUtils.frameFromPositionAndRPY(msg.q[:3], np.degrees(msg.q[3:])) axes = transformUtils.getAxesFromTransform(T) # vis.updateFrame(T, 'vicon camera') # return camera = app.view.camera() camera.SetPosition(T.GetPosition()) camera.SetFocalPoint(np.array(T.GetPosition()) + axes[0]) camera.SetViewUp(axes[2]) camera.SetViewAngle(122.6) app.view.render()
def updateCamera(msg): T = transformUtils.frameFromPositionAndRPY(msg.q[:3],np.degrees(msg.q[3:])) axes = transformUtils.getAxesFromTransform(T) #vis.updateFrame(T, 'vicon camera') #return camera = app.view.camera() camera.SetPosition(T.GetPosition()) camera.SetFocalPoint(np.array(T.GetPosition())+axes[0]) camera.SetViewUp(axes[2]) camera.SetViewAngle(122.6) app.view.render()
def getNominalPose(self): axes = transformUtils.getAxesFromTransform(self.computeGraspFrame().transform) yaxis = axes[1] yawDesired = np.arctan2(yaxis[1], yaxis[0]) seedDistance = 1 nominalPose = self.ikPlanner.jointController.getPose('q_nom') nominalPose[0] = (self.computeGraspFrame().transform.GetPosition()[0] - seedDistance*yaxis[0]) nominalPose[1] = (self.computeGraspFrame().transform.GetPosition()[1] - seedDistance*yaxis[1]) nominalPose[5] = yawDesired if self.scribeDirection == 1: # Clockwise nominalPose = self.ikPlanner.getMergedPostureFromDatabase(nominalPose, 'valve', 'reach-nominal-cw', side=self.graspingHand) else: # Counter-clockwise nominalPose = self.ikPlanner.getMergedPostureFromDatabase(nominalPose, 'valve', 'reach-nominal-ccw', side=self.graspingHand) return nominalPose
def testTransform(): ''' test transformFromPose --> getAxesFromTransform is same as quat --> matrix ''' quat = transformations.random_quaternion() pos = np.random.rand(3) frame = transformUtils.transformFromPose(pos, quat) axes = transformUtils.getAxesFromTransform(frame) mat = transformUtils.getNumpyFromTransform(frame) assert np.allclose(mat[:3, :3], np.array(axes).transpose()) mat2 = transformations.quaternion_matrix(quat) mat2[:3, 3] = pos print mat print mat2 assert np.allclose(mat, mat2)
def createUtorsoGazeConstraints(self, tspan): constraints = [] g = ik.WorldGazeDirConstraint() g.linkName = 'utorso' g.targetFrame = vtk.vtkTransform() axes = transformUtils.getAxesFromTransform(self.polaris.leftFootEgressOutsideFrame.transform) g.targetAxis = axes[0] g.bodyAxis = [1,0,0] g.coneThreshold = self.coneThreshold g.tspan = tspan constraints.append(g) g = ik.WorldGazeDirConstraint() g.linkName = 'utorso' g.targetFrame = vtk.vtkTransform() g.targetAxis = [0,0,1] g.bodyAxis = [0,0,1] g.coneThreshold = self.coneThreshold g.tspan = tspan constraints.append(g) return constraints
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)
def getCorners(self): ''' Return a 4x3 numpy array representing the world xyz positions of the four corners of the block top. Corners are listed clockwise from far right. ''' width = self.rectWidth depth = self.rectDepth width = max(width, 0.39) #depth = max(depth, 0.38) xaxis, yaxis, zaxis = transformUtils.getAxesFromTransform(self.cornerTransform) xedge = np.array(xaxis)*depth yedge = np.array(yaxis)*width c1 = np.array(self.cornerTransform.GetPosition()) + (np.array(yaxis)*self.rectWidth*0.5) - yedge*0.5 c2 = c1 - xedge c3 = c1 - xedge + yedge c4 = c1 + yedge return np.array([c3, c4, c1, c2])
def fitRunningBoardAtFeet(self): # get stance frame startPose = self.getPlanningStartPose() stanceFrame = self.robotSystem.footstepsDriver.getFeetMidPoint(self.robotSystem.robotStateModel, useWorldZ=False) stanceFrameAxes = transformUtils.getAxesFromTransform(stanceFrame) # get pointcloud and extract search region covering the running board polyData = segmentation.getCurrentRevolutionData() polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01) _, polyData = segmentation.removeGround(polyData) polyData = segmentation.cropToBox(polyData, stanceFrame, [1.0, 1.0, 0.1]) if not polyData.GetNumberOfPoints(): print 'empty search region point cloud' return vis.updatePolyData(polyData, 'running board search points', parent=segmentation.getDebugFolder(), color=[0,1,0], visible=False) # extract maximal points along the stance x axis perpAxis = stanceFrameAxes[0] edgeAxis = stanceFrameAxes[1] edgePoints = segmentation.computeEdge(polyData, edgeAxis, perpAxis) edgePoints = vnp.getVtkPolyDataFromNumpyPoints(edgePoints) vis.updatePolyData(edgePoints, 'edge points', parent=segmentation.getDebugFolder(), visible=True) # ransac fit a line to the edge points linePoint, lineDirection, fitPoints = segmentation.applyLineFit(edgePoints) if np.dot(lineDirection, stanceFrameAxes[1]) < 0: lineDirection = -lineDirection linePoints = segmentation.thresholdPoints(fitPoints, 'ransac_labels', [1.0, 1.0]) dists = np.dot(vnp.getNumpyFromVtk(linePoints, 'Points')-linePoint, lineDirection) p1 = linePoint + lineDirection*np.min(dists) p2 = linePoint + lineDirection*np.max(dists) vis.updatePolyData(fitPoints, 'line fit points', parent=segmentation.getDebugFolder(), colorByName='ransac_labels', visible=False) # compute a new frame that is in plane with the stance frame # and matches the orientation and position of the detected edge origin = np.array(stanceFrame.GetPosition()) normal = np.array(stanceFrameAxes[2]) # project stance origin to edge, then back to foot frame originProjectedToEdge = linePoint + lineDirection*np.dot(origin - linePoint, lineDirection) originProjectedToPlane = segmentation.projectPointToPlane(originProjectedToEdge, origin, normal) zaxis = np.array(stanceFrameAxes[2]) yaxis = np.array(lineDirection) xaxis = np.cross(yaxis, zaxis) xaxis /= np.linalg.norm(xaxis) yaxis = np.cross(zaxis, xaxis) yaxis /= np.linalg.norm(yaxis) d = DebugData() d.addSphere(p1, radius=0.005) d.addSphere(p2, radius=0.005) d.addLine(p1, p2) d.addSphere(originProjectedToEdge, radius=0.001, color=[1,0,0]) d.addSphere(originProjectedToPlane, radius=0.001, color=[0,1,0]) d.addLine(originProjectedToPlane, origin, color=[0,1,0]) d.addLine(originProjectedToEdge, origin, color=[1,0,0]) vis.updatePolyData(d.getPolyData(), 'running board edge', parent=segmentation.getDebugFolder(), colorByName='RGB255', visible=False) # update the running board box affordance position and orientation to # fit the detected edge box = self.spawnRunningBoardAffordance() boxDimensions = box.getProperty('Dimensions') t = transformUtils.getTransformFromAxesAndOrigin(xaxis, yaxis, zaxis, originProjectedToPlane) t.PreMultiply() t.Translate(-boxDimensions[0]/2.0, 0.0, -boxDimensions[2]/2.0) box.getChildFrame().copyFrame(t) self.initialize()
def planInsertTraj(self, speed, lockFeet=True, lockBase=None, resetBase=False, wristAngleCW=0, startPose=None, verticalOffset=0.01, usePoses=False, resetPoses=True, planFromCurrentRobotState=False, retract=False): ikParameters = IkParameters( usePointwise=False, maxDegreesPerSecond=speed, numberOfAddedKnots=1, quasiStaticShrinkFactor=self.quasiStaticShrinkFactor, fixInitialState=planFromCurrentRobotState) _, yaxis, _ = transformUtils.getAxesFromTransform( self.computeGraspFrame().transform) yawDesired = np.arctan2(yaxis[1], yaxis[0]) if startPose is None: startPose = self.getPlanningStartPose() nominalPose = self.getNominalPose() self.ikPlanner.addPose(nominalPose, self.nominalPoseName) self.ikPlanner.addPose(startPose, self.startPoseName) self.ikPlanner.reachingSide = self.graspingHand constraints = [] constraints.extend( self.createBaseConstraints(resetBase, lockBase, lockFeet, yawDesired)) constraints.append(self.createBackPostureConstraint()) constraints.append(self.ikPlanner.createQuasiStaticConstraint()) constraints.extend(self.createFootConstraints(lockFeet)) constraints.append( self.ikPlanner.createLockedArmPostureConstraint( self.startPoseName)) constraints.append( self.ikPlanner.createKneePostureConstraint([0.7, 2.5])) constraints.append(self.createElbowPostureConstraint()) constraints.append(self.createStaticTorqueConstraint()) constraints.append(self.createHandGazeConstraint()) constraints.append(self.createHandFixedOrientConstraint()) constraints.append( self.createWristAngleConstraint(wristAngleCW, planFromCurrentRobotState)) constraints.extend( self.createAllHandPositionConstraints(self.coaxialTol, retract)) if retract: startPoseName = self.getStartPoseName(planFromCurrentRobotState, True, usePoses) endPoseName = self.getEndPoseName(True, usePoses) endPose = self.ikPlanner.jointController.getPose(endPoseName) endPose = self.ikPlanner.mergePostures( endPose, robotstate.matchJoints('lwy'), startPose) endPoseName = 'q_retract' self.ikPlanner.addPose(endPose, endPoseName) else: startPoseName = self.getStartPoseName(planFromCurrentRobotState, retract, usePoses) endPoseName = self.getEndPoseName(retract, usePoses) plan = self.ikPlanner.runIkTraj(constraints, startPoseName, endPoseName, self.nominalPoseName, ikParameters=ikParameters) if resetPoses and not retract and max(plan.plan_info) <= 10: self.setReachAndTouchPoses(plan) return plan
def fitRunningBoardAtFeet(self): # get stance frame startPose = self.getPlanningStartPose() stanceFrame = self.robotSystem.footstepsDriver.getFeetMidPoint( self.robotSystem.robotStateModel, useWorldZ=False) stanceFrameAxes = transformUtils.getAxesFromTransform(stanceFrame) # get pointcloud and extract search region covering the running board polyData = segmentation.getCurrentRevolutionData() polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01) _, polyData = segmentation.removeGround(polyData) polyData = segmentation.cropToBox(polyData, stanceFrame, [1.0, 1.0, 0.1]) if not polyData.GetNumberOfPoints(): print 'empty search region point cloud' return vis.updatePolyData(polyData, 'running board search points', parent=segmentation.getDebugFolder(), color=[0, 1, 0], visible=False) # extract maximal points along the stance x axis perpAxis = stanceFrameAxes[0] edgeAxis = stanceFrameAxes[1] edgePoints = segmentation.computeEdge(polyData, edgeAxis, perpAxis) edgePoints = vnp.getVtkPolyDataFromNumpyPoints(edgePoints) vis.updatePolyData(edgePoints, 'edge points', parent=segmentation.getDebugFolder(), visible=True) # ransac fit a line to the edge points linePoint, lineDirection, fitPoints = segmentation.applyLineFit( edgePoints) if np.dot(lineDirection, stanceFrameAxes[1]) < 0: lineDirection = -lineDirection linePoints = segmentation.thresholdPoints(fitPoints, 'ransac_labels', [1.0, 1.0]) dists = np.dot( vnp.getNumpyFromVtk(linePoints, 'Points') - linePoint, lineDirection) p1 = linePoint + lineDirection * np.min(dists) p2 = linePoint + lineDirection * np.max(dists) vis.updatePolyData(fitPoints, 'line fit points', parent=segmentation.getDebugFolder(), colorByName='ransac_labels', visible=False) # compute a new frame that is in plane with the stance frame # and matches the orientation and position of the detected edge origin = np.array(stanceFrame.GetPosition()) normal = np.array(stanceFrameAxes[2]) # project stance origin to edge, then back to foot frame originProjectedToEdge = linePoint + lineDirection * np.dot( origin - linePoint, lineDirection) originProjectedToPlane = segmentation.projectPointToPlane( originProjectedToEdge, origin, normal) zaxis = np.array(stanceFrameAxes[2]) yaxis = np.array(lineDirection) xaxis = np.cross(yaxis, zaxis) xaxis /= np.linalg.norm(xaxis) yaxis = np.cross(zaxis, xaxis) yaxis /= np.linalg.norm(yaxis) d = DebugData() d.addSphere(p1, radius=0.005) d.addSphere(p2, radius=0.005) d.addLine(p1, p2) d.addSphere(originProjectedToEdge, radius=0.001, color=[1, 0, 0]) d.addSphere(originProjectedToPlane, radius=0.001, color=[0, 1, 0]) d.addLine(originProjectedToPlane, origin, color=[0, 1, 0]) d.addLine(originProjectedToEdge, origin, color=[1, 0, 0]) vis.updatePolyData(d.getPolyData(), 'running board edge', parent=segmentation.getDebugFolder(), colorByName='RGB255', visible=False) # update the running board box affordance position and orientation to # fit the detected edge box = self.spawnRunningBoardAffordance() boxDimensions = box.getProperty('Dimensions') t = transformUtils.getTransformFromAxesAndOrigin( xaxis, yaxis, zaxis, originProjectedToPlane) t.PreMultiply() t.Translate(-boxDimensions[0] / 2.0, 0.0, -boxDimensions[2] / 2.0) box.getChildFrame().copyFrame(t) self.initialize()