def loadRobotModel(name, view=None, parent='planning', urdfFile=None, color=None, visible=True, colorMode='URDF Colors'):
if not urdfFile:
urdfFile = drcargs.getDirectorConfig()['urdfConfig']['default']
if isinstance(parent, str):
parent = om.getOrCreateContainer(parent)
model = loadRobotModelFromFile(urdfFile)
if not model:
raise Exception('Error loading robot model from file: %s' % urdfFile)
obj = RobotModelItem(model)
om.addToObjectModel(obj, parent)
obj.setProperty('Visible', visible)
obj.setProperty('Name', name)
obj.setProperty('Color', color or getRobotGrayColor())
if colorMode == 'Textures':
obj.setProperty('Color Mode', 1)
elif colorMode == 'Solid Color':
obj.setProperty('Color Mode', 0)
elif colorMode == 'URDF Colors':
obj.setProperty('Color Mode', 2)
if view is not None:
obj.addToView(view)
jointController = jointcontrol.JointController([obj])
fixedPointFile = drcargs.getDirectorConfig()['fixedPointFile']
jointController.setPose('q_nom', jointController.loadPoseFromFile(fixedPointFile))
return obj, jointController
def init():
global imageManager
imageManager = ImageManager()
global cameraView
cameraView = CameraView(imageManager)
if "modelName" in drcargs.getDirectorConfig():
_modelName = drcargs.getDirectorConfig()['modelName']
cameraNames = imageManager.queue.getCameraNames()
if "CAMERA_LEFT" in cameraNames:
addCameraView('CAMERA_LEFT', 'Head camera')
if "OPENNI_FRAME_LEFT" in cameraNames:
addCameraView('OPENNI_FRAME_LEFT', 'OpenNI')
#import bot_core as lcmbotcore
#addCameraView('CAMERA', 'Head camera right', 'CAMERA_RIGHT', lcmbotcore.images_t.RIGHT)
#addCameraView('CAMERA', 'Head camera depth', 'CAMERA_DISPARITY', lcmbotcore.images_t.DISPARITY_ZIPPED)
if "atlas" in _modelName or "valkyrie" in _modelName:
addCameraView('CAMERACHEST_LEFT', 'Chest left')
addCameraView('CAMERACHEST_RIGHT', 'Chest right')
if "atlas" in drcargs.getDirectorConfig()['modelName']:
addCameraView('CAMERALHAND', 'Hand left')
addCameraView('CAMERARHAND', 'Hand right')
if "KINECT_RGB" in cameraNames:
addCameraView('KINECT_RGB', 'Kinect RGB')
def init():
global imageManager
imageManager = ImageManager()
global cameraView
cameraView = CameraView(imageManager)
if "modelName" in drcargs.getDirectorConfig():
_modelName = drcargs.getDirectorConfig()["modelName"]
cameraNames = imageManager.queue.getCameraNames()
if "CAMERA_LEFT" in cameraNames:
addCameraView("CAMERA_LEFT", "Head camera")
if "OPENNI_FRAME_LEFT" in cameraNames:
addCameraView("OPENNI_FRAME_LEFT", "OpenNI")
# import bot_core as lcmbotcore
# addCameraView('CAMERA', 'Head camera right', 'CAMERA_RIGHT', lcmbotcore.images_t.RIGHT)
# addCameraView('CAMERA', 'Head camera depth', 'CAMERA_DISPARITY', lcmbotcore.images_t.DISPARITY_ZIPPED)
if "atlas" in _modelName or "valkyrie" in _modelName:
addCameraView("CAMERACHEST_LEFT", "Chest left")
addCameraView("CAMERACHEST_RIGHT", "Chest right")
if "atlas" in drcargs.getDirectorConfig()["modelName"]:
addCameraView("CAMERALHAND", "Hand left")
addCameraView("CAMERARHAND", "Hand right")
if "KINECT_RGB" in cameraNames:
addCameraView("KINECT_RGB", "Kinect RGB")
def __init__(self, channel):
self.leftFootLink = drcargs.getDirectorConfig()['leftFootLink']
self.rightFootLink = drcargs.getDirectorConfig()['rightFootLink']
footContactSub = lcmUtils.addSubscriber(
channel, lcmdrc.foot_contact_estimate_t, self.onFootContact)
footContactSub.setSpeedLimit(60)
def init():
global imageManager
imageManager = ImageManager()
global cameraView
cameraView = CameraView(imageManager)
if "modelName" in drcargs.getDirectorConfig():
_modelName = drcargs.getDirectorConfig()['modelName']
cameraNames = imageManager.queue.getCameraNames()
if "MULTISENSE_CAMERA_LEFT" in cameraNames:
addCameraView('MULTISENSE_CAMERA_LEFT', 'Head camera')
if "OPENNI_FRAME_LEFT" in cameraNames:
addCameraView('OPENNI_FRAME_LEFT', 'OpenNI')
#import bot_core as lcmbotcore
#addCameraView('MULTISENSE_CAMERA', 'Head camera right', 'MULTISENSE_CAMERA_RIGHT', lcmbotcore.images_t.RIGHT)
#addCameraView('MULTISENSE_CAMERA', 'Head camera depth', 'MULTISENSE_CAMERA_DISPARITY', lcmbotcore.images_t.DISPARITY_ZIPPED)
if "atlas" in _modelName or "valkyrie" in _modelName:
addCameraView('CAMERACHEST_LEFT', 'Chest left')
addCameraView('CAMERACHEST_RIGHT', 'Chest right')
if "atlas" in drcargs.getDirectorConfig()['modelName']:
addCameraView('CAMERALHAND', 'Hand left')
addCameraView('CAMERARHAND', 'Hand right')
if "KINECT_RGB" in cameraNames:
addCameraView('KINECT_RGB', 'Kinect RGB')
def __init__(self, planningUtils, robotStateModel,
robotStateJointController, teleopRobotModel,
teleopJointController, ikPlanner, manipPlanner,
affordanceManager, showPlanFunction, hidePlanFunction,
footDriver):
self.planningUtils = 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.footDriver = footDriver
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(':/ui/ddMotionPlanningPanel.ui')
assert uifile.open(uifile.ReadOnly)
self.widget = loader.load(uifile)
self.ui = WidgetDict(self.widget.children())
# Check motion planning mode
self.ui.mpModeButton.connect('clicked()', self.onMotionPlanningMode)
# End-pose planning
self.ui.handCombo.connect('currentIndexChanged(const QString&)',
self.onHandChanged)
self.ui.baseComboBox.connect('currentIndexChanged(const QString&)',
self.onBaseConstraintChanged)
self.ui.backComboBox.connect('currentIndexChanged(const QString&)',
self.onBackConstraintChanged)
self.ui.feetComboBox.connect('currentIndexChanged(const QString&)',
self.onFeetConstraintChanged)
self.ui.otherHandComboBox.connect(
'currentIndexChanged(const QString&)',
self.onOtherHandConstraintChanged)
self.ui.fpButton.connect('clicked()', self.onSearchFinalPose)
if 'kneeJointLimits' in drcargs.getDirectorConfig():
self.kneeJointLimits = drcargs.getDirectorConfig(
)['kneeJointLimits']
self.constraintSet = None
self.palmOffsetDistance = 0.0
# Foot step planning
self.placer = None
self.ui.walkingPlanButton.connect('clicked()', self.onWalkingPlan)
self.ui.teleportRobotButton.connect('clicked()',
self.onTeleportRobotToStanceFrame)
# Motion Planning
self.ui.motionPlanButton.connect('clicked()', self.onMotionPlan)
self.deactivate()
def __init__(self, handType, robotModel):
'''
handType is of the form 'left_robotiq' or 'right_valkyrie'
'''
def toFrame(xyzrpy):
rpy = [math.degrees(rad) for rad in xyzrpy[3:]]
return transformUtils.frameFromPositionAndRPY(xyzrpy[:3], rpy)
self.side, self.handType = handType.split('_')
assert self.side in ('left', 'right')
thisCombination = None
handCombinations = drcargs.getDirectorConfig()['handCombinations']
numberOfHands = len(handCombinations)
for i in range(0, numberOfHands):
if (handCombinations[i]['side'] == self.side):
thisCombination = handCombinations[i]
break
assert thisCombination is not None
self.handLinkName = thisCombination['handLinkName']
self.handUrdf = thisCombination['handUrdf']
self.loadHandModel()
self.initHandTransforms(robotModel, thisCombination)
def sliderChanged(self, jointName):
slider = self.slidersMap[jointName]
jointIndex = self.toJointIndex(jointName)
jointValue = self.toJointValue(jointIndex,
slider.value / float(self.sliderMax))
if ('arm' in jointName and self.mirrorArms) or ('leg' in jointName
and self.mirrorLegs):
mirrorJoint = jointName.replace(
'l_',
'r_') if jointName.startswith('l_') else jointName.replace(
'r_', 'l_')
mirrorIndex = self.toJointIndex(mirrorJoint)
mirrorValue = jointValue
if mirrorJoint in drcargs.getDirectorConfig(
)['mirrorJointSignFlips']:
mirrorValue = -mirrorValue
self.endPose[mirrorIndex] = mirrorValue
self.endPose[jointIndex] = jointValue
self.updateLabel(jointName, jointValue)
# this is what is causing the position goal to be published using the commandStream.setGoalPose function
# call
self.showPose(self.endPose)
self.updateSliders()
def __init__(self):
self.app = ConsoleApp()
self.view = self.app.createView()
self.robotSystem = robotsystem.create(self.view)
self.config = drcargs.getDirectorConfig()
jointGroups = self.config['teleopJointGroups']
self.jointTeleopPanel = JointTeleopPanel(self.robotSystem, jointGroups)
self.jointCommandPanel = JointCommandPanel(self.robotSystem)
self.jointCommandPanel.ui.speedSpinBox.setEnabled(False)
self.jointCommandPanel.ui.mirrorArmsCheck.setChecked(self.jointTeleopPanel.mirrorArms)
self.jointCommandPanel.ui.mirrorLegsCheck.setChecked(self.jointTeleopPanel.mirrorLegs)
self.jointCommandPanel.ui.resetButton.connect('clicked()', self.resetJointTeleopSliders)
self.jointCommandPanel.ui.mirrorArmsCheck.connect('clicked()', self.mirrorJointsChanged)
self.jointCommandPanel.ui.mirrorLegsCheck.connect('clicked()', self.mirrorJointsChanged)
self.widget = QtGui.QWidget()
gl = QtGui.QGridLayout(self.widget)
gl.addWidget(self.app.showObjectModel(), 0, 0, 4, 1) # row, col, rowspan, colspan
gl.addWidget(self.view, 0, 1, 1, 1)
gl.addWidget(self.jointCommandPanel.widget, 1, 1, 1, 1)
gl.addWidget(self.jointTeleopPanel.widget, 0, 2, -1, 1)
gl.setRowStretch(0,1)
gl.setColumnStretch(1,1)
#self.sub = lcmUtils.addSubscriber('COMMITTED_ROBOT_PLAN', lcmdrc.robot_plan_t, self.onRobotPlan)
lcmUtils.addSubscriber('STEERING_COMMAND_POSITION_GOAL', lcmdrc.joint_position_goal_t, self.onSingleJointPositionGoal)
lcmUtils.addSubscriber('THROTTLE_COMMAND_POSITION_GOAL', lcmdrc.joint_position_goal_t, self.onSingleJointPositionGoal)
def __init__(self):
self.app = ConsoleApp()
self.view = self.app.createView()
self.robotSystem = robotsystem.create(self.view)
self.config = drcargs.getDirectorConfig()
jointGroups = self.config['teleopJointGroups']
self.jointTeleopPanel = JointTeleopPanel(self.robotSystem, jointGroups)
self.jointCommandPanel = JointCommandPanel(self.robotSystem)
self.jointCommandPanel.ui.speedSpinBox.setEnabled(False)
self.jointCommandPanel.ui.mirrorArmsCheck.setChecked(self.jointTeleopPanel.mirrorArms)
self.jointCommandPanel.ui.mirrorLegsCheck.setChecked(self.jointTeleopPanel.mirrorLegs)
self.jointCommandPanel.ui.resetButton.connect('clicked()', self.resetJointTeleopSliders)
self.jointCommandPanel.ui.mirrorArmsCheck.connect('clicked()', self.mirrorJointsChanged)
self.jointCommandPanel.ui.mirrorLegsCheck.connect('clicked()', self.mirrorJointsChanged)
self.widget = QtGui.QWidget()
gl = QtGui.QGridLayout(self.widget)
gl.addWidget(self.app.showObjectModel(), 0, 0, 4, 1) # row, col, rowspan, colspan
gl.addWidget(self.view, 0, 1, 1, 1)
gl.addWidget(self.jointCommandPanel.widget, 1, 1, 1, 1)
gl.addWidget(self.jointTeleopPanel.widget, 0, 2, -1, 1)
gl.setRowStretch(0,1)
gl.setColumnStretch(1,1)
#self.sub = lcmUtils.addSubscriber('COMMITTED_ROBOT_PLAN', lcmdrc.robot_plan_t, self.onRobotPlan)
lcmUtils.addSubscriber('STEERING_COMMAND_POSITION_GOAL', lcmdrc.joint_position_goal_t, self.onSingleJointPositionGoal)
lcmUtils.addSubscriber('THROTTLE_COMMAND_POSITION_GOAL', lcmdrc.joint_position_goal_t, self.onSingleJointPositionGoal)
def __init__(self, handType, robotModel):
'''
handType is of the form 'left_robotiq' or 'right_valkyrie'
'''
def toFrame(xyzrpy):
rpy = [math.degrees(rad) for rad in xyzrpy[3:]]
return transformUtils.frameFromPositionAndRPY(xyzrpy[:3], rpy)
self.side, self.handType = handType.split('_')
assert self.side in ('left', 'right')
thisCombination = None
handCombinations = drcargs.getDirectorConfig()['handCombinations']
numberOfHands = len(handCombinations)
for i in range(0, numberOfHands ):
if (handCombinations[i]['side'] == self.side):
thisCombination = handCombinations[i]
break
assert thisCombination is not None
self.handLinkName = thisCombination['handLinkName']
self.handUrdf = thisCombination['handUrdf']
self.loadHandModel()
self.initHandTransforms(robotModel, thisCombination)
def getHeadLink(self):
headLink = drcargs.getDirectorConfig().get('headLink')
if not headLink:
import warnings
warnings.warn(
"headLink is not defined in director config - certain features will be broken"
)
return headLink
def __init__(self, robotModel, imageManager, cameraName):
self.robotModel = robotModel
self.cameraName = cameraName
self.imageManager = imageManager
self.rayLength = 2.0
self.headLink = drcargs.getDirectorConfig()['headLink']
robotModel.connectModelChanged(self.update)
self.update(robotModel)
def getRobotStateJointNames():
global _robotStateJointNames
if _robotStateJointNames:
return _robotStateJointNames
else:
_robotStateJointNames = drcargs.getDirectorConfig()["robotStateJointNames"]
return _robotStateJointNames
def getDrakePoseJointNames():
global _drakePoseJointNames
if _drakePoseJointNames:
return _drakePoseJointNames
else:
_drakePoseJointNames = drcargs.getDirectorConfig()['drakeJointNames']
return _drakePoseJointNames
def getRobotStateJointNames():
global _robotStateJointNames
if _robotStateJointNames:
return _robotStateJointNames
else:
_robotStateJointNames = drcargs.getDirectorConfig()['robotStateJointNames']
return _robotStateJointNames
def getDrakePoseJointNames():
global _drakePoseJointNames
if _drakePoseJointNames:
return _drakePoseJointNames
else:
_drakePoseJointNames = drcargs.getDirectorConfig()["drakeJointNames"]
return _drakePoseJointNames
def __init__(self, planningUtils, robotStateModel, robotStateJointController, teleopRobotModel, teleopJointController, ikPlanner, manipPlanner, affordanceManager, showPlanFunction, hidePlanFunction, footDriver):
self.planningUtils = 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.footDriver = footDriver
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(':/ui/ddMotionPlanningPanel.ui')
assert uifile.open(uifile.ReadOnly)
self.widget = loader.load(uifile)
self.ui = WidgetDict(self.widget.children())
# Check motion planning mode
self.ui.mpModeButton.connect('clicked()', self.onMotionPlanningMode)
# End-pose planning
self.ui.handCombo.connect('currentIndexChanged(const QString&)', self.onHandChanged)
self.ui.baseComboBox.connect('currentIndexChanged(const QString&)', self.onBaseConstraintChanged)
self.ui.backComboBox.connect('currentIndexChanged(const QString&)', self.onBackConstraintChanged)
self.ui.feetComboBox.connect('currentIndexChanged(const QString&)', self.onFeetConstraintChanged)
self.ui.otherHandComboBox.connect('currentIndexChanged(const QString&)', self.onOtherHandConstraintChanged)
self.ui.fpButton.connect('clicked()', self.onSearchFinalPose)
if 'kneeJointLimits' in drcargs.getDirectorConfig():
self.kneeJointLimits = drcargs.getDirectorConfig()['kneeJointLimits']
self.constraintSet = None
self.palmOffsetDistance = 0.0
# Foot step planning
self.placer = None
self.ui.walkingPlanButton.connect('clicked()', self.onWalkingPlan)
self.ui.teleportRobotButton.connect('clicked()', self.onTeleportRobotToStanceFrame)
# Motion Planning
self.ui.motionPlanButton.connect('clicked()', self.onMotionPlan)
self.deactivate()
def __init__(self, robotModel, defaultLeftHandType=None, defaultRightHandType=None):
self.robotModel = robotModel
self.defaultHandTypes = {}
self.loaders = {}
handCombinations = drcargs.getDirectorConfig().get('handCombinations', [])
for description in handCombinations:
handType = description['handType']
side = description['side']
self.defaultHandTypes[side] = handType
def __init__(self, robotModel, defaultLeftHandType=None, defaultRightHandType=None):
self.robotModel = robotModel
self.defaultHandTypes = {}
self.loaders = {}
handCombinations = drcargs.getDirectorConfig().get('handCombinations', [])
for description in handCombinations:
handType = description['handType']
side = description['side']
self.defaultHandTypes[side] = handType
def initDirectorConfig(self, robotSystem):
from director import drcargs
directorConfig = drcargs.getDirectorConfig()
if 'colorMode' not in directorConfig:
defaultColorMode = 'URDF Colors'
directorConfig['colorMode'] = defaultColorMode
assert directorConfig['colorMode'] in ['URDF Colors', 'Solid Color', 'Textures']
return FieldContainer(directorConfig=directorConfig)
def initDirectorConfig(self, robotSystem):
from director import drcargs
directorConfig = drcargs.getDirectorConfig()
if 'colorMode' not in directorConfig:
defaultColorMode = 'URDF Colors'
directorConfig['colorMode'] = defaultColorMode
assert directorConfig['colorMode'] in ['URDF Colors', 'Solid Color', 'Textures']
return FieldContainer(directorConfig=directorConfig)
def __init__(self, handType, robotModel):
'''
handType is of the form 'left_robotiq' or 'right_valkyrie'
'''
def toFrame(xyzrpy):
rpy = [math.degrees(rad) for rad in xyzrpy[3:]]
return transformUtils.frameFromPositionAndRPY(xyzrpy[:3], rpy)
self.side, self.handType = handType.split('_')
assert self.side in ('left', 'right')
thisCombination = None
handCombinations = drcargs.getDirectorConfig()['handCombinations']
numberOfHands = len(handCombinations)
for i in range(0, numberOfHands):
if (handCombinations[i]['side'] == self.side):
thisCombination = handCombinations[i]
break
assert thisCombination is not None
self.handLinkName = thisCombination['handLinkName']
self.handUrdf = thisCombination['handUrdf']
handRootLink = thisCombination['handRootLink']
robotMountLink = thisCombination['robotMountLink']
palmLink = thisCombination['palmLink']
self.loadHandModel()
baseToHandRoot = self.getLinkToLinkTransform(self.handModel,
'plane::xy::base',
handRootLink)
robotMountToHandRoot = self.getLinkToLinkTransform(
robotModel, robotMountLink, handRootLink)
robotMountToHandLink = self.getLinkToLinkTransform(
robotModel, robotMountLink, self.handLinkName)
robotMountToPalm = self.getLinkToLinkTransform(robotModel,
robotMountLink,
palmLink)
t = vtk.vtkTransform()
t.PostMultiply()
t.Concatenate(baseToHandRoot)
t.Concatenate(robotMountToHandRoot.GetLinearInverse())
t.Concatenate(robotMountToPalm)
self.modelToPalm = t
self.handLinkToPalm = self.getLinkToLinkTransform(
robotModel, self.handLinkName, palmLink)
self.palmToHandLink = self.handLinkToPalm.GetLinearInverse()
def __init__(self, handType, robotModel):
'''
handType is of the form 'left_robotiq' or 'right_valkyrie'
'''
def toFrame(xyzrpy):
rpy = [math.degrees(rad) for rad in xyzrpy[3:]]
return transformUtils.frameFromPositionAndRPY(xyzrpy[:3], rpy)
self.side, self.handType = handType.split('_')
assert self.side in ('left', 'right')
thisCombination = None
handCombinations = drcargs.getDirectorConfig()['handCombinations']
numberOfHands = len(handCombinations)
for i in range(0, numberOfHands ):
if (handCombinations[i]['side'] == self.side):
thisCombination = handCombinations[i]
break
assert thisCombination is not None
self.handLinkName = thisCombination['handLinkName']
self.handUrdf = thisCombination['handUrdf']
handRootLink = thisCombination['handRootLink']
robotMountLink = thisCombination['robotMountLink']
palmLink = thisCombination['palmLink']
self.loadHandModel()
baseToHandRoot = self.getLinkToLinkTransform(self.handModel, 'plane::xy::base', handRootLink)
robotMountToHandRoot = self.getLinkToLinkTransform(robotModel, robotMountLink, handRootLink)
robotMountToHandLink = self.getLinkToLinkTransform(robotModel, robotMountLink, self.handLinkName)
robotMountToPalm = self.getLinkToLinkTransform(robotModel, robotMountLink, palmLink)
t = vtk.vtkTransform()
t.PostMultiply()
t.Concatenate(baseToHandRoot)
t.Concatenate(robotMountToHandRoot.GetLinearInverse())
t.Concatenate(robotMountToPalm)
self.modelToPalm = t
self.handLinkToPalm = self.getLinkToLinkTransform(robotModel, self.handLinkName, palmLink)
self.palmToHandLink = self.handLinkToPalm.GetLinearInverse()
def setNeckPitch(neckPitchDegrees):
assert neckPitchDegrees <= 90 and neckPitchDegrees >= -90
jointGroups = drcargs.getDirectorConfig()['teleopJointGroups']
jointGroupNeck = filter(lambda group: group['name'] == 'Neck',
jointGroups)
if (len(jointGroupNeck) == 1):
neckJoints = jointGroupNeck[0]['joints']
else:
return
# Assume first neck joint is the pitch joint
m = lcmbotcore.joint_angles_t()
m.utime = getUtime()
m.num_joints = 1
m.joint_name = [neckJoints[0]]
m.joint_position = [math.radians(neckPitchDegrees)]
lcmUtils.publish('DESIRED_NECK_ANGLES', m)
def sliderChanged(self, jointName):
slider = self.slidersMap[jointName]
jointIndex = self.toJointIndex(jointName)
jointValue = self.toJointValue(jointIndex, slider.value / float(self.sliderMax))
if ('arm' in jointName and self.mirrorArms) or ('leg' in jointName and self.mirrorLegs):
mirrorJoint = jointName.replace('l_', 'r_') if jointName.startswith('l_') else jointName.replace('r_', 'l_')
mirrorIndex = self.toJointIndex(mirrorJoint)
mirrorValue = jointValue
if mirrorJoint in drcargs.getDirectorConfig()['mirrorJointSignFlips']:
mirrorValue = -mirrorValue
self.endPose[mirrorIndex] = mirrorValue
self.endPose[jointIndex] = jointValue
self.updateLabel(jointName, jointValue)
self.showPose(self.endPose)
self.updateSliders()
def setNeckPitch(neckPitchDegrees):
assert neckPitchDegrees <= 90 and neckPitchDegrees >= -90
jointGroups = drcargs.getDirectorConfig()['teleopJointGroups']
jointGroupNeck = filter(lambda group: group['name'] == 'Neck', jointGroups)
neckJoints = jointGroupNeck[0]['joints']
if (len(neckJoints) == 1): # Atlas
jointPositions = [math.radians(neckPitchDegrees)]
elif (len(neckJoints) == 3): # Valkyrie, assuming joint 0 is lowerNeckPitch
jointPositions = [math.radians(neckPitchDegrees), 0, 0]
else:
return
# Assume first neck joint is the pitch joint
m = lcmbotcore.joint_angles_t()
m.utime = getUtime()
m.num_joints = len(neckJoints)
m.joint_name = neckJoints
m.joint_position = jointPositions
lcmUtils.publish('DESIRED_NECK_ANGLES', m)
def setNeckPitch(neckPitchDegrees):
assert neckPitchDegrees <= 90 and neckPitchDegrees >= -90
jointGroups = drcargs.getDirectorConfig()['teleopJointGroups']
jointGroupNeck = filter(lambda group: group['name'] == 'Neck', jointGroups)
neckJoints = jointGroupNeck[0]['joints']
if (len(neckJoints) == 1): # Atlas
jointPositions = [math.radians(neckPitchDegrees)]
elif (len(neckJoints) == 3): # Valkyrie, assuming joint 0 is lowerNeckPitch
jointPositions = [math.radians(neckPitchDegrees), 0, 0]
else:
return
# Assume first neck joint is the pitch joint
m = lcmbotcore.joint_angles_t()
m.utime = getUtime()
m.num_joints = len(neckJoints)
m.joint_name = neckJoints
m.joint_position = jointPositions
lcmUtils.publish('DESIRED_NECK_ANGLES', m)
def sliderChanged(self, jointName):
slider = self.slidersMap[jointName]
jointIndex = self.toJointIndex(jointName)
jointValue = self.toJointValue(jointIndex, slider.value / float(self.sliderMax))
if ('arm' in jointName and self.mirrorArms) or ('leg' in jointName and self.mirrorLegs):
mirrorJoint = jointName.replace('l_', 'r_') if jointName.startswith('l_') else jointName.replace('r_', 'l_')
mirrorIndex = self.toJointIndex(mirrorJoint)
mirrorValue = jointValue
if mirrorJoint in drcargs.getDirectorConfig()['mirrorJointSignFlips']:
mirrorValue = -mirrorValue
self.endPose[mirrorIndex] = mirrorValue
self.endPose[jointIndex] = jointValue
self.updateLabel(jointName, jointValue)
# this is what is causing the position goal to be published using the commandStream.setGoalPose function
# call
self.showPose(self.endPose)
self.updateSliders()
def sliderChanged(self, jointName):
slider = self.slidersMap[jointName]
jointIndex = self.toJointIndex(jointName)
jointValue = self.toJointValue(jointIndex,
slider.value / float(self.sliderMax))
if ('arm' in jointName and self.mirrorArms) or ('leg' in jointName
and self.mirrorLegs):
mirrorJoint = jointName.replace(
'l_',
'r_') if jointName.startswith('l_') else jointName.replace(
'r_', 'l_')
mirrorIndex = self.toJointIndex(mirrorJoint)
mirrorValue = jointValue
if mirrorJoint in drcargs.getDirectorConfig(
)['mirrorJointSignFlips']:
mirrorValue = -mirrorValue
self.endPose[mirrorIndex] = mirrorValue
self.endPose[jointIndex] = jointValue
self.updateLabel(jointName, jointValue)
self.showPose(self.endPose)
self.updateSliders()
def isCompatibleWithConfig():
return "headLink" in drcargs.getDirectorConfig()
def __init__(self, channel):
self.leftFootLink = drcargs.getDirectorConfig()['leftFootLink']
self.rightFootLink = drcargs.getDirectorConfig()['rightFootLink']
footContactSub = lcmUtils.addSubscriber(channel, lcmdrc.foot_contact_estimate_t, self.onFootContact)
footContactSub.setSpeedLimit(60)
useBlackoutText = False
useRandomWalk = True
useCOPMonitor = True
useCourseModel = False
useLimitJointsSentToPlanner = False
useFeetlessRobot = False
# Sensor Flags
useKinect = False
useMultisense = True
useOpenniDepthImage = False
poseCollection = PythonQt.dd.ddSignalMap()
costCollection = PythonQt.dd.ddSignalMap()
if 'userConfig' in drcargs.getDirectorConfig():
if 'fixedBaseArm' in drcargs.getDirectorConfig()['userConfig']:
ikPlanner.fixedBaseArm = True
if 'disableComponents' in drcargs.getDirectorConfig():
for component in drcargs.getDirectorConfig()['disableComponents']:
print "Disabling", component
locals()[component] = False
if 'enableComponents' in drcargs.getDirectorConfig():
for component in drcargs.getDirectorConfig()['enableComponents']:
print "Enabling", component
locals()[component] = True
if useSpreadsheet:
def getHeadLink(self):
return drcargs.getDirectorConfig().get('headLink')
def init(self, view=None, globalsDict=None):
view = view or applogic.getCurrentRenderView()
useRobotState = True
usePerception = True
useFootsteps = True
useHands = True
usePlanning = True
useAtlasDriver = True
useAtlasConvexHull = False
useWidgets = False
directorConfig = drcargs.getDirectorConfig()
neckPitchJoint = 'neck_ay'
colorMode = 'URDF Colors'
if 'colorMode' in directorConfig:
assert directorConfig['colorMode'] in ['URDF Colors', 'Solid Color', 'Textures']
colorMode = directorConfig['colorMode']
if useAtlasDriver:
atlasDriver = atlasdriver.init(None)
if useRobotState:
robotStateModel, robotStateJointController = roboturdf.loadRobotModel('robot state model', view, urdfFile=directorConfig['urdfConfig']['robotState'], parent='sensors', color=roboturdf.getRobotGrayColor(), visible=True, colorMode=colorMode)
robotStateJointController.setPose('EST_ROBOT_STATE', robotStateJointController.getPose('q_nom'))
roboturdf.startModelPublisherListener([(robotStateModel, robotStateJointController)])
robotStateJointController.addLCMUpdater('EST_ROBOT_STATE')
segmentationroutines.SegmentationContext.initWithRobot(robotStateModel)
if usePerception:
multisenseDriver, mapServerSource = perception.init(view)
def getNeckPitch():
return robotStateJointController.q[robotstate.getDrakePoseJointNames().index( neckPitchJoint )]
neckDriver = perception.NeckDriver(view, getNeckPitch)
def getSpindleAngleFunction():
if (robotStateJointController.lastRobotStateMessage):
if ('hokuyo_joint' in robotStateJointController.lastRobotStateMessage.joint_name):
index = robotStateJointController.lastRobotStateMessage.joint_name.index('hokuyo_joint')
return (float(robotStateJointController.lastRobotStateMessage.utime)/(1e6),
robotStateJointController.lastRobotStateMessage.joint_position[index])
return (0, 0)
spindleMonitor = perception.SpindleMonitor(getSpindleAngleFunction)
robotStateModel.connectModelChanged(spindleMonitor.onRobotStateChanged)
if useHands:
rHandDriver = handdriver.RobotiqHandDriver(side='right')
lHandDriver = handdriver.RobotiqHandDriver(side='left')
if useFootsteps:
footstepsDriver = footstepsdriver.FootstepsDriver(robotStateJointController)
irisDriver = irisdriver.IRISDriver(robotStateJointController, footstepsDriver.params)
raycastDriver = raycastdriver.RaycastDriver()
if usePlanning:
ikRobotModel, ikJointController = roboturdf.loadRobotModel('ik model', urdfFile=directorConfig['urdfConfig']['ik'], parent=None)
om.removeFromObjectModel(ikRobotModel)
ikJointController.addPose('q_end', ikJointController.getPose('q_nom'))
ikJointController.addPose('q_start', ikJointController.getPose('q_nom'))
if 'leftFootLink' in directorConfig:
ikServer = ik.AsyncIKCommunicator(directorConfig['urdfConfig']['ik'], directorConfig['fixedPointFile'], directorConfig['leftFootLink'], directorConfig['rightFootLink'], directorConfig['pelvisLink'])
else: # assume that robot has no feet e.g. fixed base arm
ikServer = ik.AsyncIKCommunicator(directorConfig['urdfConfig']['ik'], directorConfig['fixedPointFile'], '', '', '')
def startIkServer():
ikServer.startServerAsync()
ikServer.comm.writeCommandsToLogFile = True
#startIkServer()
playbackRobotModel, playbackJointController = roboturdf.loadRobotModel('playback model', view, urdfFile=directorConfig['urdfConfig']['playback'], parent='planning', color=roboturdf.getRobotOrangeColor(), visible=False, colorMode=colorMode)
teleopRobotModel, teleopJointController = roboturdf.loadRobotModel('teleop model', view, urdfFile=directorConfig['urdfConfig']['teleop'], parent='planning', color=roboturdf.getRobotBlueColor(), visible=False, colorMode=colorMode)
if useAtlasConvexHull:
chullRobotModel, chullJointController = roboturdf.loadRobotModel('convex hull atlas', view, urdfFile=urdfConfig['chull'], parent='planning',
color=roboturdf.getRobotOrangeColor(), visible=False)
playbackJointController.models.append(chullRobotModel)
planPlayback = planplayback.PlanPlayback()
handFactory = roboturdf.HandFactory(robotStateModel)
handModels = []
for side in ['left', 'right']:
if side in handFactory.defaultHandTypes:
handModels.append(handFactory.getLoader(side))
ikPlanner = ikplanner.IKPlanner(ikServer, ikRobotModel, ikJointController, handModels)
manipPlanner = robotplanlistener.ManipulationPlanDriver(ikPlanner)
affordanceManager = affordancemanager.AffordanceObjectModelManager(view)
affordanceitems.MeshAffordanceItem.getMeshManager().initLCM()
affordanceitems.MeshAffordanceItem.getMeshManager().collection.sendEchoRequest()
affordanceManager.collection.sendEchoRequest()
segmentation.affordanceManager = affordanceManager
plannerPub = plannerPublisher.PlannerPublisher(ikPlanner,affordanceManager,ikRobotModel)
ikPlanner.setPublisher(plannerPub)
# This joint angle is mapped to the Multisense panel
neckPitchJoint = ikPlanner.neckPitchJoint
applogic.resetCamera(viewDirection=[-1,0,0], view=view)
if useWidgets:
playbackPanel = playbackpanel.PlaybackPanel(planPlayback, playbackRobotModel, playbackJointController,
robotStateModel, robotStateJointController, manipPlanner)
teleopPanel = teleoppanel.TeleopPanel(robotStateModel, robotStateJointController, teleopRobotModel, teleopJointController,
ikPlanner, manipPlanner, playbackPanel.setPlan, playbackPanel.hidePlan)
footstepsDriver.walkingPlanCallback = playbackPanel.setPlan
manipPlanner.connectPlanReceived(playbackPanel.setPlan)
viewBehaviors = None
robotSystemArgs = dict(locals())
for arg in ['globalsDict', 'self']:
del robotSystemArgs[arg]
if globalsDict is not None:
globalsDict.update(robotSystemArgs)
robotSystem = FieldContainer(**robotSystemArgs)
robotSystem.viewBehaviors = robotviewbehaviors.RobotViewBehaviors(view, robotSystem)
return robotSystem
def getHeadLink(self):
headLink = drcargs.getDirectorConfig().get('headLink')
if not headLink:
import warnings
warnings.warn("headLink is not defined in director config - certain features will be broken")
return headLink
def isCompatibleWithConfig():
return 'headLink' in drcargs.getDirectorConfig()
def getEndEffectorLinkName():
config = drcargs.getDirectorConfig()['endEffectorConfig']
linkName = config['endEffectorLinkNames'][0]
assert linkName == ikPlanner.getHandLink()
return linkName
def init(self, view=None, globalsDict=None):
view = view or applogic.getCurrentRenderView()
useRobotState = True
usePerception = True
useFootsteps = True
useHands = True
usePlanning = True
useAtlasDriver = True
useAtlasConvexHull = False
useWidgets = False
directorConfig = drcargs.getDirectorConfig()
neckPitchJoint = 'neck_ay'
colorMode = 'URDF Colors'
if 'colorMode' in directorConfig:
assert directorConfig['colorMode'] in [
'URDF Colors', 'Solid Color', 'Textures'
]
colorMode = directorConfig['colorMode']
if useAtlasDriver:
atlasDriver = atlasdriver.init(None)
if useRobotState:
robotStateModel, robotStateJointController = roboturdf.loadRobotModel(
'robot state model',
view,
urdfFile=directorConfig['urdfConfig']['robotState'],
parent='sensors',
color=roboturdf.getRobotGrayColor(),
visible=True,
colorMode=colorMode)
robotStateJointController.setPose(
'EST_ROBOT_STATE', robotStateJointController.getPose('q_nom'))
roboturdf.startModelPublisherListener([
(robotStateModel, robotStateJointController)
])
robotStateJointController.addLCMUpdater('EST_ROBOT_STATE')
segmentationroutines.SegmentationContext.initWithRobot(
robotStateModel)
if usePerception:
multisenseDriver, mapServerSource = perception.init(view)
def getNeckPitch():
return robotStateJointController.q[
robotstate.getDrakePoseJointNames().index(neckPitchJoint)]
neckDriver = perception.NeckDriver(view, getNeckPitch)
def getSpindleAngleFunction():
if (robotStateJointController.lastRobotStateMessage):
if ('hokuyo_joint' in robotStateJointController.
lastRobotStateMessage.joint_name):
index = robotStateJointController.lastRobotStateMessage.joint_name.index(
'hokuyo_joint')
return (float(robotStateJointController.
lastRobotStateMessage.utime) / (1e6),
robotStateJointController.
lastRobotStateMessage.joint_position[index])
return (0, 0)
spindleMonitor = perception.SpindleMonitor(getSpindleAngleFunction)
robotStateModel.connectModelChanged(
spindleMonitor.onRobotStateChanged)
if useHands:
rHandDriver = handdriver.RobotiqHandDriver(side='right')
lHandDriver = handdriver.RobotiqHandDriver(side='left')
if useFootsteps:
footstepsDriver = footstepsdriver.FootstepsDriver(
robotStateJointController)
irisDriver = irisdriver.IRISDriver(robotStateJointController,
footstepsDriver.params)
raycastDriver = raycastdriver.RaycastDriver()
if usePlanning:
ikRobotModel, ikJointController = roboturdf.loadRobotModel(
'ik model',
urdfFile=directorConfig['urdfConfig']['ik'],
parent=None)
om.removeFromObjectModel(ikRobotModel)
ikJointController.addPose('q_end',
ikJointController.getPose('q_nom'))
ikJointController.addPose('q_start',
ikJointController.getPose('q_nom'))
if 'leftFootLink' in directorConfig:
ikServer = ik.AsyncIKCommunicator(
directorConfig['urdfConfig']['ik'],
directorConfig['fixedPointFile'],
directorConfig['leftFootLink'],
directorConfig['rightFootLink'],
directorConfig['pelvisLink'])
else: # assume that robot has no feet e.g. fixed base arm
ikServer = ik.AsyncIKCommunicator(
directorConfig['urdfConfig']['ik'],
directorConfig['fixedPointFile'], '', '', '')
def startIkServer():
ikServer.startServerAsync()
ikServer.comm.writeCommandsToLogFile = True
#startIkServer()
playbackRobotModel, playbackJointController = roboturdf.loadRobotModel(
'playback model',
view,
urdfFile=directorConfig['urdfConfig']['playback'],
parent='planning',
color=roboturdf.getRobotOrangeColor(),
visible=False,
colorMode=colorMode)
teleopRobotModel, teleopJointController = roboturdf.loadRobotModel(
'teleop model',
view,
urdfFile=directorConfig['urdfConfig']['teleop'],
parent='planning',
color=roboturdf.getRobotBlueColor(),
visible=False,
colorMode=colorMode)
if useAtlasConvexHull:
chullRobotModel, chullJointController = roboturdf.loadRobotModel(
'convex hull atlas',
view,
urdfFile=urdfConfig['chull'],
parent='planning',
color=roboturdf.getRobotOrangeColor(),
visible=False)
playbackJointController.models.append(chullRobotModel)
planPlayback = planplayback.PlanPlayback()
handFactory = roboturdf.HandFactory(robotStateModel)
handModels = []
for side in ['left', 'right']:
if side in handFactory.defaultHandTypes:
handModels.append(handFactory.getLoader(side))
ikPlanner = ikplanner.IKPlanner(ikServer, ikRobotModel,
ikJointController, handModels)
manipPlanner = robotplanlistener.ManipulationPlanDriver(ikPlanner)
affordanceManager = affordancemanager.AffordanceObjectModelManager(
view)
affordanceitems.MeshAffordanceItem.getMeshManager().initLCM()
affordanceitems.MeshAffordanceItem.getMeshManager(
).collection.sendEchoRequest()
affordanceManager.collection.sendEchoRequest()
segmentation.affordanceManager = affordanceManager
plannerPub = plannerPublisher.PlannerPublisher(
ikPlanner, affordanceManager, ikRobotModel)
ikPlanner.setPublisher(plannerPub)
# This joint angle is mapped to the Multisense panel
neckPitchJoint = ikPlanner.neckPitchJoint
applogic.resetCamera(viewDirection=[-1, 0, 0], view=view)
if useWidgets:
playbackPanel = playbackpanel.PlaybackPanel(
planPlayback, playbackRobotModel, playbackJointController,
robotStateModel, robotStateJointController, manipPlanner)
teleopPanel = teleoppanel.TeleopPanel(
robotStateModel, robotStateJointController, teleopRobotModel,
teleopJointController, ikPlanner, manipPlanner,
playbackPanel.setPlan, playbackPanel.hidePlan)
footstepsDriver.walkingPlanCallback = playbackPanel.setPlan
manipPlanner.connectPlanReceived(playbackPanel.setPlan)
viewBehaviors = None
robotSystemArgs = dict(locals())
for arg in ['globalsDict', 'self']:
del robotSystemArgs[arg]
if globalsDict is not None:
globalsDict.update(robotSystemArgs)
robotSystem = FieldContainer(**robotSystemArgs)
robotSystem.viewBehaviors = robotviewbehaviors.RobotViewBehaviors(
view, robotSystem)
return robotSystem
def searchFinalPose(self, constraints, side, eeName, eePose,
nominalPoseName, capabilityMapFile, ikParameters):
commands = []
commands.append('default_shrink_factor = %s;' %
ikParameters.quasiStaticShrinkFactor)
constraintNames = []
for constraintId, constraint in enumerate(constraints):
if not constraint.enabled:
continue
constraint.getCommands(commands,
constraintNames,
suffix='_%d' % constraintId)
commands.append('\n')
commands.append('eeId = r.findLinkId(\'{:s}\');'.format(eeName))
commands.append('additional_constraints = {};')
commands.append('goal_constraints = {};')
commands.append(
'capability_map = CapabilityMap([\'{:s}\', \'/{:s}\']);'.format(
os.path.dirname(drcargs.args().directorConfigFile),
drcargs.getDirectorConfig()['capabilityMapFile']))
for constraint in constraintNames:
commands.append(
'if isa({0:s}, \'Point2PointDistanceConstraint\') && {0:s}.body_a.idx == eeId '
'|| isa({0:s}, \'EulerConstraint\') && {0:s}.body == eeId '
'goal_constraints = {{goal_constraints{{:}}, {0:s}}}; else '
'additional_constraints = {{additional_constraints{{:}}, {0:s}}};end'
.format(constraint))
commands.append('cost = Point(r.getPositionFrame(),10);')
commands.append(
'for i = r.getNumBodies():-1:1 '
'if all(r.getBody(i).parent > 0) && all(r.getBody(r.getBody(i).parent).position_num > 0) '
'cost(r.getBody(r.getBody(i).parent).position_num) = '
'cost(r.getBody(r.getBody(i).parent).position_num) + cost(r.getBody(i).position_num);end;end'
)
commands.append('cost(1:6) = max(cost(7:end))/2;')
commands.append('cost = cost/min(cost);')
commands.append('Q = diag(cost);')
commands.append('ikoptions = IKoptions(r);')
commands.append(
'ikoptions = ikoptions.setMajorIterationsLimit({:d});'.format(
ikParameters.majorIterationsLimit))
commands.append('ikoptions = ikoptions.setQ(Q);')
commands.append(
'ikoptions = ikoptions.setMajorOptimalityTolerance({:f});'.format(
ikParameters.majorOptimalityTolerance))
# commands.append('{:s}'.format(ConstraintBase.toColumnVectorString(xGoal)))
commands.append(
'fpp = FinalPoseProblem(r, eeId, reach_start, {:s}, additional_constraints,'
'{:s}, \'capabilitymap\', capability_map, \'ikoptions\', ikoptions, \'graspinghand\', \'{:s}\');'
.format(ConstraintBase.toColumnVectorString(eePose),
nominalPoseName, side))
commands.append('[x_goal, info] = fpp.findFinalPose();')
self.comm.sendCommands(commands)
info = self.comm.getFloatArray('info')[0]
if info == 1:
endPose = self.comm.getFloatArray('x_goal(8:end)')
else:
endPose = []
return endPose, info
def searchFinalPose(self, constraints, side, eeName, eePose, nominalPoseName, capabilityMapFile, ikParameters):
commands = []
commands.append('default_shrink_factor = %s;' % ikParameters.quasiStaticShrinkFactor)
constraintNames = []
for constraintId, constraint in enumerate(constraints):
if not constraint.enabled:
continue
constraint.getCommands(commands, constraintNames, suffix='_%d' % constraintId)
commands.append('\n')
commands.append('eeId = r.findLinkId(\'{:s}\');'.format(eeName))
commands.append('additional_constraints = {};')
commands.append('goal_constraints = {};')
commands.append('capability_map = CapabilityMap([\'{:s}\', \'/{:s}\']);'.format(os.path.dirname(drcargs.args().directorConfigFile), drcargs.getDirectorConfig()['capabilityMapFile']))
for constraint in constraintNames:
commands.append('if isa({0:s}, \'Point2PointDistanceConstraint\') && {0:s}.body_a.idx == eeId '
'|| isa({0:s}, \'EulerConstraint\') && {0:s}.body == eeId '
'goal_constraints = {{goal_constraints{{:}}, {0:s}}}; else '
'additional_constraints = {{additional_constraints{{:}}, {0:s}}};end'.format(constraint))
commands.append('cost = Point(r.getPositionFrame(),10);')
commands.append('for i = r.getNumBodies():-1:1 '
'if all(r.getBody(i).parent > 0) && all(r.getBody(r.getBody(i).parent).position_num > 0) '
'cost(r.getBody(r.getBody(i).parent).position_num) = '
'cost(r.getBody(r.getBody(i).parent).position_num) + cost(r.getBody(i).position_num);end;end')
commands.append('cost(1:6) = max(cost(7:end))/2;')
commands.append('cost = cost/min(cost);')
commands.append('Q = diag(cost);')
commands.append('ikoptions = IKoptions(r);')
commands.append('ikoptions = ikoptions.setMajorIterationsLimit({:d});'.format(ikParameters.majorIterationsLimit))
commands.append('ikoptions = ikoptions.setQ(Q);')
commands.append('ikoptions = ikoptions.setMajorOptimalityTolerance({:f});' .format(ikParameters.majorOptimalityTolerance))
# commands.append('{:s}'.format(ConstraintBase.toColumnVectorString(xGoal)))
commands.append('fpp = FinalPoseProblem(r, eeId, reach_start, {:s}, additional_constraints,'
'{:s}, \'capabilitymap\', capability_map, \'ikoptions\', ikoptions, \'graspinghand\', \'{:s}\');'.format(ConstraintBase.toColumnVectorString(eePose), nominalPoseName, side))
commands.append('[x_goal, info] = fpp.findFinalPose();')
self.comm.sendCommands(commands)
info = self.comm.getFloatArray('info')[0]
if info == 1:
endPose = self.comm.getFloatArray('x_goal(8:end)')
else:
endPose = []
return endPose, info
def getGraspToHandLink():
config = drcargs.getDirectorConfig()['endEffectorConfig']
return transformUtils.frameFromPositionAndRPY(
config['graspOffsetFrame'][0],
np.degrees(config['graspOffsetFrame'][1]))
useBlackoutText = False
useRandomWalk = True
useCOPMonitor = True
useCourseModel = False
useLimitJointsSentToPlanner = False
useFeetlessRobot = False
# Sensor Flags
useKinect = False
useMultisense = True
useOpenniDepthImage = False
poseCollection = PythonQt.dd.ddSignalMap()
costCollection = PythonQt.dd.ddSignalMap()
if 'userConfig' in drcargs.getDirectorConfig():
if 'fixedBaseArm' in drcargs.getDirectorConfig()['userConfig']:
ikPlanner.fixedBaseArm = True
if 'disableComponents' in drcargs.getDirectorConfig():
for component in drcargs.getDirectorConfig()['disableComponents']:
print "Disabling", component
locals()[component] = False
if 'enableComponents' in drcargs.getDirectorConfig():
for component in drcargs.getDirectorConfig()['enableComponents']:
print "Enabling", component
locals()[component] = True
if useSpreadsheet:
spreadsheet.init(poseCollection, costCollection)
