Coverage for /home/travis/build/RobotLocomotion/director/build/src/director-build/lib/python2.7/dist-packages/director/teleoppanel : 0%

constraints.append(ikPlanner.createFixedLinkConstraints(startPoseName, ikPlanner.leftFootLink, tspan=[0.0, 1.0], lowerBound=-0.0001*np.ones(3), upperBound=0.0001*np.ones(3), angleToleranceInDegrees=0.1))

elif self.getLFootConstraint() == 'constrained':

constraints.extend(ikPlanner.createSixDofLinkConstraints(startPoseName, ikPlanner.leftFootLink, tspan=[1.0, 1.0]))

elif self.getLFootConstraint() == 'sliding':

constraints.extend(ikPlanner.createSlidingFootConstraints(startPoseName)[:2])

if self.getRFootConstraint() == 'fixed':

constraints.append(ikPlanner.createFixedLinkConstraints(startPoseName, ikPlanner.rightFootLink, tspan=[0.0, 1.0], lowerBound=-0.0001*np.ones(3), upperBound=0.0001*np.ones(3), angleToleranceInDegrees=0.1))

elif self.getRFootConstraint() == 'constrained':

constraints.extend(ikPlanner.createSixDofLinkConstraints(startPoseName, ikPlanner.rightFootLink, tspan=[1.0, 1.0]))

elif self.getRFootConstraint() == 'sliding':

constraints.extend(ikPlanner.createSlidingFootConstraints(startPoseName)[2:])

if self.getBackConstraint() == 'fixed':

constraints.append(ikPlanner.createLockedBackPostureConstraint(startPoseName))

ikPlanner.setBackLocked(True)

elif self.getBackConstraint() == 'limited':

constraints.append(ikPlanner.createMovingBackLimitedPostureConstraint())

ikPlanner.setBackLocked(False)

elif self.getBackConstraint() == 'free':

constraints.append(ikPlanner.createMovingBackPostureConstraint())

ikPlanner.setBackLocked(False)

if self.getBaseConstraint() == 'fixed':

constraints.append(ikPlanner.createLockedBasePostureConstraint(startPoseName, lockLegs=False))

ikPlanner.setBaseLocked(True)

if self.getBaseConstraint() == 'constrained':

constraints.extend(ikPlanner.createSixDofLinkConstraints(startPoseName, ikPlanner.pelvisLink, tspan=[1.0, 1.0]))

ikPlanner.setBaseLocked(False)

elif self.getBaseConstraint() == 'xyz only':

constraints.append(ikPlanner.createXYZMovingBasePostureConstraint(startPoseName))

constraints.append(ikPlanner.createKneePostureConstraint(self.kneeJointLimits))

ikPlanner.setBaseLocked(False)

elif self.getBaseConstraint() == 'z only':

constraints.append(ikPlanner.createZMovingBasePostureConstraint(startPoseName))

constraints.append(ikPlanner.createKneePostureConstraint(self.kneeJointLimits))

ikPlanner.setBaseLocked(False)

elif self.getBaseConstraint() == 'limited':

constraints.append(ikPlanner.createMovingBaseSafeLimitsConstraint())

constraints.append(ikPlanner.createKneePostureConstraint(self.kneeJointLimits))

ikPlanner.setBaseLocked(False)

elif self.getBaseConstraint() == 'free':

constraints.append(ikPlanner.createKneePostureConstraint(self.kneeJointLimits))

ikPlanner.setBaseLocked(False)

# Fixed Base Arm: Remove all except the fixed base constraint

if ikPlanner.fixedBaseArm:

constraints = []

constraints.append(ikPlanner.createLockedBasePostureConstraint(startPoseName, lockLegs=False))

if ikPlanner.robotNoFeet:

constraints = []

constraints.append(ikPlanner.createLockedBasePostureConstraint(startPoseName))

# Only add Back constraints if robot has a back

if ikPlanner.getJointGroup('Back'):

if self.getBackConstraint() == 'fixed':

constraints.append(ikPlanner.createLockedBackPostureConstraint(startPoseName))

ikPlanner.setBackLocked(True)

elif self.getBackConstraint() == 'limited':

constraints.append(ikPlanner.createMovingBackLimitedPostureConstraint())

ikPlanner.setBackLocked(False)

elif self.getBackConstraint() == 'free':

constraints.append(ikPlanner.createMovingBackPostureConstraint())

ikPlanner.setBackLocked(False)

# Don't generate end effector constraints if in final pose planning

if not self.getCheckboxState(self.ui.finalPosePlanningOptions):

for handModel in ikPlanner.handModels:

side = handModel.side

if (side == "left"):

thisHandConstraint = self.getLHandConstraint()

elif (side == "right"):

thisHandConstraint = self.getRHandConstraint()

linkName = ikPlanner.getHandLink(side)

graspToHand = ikPlanner.newPalmOffsetGraspToHandFrame(side, self.palmOffsetDistance)

graspToWorld = self.getGoalFrame(linkName)

p, q = ikPlanner.createPositionOrientationGraspConstraints(side, graspToWorld, graspToHand)

