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)
