def main():
app = ConsoleApp()
view = app.createView()
vis_item = {"current": None}
def handle_data(msg):
# msg = lcmdrake.lcmt_viewer_geometry_data.decode(msg_data)
side_length = int(round(np.power(len(msg.float_data), 1. / 3)))
data = np.reshape(msg.float_data,
(side_length, side_length, side_length))
# convert to a vtkImageData
image = numpyToVtkImage(data)
# compute iso contour as value 0.5
polyData = applyContourFilter(image, 0.0)
# show data
if vis_item["current"] is not None:
vis_item["current"].removeFromAllViews()
vis_item["current"] = vis.showPolyData(polyData, 'contour')
view.resetCamera()
lcmUtils.addSubscriber("FIELD_DATA", lcmdrake.lcmt_viewer_geometry_data,
handle_data)
# start app
view.show()
view.resetCamera()
app.start()
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 __init__(self, drillDemo, robotModel, playbackRobotModel, teleopRobotModel, footstepPlanner, manipPlanner, ikPlanner,
lhandDriver, rhandDriver, atlasDriver, multisenseDriver, affordanceFitFunction, sensorJointController,
planPlaybackFunction, showPoseFunction, cameraView, segmentationpanel):
self.drillDemo = drillDemo
self.robotModel = robotModel
self.playbackRobotModel = playbackRobotModel # not used inside the demo
self.teleopRobotModel = teleopRobotModel # not used inside the demo
self.footstepPlanner = footstepPlanner
self.manipPlanner = manipPlanner
self.ikPlanner = ikPlanner
self.lhandDriver = lhandDriver
self.rhandDriver = rhandDriver
self.atlasDriver = atlasDriver
self.multisenseDriver = multisenseDriver
self.affordanceFitFunction = affordanceFitFunction
self.sensorJointController = sensorJointController
self.planPlaybackFunction = planPlaybackFunction
self.showPoseFunction = showPoseFunction
self.cameraView = cameraView
self.segmentationpanel = segmentationpanel
self.pointerTracker = None
self.projectCallback = None
self.drillYawSliderValue = 0.0
self.segmentationpanel.init() # TODO: check with Pat. I added dependency on segmentationpanel, but am sure its appropriate
self.defaultGraspingHand = "left"
self.imageViewName = 'CAMERALHAND'
self.setGraspingHand(self.defaultGraspingHand)
self.TLDResultMsg = lcmdrc.image_roi_t()
self.tldsub = lcmUtils.addSubscriber('TLD_OBJECT_ROI_RESULT', lcmdrc.image_roi_t, self.TLDReceived)
self.targetsub = lcmUtils.addSubscriber('REACH_TARGET_POSE', bot_frames.update_t, self.TargetReceived)
self.autoMode = False
self.drill = Drill()
def main():
global app, view, nav_data
nav_data = np.array([[0, 0, 0]])
lcmUtils.addSubscriber(".*_NAV$", node_nav_t, handleNavData)
app = ConsoleApp()
app.setupGlobals(globals())
app.showPythonConsole()
view = app.createView()
view.show()
global d
d = DebugData()
d.addLine([0,0,0], [1,0,0], color=[0,1,0])
d.addSphere((0,0,0), radius=0.02, color=[1,0,0])
#vis.showPolyData(d.getPolyData(), 'my debug geometry', colorByName='RGB255')
startSwarmVisualization()
app.start()
def __init__(self):
self.app = ConsoleApp()
self.view = self.app.createView()
self.robotSystem = robotsystem.create(self.view)
jointGroups = getJointGroups()
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 main():
global app, view, nav_data
nav_data = np.array([[0, 0, 0]])
lcmUtils.addSubscriber(".*_NAV$", node_nav_t, handleNavData)
app = ConsoleApp()
app.setupGlobals(globals())
app.showPythonConsole()
view = app.createView()
view.show()
global d
d = DebugData()
d.addLine([0, 0, 0], [1, 0, 0], color=[0, 1, 0])
d.addSphere((0, 0, 0), radius=0.02, color=[1, 0, 0])
#vis.showPolyData(d.getPolyData(), 'my debug geometry', colorByName='RGB255')
startSwarmVisualization()
app.start()
def __init__(self):
self.app = ConsoleApp()
self.view = self.app.createView()
self.robotSystem = robotsystem.create(self.view)
jointGroups = getJointGroups()
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 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 _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))
def _setupSubscriptions(self):
lcmUtils.addSubscriber('PLAN_EXECUTION_STATUS', lcmdrc.plan_status_t, self.onControllerStatus)
lcmUtils.addSubscriber('ATLAS_BATTERY_DATA', lcmdrc.atlas_battery_data_t, self.onAtlasBatteryData)
lcmUtils.addSubscriber('ATLAS_ELECTRIC_ARM_STATUS', lcmdrc.atlas_electric_arm_status_t, self.onAtlasElectricArmStatus)
lcmUtils.addSubscriber('CONTROLLER_RATE', lcmdrc.message_rate_t, self.onControllerRate)
sub = lcmUtils.addSubscriber('ATLAS_STATUS', lcmdrc.atlas_status_t, self.onAtlasStatus)
sub.setSpeedLimit(60)
sub = lcmUtils.addSubscriber('ATLAS_STATE_EXTRA', lcmdrc.atlas_state_extra_t, self.onAtlasStateExtra)
sub.setSpeedLimit(5)
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 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 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 setEnabled(self, enabled):
if enabled and not self.subscriber:
self.subscriber = lcmUtils.addSubscriber('LCMGL.*',
callback=self.onMessage)
elif not enabled and self.subscriber:
lcmUtils.removeSubscriber(self.subscriber)
self.subscriber = None
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, 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, 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 _setupSubscriptions(self):
lcmUtils.addSubscriber('PLAN_EXECUTION_STATUS', lcmdrc.plan_status_t,
self.onControllerStatus)
lcmUtils.addSubscriber('ATLAS_BATTERY_DATA',
lcmdrc.atlas_battery_data_t,
self.onAtlasBatteryData)
lcmUtils.addSubscriber('ATLAS_ELECTRIC_ARM_STATUS',
lcmdrc.atlas_electric_arm_status_t,
self.onAtlasElectricArmStatus)
lcmUtils.addSubscriber('CONTROLLER_RATE', lcmdrc.message_rate_t,
self.onControllerRate)
sub = lcmUtils.addSubscriber('ATLAS_STATUS', lcmdrc.atlas_status_t,
self.onAtlasStatus)
sub.setSpeedLimit(60)
sub = lcmUtils.addSubscriber('ATLAS_STATE_EXTRA',
lcmdrc.atlas_state_extra_t,
self.onAtlasStateExtra)
sub.setSpeedLimit(5)
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 _setupOnce(self):
'''
This is setup code that is called that first time continuous walking is
executed.
'''
if self._setupComplete:
return
# use a different classifier to scott:
#footContactSubContinuous = lcmUtils.addSubscriber('FOOT_CONTACT_ESTIMATE_SLOW', lcmdrc.foot_contact_estimate_t, self.onFootContactContinuous)
#footContactSubContinuous.setSpeedLimit(60)
lcmUtils.addSubscriber('FOOTSTEP_PLAN_RESPONSE', lcmdrc.footstep_plan_t, self.onFootstepPlanContinuous)# additional git decode stuff removed
lcmUtils.addSubscriber('NEXT_EXPECTED_DOUBLE_SUPPORT', lcmdrc.footstep_plan_t, self.onNextExpectedDoubleSupport)
stepParamsSub = lcmUtils.addSubscriber('ATLAS_STEP_PARAMS', lcmdrc.atlas_behavior_step_params_t, self.onAtlasStepParams)
stepParamsSub.setSpeedLimit(60)
self.footstepsPanel.driver.applyDefaults('BDI')
def _setupOnce(self):
'''
This is setup code that is called that first time continuous walking is
executed.
'''
if self._setupComplete:
return
# use a different classifier to scott:
#footContactSubContinuous = lcmUtils.addSubscriber('FOOT_CONTACT_ESTIMATE_SLOW', lcmdrc.foot_contact_estimate_t, self.onFootContactContinuous)
#footContactSubContinuous.setSpeedLimit(60)
lcmUtils.addSubscriber('FOOTSTEP_PLAN_RESPONSE', lcmdrc.footstep_plan_t, self.onFootstepPlanContinuous)# additional git decode stuff removed
lcmUtils.addSubscriber('NEXT_EXPECTED_DOUBLE_SUPPORT', lcmdrc.footstep_plan_t, self.onNextExpectedDoubleSupport)
stepParamsSub = lcmUtils.addSubscriber('ATLAS_STEP_PARAMS', lcmdrc.atlas_behavior_step_params_t, self.onAtlasStepParams)
stepParamsSub.setSpeedLimit(60)
self.footstepsPanel.driver.applyDefaults('BDI')
def __init__(self, channel, statusBar=None):
self.sub = lcmUtils.addSubscriber(channel, lcmdrc.atlas_state_t, self.onAtlasState)
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 _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 __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,
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, 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 __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 __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 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, 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 init(self):
self.subscriber = lcmUtils.addSubscriber("POSE_BODY", lcmbotcore.pose_t, self.onBDIPose)
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 __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 __init__(self, outputConsole):
self.outputConsole = outputConsole
lcmUtils.addSubscriber('SYSTEM_STATUS', lcmdrc.system_status_t,
self.onSystemStatus)
camera = app.view.camera()
#camera.SetPosition([-.,.5,1.5])
camera.SetFocalPoint((.9*np.array(camera.GetFocalPoint())+.1*pos).tolist())
#camera.SetViewAngle(100)
camera.SetViewUp([0,0,1])
app.view.render()
if __name__=='__main__':
# use global so the variable is available in the python console
global app
app = DrakeVisualizerApp()
lcmUtils.addSubscriber('DRAW_POLYTOPE', polytopes_t, drawPolytope)
# to have the camera follow the camera log
#lcmUtils.addSubscriber('sbach_camera', vicon_pos_t, updateCamera)
# to have the camera follow the robot
quad_cam = QuadCamera()
lcmUtils.addSubscriber('DRAKE_VIEWER_DRAW', lcmdrake.lcmt_viewer_draw, quad_cam.onRobotDraw)
# to plot the strings obstacles course
setupStrings()
applogic.setBackgroundColor([0.3, 0.3, 0.35], [0.95,0.95,1])
app.mainWindow.show()
app.start()
def initICPCallback():
global _icpSub
_icpSub = lcmUtils.addSubscriber('MAP_LOCAL_CORRECTION',
lcmbotcore.rigid_transform_t,
onICPCorrection)
stat.update(len(messageData))
if channel not in channelToMsg:
channelToMsg[channel] = lcmspy.getMessageTypeFullName(
msgType) if msgType else '<unknown msg type>'
if timer.elapsed() > 3:
printStats()
timer.reset()
for stat in stats.values():
stat.reset()
#msg = lcmspy.decodeMessage(messageData)
sub = lcmUtils.addSubscriber(channel='.+', callback=onMessage)
sub.setNotifyAllMessagesEnabled(True)
from PythonQt import QtGui, QtCore
table = QtGui.QTableWidget()
table.setColumnCount(3)
table.setHorizontalHeaderLabels(['channel', 'type', 'bandwidth'])
table.verticalHeader().setVisible(False)
table.show()
app.start()
def onFootContact(msg):
leftInContact = msg.left_contact > 0.0
rightInContact = msg.right_contact > 0.0
for linkName, inContact in [['l_foot', msg.left_contact > 0.0], ['r_foot', msg.right_contact > 0.0]]:
if inContact:
robotHighlighter.highlightLink(linkName, [0, 0, 1])
else:
robotHighlighter.dehighlightLink(linkName)
#robotStateModel.model.setLinkColor(drcargs.getDirectorConfig()['leftFootLink'], contactColor if leftInContact else noContactColor)
#robotStateModel.model.setLinkColor(drcargs.getDirectorConfig()['rightFootLink'], contactColor if rightInContact else noContactColor)
footContactSub = lcmUtils.addSubscriber('FOOT_CONTACT_ESTIMATE', lcmdrc.foot_contact_estimate_t, onFootContact)
footContactSub.setSpeedLimit(60)
if useFallDetectorVis:
def onPlanStatus(msg):
links = ['pelvis', 'utorso']
if msg.plan_type == lcmdrc.plan_status_t.RECOVERING:
for link in links:
robotHighlighter.highlightLink(link, [1,0.4,0.0])
elif msg.plan_type == lcmdrc.plan_status_t.BRACING:
for link in links:
robotHighlighter.highlightLink(link, [1, 0, 0])
else:
for link in links:
robotHighlighter.dehighlightLink(link)
def setEnabled(self, enabled):
if enabled and not self.subscriber:
self.subscriber = lcmUtils.addSubscriber('LCMGL.*', callback=self.onMessage)
elif not enabled and self.subscriber:
lcmUtils.removeSubscriber(self.subscriber)
self.subscriber = None
def initICPCallback():
global _icpSub
_icpSub = lcmUtils.addSubscriber('MAP_LOCAL_CORRECTION', lcmbotcore.rigid_transform_t, onICPCorrection)
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):
sub0 = lcmUtils.addSubscriber('AUTONOMOUS_TEST_WALKING',
lcmdrc.utime_t, self.autonomousTest)
def startModelPublisherListener(modelsToReload):
global _modelsToReload
_modelsToReload = modelsToReload
lcmUtils.addSubscriber('ROBOT_MODEL', lcmdrc.robot_urdf_t, onModelPublisherString)
def startModelPublisherListener(modelsToReload):
global _modelsToReload
_modelsToReload = modelsToReload
lcmUtils.addSubscriber('ROBOT_MODEL', lcmdrc.robot_urdf_t, onModelPublisherString)
camera = app.view.camera()
# camera.SetPosition([-.,.5,1.5])
camera.SetFocalPoint((0.9 * np.array(camera.GetFocalPoint()) + 0.1 * pos).tolist())
# camera.SetViewAngle(100)
camera.SetViewUp([0, 0, 1])
app.view.render()
if __name__ == "__main__":
# use global so the variable is available in the python console
global app
app = DrakeVisualizerApp()
lcmUtils.addSubscriber("DRAW_POLYTOPE", polytopes_t, drawPolytope)
# to have the camera follow the camera log
# lcmUtils.addSubscriber('sbach_camera', vicon_pos_t, updateCamera)
# to have the camera follow the robot
quad_cam = QuadCamera()
lcmUtils.addSubscriber("DRAKE_VIEWER_DRAW", lcmdrake.lcmt_viewer_draw, quad_cam.onRobotDraw)
# to plot the strings obstacles course
setupStrings()
applogic.setBackgroundColor([0.3, 0.3, 0.35], [0.95, 0.95, 1])
app.mainWindow.show()
app.start()
def _setupSubscriptions(self):
sub0 = lcmUtils.addSubscriber('AUTONOMOUS_TEST_WALKING', lcmdrc.utime_t, self.autonomousTest)
stat.update(len(messageData))
if channel not in channelToMsg:
channelToMsg[channel] = lcmspy.getMessageTypeFullName(msgType) if msgType else '<unknown msg type>'
if timer.elapsed() > 3:
printStats()
timer.reset()
for stat in stats.values():
stat.reset()
#msg = lcmspy.decodeMessage(messageData)
sub = lcmUtils.addSubscriber(channel='.+', callback=onMessage)
sub.setNotifyAllMessagesEnabled(True)
from PythonQt import QtGui, QtCore
table = QtGui.QTableWidget()
table.setColumnCount(3)
table.setHorizontalHeaderLabels(['channel', 'type', 'bandwidth'])
table.verticalHeader().setVisible(False)
table.show()
def init(self):
self.subscriber = lcmUtils.addSubscriber('POSE_BODY',
lcmbotcore.pose_t,
self.onBDIPose)
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))
def __init__(self, onContactCallback, graspingHand):
lcmUtils.addSubscriber('WRIST_STRAIN_GAUGES', lcmdrc.atlas_strain_gauges_t, self.handleFTSignal)
self.onContactCallback = onContactCallback
self.graspingHand = graspingHand