g = ikPlanner.createGazeGraspConstraint(side, graspToWorld, graspToHand, targetAxis=list(self.palmGazeAxis), bodyAxis=list(self.palmGazeAxis))

p.tspan = [1.0, 1.0]

q.tspan = [1.0, 1.0]

g.tspan = [1.0, 1.0]

if thisHandConstraint == 'arm fixed':

if (side == "left"):

constraints.append(ikPlanner.createLockedLeftArmPostureConstraint(startPoseName))

elif (side == "right"):

constraints.append(ikPlanner.createLockedRightArmPostureConstraint(startPoseName))

ikPlanner.setArmLocked(side,True)

elif thisHandConstraint == 'ee fixed':

constraints.extend([p, q])

ikPlanner.setArmLocked(side,False)

elif thisHandConstraint == 'position':

constraints.extend([p])

ikPlanner.setArmLocked(side,False)

elif thisHandConstraint == 'gaze':

constraints.extend([p, g])

ikPlanner.setArmLocked(side,False)

elif thisHandConstraint == 'orbit':

graspToHand = ikPlanner.newPalmOffsetGraspToHandFrame(side, distance=0.07)

constraints.extend(ikPlanner.createGraspOrbitConstraints(side, graspToWorld, graspToHand))

constraints[-3].tspan = [1.0, 1.0]

if ikPlanner.defaultIkParameters.useCollision != 'none':

constraints[-2].tspan = [0.5, 1.0]

constraints[-1].tspan = [0.5, 1.0]

else:

constraints[-2].tspan = [1.0, 1.0]

constraints[-1].tspan = [1.0, 1.0]

ikPlanner.setArmLocked(side,False)

elif thisHandConstraint == 'free':

ikPlanner.setArmLocked(side,False)

if hasattr(self,'reachSide'):

if self.reachSide == 'left':

endEffectorName = ikPlanner.handModels[0].handLinkName # 'l_hand'

else:

endEffectorName = ikPlanner.handModels[1].handLinkName # 'r_hand'

constraints.append(ikPlanner.createActiveEndEffectorConstraint(endEffectorName,ikPlanner.getPalmPoint(self.reachSide)))

self.constraintSet = ikplanner.ConstraintSet(ikPlanner, constraints, 'reach_end', startPoseName)

handLinks = []

for handModel in ikPlanner.handModels: handLinks.append(handModel.handLinkName)

for constraint in constraints:

if hasattr(constraint, 'linkName') and constraint.linkName in handLinks:

continue

if isinstance(constraint, ikconstraints.PositionConstraint):

frameObj = self.getGoalFrame(constraint.linkName)

if frameObj:

constraint.referenceFrame = frameObj.transform

elif isinstance(constraint, ikconstraints.QuatConstraint):

frameObj = self.getGoalFrame(constraint.linkName)

if frameObj:

constraint.quaternion = frameObj.transform

elif isinstance(constraint, ikconstraints.WorldGazeDirConstraint):

frameObj = self.getGoalFrame(constraint.linkName)

if frameObj:

constraint.targetFrame = frameObj.transform

if not self.getCheckboxState(self.ui.finalPosePlanningOptions):

self.onGoalFrameModified(None)

om.removeFromObjectModel(self.getConstraintFolder())

folder = self.getConstraintFolder()

for i, pc in enumerate(constraints):

constraintItem = ConstraintItem(pc)

om.addToObjectModel(constraintItem, parentObj=folder)

def addHandMesh(self, handModel, goalFrame):

handObj = handModel.newPolyData('reach goal left hand', self.panel.teleopRobotModel.views[0], parent=goalFrame)

handFrame = handObj.children()[0]

handFrame.copyFrame(goalFrame.transform)

frameSync = vis.FrameSync()

frameSync.addFrame(goalFrame)

frameSync.addFrame(handFrame)

goalFrame.sync = frameSync

@staticmethod

def removePlanFolder():

om.removeFromObjectModel(om.findObjectByName('teleop plan'))

@staticmethod

def getConstraintFrameFolder():

return om.getOrCreateContainer('constraint frames', parentObj=om.getOrCreateContainer('teleop plan', parentObj=om.findObjectByName('planning')))

@staticmethod

def getConstraintFolder():

return om.getOrCreateContainer('ik constraints', parentObj=om.getOrCreateContainer('teleop plan', parentObj=om.findObjectByName('planning')))

def createGoalFrames(self):

ikPlanner = self.panel.ikPlanner

startPose = self.panel.planningUtils.getPlanningStartPose()

self.removePlanFolder()

folder = self.getConstraintFrameFolder()

for handModel in ikPlanner.handModels:

side = handModel.side

linkName = ikPlanner.getHandLink(side)

frameName = '%s constraint frame' % linkName

graspToHand = ikPlanner.newPalmOffsetGraspToHandFrame(side, self.palmOffsetDistance)

graspToWorld = ikPlanner.newGraspToWorldFrame(startPose, side, graspToHand)

om.removeFromObjectModel(om.findObjectByName(frameName))

frame = vis.showFrame(graspToWorld, frameName, parent=folder, scale=0.2)

#frame.setProperty('Edit', True)

