def planCenterWeight(self, startPose=None): ikPlanner = self.robotSystem.ikPlanner if startPose is None: startPose = self.getPlanningStartPose() startPoseName = 'q_lean_right' self.robotSystem.ikPlanner.addPose(startPose, startPoseName) endPoseName = 'q_egress_end' footFixedConstraints = ikPlanner.createFixedFootConstraints(startPoseName) backConstraint = ikPlanner.createMovingBackLimitedPostureConstraint() armsLocked = ikPlanner.createLockedArmsPostureConstraints(startPoseName) constraints = [backConstraint] constraints.extend(footFixedConstraints) constraints.extend(armsLocked) constraints.append(ik.QuasiStaticConstraint(leftFootEnabled=True, rightFootEnabled=True, pelvisEnabled=False, shrinkFactor=self.quasiStaticShrinkFactor)) constraints.append(self.robotSystem.ikPlanner.createKneePostureConstraint([0.7, 2.5])) constraintSet = ConstraintSet(ikPlanner, constraints, endPoseName, startPoseName) constraintSet.seedPoseName = 'q_start' constraintSet.nominalPoseName = 'q_nom' endPose = constraintSet.runIk() keyFramePlan = constraintSet.planEndPoseGoal() poseTimes, poses = planplayback.PlanPlayback.getPlanPoses(keyFramePlan) ts = [poseTimes[0]] supportsList = [['r_foot','l_foot']] plan = self.publishPlanWithSupports(keyFramePlan, supportsList, ts, True) self.addPlan(plan) return plan
def runIK(self, targetFrame, startPose=None, graspToHandLinkFrame=None, positionTolerance=0.0, angleToleranceInDegrees=0.0, seedPoseName='q_nom'): if graspToHandLinkFrame is None: graspToHandLinkFrame = vtk.vtkTransform() if startPose is None: startPose = self.getPlanningStartPose() ikPlanner = self.robotSystem.ikPlanner startPoseName = 'reach_start' endPoseName = 'reach_end' ikPlanner.addPose(startPose, startPoseName) side = ikPlanner.reachingSide constraints = [] constraints.append( KukaPlanningUtils.createLockedBasePostureConstraint( ikPlanner, startPoseName)) # positionConstraint, orientationConstraint = ikPlanner.createPositionOrientationGraspConstraints(side, targetFrame, # graspToHandLinkFrame, # positionTolerance=positionTolerance, # angleToleranceInDegrees=angleToleranceInDegrees) positionConstraint, orientationConstraint = ikPlanner.createPositionOrientationConstraint( self.endEffectorLinkName, targetFrame, graspToHandLinkFrame, positionTolerance=positionTolerance, angleToleranceInDegrees=angleToleranceInDegrees) positionConstraint.tspan = [1.0, 1.0] orientationConstraint.tspan = [1.0, 1.0] constraints.append(positionConstraint) constraints.append(orientationConstraint) constraintSet = ConstraintSet(ikPlanner, constraints, 'reach_end', startPoseName) constraintSet.ikParameters = IkParameters() constraintSet.seedPoseName = seedPoseName print "consraintSet ", constraintSet print "constraintSet.seedPoseName ", constraintSet.seedPoseName print "constraintSet.nominalPoseName ", constraintSet.nominalPoseName 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], 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