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 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 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 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
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 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 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)
def planTurn(self, wristAngleCW=np.radians(320)): ikParameters = IkParameters(maxDegreesPerSecond=self.speedTurn) startPose = self.getPlanningStartPose() wristAngleCW = min( np.radians(320) - 0.01, max(-np.radians(160) + 0.01, wristAngleCW)) if self.graspingHand == 'left': postureJoints = {'l_arm_lwy': -np.radians(160) + wristAngleCW} else: postureJoints = {'r_arm_lwy': np.radians(160) - wristAngleCW} endPose = self.ikPlanner.mergePostures(startPose, postureJoints) plan = self.ikPlanner.computePostureGoal(startPose, endPose, ikParameters=ikParameters) app.displaySnoptInfo(1) self.addPlan(plan)
def planReach(self, verticalOffset=None, **kwargs): startPose = self.getPlanningStartPose() insert_plan = self.planInsertTraj(self.speedHigh, lockFeet=True, usePoses=True, resetPoses=True, **kwargs) info = max(insert_plan.plan_info) reachPose = robotstate.convertStateMessageToDrakePose( insert_plan.plan[0]) ikParameters = IkParameters( maxDegreesPerSecond=2 * self.speedTurn, rescaleBodyNames=[ self.ikPlanner.getHandLink(side=self.graspingHand) ], rescaleBodyPts=list( self.ikPlanner.getPalmPoint(side=self.graspingHand)), maxBodyTranslationSpeed=self.maxHandTranslationSpeed) plan = self.ikPlanner.computePostureGoal(startPose, reachPose, ikParameters=ikParameters) plan.plan_info = [info] * len(plan.plan_info) lcmUtils.publish('CANDIDATE_MANIP_PLAN', plan) self.addPlan(plan)
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