frame.connectFrameModified(self.onGoalFrameModified)

#addHandMesh(handModels[side], frame)

if not ikPlanner.fixedBaseArm and not ikPlanner.robotNoFeet:

for linkName in [ikPlanner.leftFootLink, ikPlanner.rightFootLink, ikPlanner.pelvisLink]:

frameName = linkName + ' constraint frame'

om.removeFromObjectModel(om.findObjectByName(frameName))

frame = vis.showFrame(ikPlanner.getLinkFrameAtPose(linkName, startPose), frameName, parent=folder, scale=0.2)

frame.connectFrameModified(self.onGoalFrameModified)

def newReachTeleop(self, frame, side, reachTargetObject=None):

'''

reachTarget is the object we are reaching to. For some types of plans

this object may be treated in a special way, for example, when doing

planning with collision avoidance.

'''

self.deactivate()

self.panel.jointTeleop.deactivate()

self.setBaseConstraint('xyz only')

self.setBackConstraint('limited')

self.setLFootConstraint('fixed')

self.setRFootConstraint('fixed')

self.setLHandConstraint('arm fixed')

self.setRHandConstraint('arm fixed')

if side == 'left':

if self.panel.ikPlanner.defaultIkParameters.useCollision != 'none':

self.setLHandConstraint('ee fixed')

else:

self.setLHandConstraint('ee fixed')

elif side == 'right':

if self.panel.ikPlanner.defaultIkParameters.useCollision != 'none':

self.setRHandConstraint('ee fixed')

else:

self.setRHandConstraint('ee fixed')

self.reachTargetObject = reachTargetObject

self.reachSide = side

self.activate()

return self.updateGoalFrame(self.panel.ikPlanner.getHandLink(side), frame)

def finalPosePlanningChanged(self):

if self.getCheckboxState(self.ui.finalPosePlanningOptions):

self.setCheckboxState(self.ui.eeTeleopButton, False)

if self.getFinalPoseGraspingHand() == 'left':

self.ui.lhandCombo.enabled = False

else:

self.ui.rhandCombo.enabled = False

self.deactivate()

self.initFinalPosePlanning()

else:

self.terminateFinalPosePlanning()

self.ui.lhandCombo.enabled = True

self.ui.rhandCombo.enabled = True

def rightGraspingHandButtonClicked(self):

self.ui.rightGraspingHandButton.checked = True

self.ui.leftGraspingHandButton.checked = False

self.terminateFinalPosePlanning()

self.initFinalPosePlanning()

ikplanner.getIkOptions().setProperty('RRT hand', 1)

self.ui.rhandCombo.enabled = False

self.ui.lhandCombo.enabled = True

def leftGraspingHandButtonClicked(self):

self.ui.rightGraspingHandButton.checked = False

self.ui.leftGraspingHandButton.checked = True

self.terminateFinalPosePlanning()

self.initFinalPosePlanning()

ikplanner.getIkOptions().setProperty('RRT hand', 0)

self.ui.rhandCombo.enabled = True

self.ui.lhandCombo.enabled = False

def initFinalPosePlanning(self):

if drcargs.getDirectorConfig()['modelName'] != 'valkyrie':

message = 'Final pose planning is not yet available for %s' % drcargs.getDirectorConfig()['modelName']

QtGui.QMessageBox.warning(app.getMainWindow(), 'Model not supported', message,

QtGui.QMessageBox.Ok)

self.setCheckboxState(self.ui.finalPosePlanningOptions, False)

return

pelvisFrame = self.panel.ikPlanner.robotModel.getLinkFrame(self.panel.ikPlanner.pelvisLink)

t = transformUtils.copyFrame(pelvisFrame)

t.PreMultiply()

if self.getFinalPoseGraspingHand() == 'left':

rotation = [0, 90, -90]

else:

rotation = [0, -90, -90]

t.Concatenate(transformUtils.frameFromPositionAndRPY([0.4,0,0], rotation))

handFactory = HandFactory(self.panel.teleopRobotModel)

handFactory.placeHandModelWithTransform(t, self.panel.teleopRobotModel.views[0],

self.getFinalPoseGraspingHand(), 'Final Pose End Effector', 'planning')

def terminateFinalPosePlanning(self):

finalPoseEE = om.findObjectByName('Final Pose End Effector')

om.removeFromObjectModel(finalPoseEE)

def getFinalPoseGraspingHand(self):

if self.ui.rightGraspingHandButton.checked:

return 'right'

else:

return 'left'

def searchFinalPoseClicked(self):

self.updateConstraints()

self.updateCollisionEnvironment()

frame = om.findObjectByName('Final Pose End Effector frame')

handTransform = transformUtils.copyFrame(frame.transform)

handTransform.PreMultiply()

palmToHand = self.panel.ikPlanner.getPalmToHandLink(self.getFinalPoseGraspingHand())

palmToHand = palmToHand.GetLinearInverse()

handTransform.Concatenate(palmToHand)

endPose, info = self.constraintSet.searchFinalPose(self.getFinalPoseGraspingHand(), handTransform)

if info == 1:

self.panel.showPose(self.constraintSet.endPose)

