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 onRobotPlan(self, msg):
playback = planplayback.PlanPlayback()
playback.interpolationMethod = 'pchip'
poseTimes, poses = playback.getPlanPoses(msg)
f = playback.getPoseInterpolator(poseTimes, poses)
jointController = self.robotSystem.teleopJointController
timer = simpletimer.SimpleTimer()
def setPose(pose):
jointController.setPose('plan_playback', pose)
self.jointTeleopPanel.endPose = pose
self.jointTeleopPanel.updateSliders()
commandStream.setGoalPose(pose)
def updateAnimation():
tNow = timer.elapsed()
if tNow > poseTimes[-1]:
pose = poses[-1]
setPose(pose)
return False
pose = f(tNow)
setPose(pose)
self.animationTimer = TimerCallback()
self.animationTimer.targetFps = 60
self.animationTimer.callback = updateAnimation
self.animationTimer.start()
def onRobotPlan(self, msg):
playback = planplayback.PlanPlayback()
playback.interpolationMethod = 'pchip'
poseTimes, poses = playback.getPlanPoses(msg)
f = playback.getPoseInterpolator(poseTimes, poses)
print 'received robot plan, %.2f seconds' % (poseTimes[-1] -
poseTimes[0])
commandStream.applyPlanDefaults()
commandStream.startStreaming()
timer = simpletimer.SimpleTimer()
def setPose(pose):
commandStream.setGoalPose(pose)
def updateAnimation():
tNow = timer.elapsed()
if tNow > poseTimes[-1]:
pose = poses[-1]
setPose(pose)
commandStream.applyDefaults()
print 'plan ended.'
return False
pose = f(tNow)
setPose(pose)
self.animationTimer = TimerCallback()
self.animationTimer.targetFps = 1000
self.animationTimer.callback = updateAnimation
self.animationTimer.start()
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
# listen for robot state updates
robotStateJointController.addLCMUpdater('EST_ROBOT_STATE')
# reload urdf from model publisher lcm
roboturdf.startModelPublisherListener([(robotStateModel,
robotStateJointController)])
# load playback model
playbackRobotModel, playbackJointController = roboturdf.loadRobotModel(
'playback robot model',
view,
color=roboturdf.getRobotOrangeColor(),
visible=False)
# initialize the playback panel
planPlayback = planplayback.PlanPlayback()
manipPlanner = robotplanlistener.ManipulationPlanDriver(ikPlanner=None)
playbackPanel = playbackpanel.PlaybackPanel(planPlayback, playbackRobotModel,
playbackJointController,
robotStateModel,
robotStateJointController,
manipPlanner)
manipPlanner.connectPlanReceived(playbackPanel.setPlan)
# initialize pointcloud lcm
pointcloudlcm.init(view)
# initialize camera view
cameraChannel = 'MULTISENSE_CAMERA_LEFT'
imageManager = cameraview.ImageManager()
imageManager.queue.addCameraStream(cameraChannel)