def computeSingleCameraPose(self, targetLocationWorld=[1,0,0], cameraFrameLocation=[0.22, 0, 0.89], flip=False): cameraAxis = [0,0,1] linkName = self.handFrame linkName = 'iiwa_link_7' linkFrame =self.robotSystem.robotStateModel.getLinkFrame(linkName) cameraFrame = self.robotSystem.robotStateModel.getLinkFrame(self.handFrame) cameraToLinkFrame = transformUtils.concatenateTransforms([cameraFrame, linkFrame.GetLinearInverse()]) ikPlanner = self.robotSystem.ikPlanner startPoseName = 'q_nom' endPoseName = 'reach_end' seedPoseName = 'q_nom' if flip: print "FLIPPING startPoseName" startPoseName = 'q_nom_invert_7th_joint' seedPoseName = 'q_nom_invert_7th_joint' pose = np.asarray([ 0. , 0. , 0. , 0. , 0. , 0. , 0. , -0.68 , 1.0 , -1.688 , 1.0 , -0.5635, 1.0 ]) ikPlanner.addPose(pose, startPoseName) else: print "NOT flipped" constraints = [] constraints.append(ikPlanner.createPostureConstraint(startPoseName, robotstate.matchJoints('base_'))) positionConstraint = HandEyeCalibration.createPositionConstraint(targetPosition=cameraFrameLocation, linkName=linkName, linkOffsetFrame=cameraToLinkFrame, positionTolerance=0.05) cameraGazeConstraint = HandEyeCalibration.createCameraGazeTargetConstraint(linkName=linkName, cameraToLinkFrame=cameraToLinkFrame, cameraAxis=cameraAxis, worldPoint=targetLocationWorld, coneThresholdDegrees=5.0) constraints.append(positionConstraint) constraints.append(cameraGazeConstraint) constraintSet = ConstraintSet(ikPlanner, constraints, 'reach_end', startPoseName) if flip: constraintSet.ikPlanner.addPose(pose, startPoseName) constraintSet.ikParameters = IkParameters() constraintSet.seedPoseName = seedPoseName endPose, info = constraintSet.runIk() returnData = dict() returnData['info'] = info returnData['endPose'] = endPose return returnData
def computeSingleCameraPose(self, targetLocationWorld=[1, 0, 0], cameraFrameLocation=[0.22, 0, 0.89]): cameraAxis = [0, -1, 0] # assuming we are using 'palm' as the link frame linkName = self.handFrame linkName = 'iiwa_link_7' linkFrame = self.robotSystem.robotStateModel.getLinkFrame(linkName) cameraFrame = self.robotSystem.robotStateModel.getLinkFrame( self.handFrame) cameraToLinkFrame = transformUtils.concatenateTransforms( [cameraFrame, linkFrame.GetLinearInverse()]) ikPlanner = self.robotSystem.ikPlanner startPoseName = 'q_nom' endPoseName = 'reach_end' seedPoseName = 'q_nom' constraints = [] constraints.append( ikPlanner.createPostureConstraint(startPoseName, robotstate.matchJoints('base_'))) positionConstraint = HandEyeCalibration.createPositionConstraint( targetPosition=cameraFrameLocation, linkName=linkName, linkOffsetFrame=cameraToLinkFrame, positionTolerance=0.05) cameraGazeConstraint = HandEyeCalibration.createCameraGazeTargetConstraint( linkName=linkName, cameraToLinkFrame=cameraToLinkFrame, cameraAxis=cameraAxis, worldPoint=targetLocationWorld, coneThresholdDegrees=5.0) constraints.append(positionConstraint) constraints.append(cameraGazeConstraint) constraintSet = ConstraintSet(ikPlanner, constraints, 'reach_end', startPoseName) constraintSet.ikParameters = IkParameters() constraintSet.seedPoseName = seedPoseName endPose, info = constraintSet.runIk() returnData = dict() returnData['info'] = info returnData['endPose'] = endPose return returnData
def planReachGoal(goalFrameName='reach goal', startPose=None, planTraj=True, interactive=False, seedFromStart=False): goalFrame = om.findObjectByName(goalFrameName).transform startPoseName = 'reach_start' endPoseName = 'reach_end' endEffectorLinkName = getEndEffectorLinkName() graspOffsetFrame = getGraspToHandLink() if startPose is None: startPose = robotSystem.planningUtils.getPlanningStartPose() ikPlanner.addPose(startPose, startPoseName) constraints = [] constraints.append( ikPlanner.createPostureConstraint(startPoseName, robotstate.matchJoints('base_'))) p, q = ikPlanner.createPositionOrientationConstraint( endEffectorLinkName, goalFrame, graspOffsetFrame, positionTolerance=0.0, angleToleranceInDegrees=0.0) p.tspan = [1.0, 1.0] q.tspan = [1.0, 1.0] _, _, axisConstraint = ikPlanner.createMoveOnLineConstraints( startPose, goalFrame, graspOffsetFrame) axisConstraint.tspan = np.linspace(0, 1, 10) isPregrasp = goalFrameName.startswith('pregrasp to world') isGrasp = goalFrameName.startswith('grasp to world') if isGrasp: seedFromStart = True # adjust bounds of move on line constraint axisConstraintTubeRadius = 0.3 if isPregrasp else 0.001 axisConstraint.lowerBound[0] = -axisConstraintTubeRadius axisConstraint.lowerBound[0] = -axisConstraintTubeRadius axisConstraint.upperBound[1] = axisConstraintTubeRadius axisConstraint.upperBound[1] = axisConstraintTubeRadius # allow sliding in Z axis of pinch frame # this may be overruled by the line constraint if isPregrasp: p.lowerBound[2] = -0.02 p.upperBound[2] = 0.02 # align the gripper pinch axis # with the Y axis of the goal frame g = ikconstraints.WorldGazeDirConstraint() g.linkName = endEffectorLinkName g.targetFrame = goalFrame g.targetAxis = [0, 1, 0] g.bodyAxis = list(graspOffsetFrame.TransformVector([0, 1, 0])) g.coneThreshold = np.radians(5.0) if isPregrasp else np.radians(0.0) g.tspan = [1.0, 1.0] # point the fingers along the X axis # of the goal frame pinchPivotBound = np.radians(20) if isPregrasp else np.radians(0) g2 = ikconstraints.WorldGazeDirConstraint() g2.linkName = endEffectorLinkName g2.targetFrame = goalFrame g2.targetAxis = [1, 0, 0] g2.bodyAxis = list(graspOffsetFrame.TransformVector([1, 0, 0])) g2.coneThreshold = pinchPivotBound g2.tspan = [1.0, 1.0] constraints.append(p) constraints.append(q) #constraints.append(g) #constraints.append(g2) constraints.append(axisConstraint) constraintSet = ikplanner.ConstraintSet(ikPlanner, constraints, endPoseName, startPoseName) constraintSet.ikParameters.usePointwise = True if seedFromStart: constraintSet.seedPoseName = startPoseName constraintSet.nominalPoseName = startPoseName elif isPregrasp: constraintSet.seedPoseName = 'q_nom' constraintSet.nominalPoseName = 'q_nom' global _callbackId #if _callbackId: # om.findObjectByName(goalFrameName).disconnectFrameModified(_callbackId) if interactive: def update(frame): endPose, info = constraintSet.runIk() robotSystem.teleopPanel.showPose(endPose) _callbackId = om.findObjectByName(goalFrameName).connectFrameModified( update) update(None) else: endPose, info = constraintSet.runIk() if planTraj: robotSystem.teleopPanel.hideTeleopModel() return constraintSet.runIkTraj() else: return endPose, info
def createAllButLeftLegPostureConstraint(self, poseName): joints = robotstate.matchJoints('^(?!l_leg)') return self.robotSystem.ikPlanner.createPostureConstraint(poseName, joints)
def createLockedBasePostureConstraint(ikPlanner, startPoseName): return ikPlanner.createPostureConstraint( startPoseName, robotstate.matchJoints('base_'))