app.displaySnoptInfo(info)

def onCandidateEndPose(self, msg):

if not self.getCheckboxState(self.ui.finalPosePlanningOptions) and not self.ui.eeTeleopButton.checked:

pose = robotstate.convertStateMessageToDrakePose(msg)

self.panel.showPose(pose)

class PosturePlanShortcuts(object):

def __init__(self, jointController, ikPlanner, planningUtils, widget=None):

self.jointController = jointController

self.ikPlanner = ikPlanner

self.planningUtils = planningUtils

widget = widget or app.getMainWindow()

app.addShortcut(widget, 'Ctrl+Shift+S', self.planStand)

app.addShortcut(widget, 'Ctrl+Shift+N', self.planNominal)

app.addShortcut(widget, 'Ctrl+Shift+L', functools.partial(self.planPreGrasp, 'left'))

app.addShortcut(widget, 'Ctrl+Shift+R', functools.partial(self.planPreGrasp, 'right'))

def planStand(self):

self.ikPlanner.computeStandPlan(self.planningUtils.getPlanningStartPose())

def planNominal(self):

self.ikPlanner.computeNominalPlan(self.planningUtils.getPlanningStartPose())

def planPreGrasp(self, side):

startPose = self.jointController.q # wxm

endPose = self.ikPlanner.getMergedPostureFromDatabase(startPose, 'General', 'arm up pregrasp', side=side)

self.ikPlanner.computePostureGoal(startPose, endPose)

class JointLimitChecker(object):

def __init__(self, robotModel, sensorJointController):

self.robotModel = robotModel

self.sensorJointController = sensorJointController

self.jointLimitsMin = np.array([self.robotModel.model.getJointLimits(jointName)[0] for jointName in robotstate.getDrakePoseJointNames()])

self.jointLimitsMax = np.array([self.robotModel.model.getJointLimits(jointName)[1] for jointName in robotstate.getDrakePoseJointNames()])

self.joints = robotstate.matchJoints('^(?!base_)') # all but base joints

self.inflationAmount = np.radians(0.3)

self.timer = TimerCallback(targetFps=1)

self.timer.callback = self.update

self.warningButton = None

self.action = None

def update(self):

limitData = self.checkJointLimits()

if limitData:

self.notifyUserStatusBar(limitData)

else:

self.clearStatusBarWarning()

def start(self):

self.action.checked = True

self.timer.start()

def stop(self):

self.action.checked = False

self.timer.stop()

def setupMenuAction(self):

self.action = app.addMenuAction('Tools', 'Joint Limit Checker')

self.action.setCheckable(True)

self.action.checked = self.timer.isActive()

self.action.connect('triggered()', self.onActionChanged)

def onActionChanged(self):

if self.action.checked:

self.start()

else:

self.stop()

def clearStatusBarWarning(self):

if self.warningButton:

self.warningButton.deleteLater()

self.warningButton = None

def notifyUserStatusBar(self, limitData):

if self.warningButton:

return

def showDialog():

limitData = self.checkJointLimits()

if limitData:

self.notifyUserDialog(limitData)

self.clearStatusBarWarning()

self.warningButton = QtGui.QPushButton('Joint Limit Warning')

self.warningButton.setStyleSheet("background-color:red")

self.warningButton.connect('clicked()', showDialog)

app.getMainWindow().statusBar().insertPermanentWidget(0, self.warningButton)

def notifyUserDialog(self, limitData):

message = '\n'.join(['%s by %.2f degrees' % (name, np.degrees(epsilon)) for name, epsilon, jointPosition in limitData])

message = 'The following joints have been detected to exceed joint limts specified by the model:\n\n' + message + '\n\n'

message += 'Would to like to update the joint limits used by the planning robot model? If you select no '\

'then the joint limit checker will be disabled (use the Tools menu to re-enable).'

msgBox = QtGui.QMessageBox()

msgBox.setText(message)

msgBox.addButton(QtGui.QPushButton('No'), QtGui.QMessageBox.NoRole)

msgBox.addButton(QtGui.QPushButton('Yes'), QtGui.QMessageBox.YesRole)

choice = msgBox.exec_()

if choice == 0: # No

# don't do anything except close the dialog window

return

else:

self.extendJointLimitsAsExceeded(limitData)

def extendJointLimitsAsExceeded(self, limitData):

# inflate the epsilon

limitData = [(jointName, epsilon+np.sign(epsilon)*self.inflationAmount, jointPosition) for jointName, epsilon, jointPosition in limitData]

# update limits on server

panel.ikPlanner.ikServer.updateJointLimits(limitData)

panel.ikPlanner.plannerPub.updateJointLimits(limitData)

# update limits on checker

for jointName, epsilon, jointPosition in limitData:

limitsArray = self.jointLimitsMin if epsilon < 0 else self.jointLimitsMax

limitsArray[self.toJointIndex(jointName)] += epsilon

def checkJointLimits(self):

limitData = []

for jointName in self.joints:

jointIndex = self.toJointIndex(jointName)

jointPosition = self.sensorJointController.q[jointIndex]

