def __init__(self, **kwargs):
self._add_fields(usePointwise=None,
useCollision=None,
majorIterationsLimit=None,
majorOptimalityTolerance=None,
majorFeasibilityTolerance=None,
maxDegreesPerSecond=None,
maxBaseMetersPerSecond=None,
maxBaseRPYDegreesPerSecond=None,
maxBackDegreesPerSecond=None,
accelerationParam=None,
accelerationFraction=None,
maxPlanDuration=None,
fixInitialState=None,
numberOfAddedKnots=None,
numberOfInterpolatedCollisionChecks=None,
collisionMinDistance=None,
rrtMaxEdgeLength=None,
rrtGoalBias=None,
rrtHand=None,
rrtMaxNumVertices=None,
rrtNSmoothingPasses=None,
maxBodyTranslationSpeed=None,
maxBodyRotationSpeed=None,
rescaleBodyNames=None,
rescaleBodyPts=None,
quasiStaticShrinkFactor=None)
FieldContainer.__init__(self, **kwargs)
def testEncodeDecode(self, fields):
encoded = json.dumps(fields, cls=ikconstraintencoder.ConstraintEncoder)
decoded = json.loads(encoded, object_hook=ikconstraintencoder.ConstraintDecoder)
del decoded['class']
fields = FieldContainer(**decoded)
del fields.options['class']
fields.options = ikparameters.IkParameters(**fields.options)
constraints = []
for c in fields.constraints:
objClass = getattr(ikconstraints, c['class'])
del c['class']
obj = objClass()
constraints.append(obj)
for attr, value in c.iteritems():
if isinstance(value, dict) and 'position' in value and 'quaternion' in value:
value = transformUtils.transformFromPose(value['position'], value['quaternion'])
setattr(obj, attr, value)
fields.constraints = constraints
return fields
def testEncodeDecode(self, fields):
encoded = json.dumps(fields, cls=ikconstraintencoder.ConstraintEncoder)
decoded = json.loads(encoded,
object_hook=ikconstraintencoder.ConstraintDecoder)
del decoded['class']
fields = FieldContainer(**decoded)
del fields.options['class']
fields.options = ikparameters.IkParameters(**fields.options)
constraints = []
for c in fields.constraints:
objClass = getattr(ikconstraints, c['class'])
del c['class']
obj = objClass()
constraints.append(obj)
for attr, value in c.iteritems():
if isinstance(
value, dict
) and 'position' in value and 'quaternion' in value:
value = transformUtils.transformFromPose(
value['position'], value['quaternion'])
setattr(obj, attr, value)
fields.constraints = constraints
return fields
def __init__(self, **kwargs):
self._add_fields(
usePointwise = None,
useCollision = None,
majorIterationsLimit = None,
majorOptimalityTolerance = None,
majorFeasibilityTolerance = None,
maxDegreesPerSecond = None,
maxBaseMetersPerSecond = None,
maxBaseRPYDegreesPerSecond = None,
maxBackDegreesPerSecond = None,
accelerationParam = None,
accelerationFraction = None,
maxPlanDuration = None,
fixInitialState = None,
numberOfAddedKnots = None,
numberOfInterpolatedCollisionChecks = None,
collisionMinDistance = None,
rrtMaxEdgeLength = None,
rrtGoalBias = None,
rrtHand = None,
rrtMaxNumVertices = None,
rrtNSmoothingPasses = None,
maxBodyTranslationSpeed = None,
maxBodyRotationSpeed = None,
rescaleBodyNames = None,
rescaleBodyPts = None,
quasiStaticShrinkFactor = None
)
FieldContainer.__init__(self, **kwargs)
def initPlanning(self, robotSystem):
from director import objectmodel as om
from director import planningutils
from director import roboturdf
from director import ikplanner
directorConfig = robotSystem.directorConfig
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'))
handFactory = roboturdf.HandFactory(robotSystem.robotStateModel)
handModels = []
for side in ['left', 'right']:
if side in handFactory.defaultHandTypes:
handModels.append(handFactory.getLoader(side))
ikPlanner = ikplanner.IKPlanner(ikRobotModel, ikJointController, handModels)
planningUtils = planningutils.PlanningUtils(robotSystem.robotStateModel, robotSystem.robotStateJointController)
return FieldContainer(
ikRobotModel=ikRobotModel,
ikJointController=ikJointController,
handFactory=handFactory,
handModels=handModels,
ikPlanner=ikPlanner,
planningUtils=planningUtils
)
def initMainToolBar(self, fields):
from director import viewcolors
app = fields.app
toolBar = app.addToolBar('Main Toolbar')
app.addToolBarAction(toolBar, 'Python Console', ':/images/python_logo.png', callback=app.showPythonConsole)
toolBar.addSeparator()
terrainModeAction = fields.app.addToolBarAction(toolBar, 'Camera Free Rotate', ':/images/camera_mode.png')
lightAction = fields.app.addToolBarAction(toolBar, 'Background Light', ':/images/light_bulb_icon.png')
app.addToolBarAction(toolBar, 'Reset Camera', ':/images/reset_camera.png', callback=applogic.resetCamera)
def getFreeCameraMode():
return not applogic.getCameraTerrainModeEnabled(fields.view)
def setFreeCameraMode(enabled):
applogic.setCameraTerrainModeEnabled(fields.view, not enabled)
terrainToggle = applogic.ActionToggleHelper(terrainModeAction, getFreeCameraMode, setFreeCameraMode)
viewBackgroundLightHandler = viewcolors.ViewBackgroundLightHandler(fields.viewOptions, fields.gridObj,
lightAction)
return FieldContainer(viewBackgroundLightHandler=viewBackgroundLightHandler, terrainToggle=terrainToggle)
def initGrid(self, fields):
gridObj = vis.showGrid(fields.view, parent='scene')
gridObj.setProperty('Surface Mode', 'Surface with edges')
gridObj.setProperty('Color', [0, 0, 0])
gridObj.setProperty('Alpha', 0.1)
applogic.resetCamera(viewDirection=[-1, -1, -0.3], view=fields.view)
return FieldContainer(gridObj=gridObj)
def initTeleop(self, robotSystem):
from director import roboturdf
from director import teleoppanel
directorConfig = robotSystem.directorConfig
teleopRobotModel, teleopJointController = roboturdf.loadRobotModel(
'teleop model',
robotSystem.view,
urdfFile=directorConfig['urdfConfig']['teleop'],
parent='planning',
color=roboturdf.getRobotBlueColor(),
visible=False,
colorMode=directorConfig['colorMode'])
teleopPanel = teleoppanel.TeleopPanel(
robotSystem.robotStateModel, robotSystem.robotStateJointController,
teleopRobotModel, teleopJointController, robotSystem.ikPlanner,
robotSystem.manipPlanner, robotSystem.affordanceManager,
robotSystem.playbackPanel.setPlan,
robotSystem.playbackPanel.hidePlan, robotSystem.planningUtils)
return FieldContainer(
teleopRobotModel=teleopRobotModel,
teleopJointController=teleopJointController,
teleopPanel=teleopPanel,
)
def initOutputConsole(self, fields):
from director import outputconsole
outputConsole = outputconsole.OutputConsole()
outputConsole.addToAppWindow(fields.app, visible=False)
return FieldContainer(outputConsole=outputConsole)
def initGrid(self, fields):
gridObj = vis.showGrid(fields.view, parent="scene")
gridObj.setProperty("Surface Mode", "Surface with edges")
gridObj.setProperty("Color", [0, 0, 0])
gridObj.setProperty("Alpha", 0.1)
applogic.resetCamera(viewDirection=[-1, -1, -0.3], view=fields.view)
return FieldContainer(gridObj=gridObj)
def initUndoRedo(self, fields):
undoStack = QtGui.QUndoStack()
undoView = QtGui.QUndoView(undoStack)
undoView.setEmptyLabel("Start")
undoView.setWindowTitle("History")
undoDock = fields.app.addWidgetToDock(
undoView, QtCore.Qt.LeftDockWidgetArea, visible=False
)
undoAction = undoStack.createUndoAction(undoStack)
redoAction = undoStack.createRedoAction(undoStack)
undoAction.setShortcut(QtGui.QKeySequence("Ctrl+Z"))
redoAction.setShortcut(QtGui.QKeySequence("Ctrl+Shift+Z"))
fields.app.editMenu.addAction(undoAction)
fields.app.editMenu.addAction(redoAction)
return FieldContainer(
undoDock=undoDock,
undoStack=undoStack,
undoView=undoView,
undoAction=undoAction,
redoAction=redoAction,
)
def setupFields(self,
constraints,
ikParameters,
positionCosts,
nominalPoseName="",
seedPoseName="",
endPoseName=""):
poses = ikconstraintencoder.getPlanPoses(constraints, self.ikPlanner)
poses.update(self.poses)
poses['q_nom'] = list(self.ikPlanner.jointController.getPose('q_nom'))
fields = FieldContainer(
utime=getUtime(),
poses=poses,
constraints=constraints,
seedPose=seedPoseName,
nominalPose=nominalPoseName,
endPose=endPoseName,
jointNames=self.jointNames,
jointLimits=self.jointLimits,
positionCosts=positionCosts,
affordances=self.processAffordances(),
options=ikParameters,
)
return fields
def initHandDrivers(self, robotSystem):
from director import handdriver
rHandDriver = handdriver.RobotiqHandDriver(side='right')
lHandDriver = handdriver.RobotiqHandDriver(side='left')
return FieldContainer(rHandDriver=rHandDriver, lHandDriver=lHandDriver)
def initPlayback(self, robotSystem):
from director import roboturdf
from director import planplayback
from director import playbackpanel
from director import robotplanlistener
directorConfig = robotSystem.directorConfig
manipPlanner = robotplanlistener.ManipulationPlanDriver(
robotSystem.ikPlanner)
playbackRobotModel, playbackJointController = roboturdf.loadRobotModel(
'playback model',
robotSystem.view,
urdfFile=directorConfig['urdfConfig']['playback'],
parent='planning',
color=roboturdf.getRobotOrangeColor(),
visible=False,
colorMode=directorConfig['colorMode'])
planPlayback = planplayback.PlanPlayback()
playbackPanel = playbackpanel.PlaybackPanel(
planPlayback, playbackRobotModel, playbackJointController,
robotSystem.robotStateModel, robotSystem.robotStateJointController,
manipPlanner)
manipPlanner.connectPlanReceived(playbackPanel.setPlan)
return FieldContainer(playbackRobotModel=playbackRobotModel,
playbackJointController=playbackJointController,
planPlayback=planPlayback,
manipPlanner=manipPlanner,
playbackPanel=playbackPanel)
def initRobotLinkSelector(self, robotSystem):
from director import robotlinkselector
robotLinkSelector = robotlinkselector.RobotLinkSelector()
robotSystem.viewBehaviors.addHandler(
robotSystem.viewBehaviors.LEFT_DOUBLE_CLICK_EVENT,
robotLinkSelector.onLeftDoubleClick)
return FieldContainer(robotLinkSelector=robotLinkSelector)
def initViewBehaviors(self, robotSystem):
from director import applogic
from director import robotviewbehaviors
viewBehaviors = robotviewbehaviors.RobotViewBehaviors(robotSystem.view, robotSystem)
applogic.resetCamera(viewDirection=[-1,0,0], view=robotSystem.view)
return FieldContainer(viewBehaviors=viewBehaviors)
def initGlobals(self, fields):
try:
globalsDict = fields.globalsDict
except AttributeError:
globalsDict = dict()
if globalsDict is None:
globalsDict = dict()
return FieldContainer(globalsDict=globalsDict)
def initIRISDriver(self, robotSystem):
from director import irisdriver
irisDriver = irisdriver.IRISDriver(
robotSystem.robotStateJointController,
robotSystem.footstepsDriver.params)
return FieldContainer(irisDriver=irisDriver)
def _setupLocalServer(self):
initArgs = FieldContainer(
urdfFile=self.ikPlanner.robotModel.getProperty('Filename'),
packagePaths=roboturdf.getPackagePaths())
self.ikServer = PyDrakeIkServer()
self.ikServer.initInstance(initArgs)
def initViewOptions(self, fields):
viewOptions = vis.ViewOptionsItem(fields.view)
fields.objectModel.addToObjectModel(
viewOptions,
parentObj=fields.objectModel.findObjectByName('scene'))
viewOptions.setProperty('Background color', [0.3, 0.3, 0.35])
viewOptions.setProperty('Background color 2', [0.95, 0.95, 1])
return FieldContainer(viewOptions=viewOptions)
def getDefaultOptions(self):
options = dict()
for name in self.componentGraph.getComponentNames():
options['use' + name] = True
options = FieldContainer(**options)
self.initDefaultOptions(options)
return options
def initMeasurementPanel(self, fields):
from director import measurementpanel
measurementPanel = measurementpanel.MeasurementPanel(fields.app, fields.view)
measurementDock = fields.app.addWidgetToDock(measurementPanel.widget, QtCore.Qt.RightDockWidgetArea, visible=False)
return FieldContainer(
measurementPanel=measurementPanel,
measurementDock=measurementDock
)
def initScreenGrabberPanel(self, fields):
from director.screengrabberpanel import ScreenGrabberPanel
screenGrabberPanel = ScreenGrabberPanel(fields.view)
screenGrabberDock = fields.app.addWidgetToDock(screenGrabberPanel.widget, QtCore.Qt.RightDockWidgetArea, visible=False)
return FieldContainer(
screenGrabberPanel=screenGrabberPanel,
screenGrabberDock=screenGrabberDock
)
def initCameraControlPanel(self, fields):
from director import cameracontrolpanel
cameraControlPanel = cameracontrolpanel.CameraControlPanel(fields.view)
cameraControlDock = fields.app.addWidgetToDock(cameraControlPanel.widget, QtCore.Qt.RightDockWidgetArea, visible=False)
return FieldContainer(
cameraControlPanel=cameraControlPanel,
cameraControlDock=cameraControlDock
)
def initDrakeVisualizer(self, fields):
from director import drakevisualizer
drakeVisualizer = drakevisualizer.DrakeVisualizer(fields.view)
applogic.MenuActionToggleHelper('Tools', drakeVisualizer.name,
drakeVisualizer.isEnabled,
drakeVisualizer.setEnabled)
return FieldContainer(drakeVisualizer=drakeVisualizer)
def initAffordances(self, robotSystem):
from director import affordancemanager
from director import affordanceitems
affordanceManager = affordancemanager.AffordanceObjectModelManager(
robotSystem.view)
affordanceitems.MeshAffordanceItem.getMeshManager()
return FieldContainer(affordanceManager=affordanceManager, )
def initTreeViewer(self, fields):
from director import treeviewer
treeViewer = treeviewer.TreeViewer(fields.view)
applogic.MenuActionToggleHelper('Tools', treeViewer.name,
treeViewer.isEnabled,
treeViewer.setEnabled)
return FieldContainer(treeViewer=treeViewer)
def initOpenDataHandler(self, fields):
from director import opendatahandler
openDataHandler = opendatahandler.OpenDataHandler(fields.app)
def loadData():
for filename in drcargs.args().data_files:
openDataHandler.openGeometry(filename)
fields.app.registerStartupCallback(loadData)
return FieldContainer(openDataHandler=openDataHandler)
def initPerceptionDrivers(self, robotSystem):
from director import perception
perceptionSources = perception.init(
robotSystem.view, robotSystem.robotStateJointController)
# Expand dict to keyword args, robotSystem object will have objects accessible via keys set in config
return FieldContainer(sources=list(perceptionSources.values()),
**perceptionSources)
def construct(self, options=None, **kwargs):
options = options or self.getDefaultOptions()
if isinstance(options, dict):
options = self.setDependentOptions(self.getDefaultOptions(), **options)
self._verifyOptions(options)
componentGraph = self.componentGraph
for name in componentGraph.getComponentNames():
if 'use'+name not in options.__dict__:
raise Exception('Missing use option: ' + 'use'+name)
if 'init'+name not in dir(self):
raise Exception('Missing init function: ' + 'init'+name)
initOrder = toposort_flatten(componentGraph._graph)
componentFields = OrderedDict()
defaultFields = FieldContainer(options=options, **kwargs)
for name in initOrder:
initFunction = getattr(self, 'init'+name)
isEnabled = getattr(options, 'use'+name)
if isEnabled:
dependencies = componentGraph.getComponentDependencies(name)
inputFields = self._joinFields([defaultFields] + [componentFields[dep] for dep in dependencies])
newFields = initFunction(inputFields)
if not newFields:
newFields = FieldContainer()
componentFields[name] = newFields
# for componentName, componentFields in app.iteritems():
# print componentName, 'exports fields:'
# for name in componentFields._fields:
# print ' ', name
fields = self._joinFields([defaultFields] + componentFields.values())
return fields
def initCameraBookmarksPanel(self, fields):
from director import camerabookmarks
cameraBookmarksPanel = camerabookmarks.CameraBookmarkWidget(fields.view)
cameraBookmarksDock = fields.app.addWidgetToDock(cameraBookmarksPanel.widget, QtCore.Qt.RightDockWidgetArea, visible=False)
return FieldContainer(
cameraBookmarksPanel=cameraBookmarksPanel,
cameraBookmarksDock=cameraBookmarksDock
)
def initLCMGLRenderer(self, fields):
from director import lcmgl
if lcmgl.LCMGL_AVAILABLE:
lcmglManager = lcmgl.LCMGLManager(fields.view)
applogic.MenuActionToggleHelper('Tools', 'LCMGL Renderer',
lcmglManager.isEnabled,
lcmglManager.setEnabled)
else:
lcmglManager = None
return FieldContainer(lcmglManager=lcmglManager)
def __init__(self, *strs):
kwargs = {s:s for s in strs}
assert len(kwargs) == len(strs)
FieldContainer.__init__(self, **kwargs)
class ComponentFactory(object):
def __init__(self):
self.componentGraph = ComponentGraph()
self.initFunctions = {}
self.componentFields = {}
self.defaultOptions = FieldContainer()
def register(self, factoryClass):
fact = factoryClass()
components, disabledComponents = fact.getComponents()
for name in sorted(components.keys()):
if name in self.componentGraph.getComponentNames():
raise Exception('Component %s from %s has already been registered.' % (name, factoryClass.__name__))
if not hasattr(fact, 'init'+name):
raise Exception('Missing init function for component %s' % name)
for name in disabledComponents:
if name not in components.keys():
raise Exception('Unknown component %s found in list of disabled components.' % name)
options = dict()
for name, deps in components.iteritems():
self.componentGraph.addComponent(name, deps)
self.initFunctions[name] = getattr(fact, 'init'+name)
isEnabled = name not in disabledComponents
options['use'+name] = isEnabled
self.defaultOptions._add_fields(**options)
def getDisabledOptions(self, **kwargs):
options = self.getDefaultOptions()
for opt in options._fields:
options[opt] = False
self.setDependentOptions(options, **kwargs)
return options
def getDefaultOptions(self, **kwargs):
options = FieldContainer(**dict(self.defaultOptions))
self.setDependentOptions(options, **kwargs)
return options
def _toComponentName(self, optionName):
assert optionName[:3] == 'use'
return optionName[3:]
def _toOptionName(self, componentName):
return 'use' + componentName
def _joinFields(self, fieldsList):
f = FieldContainer()
for fields in fieldsList:
f._add_fields(**dict(fields))
return f
def setDependentOptions(self, options, **kwargs):
# verify the given args exist in the options fields
for name in kwargs.keys():
if name not in options._fields:
raise Exception('unknown option given: ' + name)
for name, enabled in kwargs.iteritems():
setattr(options, name, enabled)
# if option is being enabled, also enable its dependencies
if enabled:
name = self._toComponentName(name)
dependencies = self.componentGraph.getComponentDependencies(name)
for dep in dependencies:
setattr(options, self._toOptionName(dep), True)
return options
def _verifyOptions(self, options):
for name, enabled in options:
if enabled:
name = self._toComponentName(name)
dependencies = self.componentGraph.getComponentDependencies(name)
for dep in dependencies:
if not getattr(options, self._toOptionName(dep)):
raise Exception('Component %s depends on component %s, but %s is disabled.' % (name, dep, dep))
def construct(self, options=None, **kwargs):
options = options or self.getDefaultOptions()
if isinstance(options, dict):
options = self.setDependentOptions(self.getDefaultOptions(), **options)
self._verifyOptions(options)
defaultFields = FieldContainer(options=options, **kwargs)
initOrder = toposort_flatten(self.componentGraph.getComponentGraph())
for name in initOrder:
isEnabled = getattr(options, 'use'+name)
if isEnabled:
self.initComponent(name, defaultFields)
fields = self._joinFields([defaultFields] + self.componentFields.values())
return fields
def printComponentFields(self):
for k, v in sorted(self.componentFields.items()):
print '%s:' % k
for name in v._fields:
print ' ', name
def initComponent(self, name, defaultFields):
initFunction = self.initFunctions[name]
dependencies = self.componentGraph.getComponentDependencies(name)
inputFields = self._joinFields([defaultFields] + [self.componentFields[dep] for dep in dependencies])
newFields = initFunction(inputFields)
if not newFields:
newFields = FieldContainer()
self.componentFields[name] = newFields
def _joinFields(self, fieldsList):
f = FieldContainer()
for fields in fieldsList:
f._add_fields(**dict(fields))
return f
def __init__(self):
self.componentGraph = ComponentGraph()
self.initFunctions = {}
self.componentFields = {}
self.defaultOptions = FieldContainer()
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
