def planShiftWeightOut(self, startPose=None): if startPose is None: startPose = self.getPlanningStartPose() startPoseName = 'q_egress_start' self.robotSystem.ikPlanner.addPose(startPose, startPoseName) endPoseName = 'q_egress_end' constraints = [] utorsoFrame = self.robotSystem.ikPlanner.getLinkFrameAtPose('utorso', startPose) constraints.extend(self.createUtorsoGazeConstraints([1.0, 1.0])) constraints.append(ik.QuasiStaticConstraint(leftFootEnabled=False, rightFootEnabled=True, pelvisEnabled=False, shrinkFactor=self.quasiStaticShrinkFactor)) constraints.append(self.robotSystem.ikPlanner.createXYZMovingBasePostureConstraint(startPoseName)) constraints.append(self.robotSystem.ikPlanner.createLockedLeftArmPostureConstraint(startPoseName)) constraints.append(self.robotSystem.ikPlanner.createLockedRightArmPostureConstraint(startPoseName)) constraints.append(self.robotSystem.ikPlanner.createKneePostureConstraint([0.7, 2.5])) constraints.append(self.robotSystem.ikPlanner.createFixedLinkConstraints(startPoseName, 'l_foot')) constraints.append(self.robotSystem.ikPlanner.createFixedLinkConstraints(startPoseName, 'r_foot')) constraintSet = ConstraintSet(self.robotSystem.ikPlanner, constraints, endPoseName, startPoseName) constraintSet.ikParameters = IkParameters(usePointwise=True, maxBaseMetersPerSecond=0.02) constraintSet.runIk() keyFramePlan = constraintSet.planEndPoseGoal(feetOnGround=False) 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 planGetWeightOverFeet(self): startPose = self.getPlanningStartPose() startPoseName = 'q_egress_start' self.robotSystem.ikPlanner.addPose(startPose, startPoseName) endPoseName = 'q_egress_end' constraints = [] constraints.append( ikconstraints.QuasiStaticConstraint(leftFootEnabled=True, rightFootEnabled=True, pelvisEnabled=False, shrinkFactor=0.5)) constraints.append( self.robotSystem.ikPlanner.createLockedBasePostureConstraint( startPoseName)) constraints.append( self.robotSystem.ikPlanner.createLockedLeftArmPostureConstraint( startPoseName)) constraints.append( self.robotSystem.ikPlanner.createLockedRightArmPostureConstraint( startPoseName)) constraintSet = ConstraintSet(self.robotSystem.ikPlanner, constraints, endPoseName, startPoseName) constraintSet.ikParameters = IkParameters(usePointwise=False, maxDegreesPerSecond=10) constraintSet.runIk() keyFramePlan = constraintSet.planEndPoseGoal(feetOnGround=False) poseTimes, poses = planplayback.PlanPlayback.getPlanPoses(keyFramePlan) ts = [poseTimes[0]] supportsList = [['r_foot', 'l_foot', 'pelvis']] plan = self.publishPlanWithSupports(keyFramePlan, supportsList, ts, True) self.addPlan(plan) return plan
def planStandUp(self): startPose = self.getPlanningStartPose() startPoseName = 'q_egress_start' self.robotSystem.ikPlanner.addPose(startPose, startPoseName) endPoseName = 'q_egress_end' pelvisFrame = self.robotSystem.ikPlanner.getLinkFrameAtPose('pelvis', startPose) t = transformUtils.frameFromPositionAndRPY([self.pelvisLiftX, 0, self.pelvisLiftZ], [0, 0, 0]) liftFrame = transformUtils.concatenateTransforms([t, pelvisFrame]) constraints = [] utorsoFrame = self.robotSystem.ikPlanner.getLinkFrameAtPose('utorso', startPose) g = self.createUtorsoGazeConstraints([1.0, 1.0]) constraints.append(g[1]) p = ik.PositionConstraint(linkName='pelvis', referenceFrame=liftFrame, lowerBound=np.array([0.0, -np.inf, 0.0]), upperBound=np.array([np.inf, np.inf, 0.0])) constraints.append(p) constraints.append(ik.QuasiStaticConstraint(leftFootEnabled=True, rightFootEnabled=True, pelvisEnabled=False, shrinkFactor=self.quasiStaticShrinkFactor)) constraints.append(self.robotSystem.ikPlanner.createXYZMovingBasePostureConstraint(startPoseName)) constraints.append(self.robotSystem.ikPlanner.createLockedLeftArmPostureConstraint(startPoseName)) constraints.append(self.robotSystem.ikPlanner.createLockedRightArmPostureConstraint(startPoseName)) constraints.extend(self.robotSystem.ikPlanner.createFixedFootConstraints(startPoseName)) constraints.append(self.robotSystem.ikPlanner.createKneePostureConstraint([0.7, 2.5])) constraintSet = ConstraintSet(self.robotSystem.ikPlanner, constraints, endPoseName, startPoseName) constraintSet.ikParameters = IkParameters(usePointwise=True, maxBaseMetersPerSecond=0.02) constraintSet.runIk() keyFramePlan = constraintSet.planEndPoseGoal(feetOnGround=True) 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 planFootOut(self, startPose=None, finalFootHeight=0.05): if startPose is None: startPose = self.getPlanningStartPose() startPoseName = 'q_egress_start' self.robotSystem.ikPlanner.addPose(startPose, startPoseName) endPoseName = 'q_egress_end' utorsoFrame = self.robotSystem.ikPlanner.getLinkFrameAtPose('utorso', startPose) finalLeftFootFrame = self.computeLeftFootOverPlatformFrame(startPose, finalFootHeight) constraints = [] constraints.extend(self.createUtorsoGazeConstraints([0.0, 1.0])) constraints.append(ik.QuasiStaticConstraint(leftFootEnabled=False, rightFootEnabled=True, pelvisEnabled=False, shrinkFactor=0.01)) constraints.append(self.robotSystem.ikPlanner.createMovingBaseSafeLimitsConstraint()) constraints.append(self.robotSystem.ikPlanner.createLockedLeftArmPostureConstraint(startPoseName)) constraints.append(self.robotSystem.ikPlanner.createLockedRightArmPostureConstraint(startPoseName)) #constraints.append(self.robotSystem.ikPlanner.createLockedBackPostureConstraint(startPoseName)) constraints.append(self.robotSystem.ikPlanner.createKneePostureConstraint([0.7, 2.5])) constraints.append(self.robotSystem.ikPlanner.createFixedLinkConstraints(startPoseName, 'r_foot')) constraints.extend(self.createLeftFootPoseConstraint(finalLeftFootFrame, tspan=[1,1])) constraintSet = ConstraintSet(self.robotSystem.ikPlanner, constraints, endPoseName, startPoseName) constraintSet.ikParameters = IkParameters(usePointwise=True, maxBaseRPYDegreesPerSecond=10, rescaleBodyNames=['l_foot'], rescaleBodyPts=[0.0, 0.0, 0.0], maxBodyTranslationSpeed=self.maxFootTranslationSpeed) #constraintSet.seedPoseName = 'q_start' #constraintSet.nominalPoseName = 'q_start' constraintSet.runIk() footFrame = self.robotSystem.ikPlanner.getLinkFrameAtPose('l_foot', startPose) t = transformUtils.frameFromPositionAndRPY([0, 0, self.polaris.leftFootEgressOutsideFrame.transform.GetPosition()[2]-footFrame.GetPosition()[2]], [0, 0, 0]) liftFrame = transformUtils.concatenateTransforms([footFrame, t]) vis.updateFrame(liftFrame, 'lift frame') c = ik.WorldFixedOrientConstraint() c.linkName = 'l_foot' c.tspan = [0.0, 0.1, 0.2] constraints.append(c) constraints.extend(self.createLeftFootPoseConstraint(liftFrame, tspan=[0.2, 0.2])) constraints.extend(self.createLeftFootPoseConstraint(self.polaris.leftFootEgressMidFrame, tspan=[0.5, 0.5])) constraints.extend(self.createLeftFootPoseConstraint(self.polaris.leftFootEgressOutsideFrame, tspan=[0.8, 0.8])) #plan = constraintSet.planEndPoseGoal(feetOnGround=False) keyFramePlan = constraintSet.runIkTraj() poseTimes, poses = planplayback.PlanPlayback.getPlanPoses(keyFramePlan) ts = [poseTimes[0]] supportsList = [['r_foot']] plan = self.publishPlanWithSupports(keyFramePlan, supportsList, ts, False) self.addPlan(plan) return plan
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 planLeftFootDown(self): startPose = self.getPlanningStartPose() startPoseName = 'q_footdown_start' self.robotSystem.ikPlanner.addPose(startPose, startPoseName) endPoseName = 'q_footdown_end' utorsoFrame = self.robotSystem.ikPlanner.getLinkFrameAtPose('utorso', startPose) finalLeftFootFrame = self.computeLeftFootOverPlatformFrame(startPose, 0.0) constraints = [] constraints.extend(self.createUtorsoGazeConstraints([0.0, 1.0])) constraints.append(ik.QuasiStaticConstraint(leftFootEnabled=False, rightFootEnabled=True, pelvisEnabled=False, shrinkFactor=0.01)) constraints.append(self.robotSystem.ikPlanner.createMovingBaseSafeLimitsConstraint()) constraints.append(self.robotSystem.ikPlanner.createLockedLeftArmPostureConstraint(startPoseName)) constraints.append(self.robotSystem.ikPlanner.createLockedRightArmPostureConstraint(startPoseName)) #constraints.append(self.robotSystem.ikPlanner.createLockedBackPostureConstraint(startPoseName)) constraints.append(self.robotSystem.ikPlanner.createFixedLinkConstraints(startPoseName, 'r_foot')) constraints.extend(self.createLeftFootPoseConstraint(finalLeftFootFrame, tspan=[1,1])) constraintSet = ConstraintSet(self.robotSystem.ikPlanner, constraints, endPoseName, startPoseName) constraintSet.ikParameters = IkParameters(usePointwise=True) #constraintSet.seedPoseName = 'q_start' #constraintSet.nominalPoseName = 'q_start' endPose, _ = constraintSet.runIk() keyFramePlan = constraintSet.planEndPoseGoal(feetOnGround=False) poseTimes, poses = planplayback.PlanPlayback.getPlanPoses(keyFramePlan) ts = [poseTimes[0], poseTimes[-1]] supportsList = [['r_foot'], ['r_foot','l_foot']] plan = self.publishPlanWithSupports(keyFramePlan, supportsList, ts, True) self.addPlan(plan) return plan, endPose
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 runIK(self, targetFrame, startPose=None, graspToHandLinkFrame=None, makePlan=True, positionTolerance=0.0, angleToleranceInDegrees=5.0, maxDegreesPerSecond=60): """ Sets the cameraFrame to the targetFrame using IK :param targetFrame: :return: """ 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.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( maxDegreesPerSecond=maxDegreesPerSecond) endPose, info = constraintSet.runIk() returnData = dict() returnData['info'] = info returnData['endPose'] = endPose if makePlan: plan = constraintSet.planEndPoseGoal() returnData['plan'] = plan 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
def planGetWeightOverFeet(self): startPose = self.getPlanningStartPose() startPoseName = 'q_egress_start' self.robotSystem.ikPlanner.addPose(startPose, startPoseName) endPoseName = 'q_egress_end' constraints = [] constraints.append(ik.QuasiStaticConstraint(leftFootEnabled=True, rightFootEnabled=True, pelvisEnabled=False, shrinkFactor=0.5)) constraints.append(self.robotSystem.ikPlanner.createLockedBasePostureConstraint(startPoseName)) constraints.append(self.robotSystem.ikPlanner.createLockedLeftArmPostureConstraint(startPoseName)) constraints.append(self.robotSystem.ikPlanner.createLockedRightArmPostureConstraint(startPoseName)) constraintSet = ConstraintSet(self.robotSystem.ikPlanner, constraints, endPoseName, startPoseName) constraintSet.ikParameters = IkParameters(usePointwise=False, maxDegreesPerSecond=10) constraintSet.runIk() keyFramePlan = constraintSet.planEndPoseGoal(feetOnGround=False) poseTimes, poses = planplayback.PlanPlayback.getPlanPoses(keyFramePlan) ts = [poseTimes[0]] supportsList = [['r_foot', 'l_foot', 'pelvis']] plan = self.publishPlanWithSupports(keyFramePlan, supportsList, ts, True) self.addPlan(plan) return plan