jointMin, jointMax = self.jointLimitsMin[jointIndex], self.jointLimitsMax[jointIndex]

if not (jointMin <= jointPosition <= jointMax):

epsilon = jointPosition - np.clip(jointPosition, jointMin, jointMax)

#print 'detected joint outside limit:', jointName, ' by %.3f degrees' % np.degrees(epsilon)

limitData.append((jointName, epsilon, jointPosition))

return limitData

def toJointIndex(self, jointName):

return robotstate.getDrakePoseJointNames().index(jointName)

class GeneralEndEffectorTeleopPanel(object):

def __init__(self, ikPlanner, teleopPanel, robotStateModel, robotStateJointController):

self.ikPlanner = ikPlanner

self.teleopPanel = teleopPanel

self.robotStateModel = robotStateModel

self.robotStateJointController = robotStateJointController

self.widget = QtGui.QWidget()

l = QtGui.QVBoxLayout(self.widget)

h = QtGui.QHBoxLayout()

l.addLayout(h)

h.addWidget(QtGui.QLabel('End effector:'))

self.endEffectorCombo = QtGui.QComboBox()

h.addWidget(self.endEffectorCombo)

def addButton(name, func):

b = QtGui.QPushButton(name)

b.connect('clicked()', func)

l.addWidget(b)

addButton('start ik', self.startIk)

addButton('end ik', self.endIk)

addButton('plan', self.planIk)

config = drcargs.getDirectorConfig()['endEffectorConfig']

self.endEffectorLinkNames = config['endEffectorLinkNames']

self.graspOffsetFrame = transformUtils.frameFromPositionAndRPY(config['graspOffsetFrame'][0], np.degrees(config['graspOffsetFrame'][1]))

self.fixedJoints = config['fixedJoints']

for linkName in self.endEffectorLinkNames:

self.endEffectorCombo.addItem(linkName)

def planIk(self):

startPoseName = 'reach_start'

endPoseName = 'reach_end'

startPose = self.teleopPanel.planningUtils.getPlanningStartPose()

self.ikPlanner.addPose(startPose, startPoseName)

goalMode = ikplanner.getIkOptions().getProperty('Goal planning mode')

if goalMode == 1:

plan = self.constraintSet.runIkTraj()

else:

plan = self.constraintSet.planEndPoseGoal()

self.teleopPanel.showPlan(plan)

def endIk(self):

self.teleopPanel.hideTeleopModel()

EndEffectorTeleopPanel.removePlanFolder()

def startIk(self, reachGoal=None):

EndEffectorTeleopPanel.removePlanFolder()

ikPlanner = self.ikPlanner

startPoseName = 'reach_start'

endPoseName = 'reach_end'

startPose = self.teleopPanel.planningUtils.getPlanningStartPose()

ikPlanner.addPose(startPose, startPoseName)

endEffectorLinkName = str(self.endEffectorCombo.currentText)

if reachGoal is None:

endEffectorLinkFrame = self.robotStateModel.getLinkFrame(endEffectorLinkName)

assert endEffectorLinkFrame is not None

graspToWorld = vtk.vtkTransform()

graspToWorld.PostMultiply()

graspToWorld.Concatenate(self.graspOffsetFrame)

graspToWorld.Concatenate(endEffectorLinkFrame)

reachGoal = graspToWorld

om.removeFromObjectModel('reach goal')

goalFrame = vis.showFrame(reachGoal, 'reach goal', scale=0.1, parent=EndEffectorTeleopPanel.getConstraintFrameFolder())

goalFrame.setProperty('Edit', True)

constraints = []

for pattern in self.fixedJoints:

constraints.append(ikPlanner.createPostureConstraint(startPoseName, robotstate.matchJoints(pattern)))

constraints.extend(ikPlanner.createPositionOrientationConstraint(endEffectorLinkName, goalFrame, self.graspOffsetFrame, positionTolerance=0.0, angleToleranceInDegrees=0.0))

constraints[-1].tspan = [1.0, 1.0]

constraints[-2].tspan = [1.0, 1.0]

self.constraintSet = ikplanner.ConstraintSet(ikPlanner, constraints, endPoseName, startPoseName)

def onGoalFrameModified(frame):

endPose, info = self.constraintSet.runIk()

self.teleopPanel.showPose(self.constraintSet.endPose)

app.displaySnoptInfo(info)

goalFrame.connectFrameModified(onGoalFrameModified)

onGoalFrameModified(goalFrame)

folder = EndEffectorTeleopPanel.getConstraintFolder()

for i, constraint in enumerate(constraints):

constraintItem = ConstraintItem(constraint)

om.addToObjectModel(constraintItem, parentObj=folder)

class JointTeleopPanel(object):

def __init__(self, panel, jointGroups=None):

self.panel = panel

self.ui = panel.ui

self.ui.jointTeleopButton.connect('clicked()', self.teleopButtonClicked)

self.ui.resetJointsButton.connect('clicked()', self.resetButtonClicked)

self.ui.planButton.connect('clicked()', self.planClicked)

self.timerCallback = TimerCallback()

self.timerCallback.callback = self.onTimerCallback

