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 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.getEstimatedRobotStatePose()
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 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 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 = 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 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',
lcmbotcore.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', lcmbotcore.robot_state_t)
pose = robotstate.convertStateMessageToDrakePose(msg)
pose[:6] = np.zeros(6)
self.initialize(pose)
def showEndPose(self):
endPose = robotstate.convertStateMessageToDrakePose(self.endPosePlan)
self.showPoseFunction(endPose)
def showEndPose(self):
endPose = robotstate.convertStateMessageToDrakePose(self.endPosePlan)
self.showPoseFunction(endPose)
