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.sub = lcmUtils.addSubscriber('COMMITTED_ROBOT_PLAN',
lcmdrc.robot_plan_t,
self.onRobotPlan)
lcmUtils.addSubscriber('COMMITTED_PLAN_PAUSE', lcmdrc.plan_control_t,
self.onPause)
self.animationTimer = None
def initDrakeTimeDisplay():
def onViewerDraw(msg):
t = msg.timestamp * 1e-3
vis.updateText('sim time: %.3f' % t, 'sim time')
lcmUtils.addSubscriber('DRAKE_VIEWER_DRAW', lcmbotcore.viewer_draw_t,
onViewerDraw)
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 initSubscriber(self):
self.subscriber = lcmUtils.addSubscriber(
self.channel, optitrack_frame_t, self.onMessage)
self.subscriber.setSpeedLimit(10)
self.desc_subscriber = lcmUtils.addSubscriber(
self.desc_channel, optitrack_data_descriptions_t,
self.onDescMessage)
def setEnabled(self, enabled):
if enabled and not self.subscriber:
#self.subscriber = lcmUtils.addSubscriber('LCMGL.*', callback=self.onMessage)
self.subscriber = lcmUtils.addSubscriber('OCTOMAP', callback=self.onMessage)
self.subscriber = lcmUtils.addSubscriber('OCTOMAP_REF', callback=self.onMessage)
self.subscriber = lcmUtils.addSubscriber('OCTOMAP_IN', callback=self.onMessage)
elif not enabled and self.subscriber:
lcmUtils.removeSubscriber(self.subscriber)
self.subscriber = None
def setEnabled(self, enabled):
if enabled and not self.subscriber:
#self.subscriber = lcmUtils.addSubscriber('LCMGL.*', callback=self.onMessage)
self.subscriber = lcmUtils.addSubscriber('OCTOMAP', callback=self.onMessage)
self.subscriber = lcmUtils.addSubscriber('OCTOMAP_REF', callback=self.onMessage)
self.subscriber = lcmUtils.addSubscriber('OCTOMAP_IN', callback=self.onMessage)
elif not enabled and self.subscriber:
lcmUtils.removeSubscriber(self.subscriber)
self.subscriber = None
def start(self):
sub = lcmUtils.addSubscriber('PLAN_EXECUTION_STATUS',
lcmdrc.plan_status_t, self.handleStatus)
sub.setSpeedLimit(0.2)
self.subs.append(sub)
self.subs.append(
lcmUtils.addSubscriber('FOOTSTEP_PLAN_RESPONSE',
lcmdrc.footstep_plan_t,
self.handleFootstepPlan))
def _setupSubscriptions(self):
lcmUtils.addSubscriber('PLAN_EXECUTION_STATUS', lcmdrc.plan_status_t, self.onControllerStatus)
lcmUtils.addSubscriber('ATLAS_BATTERY_DATA', atlas.battery_data_t, self.onAtlasBatteryData)
lcmUtils.addSubscriber('ATLAS_ELECTRIC_ARM_STATUS', atlas.electric_arm_status_t, self.onAtlasElectricArmStatus)
lcmUtils.addSubscriber('CONTROLLER_RATE', lcmdrc.message_rate_t, self.onControllerRate)
sub = lcmUtils.addSubscriber('ATLAS_STATUS', atlas.status_t, self.onAtlasStatus)
sub.setSpeedLimit(60)
sub = lcmUtils.addSubscriber('ATLAS_STATE_EXTRA', atlas.state_extra_t, self.onAtlasStateExtra)
sub.setSpeedLimit(5)
def _addSubscribers(self):
self.subscribers.append(
lcmUtils.addSubscriber('DRAKE_VIEWER_LOAD_ROBOT',
lcmdrake.lcmt_viewer_load_robot,
self.onViewerLoadRobot))
self.subscribers.append(
lcmUtils.addSubscriber('DRAKE_VIEWER_DRAW',
lcmdrake.lcmt_viewer_draw,
self.onViewerDraw))
self.subscribers.append(
lcmUtils.addSubscriber('DRAKE_PLANAR_LIDAR_.*',
lcmbot.planar_lidar_t,
self.onPlanarLidar,
callbackNeedsChannel=True))
def _addSubscribers(self):
self.subscribers.append(
lcmUtils.addSubscriber("DRAKE_VIEWER_LOAD_ROBOT", lcmrl.viewer_load_robot_t, self.onViewerLoadRobot)
)
self.subscribers.append(lcmUtils.addSubscriber("DRAKE_VIEWER_DRAW", lcmrl.viewer_draw_t, self.onViewerDraw))
self.subscribers.append(
lcmUtils.addSubscriber(
"DRAKE_PLANAR_LIDAR_.*", lcmbot.planar_lidar_t, self.onPlanarLidar, callbackNeedsChannel=True
)
)
self.subscribers.append(
lcmUtils.addSubscriber(
"DRAKE_POINTCLOUD_.*", lcmbot.pointcloud_t, self.onPointCloud, callbackNeedsChannel=True
)
)
def __init__(self):
self.sub = lcmUtils.addSubscriber('JOINT_POSITION_GOAL', lcmbotcore.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', lcmbotcore.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 _setupSubscriptions(self):
lcmUtils.addSubscriber('PLAN_EXECUTION_STATUS', lcmdrc.plan_status_t,
self.onControllerStatus)
lcmUtils.addSubscriber('ATLAS_BATTERY_DATA', atlas.battery_data_t,
self.onAtlasBatteryData)
lcmUtils.addSubscriber('ATLAS_ELECTRIC_ARM_STATUS',
atlas.electric_arm_status_t,
self.onAtlasElectricArmStatus)
lcmUtils.addSubscriber('CONTROLLER_RATE', lcmdrc.message_rate_t,
self.onControllerRate)
sub = lcmUtils.addSubscriber('ATLAS_STATUS', atlas.status_t,
self.onAtlasStatus)
sub.setSpeedLimit(60)
sub = lcmUtils.addSubscriber('ATLAS_STATE_EXTRA', atlas.state_extra_t,
self.onAtlasStateExtra)
sub.setSpeedLimit(5)
def addLCMUpdater(self, channelName):
'''
adds an lcm subscriber to update the joint positions from
lcm robot_state_t messages
'''
def onRobotStateMessage(msg):
if self.ignoreOldStateMessages and self.lastRobotStateMessage is not None and msg.utime < self.lastRobotStateMessage.utime:
return
poseName = channelName
pose = robotstate.convertStateMessageToDrakePose(msg)
self.lastRobotStateMessage = msg
# use joint name/positions from robot_state_t and append base_{x,y,z,roll,pitch,yaw}
jointPositions = np.hstack((msg.joint_position, pose[:6]))
jointNames = msg.joint_name + robotstate.getDrakePoseJointNames(
)[:6]
self.setPose(poseName, pose, pushToModel=False)
for model in self.models:
model.model.setJointPositions(jointPositions, jointNames)
self.subscriber = lcmUtils.addSubscriber(channelName,
bot_core.robot_state_t,
onRobotStateMessage)
self.subscriber.setSpeedLimit(60)
def __init__(self, ikPlanner):
lcmUtils.addSubscriber('CANDIDATE_MANIP_PLAN', lcmdrc.robot_plan_w_keyframes_t, self.onManipPlan)
lcmUtils.addSubscriber('CANDIDATE_ROBOT_PLAN_WITH_SUPPORTS',lcmdrc.robot_plan_with_supports_t, self.onManipPlan)
self.lastManipPlan = None
self.committedPlans = []
self.callbacks = callbacks.CallbackRegistry([self.PLAN_RECEIVED,
self.PLAN_COMMITTED,
self.USE_SUPPORTS])
self.ikPlanner = ikPlanner
self.publishPlansWithSupports = False
self.plansWithSupportsAreQuasistatic = True
self.leftFootSupportEnabled = True
self.rightFootSupportEnabled = True
self.leftHandSupportEnabled = False
self.rightHandSupportEnabled = False
self.pelvisSupportEnabled = False
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 changeSubscriptionAlt(self, newAltChannel="POSE_BODY_ALT"):
# used to monitor a different pose e.g. POSE_BODY_LOGGED in playback
self.altChannel = newAltChannel
lcmUtils.removeSubscriber ( self.altSubscribe )
self.altSubscribe = lcmUtils.addSubscriber( self.altChannel , pose_t, self.onPoseAlt)
self.altSubscribe.setSpeedLimit(60)
def changeSubscriptionBDI(self, newBDIChannel="POSE_BDI"):
# used to monitor a different pose e.g. POSE_BODY_LOGGED in playback
self.bdiChannel = newBDIChannel
lcmUtils.removeSubscriber ( self.bdiSubcribe )
self.bdiSubcribe = lcmUtils.addSubscriber( self.bdiChannel , pose_t, self.onPoseBDI)
self.bdiSubcribe.setSpeedLimit(60)
def add_subscriber(self):
if (self._subscriber is not None):
return
self._subscriber = lcmUtils.addSubscriber(
'DRAKE_VIEWER_DRAW',
messageClass=lcmbotcore.viewer_draw_t,
callback=self.handle_message)
def __init__(self, jointController,
footstepParams,
request_channel='IRIS_REGION_REQUEST',
response_channel='IRIS_REGION_RESPONSE'):
self.jointController = jointController
self.params = footstepParams
self.map_mode_map = [
lcmdrc.footstep_plan_params_t.FOOT_PLANE,
lcmdrc.footstep_plan_params_t.TERRAIN_HEIGHTS_AND_NORMALS,
lcmdrc.footstep_plan_params_t.TERRAIN_HEIGHTS_Z_NORMALS,
lcmdrc.footstep_plan_params_t.HORIZONTAL_PLANE
]
self.request_channel = request_channel
self.response_channel = response_channel
self.regions = {}
self.sub = lcmUtils.addSubscriber(self.response_channel, lcmdrc.iris_region_response_t, self.onIRISRegionResponse)
self.segmentation_sub = lcmUtils.addSubscriber('IRIS_SEGMENTATION_RESPONSE', lcmdrc.iris_region_response_t, self.onIRISRegionResponse)
def __init__(self, channel=DEFAULT_CHANNEL, frame_names=[]):
self._channel = channel
self._frame_names = frame_names
self._handlers = {}
for frame_name in self._frame_names:
self._handlers[frame_name] = LcmImageHandler()
self._subscriber = lcmUtils.addSubscriber(channel, rl.image_array_t,
self._on_message)
def add_subscriber(self):
if (self._subscriber is not None):
return
self._subscriber = lcmUtils.addSubscriber(
'CASSIE_STATE',
messageClass=dairlib.lcmt_robot_output,
callback=self.handle_message)
def add_subscriber(self):
if (self._subscriber is not None):
return
self._subscriber = lcmUtils.addSubscriber(
'DRAKE_VIEWER_DRAW',
messageClass=lcmbotcore.viewer_draw_t,
callback=self.handle_message)
def _add_subscriber(self):
if (self._subscriber is not None):
return
self._subscriber = lcmUtils.addSubscriber(
'DRAKE_DRAW_FRAMES',
messageClass = lcmrobotlocomotion.viewer_draw_t,
callback = self._handle_message)
def __init__(self, jointController,
footstepParams,
request_channel='IRIS_REGION_REQUEST',
response_channel='IRIS_REGION_RESPONSE'):
self.jointController = jointController
self.params = footstepParams
self.map_mode_map = [
lcmdrc.footstep_plan_params_t.FOOT_PLANE,
lcmdrc.footstep_plan_params_t.TERRAIN_HEIGHTS_AND_NORMALS,
lcmdrc.footstep_plan_params_t.TERRAIN_HEIGHTS_Z_NORMALS,
lcmdrc.footstep_plan_params_t.HORIZONTAL_PLANE
]
self.request_channel = request_channel
self.response_channel = response_channel
self.regions = {}
self.sub = lcmUtils.addSubscriber(self.response_channel, lcmdrc.iris_region_response_t, self.onIRISRegionResponse)
self.segmentation_sub = lcmUtils.addSubscriber('IRIS_SEGMENTATION_RESPONSE', lcmdrc.iris_region_response_t, self.onIRISRegionResponse)
def __init__(self):
self.widget = QtGui.QLabel('')
self.widget.setFixedWidth(60)
self.widget.setToolTip('Simulation time')
self.widget.setAlignment(QtCore.Qt.AlignRight)
self.setTime(0)
self.subscriber = lcmUtils.addSubscriber(timeChannel,
callback=self.onMessage)
def _add_subscriber(self):
if (self._subscriber is not None):
return
self._subscriber = lcmUtils.addSubscriber(
'DRAKE_DRAW_FRAMES',
messageClass=lcmrobotlocomotion.viewer_draw_t,
callback=self._handle_message)
def __init__(self, channel=DEFAULT_CHANNEL, frame_names=[]):
self._channel = channel
self._frame_names = frame_names
self._handlers = {}
for frame_name in self._frame_names:
self._handlers[frame_name] = LcmImageHandler()
self._subscriber = lcmUtils.addSubscriber(
channel, rl.image_array_t, self._on_message)
def __init__(self, statusBar):
self.label = QtGui.QLabel('')
statusBar.addPermanentWidget(self.label)
self.sub = lcmUtils.addSubscriber(
'IIWA_STATUS', lcmdrake.lcmt_iiwa_status, self.onIiwaStatus)
self.sub.setSpeedLimit(30)
self.label.text = '[waiting for sim status]'
def _add_subscriber(self):
if (self._subscriber is not None):
return
self._subscriber = lcmUtils.addSubscriber(
'DRAKE_POINT_CLOUD_.*',
messageClass=lcmt_point_cloud,
callback=self._handle_message,
callbackNeedsChannel=True)
self._subscriber.setNotifyAllMessagesEnabled(True)
def add_subscriber(self):
if self._sub is not None:
return
self._sub = lcmUtils.addSubscriber(
'MARKER_ARROWS',
messageClass=lcmdrakemsg.lcmt_arbitrary_arrow_collection,
callback=self.handle_message)
print self._name + " subscriber added."
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', lcmbotcore.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', lcmbotcore.atlas_command_t, self.onAtlasCommand)
self.sub.setSpeedLimit(60)
def _addSubscribers(self):
self.subscribers.append(
lcmUtils.addSubscriber('DRAKE_VIEWER_LOAD_ROBOT',
lcmrl.viewer_load_robot_t,
self.onViewerLoadRobot))
self.subscribers.append(
lcmUtils.addSubscriber('DRAKE_VIEWER_DRAW', lcmrl.viewer_draw_t,
self.onViewerDraw))
self.subscribers.append(
lcmUtils.addSubscriber('DRAKE_PLANAR_LIDAR_.*',
lcmbot.planar_lidar_t,
self.onPlanarLidar,
callbackNeedsChannel=True))
self.subscribers.append(
lcmUtils.addSubscriber('DRAKE_POINTCLOUD_.*',
lcmbot.pointcloud_t,
self.onPointCloud,
callbackNeedsChannel=True))
def add_subscriber(self):
if self._sub is not None:
return
self._sub = lcmUtils.addSubscriber(
'CONTACT_RESULTS',
messageClass=lcmdrakemsg.lcmt_contact_results_for_viz,
callback=self.handle_message)
print self._name + " subscriber added."
def add_subscriber(self):
if self._sub is not None:
return
self._sub = lcmUtils.addSubscriber(
'CONTACT_RESULTS',
messageClass=lcmdrakemsg.lcmt_contact_results_for_viz,
callback=self.handle_message)
print(self._name + ' subscriber added.')
def _add_subscriber(self):
if (self._subscriber is not None):
return
self._subscriber = lcmUtils.addSubscriber(
'DRAKE_DRAW_FRAMES.*',
messageClass=lcmrobotlocomotion.viewer_draw_t,
callback=self._handle_message,
callbackNeedsChannel=True)
self._subscriber.setNotifyAllMessagesEnabled(True)
def add_subscriber(self):
if (self._subscriber is not None):
return
channel = "INPUT_SUPERVISOR_STATUS"
self._subscriber = lcmUtils.addSubscriber(
channel,
messageClass=dairlib.lcmt_input_supervisor_status,
callback=self.handle_message)
def __init__(self, statusBar):
self.label = QtGui.QLabel('')
statusBar.addPermanentWidget(self.label)
self.sub = lcmUtils.addSubscriber('IIWA_STATUS',
lcmdrake.lcmt_iiwa_status,
self.onIiwaStatus)
self.sub.setSpeedLimit(30)
self.label.text = '[waiting for sim status]'
def setEnabled(self, enabled):
if enabled and not self.subscriber0:
self.subscriber0 = lcmUtils.addSubscriber('OBJECT_COLLECTION', callback=self.on_obj_collection_data)
self.subscriber1 = lcmUtils.addSubscriber('LINK_COLLECTION', callback=self.on_link_collection_data)
self.subscriber2 = lcmUtils.addSubscriber('POINTS_COLLECTION', callback=self.on_points_collection_data)
self.subscriber3 = lcmUtils.addSubscriber('RESET_COLLECTIONS', callback=self.on_reset_collections_data)
self.subscriber0.setNotifyAllMessagesEnabled(True)
self.subscriber1.setNotifyAllMessagesEnabled(True)
self.subscriber2.setNotifyAllMessagesEnabled(True)
self.subscriber3.setNotifyAllMessagesEnabled(True)
elif not enabled and self.subscriber0:
lcmUtils.removeSubscriber(self.subscriber0)
lcmUtils.removeSubscriber(self.subscriber1)
lcmUtils.removeSubscriber(self.subscriber2)
lcmUtils.removeSubscriber(self.subscriber3)
self.subscriber0 = None
self.subscriber1 = None
self.subscriber2 = None
self.subscriber3 = None
def __init__(self, ikPlanner):
lcmUtils.addSubscriber('CANDIDATE_MANIP_PLAN',
lcmdrc.robot_plan_w_keyframes_t,
self.onManipPlan)
lcmUtils.addSubscriber('CANDIDATE_ROBOT_PLAN_WITH_SUPPORTS',
lcmdrc.robot_plan_with_supports_t,
self.onManipPlan)
self.lastManipPlan = None
self.committedPlans = []
self.callbacks = callbacks.CallbackRegistry(
[self.PLAN_RECEIVED, self.PLAN_COMMITTED, self.USE_SUPPORTS])
self.ikPlanner = ikPlanner
self.publishPlansWithSupports = False
self.plansWithSupportsAreQuasistatic = True
self.leftFootSupportEnabled = True
self.rightFootSupportEnabled = True
self.leftHandSupportEnabled = False
self.rightHandSupportEnabled = False
self.pelvisSupportEnabled = False
def _add_subscriber(self):
if (self._subscriber is not None):
return
self._subscriber = lcmUtils.addSubscriber(
'DRAKE_DRAW_FRAMES.*',
messageClass=lcmrobotlocomotion.viewer_draw_t,
callback=self._handle_message,
callbackNeedsChannel=True)
self._subscriber.setNotifyAllMessagesEnabled(True)
def add_subscriber(self):
if self._sub is not None:
return
self._sub = lcmUtils.addSubscriber(
'CONTACT_RESULTS',
messageClass=lcmdrakemsg.lcmt_contact_results_for_viz,
callback=self.handle_message)
# Limits the rate of message handling, since redrawing is done in the
# message handler.
self._sub.setSpeedLimit(30)
print self._name + " subscriber added."
def __init__(self, channel, statusBar=None):
self.sub = lcmUtils.addSubscriber(channel, lcmbotcore.robot_state_t, self.onRobotState)
self.label = QtGui.QLabel('')
statusBar.addPermanentWidget(self.label)
self.timer = TimerCallback(targetFps=10)
self.timer.callback = self.showRate
self.timer.start()
self.l_foot_force_z = 0
self.r_foot_force_z = 0
def add_subscriber(self):
if self._sub is not None:
return
self._sub = lcmUtils.addSubscriber(
'CONTACT_RESULTS',
messageClass=lcmdrakemsg.lcmt_contact_results_for_viz,
callback=self.handle_message)
# Limits the rate of message handling, since redrawing is done in the
# message handler.
self._sub.setSpeedLimit(30)
print self._name + " subscriber added."
def __init__(self, channel, statusBar=None):
self.sub = lcmUtils.addSubscriber(channel, lcmbotcore.robot_state_t, self.onRobotState)
self.label = QtGui.QLabel('')
statusBar.addPermanentWidget(self.label)
self.timer = TimerCallback(targetFps=10)
self.timer.callback = self.showRate
self.timer.start()
self.l_foot_force_z = 0
self.r_foot_force_z = 0
def _setupSubscriptions(self):
useHistoricalLoader = False
historicalLoader = lcmUtils.HistoricalLCMLoader('drc', 'software/drc_lcmtypes/lcmtypes', os.getenv('DRC_BASE')) if useHistoricalLoader else None
lcmUtils.addSubscriber('FOOTSTEP_PLAN_RESPONSE', lcmdrc.footstep_plan_t, self.onFootstepPlan, historicalLoader)
lcmUtils.addSubscriber('WALKING_TRAJ_RESPONSE', lcmdrc.robot_plan_t, self.onWalkingPlan)
lcmUtils.addSubscriber('WALKING_SIMULATION_TRAJ_RESPONSE', lcmdrc.robot_plan_t, self.onWalkingPlan)
### Related to BDI-frame adjustment:
self.bdiSubcribe = lcmUtils.addSubscriber( self.bdiChannel , pose_t, self.onPoseBDI)
self.bdiSubcribe.setSpeedLimit(60)
sub2 = lcmUtils.addSubscriber('BDI_ADJUSTED_FOOTSTEP_PLAN', lcmdrc.footstep_plan_t, self.onBDIAdjustedFootstepPlan)
sub2.setSpeedLimit(1) # was 5 but was slow rendering
def add_subscriber(self):
if self._subs is not None:
return
self._subs = []
self._subs.append(
lcmUtils.addSubscriber(
"DEFORMABLE_MESHES_INIT",
messageClass=lcmdrakemsg.
experimental_lcmt_deformable_tri_meshes_init,
callback=self.handle_init_message,
))
self._subs.append(
lcmUtils.addSubscriber(
"DEFORMABLE_MESHES_UPDATE",
messageClass=lcmdrakemsg.
experimental_lcmt_deformable_tri_meshes_update,
callback=self.handle_update_message,
))
print(self._name + " subscribers added.")
def __init__(self, channel):
self.collection = OrderedDict()
self.collectionId = newUUID()
self.sentCommands = set()
self.sentRequest = None
self.channel = channel
self.callbacks = callbacks.CallbackRegistry([self.DESCRIPTION_UPDATED_SIGNAL,
self.DESCRIPTION_REMOVED_SIGNAL])
self.sub = lcmUtils.addSubscriber(self.channel, messageClass=lcmdrc.affordance_collection_t, callback=self._onCommandMessage)
self.sub.setNotifyAllMessagesEnabled(True)
self._modified()
def __init__(self, teleopPanel, teleopJointController, ikPlanner, view):
lcmUtils.addSubscriber("GAMEPAD_CHANNEL", lcmdrc.gamepad_cmd_t, self.onGamepadCommand)
self.speedMultiplier = 1.0
self.maxSpeedMultiplier = 3.0
self.minSpeedMultiplier = 0.2
self.travel = 1
self.angularTravel = 180
self.baseFrame = None
self.resetDeltas()
self.timer = TimerCallback()
self.timer.callback = self.tick
self.teleopPanel = teleopPanel
self.teleopJointController = teleopJointController
self.ikPlanner = ikPlanner
self.view = view
self.camera = view.camera()
self.cameraTarget = np.asarray(self.camera.GetFocalPoint())
self.endEffectorFrames = []
self.endEffectorIndex = 0
self.teleopOn = False
self.keyFramePoses = list()
self.cameraMode = False
self.timerStarted = False
def _addSubscriber(self):
self.subscriber = lcmUtils.addSubscriber(
'DIRECTOR_TREE_VIEWER_REQUEST.*',
lcmrl.viewer2_comms_t,
self.onViewerRequest,
callbackNeedsChannel=True)
# Note: from discussion with @patmarion, there's a bug in the lcmUtils subscriber
# when dealing with regex channels:
# > If you subscribe to MY_CHANNEL_*, and two messages arrive back to back
# > (MY_CHANNEL_FOO, foo_data) and (MY_CHANNEL_BAR, bar_data), then the default
# > behavior in the lcm subscriber is to only call your callback once (if notify
# > all messages = false), and it calls callback(MY_CHANNEL_FOO, bar_data)
#
# However, this can be avoided by notifying for *all* messages, which is what we
# want anyway:
self.subscriber.setNotifyAllMessagesEnabled(True)
def __init__(self, name, topic, sensorLocations, model, windowSize = 10):
self.name = name
self.sensorLocations = sensorLocations
self.frameNames = self.getFrameNames()
self.robotStateModel = model
sub = lcmUtils.addSubscriber(topic, takktile.state_t, self.drawSpheres)
sub.setSpeedLimit(60)
self.active = False
self.doTare = True
self.taredSensorValues = {}
self.tareCount = 0
self.tareWindow = windowSize
self.frames = {}
def __init__(self, model, jointController, view):
self.model = model
self.jointController = jointController
self.view = view
self.colorNoHighlight = QtGui.QColor(190, 190, 190)
self.colorHighlight = QtCore.Qt.red
self.timer = TimerCallback()
self.timer.callback = self.update
self.timer.targetFps = 60
#self.timer.start()
self.collisionStateIds = []
self.lastRobotStateMessage = None
self.lastPlanMessage = None
self.lastPlanCheckMessage = None
lcmUtils.addSubscriber('EST_ROBOT_STATE', lcmdrc.robot_state_t, self.onRobotState)
lcmUtils.addSubscriber('ROBOT_COLLISIONS', lcmdrc.robot_collision_array_t, self.onPlanCheck)
lcmUtils.addSubscriber('CANDIDATE_MANIP_PLAN', lcmdrc.robot_plan_w_keyframes_t, self.onManipPlan)
w = QtGui.QWidget()
l = QtGui.QHBoxLayout(w)
self.slider = QtGui.QSlider(QtCore.Qt.Horizontal)
self.clearButton = QtGui.QPushButton('clear')
self.zeroButton = QtGui.QPushButton('zero')
l.addWidget(self.clearButton)
l.addWidget(self.zeroButton)
l.addWidget(self.slider)
self.slider.connect(self.slider, 'valueChanged(int)', self.onSlider)
self.slider.connect(self.zeroButton, 'clicked()', self.onZeroButtonClicked)
self.slider.connect(self.clearButton, 'clicked()', self.onClearButtonClicked)
ww = QtGui.QWidget()
ll = QtGui.QVBoxLayout(ww)
ll.addWidget(self.view)
ll.addWidget(w)
ll.setMargin(0)
ww.show()
ww.resize(800, 600)
self.widget = ww
def addLCMUpdater(self, channelName):
'''
adds an lcm subscriber to update the joint positions from
lcm robot_state_t messages
'''
def onRobotStateMessage(msg):
if self.ignoreOldStateMessages and self.lastRobotStateMessage is not None and msg.utime < self.lastRobotStateMessage.utime:
return
poseName = channelName
pose = robotstate.convertStateMessageToDrakePose(msg)
self.lastRobotStateMessage = msg
# use joint name/positions from robot_state_t and append base_{x,y,z,roll,pitch,yaw}
jointPositions = np.hstack((msg.joint_position, pose[:6]))
jointNames = msg.joint_name + robotstate.getDrakePoseJointNames()[:6]
self.setPose(poseName, pose, pushToModel=False)
for model in self.models:
model.model.setJointPositions(jointPositions, jointNames)
self.subscriber = lcmUtils.addSubscriber(channelName, lcmdrc.robot_state_t, onRobotStateMessage)
self.subscriber.setSpeedLimit(60)
def __init__(self):
self.aprilTagSubsciber = lcmUtils.addSubscriber('APRIL_TAG_TO_CAMERA_LEFT', lcmbotcore.rigid_transform_t, self.onAprilTag)
pose = transformUtils.poseFromTransform(vtk.vtkTransform())
desc = dict(classname='MeshAffordanceItem', Name='polaris',
Filename='software/models/polaris/polaris_cropped.vtp', pose=pose)
self.pointcloudAffordance = segmentation.affordanceManager.newAffordanceFromDescription(desc)
self.originFrame = self.pointcloudAffordance.getChildFrame()
self.originToAprilTransform = transformUtils.transformFromPose(np.array([-0.038508 , -0.00282131, -0.01000079]),
np.array([ 9.99997498e-01, -2.10472556e-03, -1.33815696e-04, 7.46246794e-04])) # offset for . . . who knows why
# t = transformUtils.transformFromPose(np.array([ 0.14376024, 0.95920689, 0.36655712]), np.array([ 0.28745842, 0.90741428, -0.28822068, 0.10438304]))
t = transformUtils.transformFromPose(np.array([ 0.10873244, 0.93162364, 0.40509084]),
np.array([ 0.32997378, 0.88498408, -0.31780588, 0.08318602]))
self.leftFootEgressStartFrame = vis.updateFrame(t, 'left foot start', scale=0.2,visible=True, parent=self.pointcloudAffordance)
t = transformUtils.transformFromPose(np.array([ 0.265, 0.874, 0.274]),
np.array([ 0.35290731, 0.93443693, -0.04181263, 0.02314636]))
self.leftFootEgressMidFrame = vis.updateFrame(t, 'left foot mid', scale=0.2,visible=True, parent=self.pointcloudAffordance)
t = transformUtils.transformFromPose(np.array([ 0.54339115, 0.89436275, 0.26681047]),
np.array([ 0.34635985, 0.93680077, -0.04152008, 0.02674412]))
self.leftFootEgressOutsideFrame = vis.updateFrame(t, 'left foot outside', scale=0.2,visible=True, parent=self.pointcloudAffordance)
# pose = [np.array([-0.78962299, 0.44284877, -0.29539116]), np.array([ 0.54812954, 0.44571517, -0.46063251, 0.53731713])] #old location
# pose = [np.array([-0.78594663, 0.42026626, -0.23248139]), np.array([ 0.54812954, 0.44571517, -0.46063251, 0.53731713])] # updated location
pose = [np.array([-0.78594663, 0.42026626, -0.23248139]), np.array([ 0.53047159, 0.46554963, -0.48086192, 0.52022615])] # update orientation
desc = dict(classname='CapsuleRingAffordanceItem', Name='Steering Wheel', uuid=newUUID(), pose=pose,
Color=[1, 0, 0], Radius=float(0.18), Segments=20)
self.steeringWheelAffordance = segmentation.affordanceManager.newAffordanceFromDescription(desc)
pose = [np.array([-0.05907324, 0.80460545, 0.45439687]), np.array([ 0.14288327, 0.685944 , -0.703969 , 0.11615873])]
desc = dict(classname='BoxAffordanceItem', Name='pedal', Dimensions=[0.12, 0.33, 0.04], pose=pose, Color=[0,1,0])
self.pedalAffordance = segmentation.affordanceManager.newAffordanceFromDescription(desc)
# t = transformUtils.transformFromPose(np.array([ 0.04045136, 0.96565326, 0.25810111]),
# np.array([ 0.26484648, 0.88360091, -0.37065556, -0.10825996]))
# t = transformUtils.transformFromPose(np.array([ -4.34908919e-04, 9.24901627e-01, 2.65614116e-01]),
# np.array([ 0.25022251, 0.913271 , -0.32136359, -0.00708626]))
t = transformUtils.transformFromPose(np.array([ 0.0384547 , 0.89273742, 0.24140762]),
np.array([ 0.26331831, 0.915796 , -0.28055337, 0.11519963]))
self.leftFootPedalSwingFrame = vis.updateFrame(t,'left foot pedal swing', scale=0.2, visible=True, parent=self.pointcloudAffordance)
t = transformUtils.transformFromPose(np.array([-0.9012598 , -0.05709763, 0.34897024]),
np.array([ 0.03879584, 0.98950919, 0.03820214, 0.13381721]))
self.leftFootDrivingFrame = vis.updateFrame(t,'left foot driving', scale=0.2, visible=True, parent=self.pointcloudAffordance)
# t = transformUtils.transformFromPose(np.array([-0.12702725, 0.92068409, 0.27209386]),
# np.array([ 0.2062255 , 0.92155886, -0.30781119, 0.11598529]))
# t = transformUtils.transformFromPose(np.array([-0.14940375, 0.90347275, 0.23812658]),
# np.array([ 0.27150909, 0.91398724, -0.28877386, 0.0867167 ]))
# t = transformUtils.transformFromPose(np.array([-0.17331227, 0.87879312, 0.25645152]),
# np.array([ 0.26344489, 0.91567196, -0.28089824, 0.11505581]))
# self.leftFootDrivingKneeInFrame = vis.updateFrame(t,'left foot driving knee in', scale=0.2, visible=True, parent=self.pointcloudAffordance)
t = transformUtils.transformFromPose(np.array([-0.12702725, 0.92068409, 0.27209386]),
np.array([ 0.2062255 , 0.92155886, -0.30781119, 0.11598529]))
self.leftFootDrivingKneeInFrame = vis.updateFrame(t,'left foot driving knee in', scale=0.2, visible=True, parent=self.pointcloudAffordance)
t = transformUtils.transformFromPose(np.array([-0.13194951, 0.89871423, 0.24956246]),
np.array([ 0.21589082, 0.91727326, -0.30088849, 0.14651633]))
self.leftFootOnPedal = vis.updateFrame(t,'left foot on pedal', scale=0.2, visible=True, parent=self.pointcloudAffordance)
t = transformUtils.transformFromPose(np.array([ 0.17712239, 0.87619935, 0.27001509]),
np.array([ 0.33484372, 0.88280787, -0.31946488, 0.08044963]))
self.leftFootUpFrame = vis.updateFrame(t,'left foot up frame', scale=0.2, visible=True, parent=self.pointcloudAffordance)
t = transformUtils.transformFromPose(np.array([ 0.47214939, -0.04856998, 0.01375837]),
np.array([ 6.10521653e-03, 4.18621358e-04, 4.65520611e-01,
8.85015882e-01]))
self.rightHandGrabFrame = vis.updateFrame(t,'right hand grab bar', scale=0.2, visible=True, parent=self.pointcloudAffordance)
self.frameSync = vis.FrameSync()
self.frameSync.addFrame(self.originFrame)
self.frameSync.addFrame(self.pointcloudAffordance.getChildFrame(), ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootEgressStartFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootEgressMidFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootEgressOutsideFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.steeringWheelAffordance.getChildFrame(), ignoreIncoming=True)
self.frameSync.addFrame(self.pedalAffordance.getChildFrame(), ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootPedalSwingFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootDrivingFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootDrivingKneeInFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootOnPedal, ignoreIncoming=True)
self.frameSync.addFrame(self.leftFootUpFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.rightHandGrabFrame, ignoreIncoming=True)
def start(self):
sub = lcmUtils.addSubscriber('PLAN_EXECUTION_STATUS', lcmdrc.plan_status_t, self.handleStatus)
sub.setSpeedLimit(0.2)
self.subs.append(sub)
self.subs.append(lcmUtils.addSubscriber('FOOTSTEP_PLAN_RESPONSE', lcmdrc.footstep_plan_t, self.handleFootstepPlan))
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 initSubscriber(self):
self.subscriber = lcmUtils.addSubscriber(self.channel, lcmvicon.vicon_t, self.onMessage)
self.subscriber.setSpeedLimit(10)