self.jointLimitsMin = np.array([self.panel.teleopRobotModel.model.getJointLimits(jointName)[0] for jointName in robotstate.getDrakePoseJointNames()])

self.jointLimitsMax = np.array([self.panel.teleopRobotModel.model.getJointLimits(jointName)[1] for jointName in robotstate.getDrakePoseJointNames()])

# this need to be generalized

if 'baseZJointLimits' in drcargs.getDirectorConfig():

baseZLimits = drcargs.getDirectorConfig()['baseZJointLimits']

else: # TODO generalise so the base sliders are deactivated

baseZLimits = [-0.1, 0.1]

self.jointLimitsMin[0:6] = [-0.25, -0.25, baseZLimits[0], -math.radians(20), -math.radians(20), -math.radians(20)]

self.jointLimitsMax[0:6] = [ 0.25 , 0.25, baseZLimits[1], math.radians(20), math.radians(20), math.radians(20)]

if jointGroups is None:

# Add only these joint groups:

telopJointGroupNames = ['Back', 'Base', 'Left Arm', 'Right Arm', 'Neck']

allJointGroups = drcargs.getDirectorConfig()['teleopJointGroups']

jointGroups = []

for jointGroup in allJointGroups:

if jointGroup['name'] in telopJointGroupNames:

jointGroups.append( jointGroup )

self.jointGroups = jointGroups

self.buildTabWidget(jointGroups)

self.startPose = None

self.endPose = None

self.userJoints = {}

self.updateWidgetState()

def buildTabWidget(self, jointGroups):

self.slidersMap = {}

self.labelMap = {}

for group in jointGroups:

groupName = group['name']

joints = group['joints']

labels = group['labels']

if len(labels) != len(joints):

print 'error, joints/labels mismatch for joint group:', name

continue

jointGroupWidget = QtGui.QWidget()

gridLayout = QtGui.QGridLayout(jointGroupWidget)

gridLayout.setColumnStretch(0, 1)

for jointName, labelText in zip(joints, labels):

label = QtGui.QLabel(labelText)

numericLabel = QtGui.QLabel('0.0')

slider = QtGui.QSlider(QtCore.Qt.Vertical)

column = gridLayout.columnCount()

gridLayout.addWidget(label, 0, column)

gridLayout.addWidget(slider, 1, column)

gridLayout.addWidget(numericLabel, 2, column)

self.slidersMap[jointName] = slider

self.labelMap[slider] = numericLabel

if groupName == 'Neck':

def onSendNeckJointPositionGoal():

msg = lcmbotcore.joint_angles_t()

msg.utime = getUtime()

msg.num_joints = len(joints)

msg.joint_name = joints

msg.joint_position = [0] * len(joints)

for i, jointName in enumerate(joints):

jointIndex = self.toJointIndex(jointName)

msg.joint_position[i] = self.getJointValue(jointIndex)

lcmUtils.publish('DESIRED_NECK_ANGLES', msg)

sendNeckJointPositionGoalButton = QtGui.QPushButton('set')

sendNeckJointPositionGoalButton.connect('clicked()', onSendNeckJointPositionGoal)

gridLayout.addWidget(sendNeckJointPositionGoalButton, 3, gridLayout.columnCount())

gridLayout.setColumnStretch(gridLayout.columnCount(), 1)

self.ui.tabWidget.addTab(jointGroupWidget, groupName)

self.signalMapper = QtCore.QSignalMapper()

self.sliderMax = 1000.0

for jointName, slider in self.slidersMap.iteritems():

slider.connect('valueChanged(int)', self.signalMapper, 'map()')

self.signalMapper.setMapping(slider, jointName)

slider.setMaximum(self.sliderMax)

self.signalMapper.connect('mapped(const QString&)', self.sliderChanged)

def planClicked(self):

if not self.ui.jointTeleopButton.checked:

return

self.computeEndPose()

self.generatePlan()

def generatePlan(self):

hasBase = False

for jointIndex, jointValue in self.userJoints.iteritems():

if self.toJointName(jointIndex).startswith('base_'):

hasBase = True

plan = self.panel.ikPlanner.computePostureGoal(self.startPose, self.endPose, feetOnGround=hasBase)

self.panel.showPlan(plan)

def teleopButtonClicked(self):

if self.ui.jointTeleopButton.checked:

self.activate()

else:

self.deactivate()

def activate(self):

self.timerCallback.stop()

self.panel.jointTeleopActivated()

self.resetPose()

self.updateWidgetState()

def deactivate(self):

self.ui.jointTeleopButton.blockSignals(True)

self.ui.jointTeleopButton.checked = False

self.ui.jointTeleopButton.blockSignals(False)

self.timerCallback.stop()

self.panel.jointTeleopDeactivated()

self.updateWidgetState()

def updateWidgetState(self):

enabled = self.ui.jointTeleopButton.checked

for slider in self.slidersMap.values():

slider.setEnabled(enabled)

self.ui.resetJointsButton.setEnabled(enabled)

if not enabled:

self.timerCallback.start()

def resetButtonClicked(self):

self.resetPose()

self.panel.showPose(self.endPose)

def resetPose(self):

self.userJoints = {}

