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 setReachAndTouchPoses(self, plan=None): if plan is None: self.reachPoseName = None self.touchPoseName = None self.reachPose = None self.touchPose = None else: self.reachPoseName = 'q_reach' self.touchPoseName = 'q_touch' self.reachPose = robotstate.convertStateMessageToDrakePose(plan.plan[0]) self.touchPose = robotstate.convertStateMessageToDrakePose(plan.plan[-1]) self.ikPlanner.addPose(self.reachPose, self.reachPoseName) self.ikPlanner.addPose(self.touchPose, self.touchPoseName)
def getPlanningStartPose(self): if self.planFromCurrentRobotState: return self.getEstimatedRobotStatePose() else: if self.plans: return robotstate.convertStateMessageToDrakePose(self.plans[-1].plan[-1]) else: return self.getEstimatedRobotStatePose()
def getPlanningStartPose(self): if self.planFromCurrentRobotState: return self.sensorJointController.getPose('EST_ROBOT_STATE') else: if self.plans: return robotstate.convertStateMessageToDrakePose( self.plans[-1].plan[-1]) else: return self.getEstimatedRobotStatePose()
def onRobotStateMessage(msg): if self.ignoreOldStateMessages and self.lastRobotStateMessage is not None and msg.utime < self.lastRobotStateMessage.utime: return poseName = channelName pose = robotstate.convertStateMessageToDrakePose(msg) self.lastRobotStateMessage = msg # use joint name/positions from robot_state_t and append base_{x,y,z,roll,pitch,yaw} jointPositions = np.hstack((msg.joint_position, pose[:6])) jointNames = msg.joint_name + robotstate.getDrakePoseJointNames()[:6] self.setPose(poseName, pose, pushToModel=False) for model in self.models: model.model.setJointPositions(jointPositions, jointNames)
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 computeStanceFrame(self, useIkTraj=False): objectTransform = transformUtils.copyFrame(self.computeGraspFrame().transform) if useIkTraj: startPose = self.getNominalPose() plan = self.planInsertTraj(self.speedLow, lockFeet=False, lockBase=False, resetPoses=True, startPose=startPose) stancePose = robotstate.convertStateMessageToDrakePose(plan.plan[0]) stanceRobotModel = self.ikPlanner.getRobotModelAtPose(stancePose) self.nominalPelvisXYZ = stancePose[:3] robotStance = self.footstepPlanner.getFeetMidPoint(stanceRobotModel) else: robotStance = self.computeRobotStanceFrame(objectTransform, self.computeRelativeStanceTransform()) stanceFrame = vis.updateFrame(robotStance, 'valve grasp stance', parent=self.getValveAffordance(), visible=False, scale=0.2) stanceFrame.addToView(app.getDRCView()) return stanceFrame
def getPlanPoses(msgOrList): if isinstance(msgOrList, list): messages = msgOrList allPoseTimes, allPoses = PlanPlayback.getPlanPoses(messages[0]) for msg in messages[1:]: poseTimes, poses = PlanPlayback.getPlanPoses(msg) poseTimes += allPoseTimes[-1] allPoseTimes = np.hstack((allPoseTimes, poseTimes[1:])) allPoses += poses[1:] return allPoseTimes, allPoses else: msg = robotstate.asRobotPlan(msgOrList) poses = [] poseTimes = [] for plan in msg.plan: pose = robotstate.convertStateMessageToDrakePose(plan) poseTimes.append(plan.utime / 1e6) poses.append(pose) return np.array(poseTimes), poses
def onJointPositionGoal(self, msg): #lcmUtils.publish('ATLAS_COMMAND', msg) commandStream.startStreaming() pose = robotstate.convertStateMessageToDrakePose(msg) self.setGoalPose(pose)
def waitForRobotState(self): msg = lcmUtils.captureMessage('EST_ROBOT_STATE', lcmdrc.robot_state_t) pose = robotstate.convertStateMessageToDrakePose(msg) pose[:6] = np.zeros(6) self.initialize(pose)
def onJointPositionGoal(self, msg): #lcmUtils.publish('ATLAS_COMMAND', msg) commandStream.startStreaming() pose = robotstate.convertStateMessageToDrakePose(msg) self.setGoalPose(pose)
def waitForRobotState(self): msg = lcmUtils.captureMessage('EST_ROBOT_STATE', lcmdrc.robot_state_t) pose = robotstate.convertStateMessageToDrakePose(msg) pose[:6] = np.zeros(6) self.initialize(pose)
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 showEndPose(self): endPose = robotstate.convertStateMessageToDrakePose(self.endPosePlan) self.showPoseFunction(endPose)
def showEndPose(self): endPose = robotstate.convertStateMessageToDrakePose(self.endPosePlan) self.showPoseFunction(endPose)