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 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 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 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 planPinchReach(self, maxDegreesPerSecond=None): if maxDegreesPerSecond is None: maxDegreesPerSecond = 10 ikPlanner = self.ikPlanner targetFrame = om.findObjectByName('pinch reach frame').transform pinchToHand = self.getPinchToHandFrame() startPose = self.getPlanningStartPose() constraintSet = self.computeGraspPose(startPose, targetFrame, graspToHand=pinchToHand) constraintSet.ikParameters = IkParameters( maxDegreesPerSecond=maxDegreesPerSecond) seedPose = ikPlanner.getMergedPostureFromDatabase(startPose, 'surprise:switch', 'above_switch', side='right') seedPoseName = 'q_above_switch' self.robotSystem.ikPlanner.addPose(seedPose, seedPoseName) constraintSet.seedPoseName = seedPoseName constraintSet.nominalPoseName = seedPoseName endPose, info = constraintSet.runIk() plan = constraintSet.planEndPoseGoal() self.addPlan(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 planFootEgress(self): def saveOriginalTraj(name): commands = ['%s = qtraj_orig;' % name] self.robotSystem.ikServer.comm.sendCommands(commands) def concatenateAndRescaleTrajectories(trajectoryNames, concatenatedTrajectoryName, junctionTimesName, ikParameters): commands = [] commands.append('joint_v_max = repmat(%s*pi/180, r.getNumVelocities()-6, 1);' % ikParameters.maxDegreesPerSecond) commands.append('xyz_v_max = repmat(%s, 3, 1);' % ikParameters.maxBaseMetersPerSecond) commands.append('rpy_v_max = repmat(%s*pi/180, 3, 1);' % ikParameters.maxBaseRPYDegreesPerSecond) commands.append('v_max = [xyz_v_max; rpy_v_max; joint_v_max];') commands.append("max_body_translation_speed = %r;" % ikParameters.maxBodyTranslationSpeed) commands.append("max_body_rotation_speed = %r;" % ikParameters.maxBodyRotationSpeed) commands.append('rescale_body_ids = [%s];' % (','.join(['links.%s' % linkName for linkName in ikParameters.rescaleBodyNames]))) commands.append('rescale_body_pts = reshape(%s, 3, []);' % ik.ConstraintBase.toColumnVectorString(ikParameters.rescaleBodyPts)) commands.append("body_rescale_options = struct('body_id',rescale_body_ids,'pts',rescale_body_pts,'max_v',max_body_translation_speed,'max_theta',max_body_rotation_speed,'robot',r);") commands.append('trajectories = {};') for name in trajectoryNames: commands.append('trajectories{end+1} = %s;' % name) commands.append('[%s, %s] = concatAndRescaleTrajectories(trajectories, v_max, %s, %s, body_rescale_options);' % (concatenatedTrajectoryName, junctionTimesName, ikParameters.accelerationParam, ikParameters.accelerationFraction)) commands.append('s.publishTraj(%s, 1);' % concatenatedTrajectoryName) self.robotSystem.ikServer.comm.sendCommands(commands) return self.robotSystem.ikServer.comm.getFloatArray(junctionTimesName) self.planShiftWeightOut() shiftWeightName = 'qtraj_shift_weight' saveOriginalTraj(shiftWeightName) nextStartPose = robotstate.convertStateMessageToDrakePose(self.plans[-1].plan.plan[-1]) self.planFootOut(startPose=nextStartPose, finalFootHeight=0.0) footOutName = 'qtraj_foot_out' saveOriginalTraj(footOutName) nextStartPose = robotstate.convertStateMessageToDrakePose(self.plans[-1].plan.plan[-1]) self.planCenterWeight(startPose=nextStartPose) centerWeightName = 'qtraj_center_weight' saveOriginalTraj(centerWeightName) ikParameters = IkParameters(usePointwise=True, maxBaseRPYDegreesPerSecond=10, rescaleBodyNames=['l_foot'], rescaleBodyPts=[0.0, 0.0, 0.0], maxBodyTranslationSpeed=self.maxFootTranslationSpeed) ikParameters = self.robotSystem.ikPlanner.mergeWithDefaultIkParameters(ikParameters) listener = self.robotSystem.ikPlanner.getManipPlanListener() supportTimes = concatenateAndRescaleTrajectories([shiftWeightName, footOutName, centerWeightName], 'qtraj_foot_egress', 'ts', ikParameters) keyFramePlan = listener.waitForResponse() listener.finish() supportsList = [] supportsList.append(['l_foot', 'r_foot']) supportsList.append(['r_foot']) supportsList.append(['l_foot', 'r_foot']) supportsList.append(['l_foot', 'r_foot']) plan = self.publishPlanWithSupports(keyFramePlan, supportsList, supportTimes, True) self.addPlan(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 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 planArmsForward(self): q0 = self.getPlanningStartPose() q1 = self.robotSystem.ikPlanner.getMergedPostureFromDatabase(q0, 'General', 'hands-forward', side='left') q2 = self.robotSystem.ikPlanner.getMergedPostureFromDatabase(q1, 'General', 'hands-forward', side='right') a = 0.25 q1 = (1-a)*q1 + a*np.array(q2) ikParameters = IkParameters(usePointwise=True, maxBaseRPYDegreesPerSecond=10, rescaleBodyNames=['l_hand', 'r_hand'], rescaleBodyPts=list(self.robotSystem.ikPlanner.getPalmPoint(side='left')) + list(self.robotSystem.ikPlanner.getPalmPoint(side='right')), maxBodyTranslationSpeed=3*self.maxHandTranslationSpeed) plan = self.robotSystem.ikPlanner.computeMultiPostureGoal([q0, q1, q2], ikParameters=ikParameters) self.addPlan(plan) return plan
def movePose(self, joint_positions, maxDegreesPerSecond=30): assert isinstance(joint_positions, dict) self.manipPlanner.lastPlan = None startPose = self.robotSystem.robotStateJointController.q endPose = self.ikPlanner.mergePostures(startPose, joint_positions) ikParameters = IkParameters(maxDegreesPerSecond=maxDegreesPerSecond) plan = self.ikPlanner.computePostureGoal(startPose, endPose, ikParameters=ikParameters) self.manipPlanner.commitManipPlan(plan) # block until this plan is over planDuration = plan.plan[-1].utime/1e6 print "running plan of duration %s seconds " %(planDuration) time.sleep(1.1*planDuration)
def planReach(self): ikPlanner = self.robotSystem.ikPlanner startPose = self.getPlanningStartPose() startPoseName = 'q_reach_start' endPoseName = 'q_reach_end' self.robotSystem.ikPlanner.addPose(startPose, startPoseName) side = 'right' movingReachConstraint = ikPlanner.createMovingReachConstraints( startPoseName, lockBase=True, lockBack=True, lockArm=True, side=side) palmToHand = ikPlanner.getPalmToHandLink(side=side) targetFrame = om.findObjectByName('reach frame').transform poseConstraints = ikPlanner.createPositionOrientationGraspConstraints( side, targetFrame, graspToHandLinkFrame=palmToHand, angleToleranceInDegrees=5.0) constraints = [] constraints.extend(movingReachConstraint) constraints.extend(poseConstraints) constraintSet = ikplanner.ConstraintSet(ikPlanner, constraints, endPoseName, startPoseName) constraintSet.ikParameters = IkParameters(maxDegreesPerSecond=30) seedPose = ikPlanner.getMergedPostureFromDatabase(startPose, 'surprise:switch', 'above_switch', side='right') seedPoseName = 'q_above_switch' self.robotSystem.ikPlanner.addPose(seedPose, seedPoseName) constraintSet.seedPoseName = seedPoseName constraintSet.nominalPoseName = seedPoseName endPose, info = constraintSet.runIk() plan = constraintSet.planEndPoseGoal() self.addPlan(plan)
def planArmsPrep2(self, startPose=None): ikPlanner = self.robotSystem.ikPlanner if startPose is None: startPose = self.getPlanningStartPose() startPoseName = 'q_arms_prep2_start' self.robotSystem.ikPlanner.addPose(startPose, startPoseName) endPose = ikPlanner.getMergedPostureFromDatabase(startPose, 'surprise:switch', 'reach_up_1', side='right') ikParameters = IkParameters(maxDegreesPerSecond=30) plan = ikPlanner.computePostureGoal(startPose, endPose, feetOnGround=False, ikParameters=ikParameters) self.addPlan(plan)