self.computeEndPose()

self.updateSliders()

def onTimerCallback(self):

if not self.ui.tabWidget.visible:

return

self.resetPose()

def toJointIndex(self, jointName):

return robotstate.getDrakePoseJointNames().index(jointName)

def toJointName(self, jointIndex):

return robotstate.getDrakePoseJointNames()[jointIndex]

def toJointValue(self, jointIndex, sliderValue):

assert 0.0 <= sliderValue <= 1.0

jointRange = self.jointLimitsMin[jointIndex], self.jointLimitsMax[jointIndex]

return jointRange[0] + (jointRange[1] - jointRange[0])*sliderValue

def toSliderValue(self, jointIndex, jointValue):

jointRange = self.jointLimitsMin[jointIndex], self.jointLimitsMax[jointIndex]

#if jointValue < jointRange[0] or jointValue > jointRange[1]:

# print 'warning: joint %s value %f is out of expected range [%f, %f]' % (self.toJointName(jointIndex), jointValue, jointRange[0], jointRange[1])

return (jointValue - jointRange[0]) / (jointRange[1] - jointRange[0])

def getSlider(self, joint):

jointName = self.toJointName(joint) if isinstance(joint, int) else joint

return self.slidersMap[jointName]

def computeBaseJointOffsets(self):

if self.panel.ikPlanner.robotNoFeet:

baseReferenceFrame = vtk.vtkTransform()

else:

from director import footstepsdriver

baseReferenceFrame = footstepsdriver.FootstepsDriver.getFeetMidPoint(self.panel.ikPlanner.getRobotModelAtPose(self.startPose))

baseReferenceWorldPos = np.array(baseReferenceFrame.GetPosition())

baseReferenceWorldYaw = math.radians(baseReferenceFrame.GetOrientation()[2])

self.baseJointOffsets = {

'base_x' : baseReferenceWorldPos[0],

'base_y' : baseReferenceWorldPos[1],

'base_z' : baseReferenceWorldPos[2],

'base_yaw' : baseReferenceWorldYaw,

}

def computeEndPose(self):

self.startPose = self.panel.planningUtils.getPlanningStartPose()

self.endPose = self.startPose.copy()

hasBase = False

for jointIndex, jointValue in self.userJoints.iteritems():

jointName = self.toJointName(jointIndex)

self.endPose[jointIndex] = jointValue

if jointName.startswith('base_'):

hasBase = True

if hasBase:

ikPlanner = self.panel.ikPlanner

startPoseName = 'posture_goal_start'

ikPlanner.addPose(self.startPose, startPoseName)

endPoseName = 'posture_goal_end'

ikPlanner.addPose(self.endPose, endPoseName)

jointNamesAll = self.slidersMap.keys()

# remove leg joints

jointNames = []

for name in jointNamesAll:

if not 'leg' in name:

jointNames.append(name)

# uncomment to constraint only joints adjusted by user

#jointNames = [self.toJointName(jointIndex) for jointIndex in sorted(self.userJoints.keys())]

p = ikPlanner.createPostureConstraint(endPoseName, jointNames)

constraints = [p]

constraints.extend(ikPlanner.createFixedFootConstraints(startPoseName))

constraints.append(ikPlanner.createQuasiStaticConstraint())

self.endPose, info = ikPlanner.ikServer.runIk(constraints, ikPlanner.defaultIkParameters, nominalPostureName=startPoseName, seedPostureName='q_end')

app.displaySnoptInfo(info)

def getJointValue(self, jointIndex):

return self.endPose[jointIndex]

def sliderChanged(self, jointName):

slider = self.slidersMap[jointName]

jointIndex = self.toJointIndex(jointName)

jointValue = self.toJointValue(jointIndex, slider.value / float(self.sliderMax))

self.userJoints[jointIndex] = jointValue

if jointName.startswith('base_'):

self.computeBaseJointOffsets()

self.userJoints[jointIndex] += self.baseJointOffsets.get(jointName, 0.0)

self.computeEndPose()

self.panel.showPose(self.endPose)

self.updateLabel(jointName, jointValue)

def updateLabel(self, jointName, jointValue):

slider = self.slidersMap[jointName]

label = self.labelMap[slider]

if jointName in ['base_x', 'base_y', 'base_z']:

label.text = str('%.3f' % jointValue).center(5, ' ')

else:

label.text = str('%.1f' % math.degrees(jointValue)).center(5, ' ')

def updateSliders(self):

baseJointOffsets = None

for jointName, slider in self.slidersMap.iteritems():

jointIndex = self.toJointIndex(jointName)

jointValue = self.getJointValue(jointIndex)

if (self.panel.ikPlanner.fixedBaseArm==False):

if jointName.startswith('base_'):

if baseJointOffsets is None:

baseJointOffsets = self.computeBaseJointOffsets()

jointValue -= self.baseJointOffsets.get(jointName, 0.0)

slider.blockSignals(True)

slider.setValue(self.toSliderValue(jointIndex, jointValue)*self.sliderMax)

slider.blockSignals(False)

self.updateLabel(jointName, jointValue)

class TeleopPanel(object):

