def __init__(self, jointController):
self.jointController = jointController
self.lastFootstepPlan = None
self.lastFootstepRequest = None
self.goalSteps = None
self.lastWalkingPlan = None
self.walkingPlanCallback = None
self.default_step_params = DEFAULT_STEP_PARAMS
self.contact_slices = DEFAULT_CONTACT_SLICES
self.show_contact_slices = False
self.toolbarWidget = None
### Stuff pertaining to rendering BDI-frame steps
self.pose_bdi = None
self.bdi_plan = None
self.bdi_plan_adjusted = None
view = app.getDRCView()
self.bdiRobotModel, self.bdiJointController = roboturdf.loadRobotModel('bdi model', view, parent='bdi model', color=roboturdf.getRobotOrangeColor(), visible=False)
self.bdiRobotModel.setProperty('Visible', False)
self.showBDIPlan = False # hide the BDI plans when created
self.bdiChannel = "POSE_BDI"
self.bdiSubcribe = None
#enable this to used the bdi model to render a different state
#self.bdiJointController.addLCMUpdater("EST_ROBOT_STATE_ALT")
self._setupSubscriptions()
self._setupProperties()
self.showToolbarWidget()
# If we're a consoleapp and have no main window execButton won't exist
if hasattr(self, 'execButton'):
self.execButton.setEnabled(False)
self.committedPlans = []
def __init__(self, jointController):
self.jointController = jointController
self.lastFootstepPlan = None
self.lastFootstepRequest = None
self.goalSteps = None
self.lastWalkingPlan = None
self.walkingPlanCallback = None
self.default_step_params = DEFAULT_STEP_PARAMS
self.contact_slices = DEFAULT_CONTACT_SLICES
self.show_contact_slices = False
self.toolbarWidget = None
### Stuff pertaining to rendering BDI-frame steps
self.pose_bdi = None
self.bdi_plan = None
self.bdi_plan_adjusted = None
view = app.getDRCView()
self.bdiRobotModel, self.bdiJointController = roboturdf.loadRobotModel('bdi model', view, parent='bdi model', color=roboturdf.getRobotOrangeColor(), visible=False)
self.bdiRobotModel.setProperty('Visible', False)
self.showBDIPlan = False # hide the BDI plans when created
self.bdiChannel = "POSE_BDI"
self.bdiSubcribe = None
#enable this to used the bdi model to render a different state
#self.bdiJointController.addLCMUpdater("EST_ROBOT_STATE_ALT")
self._setupSubscriptions()
self._setupProperties()
self.showToolbarWidget()
# If we're a consoleapp and have no main window execButton won't exist
if hasattr(self, 'execButton'):
self.execButton.setEnabled(False)
self.committedPlans = []
def __init__(self):
self.timer = TimerCallback(targetFps=10)
#self.timer.disableScheduledTimer()
self.app = ConsoleApp()
self.robotModel, self.jointController = roboturdf.loadRobotModel(
'robot model')
self.fpsCounter = simpletimer.FPSCounter()
self.drakePoseJointNames = robotstate.getDrakePoseJointNames()
self.fpsCounter.printToConsole = True
self.timer.callback = self._tick
self._initialized = False
self.publishChannel = 'JOINT_POSITION_GOAL'
# self.lastCommandMessage = newAtlasCommandMessageAtZero()
self._numPositions = len(robotstate.getDrakePoseJointNames())
self._previousElapsedTime = 100
self._baseFlag = 0
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.useControllerFlag = False
self.drivingGainsFlag = False
self.applyDefaults()
def __init__(self):
self.app = ConsoleApp()
self.view = self.app.createView()
self.robotModel, self.jointController = roboturdf.loadRobotModel('robot model', self.view)
self.jointController.setZeroPose()
self.view.show()
self.sub = lcmUtils.addSubscriber('ATLAS_COMMAND', lcmdrc.atlas_command_t, self.onAtlasCommand)
self.sub.setSpeedLimit(60)
def __init__(self):
self.app = ConsoleApp()
self.view = self.app.createView()
self.robotModel, self.jointController = roboturdf.loadRobotModel('robot model', self.view)
self.jointController.setZeroPose()
self.view.show()
self.sub = lcmUtils.addSubscriber('ATLAS_COMMAND', lcmdrc.atlas_command_t, self.onAtlasCommand)
self.sub.setSpeedLimit(60)
def __init__(self):
self.sub = lcmUtils.addSubscriber('JOINT_POSITION_GOAL', lcmdrc.robot_state_t, self.onJointPositionGoal)
self.sub = lcmUtils.addSubscriber('SINGLE_JOINT_POSITION_GOAL', lcmdrc.joint_position_goal_t, self.onSingleJointPositionGoal)
lcmUtils.addSubscriber('COMMITTED_PLAN_PAUSE', lcmdrc.plan_control_t, self.onPause)
self.debug = False
if self.debug:
self.app = ConsoleApp()
self.view = self.app.createView()
self.robotModel, self.jointController = roboturdf.loadRobotModel('robot model', self.view)
self.jointController.setPose('ATLAS_COMMAND', commandStream._currentCommandedPose)
self.view.show()
self.timer = TimerCallback(targetFps=30)
self.timer.callback = self.onDebug
def __init__(self):
self.sub = lcmUtils.addSubscriber('JOINT_POSITION_GOAL', lcmdrc.robot_state_t, self.onJointPositionGoal)
self.sub = lcmUtils.addSubscriber('SINGLE_JOINT_POSITION_GOAL', lcmdrc.joint_position_goal_t, self.onSingleJointPositionGoal)
lcmUtils.addSubscriber('COMMITTED_PLAN_PAUSE', lcmdrc.plan_control_t, self.onPause)
self.debug = False
if self.debug:
self.app = ConsoleApp()
self.view = self.app.createView()
self.robotModel, self.jointController = roboturdf.loadRobotModel('robot model', self.view)
self.jointController.setPose('ATLAS_COMMAND', commandStream._currentCommandedPose)
self.view.show()
self.timer = TimerCallback(targetFps=30)
self.timer.callback = self.onDebug
def __init__(self):
self.timer = TimerCallback(targetFps=10)
#self.timer.disableScheduledTimer()
self.app = ConsoleApp()
self.robotModel, self.jointController = roboturdf.loadRobotModel('robot model')
self.fpsCounter = simpletimer.FPSCounter()
self.drakePoseJointNames = robotstate.getDrakePoseJointNames()
self.fpsCounter.printToConsole = True
self.timer.callback = self._tick
self._initialized = False
self.publishChannel = 'JOINT_POSITION_GOAL'
# self.lastCommandMessage = newAtlasCommandMessageAtZero()
self._numPositions = len(robotstate.getDrakePoseJointNames())
self._previousElapsedTime = 100
self._baseFlag = 0;
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.useControllerFlag = False
self.drivingGainsFlag = False
self.applyDefaults()
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 = self.getDirectorConfig()
neckPitchJoint = 'neck_ay'
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)
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.getRobotBlueColor(), visible=False)
teleopRobotModel, teleopJointController = roboturdf.loadRobotModel('teleop model', view, urdfFile=directorConfig['urdfConfig']['teleop'], parent='planning', color=roboturdf.getRobotBlueColor(), visible=False)
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()
if (roboturdf.numberOfHands == 1):
handFactory = roboturdf.HandFactory(robotStateModel)
handModels = [handFactory.getLoader(side) for side in ['left']]
elif (roboturdf.numberOfHands == 2):
handFactory = roboturdf.HandFactory(robotStateModel)
handModels = [handFactory.getLoader(side) for side in ['left', 'right']]
else:
handFactory = None
handModels = []
ikPlanner = ikplanner.IKPlanner(ikServer, ikRobotModel, ikJointController, handModels)
manipPlanner = robotplanlistener.ManipulationPlanDriver(ikPlanner)
affordanceManager = affordancemanager.AffordanceObjectModelManager(view)
affordanceitems.MeshAffordanceItem.getMeshManager().collection.sendEchoRequest()
affordanceManager.collection.sendEchoRequest()
segmentation.affordanceManager = affordanceManager
plannerPub = plannerPublisher.PlannerPublisher(ikPlanner,affordanceManager)
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)
robotSystemArgs = dict(locals())
for arg in ['globalsDict', 'self']:
del robotSystemArgs[arg]
if globalsDict is not None:
globalsDict.update(robotSystemArgs)
robotSystem = FieldContainer(**robotSystemArgs)
viewbehaviors.ViewBehaviors.addRobotBehaviors(robotSystem)
return robotSystem
from ddapp import segmentation
from ddapp import segmentationroutines
from ddapp import applogic
from ddapp import visualization as vis
from ddapp import roboturdf
app = ConsoleApp()
# create a view
view = app.createView()
segmentation._defaultSegmentationView = view
robotStateModel, robotStateJointController = roboturdf.loadRobotModel(
'robot state model',
view,
parent='sensors',
color=roboturdf.getRobotGrayColor(),
visible=True)
segmentationroutines.SegmentationContext.initWithRobot(robotStateModel)
# load poly data
dataDir = app.getTestingDataDirectory()
polyData = ioUtils.readPolyData(
os.path.join(dataDir, 'valve/valve-sparse-stereo.pcd'))
vis.showPolyData(polyData,
'pointcloud snapshot original',
colorByName='rgb_colors')
polyData = segmentationroutines.sparsifyStereoCloud(polyData)
vis.showPolyData(polyData, 'pointcloud snapshot')
# fit valve and lever
from ddapp import segmentation
from ddapp import segmentationroutines
from ddapp import applogic
from ddapp import visualization as vis
from ddapp import roboturdf
app = ConsoleApp()
# create a view
view = app.createView()
segmentation._defaultSegmentationView = view
segmentation.initAffordanceManager(view)
robotStateModel, robotStateJointController = roboturdf.loadRobotModel('robot state model', view, parent='sensors', color=roboturdf.getRobotGrayColor(), visible=True)
segmentationroutines.SegmentationContext.initWithRobot(robotStateModel)
# load poly data
dataDir = app.getTestingDataDirectory()
polyData = ioUtils.readPolyData(os.path.join(dataDir, 'valve/valve-sparse-stereo.pcd'))
vis.showPolyData(polyData, 'pointcloud snapshot original', colorByName='rgb_colors')
polyData = segmentationroutines.sparsifyStereoCloud( polyData )
vis.showPolyData(polyData, 'pointcloud snapshot')
# fit valve and lever
segmentation.segmentValveWallAuto(.2, mode='valve', removeGroundMethod=segmentation.removeGroundSimple )
if app.getTestingInteractiveEnabled():
v = applogic.getCurrentView()
def onMousePress(self, displayPoint, modifiers=None):
print self.linkName, self.pickedPoint
###########################
app = ConsoleApp()
app.setupGlobals(globals())
view = app.createView()
view.show()
view.resize(1080, 768)
robotModel, jointController = roboturdf.loadRobotModel(
'robot model',
view,
parent='scene',
color=roboturdf.getRobotGrayColor(),
visible=True)
robotModel.addToView(view)
widget = LinkWidget(view, robotModel)
widget.start()
app.viewOptions.setProperty('Gradient background', False)
app.viewOptions.setProperty('Background color', [1, 1, 1])
app.viewOptions.setProperty('Orientation widget', False)
app.gridObj.setProperty('Color', [0, 0, 0])
app.gridObj.setProperty('Surface Mode', 'Surface with edges')
app.start()
self.linkName = linkName
self.pickedPoint = pickedPoint
def onMousePress(self, displayPoint, modifiers=None):
print self.linkName, self.pickedPoint
###########################
app = ConsoleApp()
app.setupGlobals(globals())
view = app.createView()
view.show()
view.resize(1080, 768)
robotModel, jointController = roboturdf.loadRobotModel('robot model', view, parent='scene', color=roboturdf.getRobotGrayColor(), visible=True)
robotModel.addToView(view)
widget = LinkWidget(view, robotModel)
widget.start()
app.viewOptions.setProperty('Gradient background', False)
app.viewOptions.setProperty('Background color', [1,1,1])
app.viewOptions.setProperty('Orientation widget', False)
app.gridObj.setProperty('Color', [0,0,0])
app.gridObj.setProperty('Surface Mode', 'Surface with edges')
app.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 = self.getDirectorConfig()
neckPitchJoint = 'neck_ay'
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)
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.getRobotBlueColor(),
visible=False)
teleopRobotModel, teleopJointController = roboturdf.loadRobotModel(
'teleop model',
view,
urdfFile=directorConfig['urdfConfig']['teleop'],
parent='planning',
color=roboturdf.getRobotBlueColor(),
visible=False)
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()
if (roboturdf.numberOfHands == 1):
handFactory = roboturdf.HandFactory(robotStateModel)
handModels = [handFactory.getLoader(side) for side in ['left']]
elif (roboturdf.numberOfHands == 2):
handFactory = roboturdf.HandFactory(robotStateModel)
handModels = [
handFactory.getLoader(side) for side in ['left', 'right']
]
else:
handFactory = None
handModels = []
ikPlanner = ikplanner.IKPlanner(ikServer, ikRobotModel,
ikJointController, handModels)
manipPlanner = robotplanlistener.ManipulationPlanDriver(ikPlanner)
affordanceManager = affordancemanager.AffordanceObjectModelManager(
view)
affordanceitems.MeshAffordanceItem.getMeshManager(
).collection.sendEchoRequest()
affordanceManager.collection.sendEchoRequest()
segmentation.affordanceManager = affordanceManager
plannerPub = plannerPublisher.PlannerPublisher(
ikPlanner, affordanceManager)
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)
robotSystemArgs = dict(locals())
for arg in ['globalsDict', 'self']:
del robotSystemArgs[arg]
if globalsDict is not None:
globalsDict.update(robotSystemArgs)
robotSystem = FieldContainer(**robotSystemArgs)
viewbehaviors.ViewBehaviors.addRobotBehaviors(robotSystem)
return robotSystem