def __init__(self, robotStateModel, robotStateJointController, teleopRobotModel, teleopJointController, ikPlanner, manipPlanner, affordanceManager, showPlanFunction, hidePlanFunction, planningUtils):

self.robotStateModel = robotStateModel

self.robotStateJointController = robotStateJointController

self.teleopRobotModel = teleopRobotModel

self.teleopJointController = teleopJointController

self.ikPlanner = ikPlanner

self.manipPlanner = manipPlanner

self.affordanceManager = affordanceManager

self.showPlanFunction = showPlanFunction

self.hidePlanFunction = hidePlanFunction

self.planningUtils = planningUtils

manipPlanner.connectPlanCommitted(self.onPlanCommitted)

loader = QtUiTools.QUiLoader()

uifile = QtCore.QFile(':/ui/ddTeleopPanel.ui')

assert uifile.open(uifile.ReadOnly)

self.widget = loader.load(uifile)

uifile.close()

self.ui = WidgetDict(self.widget.children())

self.ui.postureDatabaseButton.connect('clicked()', self.onPostureDatabaseClicked)

self.endEffectorTeleop = EndEffectorTeleopPanel(self)

self.jointTeleop = JointTeleopPanel(self)

if 'endEffectorConfig' in drcargs.getDirectorConfig():

self.ui.endEffectorTeleopFrame.setVisible(False)

self.generalEndEffectorTeleopPanel = GeneralEndEffectorTeleopPanel(ikPlanner, self, robotStateModel, robotStateJointController)

self.widget.layout().addWidget(self.generalEndEffectorTeleopPanel.widget, 0, 0, 1, 2)

app.addShortcut(app.getMainWindow(), 'Ctrl+Shift+P', self.ui.planButton.click)

PythonQt.dd.ddGroupBoxHider(self.ui.paramsContainer)

PythonQt.dd.ddGroupBoxHider(self.ui.finalPosePlanningOptions)

def onPostureDatabaseClicked(self):

ikplanner.RobotPoseGUIWrapper.initCaptureMethods(self.robotStateJointController, self.teleopJointController)

ikplanner.RobotPoseGUIWrapper.show()

def disableJointTeleop(self):

self.ui.jointTeleopFrame.setEnabled(False)

def disableEndEffectorTeleop(self):

self.ui.endEffectorTeleopFrame.setEnabled(False)

def jointTeleopActivated(self):

self.disableEndEffectorTeleop()

def endEffectorTeleopActivated(self):

self.disableJointTeleop()

def endEffectorTeleopDeactivated(self):

self.hideTeleopModel()

self.enablePanels()

def jointTeleopDeactivated(self):

self.hideTeleopModel()

self.enablePanels()

def enablePanels(self):

self.ui.endEffectorTeleopFrame.setEnabled(True)

self.ui.jointTeleopFrame.setEnabled(True)

def onPlanCommitted(self, plan):

self.hideTeleopModel()

def hideTeleopModel(self):

self.teleopRobotModel.setProperty('Visible', False)

self.robotStateModel.setProperty('Visible', True)

self.robotStateModel.setProperty('Alpha', 1.0)

def showTeleopModel(self):

self.teleopRobotModel.setProperty('Visible', True)

self.robotStateModel.setProperty('Visible', True)

self.robotStateModel.setProperty('Alpha', 0.1)

def showPose(self, pose):

self.teleopJointController.setPose('teleop_pose', pose)

self.hidePlanFunction()

self.showTeleopModel()

def showPlan(self, plan):

self.hideTeleopModel()

self.showPlanFunction(plan)

def extendJointLimitsForTesting(teleopPanel, jointLimitChecker):

# add +/- 3 degrees to joint teleop sliders

jointTeleop = teleopPanel.jointTeleop

extra = np.zeros(len(jointTeleop.jointLimitsMin))

extra += np.deg2rad(3.0)

jointTeleop.jointLimitsMin -= extra

jointTeleop.jointLimitsMax += extra

# add +/- 4 degrees to planner joint limits

limitDataMin = [(name, -np.deg2rad(4.0)) for name in jointLimitChecker.joints]

limitDataMax = [(name, np.deg2rad(4.0)) for name in jointLimitChecker.joints]

teleopPanel.ikPlanner.ikServer.updateJointLimits(limitDataMin)

teleopPanel.ikPlanner.ikServer.updateJointLimits(limitDataMax)

def _getAction():

return app.getToolBarActions()['ActionTeleopPanel']

def addPanelToMainWindow(teleopPanel):

global panel

global dock

panel = teleopPanel

dock = app.addWidgetToDock(panel.widget, action=_getAction())

dock.hide()

def init(robotStateModel, robotStateJointController, teleopRobotModel, teleopJointController, ikPlanner, manipPlanner, affordanceManager, showPlanFunction, hidePlanFunction, planningUtils):

panel = TeleopPanel(robotStateModel, robotStateJointController, teleopRobotModel, teleopJointController, ikPlanner, manipPlanner, affordanceManager, showPlanFunction, hidePlanFunction, planningUtils)

addPanelToMainWindow(panel)

return panel

« index coverage.py v3.7.1